木板抓取冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)求解

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

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

0 引言

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

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

1 冗余機(jī)械臂正運(yùn)動(dòng)學(xué)

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

表1 冗余機(jī)械臂D-H參數(shù)

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

Rot(z,θi)

(1)

(2)

2 冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)求解

2.1 θ4的求解

(3)

根據(jù)前述肘部自運(yùn)動(dòng)分析，關(guān)節(jié)4的角度θ4與臂型角無關(guān)，θ4直接由Δsew唯一確定，根據(jù)參考坐標(biāo)系0建立的特點(diǎn)，可直接得

(4)

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

(5)

(6)

2.2 臂平面為參考平面時(shí)的求解

(7)

由正運(yùn)動(dòng)學(xué)模型結(jié)合參考平面相關(guān)向量可知機(jī)械臂末端位置向量有如下關(guān)系：

(8)

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

(9)

對式(9)轉(zhuǎn)換：

(10)

(11)

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

(12)

(13)

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

當(dāng)臂平面繞著虛擬軸sw旋轉(zhuǎn)時(shí)，臂型角為φ，從參考臂型角到實(shí)際臂型角的自運(yùn)動(dòng)旋轉(zhuǎn)變換根據(jù)Rodrigues公式可得：

(14)

(15)

因此，可求得

(16)

又由D-H坐標(biāo)變換可得

(17)

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

(18)

2.4 θ5、θ6和θ7求解

由正運(yùn)動(dòng)學(xué)模型可知機(jī)械臂末端姿態(tài)矩陣有如下關(guān)系

(19)

(20)

而根據(jù)D-H坐標(biāo)變換又有

(21)

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

(22)

2.5 機(jī)械臂奇異點(diǎn)分析

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

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

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

3 逆解的優(yōu)選

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

已知兩個(gè)相鄰的軌跡點(diǎn)A和B，機(jī)械臂末端在當(dāng)前軌跡點(diǎn)A時(shí)時(shí)對應(yīng)的機(jī)械臂的7個(gè)關(guān)節(jié)角度為qj，機(jī)械臂末端在軌跡點(diǎn)B時(shí)對應(yīng)機(jī)械臂的逆解為qk，則需對在B點(diǎn)時(shí)的機(jī)械臂關(guān)節(jié)末端位姿進(jìn)行逆運(yùn)動(dòng)學(xué)求解并選解，以達(dá)到兩個(gè)時(shí)刻之間的行程最短，考慮到機(jī)械臂抓取木板運(yùn)動(dòng)過程中不同關(guān)節(jié)對末端位姿的影響，用加權(quán)重的構(gòu)型空間距離作為評價(jià)函數(shù)，有

d=‖Qj-Qk‖2。

(23)

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

4 實(shí)驗(yàn)

4.1 有效性測試

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

4.2 算法性能驗(yàn)證

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

(1)隨機(jī)生成一組關(guān)節(jié)角度q0。

(2)用該關(guān)節(jié)角度計(jì)算機(jī)械臂運(yùn)動(dòng)學(xué)正解M0。

(3)計(jì)算q0時(shí)刻的臂型角φ0。

(4)利用M0計(jì)算機(jī)械臂運(yùn)動(dòng)學(xué)逆解。若實(shí)驗(yàn)組為數(shù)值解方法，設(shè)置其算法最高迭代次數(shù)為120，如果120次迭代內(nèi)找到解，則返回逆解q，無返回則求解失??；若實(shí)驗(yàn)組為解析解方法，設(shè)定算法中的臂型角為φ0，所有返回的逆解組為qlist。

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

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

表2 不同方法實(shí)驗(yàn)結(jié)果對比

4.3 木板抓取應(yīng)用試驗(yàn)

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

(24)

(25)

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

5 結(jié)束語

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

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