徐呈藝，劉 英，賈民平，肖 軼,曹 健

(1.南京林業(yè)大學 機械電子工程學院，江蘇 南京 210037；2.東南大學 機械工程學院，江蘇 南京 211189；3.南通職業(yè)大學 機械工程學院，江蘇 南通 226007)

0 引言

機械臂逆運動學是路徑規(guī)劃和伺服控制的基礎。冗余自由度機械臂同一末端位姿會對應無數組運動學逆解[1]，具有較好的靈活性和避障能力[2-4]，可以更好地應用于工業(yè)、農業(yè)和服務業(yè)。但逆運動學求解比較困難[5]，目前國內外對其進行了大量的研究，主要包括代數數值法如雅可比矩陣偽逆法[6-7]、二次規(guī)劃反向算法[8]；迭代算法如神經網絡算法[9-10]、粒子群算法[11-12]、加權最小范數法[13]等；退化自由度法[14-15]；解析法[16-17]。文獻[6-8]的代數數值法普遍都存在精度不高的問題，且每次只返回一個解，無法確定全部解。文獻[9-13]的迭代方法運算量大、求解速度慢，不能滿足機器人實時伺服控制要求；文獻[14-15]的退化自由度法降低了機械臂的靈活性，失去了冗余機械臂的主要優(yōu)點；文獻[16-17]解析法求解過程采用位姿分離法，但未實現(xiàn)冗余度解耦，求解不全面；文獻[18]引入自運動建立前3個關節(jié)姿態(tài)單位向量，求解后再進行逆運動運算；文獻[19]引入自運動結合機械臂方向角以及人臂特性進行求解關節(jié)變量，求解較復雜。目前，引入自運動的研究技術文獻中在臂型參考面時對前3個關節(jié)姿態(tài)矩陣求解較復雜，奇異點分析較少。

本文以用于木板抓取的七自由度擬人機械臂為對象，充分考慮七自由度機械臂的幾何構型與向量特性，在建立正運動學模型的基礎上，對該類機械臂逆運動學求解引入臂型角，并采用機械臂自運動結合位姿分離方法，其中由機械臂關節(jié)末端位置向量求解前4個關節(jié)角，由關節(jié)末端矩陣求解后3個關節(jié)角，分析機械臂奇異點，并對多組逆解進行優(yōu)選。通過控制機械臂沿給定軌跡運動，對比其他算法時效與成功率等，驗證該算法的性能，最后應用于木板抓取實例，驗證其實用效果。

1 冗余機械臂正運動學

機械臂正運動學是后續(xù)逆運動學求解的基礎。仿人雙臂七軸冗余機械臂外形如圖1所示，左右兩臂完全對稱，關節(jié)都為轉動副，以其左臂作為研究對象。采用D-H法[20-22]建立關節(jié)坐標系，如圖2所示，其中(Xb,Zb)為機械臂基座坐標系，(X0,Z0)為機械臂參考坐標系，(Xi,Zi)為機械臂關節(jié)i(i=1～7)的坐標系，(Xt,Zt)為機械臂工具坐標系，有D-H參數如表1所示。表中：ai為沿著xi方向從zi到zi+1的距離；αi為繞著xi方向從zi到zi+1的角度；di為沿著zi方向從xi-1到xi的距離；θi為繞著zi方向從xi-1到xi的角度。

表1 冗余機械臂D-H參數

D-H參數可以用來描述相鄰兩個關節(jié)之間的變換關系，則相鄰兩個關節(jié)的變換矩陣為

Rot(z,θi)

(1)

(2)

2 冗余機械臂逆運動學求解

2.1 θ4的求解

(3)

根據前述肘部自運動分析，關節(jié)4的角度θ4與臂型角無關，θ4直接由Δsew唯一確定，根據參考坐標系0建立的特點，可直接得

(4)

其中dse=d3，dew=d5，可解得

(5)

(6)

2.2 臂平面為參考平面時的求解

(7)

由正運動學模型結合參考平面相關向量可知機械臂末端位置向量有如下關系：

(8)

將式(7)代入上式展開運算則有：

