服務機器人解耦球型手臂運動控制方法

謝艷艷，胡艷華，甘 戈

（1.內蒙古機電職業(yè)技術學院，內蒙古 呼和浩特 010051；2.內蒙古工業(yè)大學，內蒙古 呼和浩特 010051；3.中國科學院大學，北京 100049）

1 引言

根據(jù)市場需求，現(xiàn)有六自由度手臂由于在狹小的工作空間中，自由度少，靈活性差。七自由度手臂應運而生，符合人體工程學需求，具備人手所能實現(xiàn)的一些較為復雜的功能。針對六自由度手臂不足，新七自由度功能性手臂，其中肩部、肘部各2個自由度，手腕處3個自由度，通過各關節(jié)之間的運動配合實現(xiàn)所需功能。

然而該服務機器人手臂設計中，手腕是該機器人手臂靈活性的保障。近年來，國內外學者設計各種類型的手腕機制，具體可以分為三類:（1）球形的手腕，三個關節(jié)軸交點，位置和姿態(tài)的解耦，逆運動學分析很簡單，結構緊湊，繁重的工作比大，大多采用球形腕關節(jié)空間機器人和其他特殊場合。（2）非球面的手腕，不符合三軸，結構緊湊，運動是不可分割的。（3）平行結構的手腕，手腕并聯(lián)機構的實際小型化。

國內外學者對服務機器人手臂的研究如下：如針對文獻[3]提出的pitch-yaw-roll三自由度手腕機構存在pitch、yaw運動耦合干涉問題，文獻[4]提出了一種雙環(huán)解耦方法的新結構。國內某大學的科研人員對Trallfa手腕進行了分析和仿制，但是這種機械手腕的球齒是由分布在球冠上的凸齒和凹齒等特殊齒輪副組成（凹凸齒），存在傳動原理誤差且加工制造困難。文獻[6]提出一種基于斜齒輪的三自由度手腕。這種機械結構的第二條軸線與共線的一、三軸線存在一個斜角，繞每個軸線的轉動合成手腕三自由度的旋轉運動，這種結構存在受載荷時跳躍的問題。文獻[7]提出了一種由錐齒輪和萬向節(jié)傳動的三自由度機械手腕關節(jié)，但僅僅是停留在概念設計上。文獻[8]基于主動萬向節(jié)提出了雙主動萬向節(jié)并設計了兩自由度的擬人手腕關節(jié)。雙主動萬向節(jié)可以確保完全的自由滾轉運動并且可以主動控制關節(jié)的柔順性，手腕可以很好的模仿人類手部運動。然而這種結構只具有俯仰與偏航兩個自由度。基于上述情況，提出一種新型七自由度的服務機器人手臂。手臂的位置由前四軸共同決定。姿態(tài)由三自由度的三主動驅動萬向節(jié)手腕。該腕部結構緊湊且小巧靈活，可實現(xiàn)俯仰、偏擺，其顯著特點是可以實現(xiàn)大角度自轉運動。該服務機器人七自由度機械手臂適用于狹小作業(yè)空間，如開啟閥門等。可有效的應用于工業(yè)、航天等領域。

2 機械結構及工作空間

2.1 球形腕結構

肩關節(jié)、肘關節(jié)設計雙自由度機構，與原有4自由度手臂相同，只是在電機布置上有所差別，如圖1所示。電機通過錐齒輪傳動實現(xiàn)手臂外展，偏置電機通過直齒輪傳動實現(xiàn)手臂前后擺動[9-10]。肘關節(jié)與肩關節(jié)結構原理相同。末端安裝與靈巧手等外圍部件的安裝接口，通過偏心法蘭對直齒輪傳動系統(tǒng)進行消隙。手臂內置肘部電機與腕部電機的驅動板卡，便于后期的調試與維護。同時手臂內可安裝觸摸傳感器模塊，通過對人體的感知可做出相應的語音或表情等動作，提高與人的互動性與趣味性。

2.2 球形腕結構

三自由度手腕關節(jié)的3D模型外形與剖切圖，如圖2所示。

圖2 手腕3D模型外形圖Fig.2 Wrist Outline Drawing 3D Model

新型手臂機構設計，主要包括2自由度肩部、2自由度肘部、3自由度腕部機構。肩部采用直齒輪傳動系統(tǒng)與錐齒輪傳動系統(tǒng)。通過偏心法蘭對直齒輪傳動系統(tǒng)進行消隙，通過調整套筒對錐齒輪傳動系統(tǒng)進行消隙。兩個自由度均安裝機械限位機構，肘部結構與肩部結構相同。腕部利用內外雙萬向節(jié)實現(xiàn)手腕自轉，利用上下半球的耦合運動實現(xiàn)手腕的側擺與俯仰。三個電機后置，通過直齒輪傳動系統(tǒng)傳遞動力。

3 運動學正分析

3.1 手臂分析

轉換矩陣（1～4），根據(jù)各個關節(jié)的空間幾何關系可以獲取到相鄰兩個坐標系的變換矩陣，如下所示：

3.2 手腕求解

手腕末端輸出的位姿由手腕的三個自由度決定。在手腕球心建立坐標系，如圖3所示。