(9)

對式(9)轉換：

(10)

(11)

將(7)式代入式(8)展開運算有

(12)

(13)

2.3 θ1、θ2和θ3的求解

當臂平面繞著虛擬軸sw旋轉時，臂型角為φ，從參考臂型角到實際臂型角的自運動旋轉變換根據Rodrigues公式可得：

(14)

(15)

因此，可求得

(16)

又由D-H坐標變換可得

(17)

聯(lián)立式(16)和式(17)可解得

(18)

2.4 θ5、θ6和θ7求解

由正運動學模型可知機械臂末端姿態(tài)矩陣有如下關系

(19)

(20)

而根據D-H坐標變換又有

(21)

聯(lián)立式(20)和式(21)可解得

(22)

2.5 機械臂奇異點分析

當θ4只有一個解時，即式(5)與式(6)相等，此時θ4=π/2，關節(jié)3與關節(jié)5旋轉軸線重合，自由度減少，故為奇異點，θ3與θ5無法求解，但它們的和可求。

當θ2=0時，式(18)的θ1與θ3無法求解，此時關節(jié)1和關節(jié)3旋轉軸線重合，自由度減少，故為奇異點，θ1與θ3的和可求。

當θ6=0時，式(22)的θ5與θ7無法求解，此時關節(jié)5和關節(jié)7旋轉軸線重合，自由度減少，故為奇異點，θ5與θ7的和可求。

3 逆解的優(yōu)選

在給定機械臂關節(jié)末端位姿與臂型角的情況下，根據前面分析的逆解求解過程一般有8組解，如圖4所示。而實際上，當機械臂達到目標位姿狀態(tài)時，其各關節(jié)角度是確定的。8組解在不考慮限位情況下都可以滿足要求，只是每組的相同關節(jié)會存在不同轉角。為了保證機械臂在抓取木板時，各關節(jié)軌跡連續(xù)，不發(fā)生突變，就要各關節(jié)跨度盡量小，即達到最短行程。

已知兩個相鄰的軌跡點A和B，機械臂末端在當前軌跡點A時時對應的機械臂的7個關節(jié)角度為qj，機械臂末端在軌跡點B時對應機械臂的逆解為qk，則需對在B點時的機械臂關節(jié)末端位姿進行逆運動學求解并選解，以達到兩個時刻之間的行程最短，考慮到機械臂抓取木板運動過程中不同關節(jié)對末端位姿的影響，用加權重的構型空間距離作為評價函數，有

d=‖Qj-Qk‖2。

(23)

式中Q=(q1,q2,q3,2q4,2q5,3q6,3q7)。由于在對應軌跡B點時機械臂逆解Qk有多組，其中d值最小對應的一組機械臂逆解即為本文所選出的機械臂最優(yōu)逆解。

4 實驗

4.1 有效性測試

在機器人操作系統(tǒng)(Robot Operating System, ROS)中，建立了木板抓取機械臂運動學模型，冗余機械臂逆運動學具體求解過程如圖5所示。首先由給定機械臂末端位姿和初始臂型角求解機械臂各關節(jié)角，臂型角初始值可設置為φ=0.785 rad，再根據構型空間最短距離對逆解優(yōu)選，然后判斷各關節(jié)角是否在最大范圍內，滿足要求則輸出關節(jié)角，否則重新調整臂型角并進行逆解計算與優(yōu)選。根據研究的逆運動學求解方法在臂型角固定的情況下控制機械臂，驗證機械臂末端是否沿著給定的圓軌跡運動。當機械臂運動完后，可在3D可視化模塊Riviz中顯示其末端軌跡，如圖6所示。機械臂運動過程中各關節(jié)角度即逆解的變化如圖7所示，整個運動過程動作平穩(wěn)流暢。在給定軌跡上采樣64個點，按照前述方法計算機械臂逆解并優(yōu)選，通過優(yōu)選的逆解得到機械臂關節(jié)末端位置，與給定軌跡上對應采樣到的到軌跡點對比，如圖8所示。給定軌跡點與逆解軌跡點匹配，即機械臂可以按照圖5的方法找到沿著給定軌跡運動的關節(jié)逆解，同時軌跡點的平均位置誤差為4.35E-10 mm，說明研究的逆解求解方法正確且精度較高。