圖3 坐標系的建立及運動參數(shù)Fig.3 The Establishment of Coordinate System and the Motion Parameters

θ2為上下兩半球轉動而在傾角為的斜面上的角度差φ=22°，其式（1）為：

4 運動學反分析

4.1 傳統(tǒng)方法缺點

一般來說傳統(tǒng)的求解機械臂各個軸的順序為θ4?θ3?θ2?θ1?θ6、θ5、θ7，在每個步驟中，都可能存在多個符合條件的角導致最終解出多組解，可以根據(jù)機械限位的情況篩選掉不在此范圍內的解。

由此可知本方法存在如下問題

（1）由于本算法限定了第三軸，的關節(jié)值為機械限位的中間值，舍棄了第三軸的其他可能性，導致反解所得的解中不一定包含示教操作過程中所記錄的真實關節(jié)值；

（2）當機械臂接近于豎直立起的狀態(tài)時會導致末端位姿的X、Y兩軸坐標接近于0，反解算法中的計算過程中有分母接近于零的情況，導致無法得出七個軸的可行解；需要限定示教操作過程中防止第二關節(jié)值逼近及，第四關節(jié)、第六關節(jié)防止逼近、和；

（3）第三軸關節(jié)值選取為軟限位的中位數(shù)，并不一定最為合適，可以根據(jù)經(jīng)驗調整第三軸關節(jié)值，使姿態(tài)更加自然。

4.2 牛頓迭代解手臂

通常情況下求解非線性方程組，方程數(shù)與未知數(shù)個數(shù)相等，對于非線性方程組。

為非線性方程組第k次迭代近似值。則計算第k+1次迭代值得牛頓迭代格式為式（2）：

通過牛頓迭代法，可以求得 θ1、θ2、θ3、θ4。

其中注意牛頓迭代的注意事項為：

此方法計算的結果完全依賴于初值的選取，而牛頓法的初值無法準確得到，故此方法僅僅在初值離解較近的時候效果較好；其他位置可能迭代出不同的結果，盡管其可能滿足末端位姿約束條件。

4.3 解析求解球形腕

沿著末端輸出軸線滾轉的第三個自由度相對于其他兩自由度獨立，可單獨考慮手腕的俯仰-偏航兩自由度。在球心用RPY角描述坐標系Σ3相對于Σ0的位姿有：先繞y軸旋轉δ角，最后繞z軸旋轉γ角；

4.4 算法驗證

對于機器人主要行走的軌跡應該為直線或正弦曲線，以正弦曲線為例，保持機器人姿態(tài)不變，末端按照正弦曲線運動10個周期（20π），得到末端軌跡，如圖4所示。

圖4 機器人運動曲線Fig.4 Robot Motion Curve

圖中紅色為預期的軌跡，而藍色為實際跟蹤的軌跡，最前端的藍色直線是由于跟蹤的軌跡和預期的軌跡。可以看到藍色與紅色是絕對重合的，驗證了運動學的準確性。

5 軟件設計流程

5.1 電機控制方案

直流電機采用H橋驅動。支持雙極性PWM和單極性PWM控制。控制環(huán)結構，如圖5所示。

圖5 電機控制方案Fig.5 Motor Control Scheme

5.2 軟件流程與模塊劃分

圖6 電機控制功能軟件流程Fig.6 Motor Control Function Of The Software Process

電機控制功能軟件流程，如圖6所示。（1）編碼器位置、速度采樣：通過DSP外部總線接口訪問FPGA數(shù)據(jù)，讀取多圈值、單圈值、速度計算間隔；（2）電流采樣：PWM中間點，即計數(shù)器PRD，觸發(fā)AD采樣。AD對每路電流對稱采樣四次求平均，共分配12路AD通道。采樣完成后進入AD中斷處理數(shù)據(jù)，并進行控制環(huán)計算；（3）PWM輸出：SVPWM算法輸出控制電壓，PWM配置成三相雙極性控制模式；（4）SPI通信的EEPROM：直接移植SPI通信程序，軟件中使用任務線程處理EEPROM的讀寫；（5）數(shù)字IO：數(shù)字IO并不直接連接DSP，而是連接FPGA。DSP通過總線地址的方式，輸出8位IO信值控制LED，讀入驅動器軸號對應的4位IO值；（6）CAN通信：直接移植CAN通信驅動程序；（7）串口SCI通信：直接移植SCI通信驅動程序。

6 結論

針對服務機器人七自由度手臂機械結構進行分析。首先利用MDH法建立了轉換坐標系，得到了各關節(jié)連桿的相對關系。其次，作為機器人底層核心技術之一的逆運動學算法一直受到硬件性能有限和迭代的初值選取難易控制等因素的制約。提出了一種基于牛頓迭代法反向解析機器人手臂。在提出的七自由度手臂其腕關節(jié)結構緊湊，重量輕、靈活運動等等，可以提高機器人的工作性能在狹窄的環(huán)境限制，特別是能夠實現(xiàn)大角度旋轉。最后闡述了軟件設計的流程，對于機器人的程序搭建有著重要的意義。