4.2 算法性能驗證

為進一步驗證研究的逆解算法的正確性，同時校驗算法的性能，使用常見的幾種算法求解逆運動學：雅可比數值解(Jacob-inv)[7]、TRAC-IK求解器[8]、Orocos項目中的KDL求解器[17]，以及本文研究的解析解算法(I-analytical)進行對比。實驗過程如下：

(1)隨機生成一組關節(jié)角度q0。

(2)用該關節(jié)角度計算機械臂運動學正解M0。

(3)計算q0時刻的臂型角φ0。

(4)利用M0計算機械臂運動學逆解。若實驗組為數值解方法，設置其算法最高迭代次數為120，如果120次迭代內找到解，則返回逆解q，無返回則求解失?。蝗魧嶒灲M為解析解方法，設定算法中的臂型角為φ0，所有返回的逆解組為qlist。

(5)如表2，成功率記錄機械臂逆運動學求解的成功概率；平均耗時記錄從計算開始所耗費的時間；平均位置誤差是首先利用得到的逆解結果計算運動學正解，再與M0進行對比，并記錄位置誤差；找到q0占比(即完備性)，判斷找到的逆解里是否有初始的q0。

(6)所有實驗進行1萬次。實驗結果如表2所示，本文算法相對其他算法平均耗時少100倍以上、成功率更高，達到100%、平均精度達到3.75E-10 mm，所求到的機械臂逆解實際只取決于計算過程中的計算機程序所取數值精度。另本文方法能找到機械臂末端某一位姿對應的所有逆解。

表2 不同方法實驗結果對比

4.3 木板抓取應用試驗

機械臂關節(jié)末端當前位姿與視覺傳感器識別的目標木板位姿之間需要插值形成機械臂末端軌跡。設當前機械臂位姿T0=(R0,p0)，需在t時刻運動到目標位姿Tt=(Rt,pt)實現(xiàn)木板抓取，為了位姿過渡更自然協(xié)調，設置位置和姿態(tài)插值分別如式(24)和式(25)所示：

(24)

(25)

插值后得到一系列位姿軌跡點，在相鄰的軌跡點之間采用前述的逆解優(yōu)選方法，根據機械臂與木板的實際位置，輸出關節(jié)角控制機械臂運動實現(xiàn)木板抓取。在ROS系統(tǒng)中設置不同的目標木板位姿，進行1 000次實驗，抓取木板均能成功，成功率為95.6%，其過程如圖9所示，機械臂運行自動、快速、協(xié)調和平穩(wěn)，達到要求。

5 結束語

本文充分考慮了用于木板抓取的七自由度機械臂幾何構型與肘部自運動特性，采用D-H法建立了有利于逆運動學求解的關節(jié)坐標系和相應的正運動學數學模型。引入臂型角并將機械臂自運動結合位姿分離法對機械臂逆運動學進行求解。由機械臂關節(jié)末端位置向量求得第4個關節(jié)角和前3個關節(jié)角的參數化解，由關節(jié)末端姿態(tài)矩陣結合已求得的關節(jié)角得到后3個關節(jié)角，同時進行奇異點分析，并通過加權重的構型空間距離作為評價函數對逆解優(yōu)選。該求解過程具有直觀簡便等特點。

通過控制機械臂沿給定軌跡運動并對比其他算法等試驗，結果表明該方法成功率更高、運算量更小、速度更快，平均運行時間0.001 21 ms左右，實驗結果精度更高，機械臂末端位置平均誤差為3.75E-10 mm左右。應用該方法在不同目標位姿進行木板抓取試驗，機械臂在抓取過程中運行平穩(wěn)流暢，抓取目標成功率高，在冗余機械臂視覺伺服等實時控制中有較好的實用意義，如通過視覺識別待抓取目標位姿，通過本文算法實現(xiàn)機械臂末端高效的定位操作。