基于旋量理論的六自由度林果采摘混聯(lián)機(jī)械臂運(yùn)動學(xué)逆解

李立君，劉 濤，高自成，廖 凱，李禹卓，許世斌

?

基于旋量理論的六自由度林果采摘混聯(lián)機(jī)械臂運(yùn)動學(xué)逆解

李立君，劉 濤，高自成，廖 凱，李禹卓，許世斌

（中南林業(yè)科技大學(xué)機(jī)電工程學(xué)院，長沙 410000）

針對傳統(tǒng)Paden-Kahan子問題求解機(jī)械臂運(yùn)動學(xué)逆解時需確定關(guān)節(jié)軸線交點(diǎn)坐標(biāo)的問題，對該子問題進(jìn)行改進(jìn)。利用末端執(zhí)行器位姿信息獲取軸線交點(diǎn)坐標(biāo)，建立物體坐標(biāo)與末端執(zhí)行器期望位姿的映射關(guān)系，結(jié)合子問題求解6自由度林果采摘機(jī)械臂運(yùn)動學(xué)逆解；根據(jù)主動關(guān)節(jié)變量取值范圍分析所求逆解的可行性，得到可行封閉解，提高機(jī)械臂控制速度、穩(wěn)定性和準(zhǔn)確性。在實驗室環(huán)境下利用所提出的算法求解10組工作目標(biāo)位置信息對應(yīng)的關(guān)節(jié)值，結(jié)果表明，所求逆解能使林果采摘機(jī)械臂到達(dá)正確位姿，末端執(zhí)行器最大位置誤差不超過夾持器最大開度的3.30%，最大姿態(tài)誤差不超過1°，滿足采摘要求。該算法為機(jī)械臂快速、穩(wěn)定及精確地控制提供技術(shù)依據(jù)。

機(jī)器人；收獲機(jī)；運(yùn)動學(xué)；旋量理論；逆運(yùn)動學(xué)；Paden-Kahan子問題；混聯(lián)機(jī)械臂；林果采摘

0 引 言

機(jī)械臂逆運(yùn)動學(xué)求解方法常與正運(yùn)動學(xué)建模方法有關(guān)。目前機(jī)械臂運(yùn)動學(xué)建模主要使用Denavit-Hartenberg（D-H）參數(shù)法、旋量法與四元數(shù)法，并有相應(yīng)的逆運(yùn)動學(xué)求解方法[1-4]。若基于旋量理論建立正運(yùn)動學(xué)模型，則常用Paden-Kahan子問題求解。Paden-Kahan子問題由Paden基于指數(shù)積公式提出[5]，該算法利用特殊點(diǎn)消去運(yùn)動學(xué)模型中的部分關(guān)節(jié)變量，使整個逆運(yùn)動學(xué)分析過程分解成若干子問題，所得子問題具有明確的幾何意義且能有效避免引入局部坐標(biāo)系所帶來的奇異性[6-7]，從而簡化求解。根據(jù)不同劃分，可得到不同子問題組合，因此Paden-Kahan子問題算法相對其他算法有較大靈活性。除此之外，運(yùn)用Paden-Kahan子問題求機(jī)械臂運(yùn)動學(xué)逆解還具有計算速度快、數(shù)值解穩(wěn)定的優(yōu)點(diǎn)[8-9]。因此，本文使用Paden-Kahan子問題分析林果采摘混聯(lián)機(jī)械臂逆運(yùn)動學(xué)問題。Murray等對子問題進(jìn)行了整理，提出了6類子問題[10]，并給出了其中最典型3類的解法。

國內(nèi)外已有學(xué)者對Paden-Kahan子問題在串、并聯(lián)機(jī)械臂運(yùn)動學(xué)逆解方面的應(yīng)用進(jìn)行了研究[11-13]。Sariyildiz等[14]將四元數(shù)與子問題算法相結(jié)合，對6R串聯(lián)機(jī)器人進(jìn)行運(yùn)動學(xué)逆解分析，并與D-H法做對比。結(jié)果表明Paden-Kahan子問題的求解速度比傳統(tǒng)D-H參數(shù)法快一倍。Gao等[15]利用經(jīng)典子問題對蟑螂仿生機(jī)器人逆運(yùn)動學(xué)進(jìn)行了求解，并給出了直線行走和定點(diǎn)旋轉(zhuǎn)運(yùn)動的關(guān)節(jié)運(yùn)動路徑，通過試驗驗證了Paden-Kahan子問題的精確性。陳慶誠等[16]針對3條不相交的關(guān)節(jié)軸線提出了一種新的子問題，對其進(jìn)行求解并應(yīng)用于6R串聯(lián)機(jī)器人逆運(yùn)動學(xué)分析，在一定程度上擴(kuò)大了子問題應(yīng)用范圍。Wang等[17]通過代數(shù)方法解決了2關(guān)節(jié)軸線互異的子問題2（即剛體依次繞2軸分別旋轉(zhuǎn)2個角度得到新的位置，求解子問題2即是求解這兩個角度。經(jīng)典Paden-Kahan子問題2必須要求兩軸相交），使子問題2求解對象擴(kuò)展至任意兩相鄰旋轉(zhuǎn)關(guān)節(jié)，并對5R串聯(lián)機(jī)器人進(jìn)行了逆運(yùn)動分析。試驗和仿真結(jié)果表明利用改進(jìn)Paden-Kahan子問題算法求解精度是徑向基函數(shù)神經(jīng)網(wǎng)絡(luò)（radial basis function neural network，RBFNN）算法的5倍，求解速度比RBFNN算法快4倍。

結(jié)合上述文獻(xiàn)，發(fā)現(xiàn)旋量理論在機(jī)械臂運(yùn)動學(xué)逆解問題的應(yīng)用中還存在以下問題：1）研究對象主要為串聯(lián)或并聯(lián)機(jī)械臂，旋量理論在混聯(lián)機(jī)械臂的運(yùn)動學(xué)逆解問題的應(yīng)用較少，僅通過單一的方法無法求得混聯(lián)機(jī)械臂運(yùn)動學(xué)逆解。2）求解過程中常運(yùn)用旋轉(zhuǎn)運(yùn)動不改變旋轉(zhuǎn)軸上點(diǎn)位置的原理利用腕關(guān)節(jié)位置矢量化簡運(yùn)動學(xué)方程組，但腕關(guān)節(jié)位置矢量本就是未知量，上述文獻(xiàn)并未給出有效的解決方法。3）求解時并未考慮關(guān)節(jié)變量取值范圍，致使本來為高度非線性的運(yùn)動學(xué)逆解問題引入非必要的運(yùn)算，降低機(jī)械臂運(yùn)動控制算法的運(yùn)行效率。

為增強(qiáng)Paden-Kahan子問題在機(jī)械臂控制上的實用性，結(jié)合混聯(lián)林果采摘機(jī)械臂特點(diǎn)，本文將其混聯(lián)結(jié)構(gòu)等效轉(zhuǎn)化為串聯(lián)結(jié)構(gòu)；推導(dǎo)機(jī)械臂位姿與腕關(guān)節(jié)位置的關(guān)系，結(jié)合Paden-Kahan子問題算法求解6自由度混聯(lián)機(jī)械臂運(yùn)動學(xué)逆解；根據(jù)關(guān)節(jié)空間范圍分析得到可行封閉解；根據(jù)機(jī)采摘機(jī)械臂工況，建立目標(biāo)位姿與機(jī)械臂期望位姿映射以獲取物體坐標(biāo)到關(guān)節(jié)空間的完整對應(yīng)關(guān)系；最后開展試驗驗證所提方法的正確性與精確性。以期為對機(jī)械臂進(jìn)行軌跡規(guī)劃、運(yùn)動控制和動力學(xué)分析提供參考。

1 林果采摘機(jī)械臂及旋量運(yùn)動模型

1.1 旋量理論與剛體運(yùn)動

旋量是一個幾何實體，通過旋量坐標(biāo)能描述任何繞某一軸旋轉(zhuǎn)和沿該軸移動的合成運(yùn)動[18-19]。Chalse已證明剛體運(yùn)動能分解為上述合成運(yùn)動[10]，即旋量運(yùn)動。旋量坐標(biāo)由特殊歐式群的李代數(shù)se(3)的6×1向量形式表示，具體表達(dá)式如下[20-21]

式中為6×1旋量坐標(biāo)，為旋量軸線的方向余弦，￠為旋量軸線上任選一點(diǎn)￠的位置矢量，為旋距。

當(dāng)關(guān)節(jié)為轉(zhuǎn)動副時，=0；當(dāng)關(guān)節(jié)為移動副時，趨于無窮大。在這2種極端情況下，旋量坐標(biāo)對應(yīng)李代數(shù)se(3)的標(biāo)準(zhǔn)4×4矩陣形式表示為

旋量坐標(biāo)可以通過如下步驟確定：

1）選擇一個合適的結(jié)構(gòu)參考位形。

2）確定此位形下每個關(guān)節(jié)軸線的方向余弦矢量及軸線上任選點(diǎn)￠的位置矢量￠。

3）根據(jù)關(guān)節(jié)運(yùn)動類型，由式（2）計算旋量坐標(biāo)。

剛體從位形一次運(yùn)動到位形的仿射矩陣由指數(shù)映射給出[10]：

式中為4×4單位矩陣，為關(guān)節(jié)變量值，為展開階數(shù)。

式（3）說明了一次剛體運(yùn)動由表示運(yùn)動方向的和表示沿該方向的運(yùn)動量組成。若剛體連續(xù)做多次螺旋運(yùn)動，經(jīng)由個關(guān)節(jié)串聯(lián)成的運(yùn)動鏈從位形運(yùn)動到位形，則這一系列的剛體運(yùn)動用指數(shù)積公式（product of exponential，POE）描述[22]：

林果采摘機(jī)械臂結(jié)構(gòu)簡圖與參考位形如圖1所示，對機(jī)械臂主運(yùn)動鏈?zhǔn)褂弥笖?shù)積公式可以得到其末端位姿為[23]

本文所用正運(yùn)動學(xué)模型建立在文獻(xiàn)[23]研究基礎(chǔ)之上，具體正運(yùn)動學(xué)建模過程可參考文獻(xiàn)[23]。

1.2 機(jī)械臂結(jié)構(gòu)分析

如式（5）所示，機(jī)械臂主運(yùn)動鏈有8個活動關(guān)節(jié)，而機(jī)械臂自由度為6。為簡化求解過程，可將其并聯(lián)移動結(jié)構(gòu)等效為串聯(lián)轉(zhuǎn)動結(jié)構(gòu)，因此需通過分析其幾何特征獲得主動移動關(guān)節(jié)與被動轉(zhuǎn)動關(guān)節(jié)之間關(guān)系。機(jī)械臂結(jié)構(gòu)參數(shù)如表1所示：

注：ξi為第i軸運(yùn)動旋量坐標(biāo)（i=1,2,…,11）；A為連桿鉸接點(diǎn)，a為連桿長度或桿間距，mm；其余同理；Os–XsYsZs為基礎(chǔ)坐標(biāo)系；Ot–XtYtZt為末端執(zhí)行器坐標(biāo)系；Pw為腕關(guān)節(jié)點(diǎn)；Pf1和Pf2為指關(guān)節(jié)點(diǎn)；Pt1和Pt2為工作目標(biāo)上的標(biāo)記點(diǎn)；pt為工作目標(biāo)位置矢量；pn為垂直于工作目標(biāo)標(biāo)線的位置矢量；如無特殊說明，后文均使用P表示剛體上的點(diǎn)，p表示該點(diǎn)的位置矢量，不同點(diǎn)通過下標(biāo)區(qū)分。

表1 機(jī)械臂結(jié)構(gòu)參數(shù)

注：θ表示沿方向的運(yùn)動量（=1,2,6,7,8,9）。

Note:θrepresents the distance of rigid body motion along, (=1,2,6,7,8,9).

結(jié)合圖1可知，、￠￠及￠￠為3個平行四邊形。對于平行四邊形結(jié)構(gòu)，其連桿上4個關(guān)節(jié)所轉(zhuǎn)過角度相同，于是有：

表2 機(jī)械臂在參考位形下的旋量參數(shù)

依次選取點(diǎn)和點(diǎn)為結(jié)點(diǎn)，由指數(shù)積公式（4）得到結(jié)構(gòu)方程：

前3個方程組相互獨(dú)立，解得：

2 逆運(yùn)動學(xué)求解

2.1 求解θ1、θ2與θ9

逆運(yùn)動分析解決的問題是根據(jù)末端執(zhí)行器的期望位姿來獲得一組或若干組關(guān)節(jié)變量，從而控制末端執(zhí)行器到達(dá)該位姿。因此末端執(zhí)行器位姿矩陣是已知的：

式中(=1,2,3)為末端執(zhí)行器方向余弦，為末端執(zhí)行器位置矢量。

根據(jù)旋量代數(shù)的基本性質(zhì)，當(dāng)旋矩=0時，若在關(guān)節(jié)軸線上取一點(diǎn)，旋轉(zhuǎn)作用對該點(diǎn)無效，即：

為求解上式，需先獲得點(diǎn)的坐標(biāo)，但在實際控制中，已知的只有末端執(zhí)行器位姿，腕關(guān)節(jié)位置無法直接獲取，從而對求解方程（13）造成困難。如圖1所示，注意到腕關(guān)節(jié)與末端執(zhí)行器的位置之間有如下關(guān)系：

根據(jù)圖1可得在參考位形的位置矢量為：

將式（14）和（15）帶入式（13）后展開得到：

根據(jù)表1所示關(guān)節(jié)變量取值范圍及連桿尺寸

聯(lián)立（9）、（10）、（16）與（17）可得到方程組（16）的解：

2.2 求解θ6和θ7

(0)根據(jù)圖1得到：

根據(jù)引入的中間點(diǎn)可以將上述子問題2分解為2個子問題1：

由此解得關(guān)節(jié)變量6和7的值：

2.3 求解θ8

8通過構(gòu)造子問題1求解，選取不在8上的點(diǎn)1，從圖1中可以得到關(guān)系：

1在參考位形中的位置矢量為

求解可得8：

16=cos(1+6)，16=sin(1+6)

3 期望位姿獲取及可行解分析

3.1 目標(biāo)姿態(tài)與末端執(zhí)行器姿態(tài)映射建立

林果采摘機(jī)械臂用于采摘油茶果，考慮到油茶果花果同期的特點(diǎn)，只要合理控制振動機(jī)構(gòu)的振幅和頻率，振動式采摘相對齒梳式、剪切式等采摘方法有較高采摘效率與較低落花率[24]，因此采摘機(jī)械臂工作方式為：通過末端執(zhí)行器夾持油茶果樹干，依靠末端執(zhí)行器上的偏心機(jī)構(gòu)產(chǎn)生振動力實現(xiàn)采摘。根據(jù)其工作方式，所求逆運(yùn)動學(xué)解應(yīng)使末端執(zhí)行器到達(dá)樹干位置并與樹干垂直。

如圖1所示，設(shè)檢測系統(tǒng)已經(jīng)測量到樹干上2標(biāo)記點(diǎn)的位置：p1=(p1xp1yp1z)T，p2=(p2xp2yp2z)T。并認(rèn)為2位于沿樹干方向更高位置，樹干在機(jī)械臂坐標(biāo)系下的6×1普呂克（plücker）坐標(biāo)可以表示為[25]

式中為樹干的普呂克坐標(biāo)，為樹干上夾持點(diǎn)的位置矢量，為樹干的方向余弦。

根據(jù)定義可計算出與：

于是將檢測裝置所獲取到的樹干位姿信息轉(zhuǎn)化為末端執(zhí)行器期望位姿：首先，為使末端執(zhí)行器到達(dá)樹干所處位置，末端執(zhí)行器應(yīng)該滿足：

其次，為使末端執(zhí)行器能順利夾持樹干，末端執(zhí)行器坐標(biāo)系的軸應(yīng)與樹干方向余弦平行，軸與垂直，即：

滿足式（29）的1有無窮多個，但考慮到機(jī)械臂碰撞體積及關(guān)節(jié)空間范圍，如圖1所示，結(jié)合路徑最短原則[26]，過機(jī)械臂坐標(biāo)系原點(diǎn)引一直線與樹干垂直于點(diǎn)，該直線的方向余弦即為滿足上述條件且唯一的1?？紤]到直線對原點(diǎn)的線矩不變，因此有：

可解得位置矢量：

于是：

此外，3與都是方向余弦向量，式（29）所給的平行約束使得3與的方向相同或者相反，再根據(jù)耗能最小原則，使末端執(zhí)行器繞8轉(zhuǎn)過的角度最小，因此有：

最后，根據(jù)右手坐標(biāo)系建立規(guī)則，得到：

從而通過式（28）、（31）、（32）和（33）建立了物體位置坐標(biāo)1和2與期望位姿1、2、3和之間的 映射。

3.2 可行解分析

式（21）及式（22）引起的多組解可能會導(dǎo)致計算出來的關(guān)節(jié)值不在關(guān)節(jié)空間范圍內(nèi)和求解緩慢的狀況，甚至?xí)率箼C(jī)械臂腕部在工作中與樹干碰撞，因此有必要根據(jù)表1所給關(guān)節(jié)變量取值范圍對解的可行性進(jìn)行討論。僅從式（21）及式（22）無法直觀地判斷哪一組解滿足要求，因此在腕關(guān)節(jié)位置處引入一局部坐標(biāo)系O–XYZ，其原點(diǎn)位置與腕關(guān)節(jié)位置相同，其坐標(biāo)軸方向與腕關(guān)節(jié)姿態(tài)矩陣所描述方向一致。根據(jù)相對運(yùn)動不變性原理，末端執(zhí)行器關(guān)于6和7的關(guān)節(jié)變量與所選參考系無關(guān)。末端執(zhí)行器初始和最終位置在腕關(guān)節(jié)坐標(biāo)系下的描述為

使用2.2節(jié)所述方法，得到與式（21）和（22）的等價解為

考慮到僅繞6和7轉(zhuǎn)動，因此可認(rèn)為始終在p為球心為半徑的球面上，的各分量均小于，從而式（34）與（35）中的根式均有意義。再由表1所給關(guān)節(jié)變量7取值范圍為–45°～45°，故相對應(yīng)求解7的Atan2函數(shù)中第二個變量的值應(yīng)該要大于0才能使求解的7始終在一、四象限內(nèi)。所以式（35）所對應(yīng)的式（22）是受關(guān)節(jié)變量行程約束的可行解。

4 試驗與結(jié)果分析

4.1 試驗設(shè)計及誤差測量

如圖2所示，以6自由度林果采摘機(jī)械臂、激光追蹤儀及夾持目標(biāo)搭建運(yùn)動學(xué)試驗平臺[23]，通過運(yùn)動學(xué)試驗驗證所建立目標(biāo)坐標(biāo)與關(guān)節(jié)變量映射的正確性及所求的精確性。圖2a右側(cè)激光追蹤儀坐標(biāo)系原點(diǎn)相對機(jī)械臂坐標(biāo)系原點(diǎn)的位置坐標(biāo)為(-500 1 500 0)T，并設(shè)此為參考位形。激光追蹤儀型號為FARO SI型，測量精度為10m。夾持目標(biāo)為一根木條，木條上有2處標(biāo)記點(diǎn)，兩點(diǎn)相距580 mm。

整個試驗分為2個階段，第一階段測試算法的可行性，第二階段測試算法的精確性。在第一階段，于機(jī)械臂工作空間內(nèi)放置一夾持目標(biāo)，其姿態(tài)隨機(jī)。測量得到2標(biāo)記點(diǎn)位置后，先通過式（28）、（31）、（32）和（33）獲得機(jī)械臂的期望位姿，再通過式（18）、（22）和（24）計算關(guān)節(jié)變量值。將關(guān)節(jié)變量值輸入至系統(tǒng)后，若機(jī)械臂能夠到達(dá)夾持目標(biāo)位置，則說明算法可行。

在第二階段，因為機(jī)械臂有移動和轉(zhuǎn)動2種關(guān)節(jié)，為使測量結(jié)果單位統(tǒng)一，所以通過比較末端執(zhí)行器的位姿來評價結(jié)果；又由于求解機(jī)械臂運(yùn)動學(xué)逆解需要用到姿態(tài)信息，而測量角度會引入較大誤差[27-28]，因此改為測量腕關(guān)節(jié)及末端執(zhí)行器兩端指關(guān)節(jié)1和2的坐標(biāo)來間接獲得末端執(zhí)行器姿態(tài)。根據(jù)圖1，可以得到測量點(diǎn)與機(jī)械臂位姿的關(guān)系：

然后通過以下步驟進(jìn)行誤差測量試驗：

1）隨機(jī)選取整數(shù)關(guān)節(jié)變量值并輸入至樣機(jī)之中。

2）測量2指關(guān)節(jié)及腕關(guān)節(jié)位置1、2與并由式（36）與（37）計算得到和=(1，2，3 )。

3）根據(jù)測量數(shù)據(jù)求解關(guān)節(jié)變量并將其輸入至樣機(jī)之中，重復(fù)步驟2）得到新的位姿與。

4）計算誤差值并重復(fù)試驗10次以評價算法的精確性，位置誤差為

e=|–| （38）

再根據(jù)文獻(xiàn)[29]所提方法先計算旋轉(zhuǎn)誤差矩陣，再化為四元數(shù)計算姿態(tài)誤差：

將其轉(zhuǎn)化為四元數(shù)得到[20]：

式中是對應(yīng)的四元數(shù)，|為所求姿態(tài)誤差。

4.2 結(jié)果與分析

4.2.1 可行性試驗與分析

進(jìn)行第一階段試驗，通過激光追蹤儀獲得標(biāo)記點(diǎn)的位置(mm)：

根據(jù)4.1節(jié)所述方法，計算得到末端執(zhí)行器期望位姿為

再求得關(guān)節(jié)變量解為：

將關(guān)節(jié)變量值輸入至樣機(jī)中，結(jié)果如圖2b與圖2c所示，末端執(zhí)行器到達(dá)目標(biāo)所在位置且其夾持器已呈現(xiàn)利于夾緊目標(biāo)的姿態(tài)，從而驗證物體坐標(biāo)到機(jī)械臂關(guān)節(jié)變量映射關(guān)系的可行性。

1. 夾持目標(biāo) 2. 標(biāo)記點(diǎn)Pt1 3. 末端執(zhí)行器坐標(biāo)系 4. 標(biāo)記點(diǎn)Pt2 5. 基礎(chǔ)坐標(biāo)系 6. 林果采摘機(jī)械臂 7. 激光追蹤儀

4.2.2 精度試驗與分析

進(jìn)行第二階段試驗，由式（38）與式（39）計算誤差值，將位置誤差各分量繪制為折線圖，如圖3所示。

從圖3可知，末端執(zhí)行器最大位置誤差為6.597 mm，小于其夾持器200 mm夾持范圍的3.30%，從而驗證算法精度能夠滿足作業(yè)要求。此外，圖3中3個軸的誤差分量分散范圍較小，而誤差曲線整體偏移零線的程度較大，表明系統(tǒng)中可能存較大的常值性系統(tǒng)誤差[30-31]，機(jī)械臂的重復(fù)定位精度較低。

姿態(tài)誤差試驗結(jié)果如表3所示。由表3可知，本文所提方法得到的姿態(tài)誤差較小，最大姿態(tài)誤差不超過1°，滿足林果采摘作業(yè)精度要求。

圖3 末端執(zhí)行器位置誤差

表3 末端執(zhí)行器姿態(tài)誤差

5 結(jié) 論

針對Paden-Kahan子問題求解林果采摘機(jī)械臂逆運(yùn)動學(xué)時需獲取關(guān)節(jié)交點(diǎn)坐標(biāo)的問題，本文結(jié)合矢量代數(shù)改進(jìn)了傳統(tǒng)子問題，提出了一種林果采摘機(jī)械臂逆運(yùn)動學(xué)分析方法。該方法首先根據(jù)機(jī)械臂幾何特征利用封閉結(jié)構(gòu)方程獲得主被動關(guān)節(jié)映射，將混聯(lián)結(jié)構(gòu)等效為串聯(lián)結(jié)構(gòu)；然后通過矢量代數(shù)獲得關(guān)節(jié)軸交點(diǎn)坐標(biāo)并結(jié)合Paden-Kahan子問題求解林果采摘機(jī)械臂運(yùn)動學(xué)方程；再根據(jù)關(guān)節(jié)空間范圍分析解的分布規(guī)律得到其可行封閉解；最后依據(jù)路徑最短原則利用普呂克坐標(biāo)將工作目標(biāo)姿態(tài)映射為末端執(zhí)行器期望位姿，得到目標(biāo)位姿與關(guān)節(jié)空間完整映射。

1）所提方法基于旋量理論，能有效避免D-H參數(shù)法引入局部坐標(biāo)系帶來的奇異性；依據(jù)油茶果采摘機(jī)械臂作業(yè)方式及路徑最短原則能直接獲得可行解從而提高求解速度，所得封閉解形式能保證其數(shù)值穩(wěn)定性；本質(zhì)屬于幾何方法，所以不受具體結(jié)構(gòu)限制并適用其它形狀的工作目標(biāo)。

2）試驗結(jié)果表明所得解能使機(jī)械臂到達(dá)指定位姿，由解驅(qū)動的位置誤差不超過夾持器最大開度的3.30%，最大姿態(tài)誤差不超過1°，滿足林果采摘要求。但試驗所得各軸誤差分量的分布范圍與其相對零線偏差較大，表明系統(tǒng)可能存在較大的常值系統(tǒng)誤差，課題組后續(xù)擬定開展機(jī)械臂誤差分析與校正工作。

[1] Singh A, Singla A. Kinematic modeling of robotic manipulators[J]. Proceedings of the National Academy of Sciences India Section A: Physical Sciences, 2016, 87(3): 303－319.

[2] An H S, Lee J H, Lee C, et al. Geometrical kinematic solution of serial spatial manipulators using screw theory[J]. Mechanism and Machine Theory, 2017, 116: 404－418.

[3] Pena C A, Guzman M A, Cardenas P F. Inverse kinematics of a 6 DOF industrial robot manipulator based on bio-inspired multi-objective optimization techniques[C]//2016 IEEE Colombian Conference on Robotics and Automation (CCRA). IEEE, 2016.

[4] Xu J, Liu Z, Cheng Q, et al. Models for three new screw-based IK sub-problems using geometric descriptions and their applications[J]. Applied Mathematical Modelling, 2018, 67: 399－412.

[5] Man C, Xun F, Li C R, et al. Kinematics analysis based on screw theory of a humanoid robot[J]. Journal of China University of Mining and Technology, 2007, 17(1): 49－52.

[6] 孫恒輝，趙愛武，李達(dá)，等. 基于新旋量子問題改進(jìn)一類6R串聯(lián)機(jī)器人逆解算法[J]. 機(jī)械工程學(xué)報，2016，52(1)：79－86. Sun Henghui, Zhao Aiwu, Li Da, et al. Improvement of the algorithm of the inverse kinematics calculation for 6R series robots based on one novel Paden-Kahan sub-problem[J]. Chinese Journal of Mechanical Engineering, 2016, 52(1): 79－86. (in Chinese with English abstract)

[7] Wang H, Lu X, Sheng C, et al. General frame for arbitrary 3R subproblems based on the POE model[J]. Robotics and Autonomous Systems, 2018, 105:138－145.

[8] Dong Y, Phan H N, Rahmani A. Modeling and kinematics study of hand[J]. International Journal of Computer Science and Applications, 2015, 12(1): 66－79.

[9] 趙杰，劉玉斌，蔡鶴皋. 一種運(yùn)動旋量逆解子問題的求解及其應(yīng)用[J]. 機(jī)器人，2005，27(2)：163－167. Zhao Jie, Liu Yubin, Cai Hegao. Solution for one type of inverse kinematics sub-problems in screw theory and its application[J]. Robot, 2005, 27(2): 163－167. (in Chinese with English abstract)

[10] Murray R M, Li Z, Sastry S S. A Mathematical Introduction to Robotic Manipulation[M]. Florida: CRC Press, 1994:19－81.

[11] 錢東海，王新峰，趙偉，等. 基于旋量理論和Paden-Kahan子問題的6自由度機(jī)器人逆解算法[J]. 機(jī)械工程學(xué)報，2009，45(9)：72－76. Qian Donghai, Wang Xinfeng, Zhao Wei, et al. Algorithm for the inverse kinematics calculation of 6-DOF robots Based on Screw throry and Paden-Kahan sub-problems[J]. Chinese Journal of Mechanical Engineering, 2009, 45(9): 72－76. (in Chinese with English abstract)

[12] 呂世增，張大衛(wèi)，劉海年. 基于吳方法的6R機(jī)器人逆運(yùn)動學(xué)旋量方程求解[J]. 機(jī)械工程學(xué)報，2010，46(17)：35－41. Lü Shizeng, Zhang Dawei, Liu Hainian. Solution of screw equation for inverse kinematics of 6R robot based on Wu’s method[J]. Chinese Journal of Mechanical Engineering, 2010, 46(17): 35－41. (in Chinese with English abstract)

[13] 李盛前，謝小鵬. 基于旋量理論和Sylvester結(jié)式法的6自由度機(jī)器人逆運(yùn)動學(xué)求解分析[J]. 農(nóng)業(yè)工程學(xué)報，2015，31(20)：48－54. Li Shengqian, Xie Xiaopeng. Anlysis of inverse kinematic solution for 6R robot based on screw theory and Sylvester resultant[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(20): 48－54. (in Chinese with English abstract)

[14] Sariyildiz E, Temeltas H. Solution of inverse kinematic problem for serial robot using quaterninons[C]//Proceedings of the IEEE International Conference on Mechatronics and Automation. Changchun: IEEE, 2009: 26－31.

[15] Gao Y, Chen W, Lu Z. Kinematics analysis and experiment of a cockroach-like robot[J]. Journal of Shanghai Jiaotong University(Science), 2011, 16(1): 71－77.

[16] 陳慶誠，朱世強(qiáng)，王宣銀，等. 基于旋量理論的串聯(lián)機(jī)器人逆解子問題求解算法[J]. 浙江大學(xué)學(xué)報：工學(xué)版，2014，48(1)：8－14. Chen Qingcheng, Zhu Shiqiang, Wang Xuanyin, et al. Inverse kinematics sub-problem solution algorithm for serial robot based on screw theory[J]. Journal of Zhejiang University: Engineering Science, 2014, 48(1): 8－14. (in Chinese with English abstract)

[17] Wang H, Lu X, Zhang Z, et al. A novel second subproblem for two arbitrary axes of robots[J]. International Journal of Advanced Robotic Systems, 2018, 15(2): 1－7.

[18] Rocha C R, Tonetto C P, Dias A. A comparison between the Denavit–Hartenberg and the screw-based methods used in kinematic modeling of robot manipulators[J]. Robotics & Computer Integrated Manufacturing, 2011, 27(4): 723－728.

[19] Li Y, Zhu S, Wang Z, et al. The kinematics analysis of a novel self-reconfigurable modular robot based on screw theory[J]. DEStech Transactions on Engineering and Technology Research, 2016: 23－30.

[20] Dai J S. Screw algebra and kinematic approaches for mechanisms and robotics[M]. London: Springer, 2014: 49－80.

[21] Kevin M L, Frank C P. Modern robotics mechanics, planning, and control[M]. Cambridge： Cambridge University Press, 2017: 59－114.

[22] Gallardo-Alvarado J. Kinematic analysis of parallel manipulators by algebraic screw theory[M]. Switzerland: Springer International Publishing, 2016: 31－63.

[23] 陽涵疆，李立君，高自成. 基于旋量理論的混聯(lián)采摘機(jī)器人正運(yùn)動學(xué)分析與試驗[J]. 農(nóng)業(yè)工程學(xué)報，2016，32(9)：53－59. Yang Hanjiang, Li Lijun, Gao Zicheng. Forward kinematics analysis and experiment of hybrid harvesting robot based on screw theory[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(9): 53－59. (in Chinese with English abstract)

[24] 王朋輝，李立君，高自成，等. 擺動式林果采摘頭設(shè)計與分析[J]. 西北林學(xué)院學(xué)報，2015，30(5)：288－291. Wang Penghui, Li Lijun, Gao Zicheng, et al. The design and analysis of oscillating fruit picking head[J]. Journal of Northwest Forestry University, 2015, 30(5): 288－291. (in Chinese with English abstract)

[25] 黃真，趙永生，趙鐵石. 高等空間機(jī)構(gòu)學(xué)[M]. 北京：高等教育出版社，2014：2－26.

[26] 李國利，姬長英，顧寶興，等. 多末端蘋果采摘機(jī)器人機(jī)械手運(yùn)動學(xué)分析與試驗[J]. 農(nóng)業(yè)機(jī)械學(xué)報，2016，47(12)：14－21. Li Guoli, Ji Changying, Gu Baoxing, et al. Kinematics analysis and experiment of apple harvesting robot manipulator with multiple end-effectors[J]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(12): 14－21. (in Chinese with English abstract)

[27] 梅東棋. 六自由度工業(yè)機(jī)器人全姿態(tài)精度補(bǔ)償方法[D]. 南京：南京航空航天大學(xué)，2015. Mei Dongqi. Compensation Method of 6R Industrial Robot positioning Accuracy[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2015. (in Chinese with English abstract)

[28] Cho Y, Do H M, Cheong J. Screw based kinematic calibration method for robot manipulators with joint compliance using circular point analysis[J]. Robotics and Computer-Integrated Manufacturing, 2018, https://doi.org/ 10.1016/j.rcim.2018.08.001.

[29] Tabandeh S, Melek W W, Clark C M. An adaptive niching genetic algorithm approach for generating multiple solutions ofserial manipulator inverse kinematics with applications to modular robots[J]. Robotica, 2010, 28(4): 493－507.

[30] Liu Y, Wan M, Xing W J, et al. Generalized actual inverse kinematic model for compensating geometric errors in five-axis machine tools[J]. International Journal of Mechanical Sciences, 2018, 145: 299－317.

[31] 王先奎. 機(jī)械制造工藝學(xué)[M]. 北京：機(jī)械工業(yè)出版社，2013：199－226.

Inverse kinematics of 6-DOF hybrid manipulator for forest-fruit harvest based on screw theory

Li Lijun, Liu Tao, Gao Zicheng, Liao Kai, Li Yuzhuo, Xu Shibin

(410000,)

A method for inverse kinematics analysis based on screw theory was presented in this paper, which can directly map the position and orientation of the working object to the joint variables of the manipulator with its application to a full inverse kinematics analysis of forest-fruit harvesting manipulator characterized by a hybrid kinematic structure, 2P4R. The solution of inverse kinematics modeling derived by screw theory was commonly realized by Paden-Kahan sub-problem method, which decomposes a full kinematics problem into sub-problem with obviously geometrical meaning through choosing appropriate point, usually, intersection of adjacent joint, such as wrist joint, to reduce the number of the variable quantity, and then close-form solution can be easily obtained. However, in practice, it is hard to gain the position of those points through measure because of their absence before end-effector actually moving to the desired position. And few researchers mentioned this issue in the relevant literature. In order to discuss this problem, firstly, a geometrical method was proposed for this issue to obtain the position of the required point, wrist joint, according to the orientation of end-effector and its geometric properties and geometric relationships through using the vector algebra method. Furthermore, a mapping between driving and driven join was gained in order to simplify the solving process of the equation set at a later, according to the solution of the structural equation of the manipulator derived by the product-of-exponentials (POEs) formula and structural character of manipulator. Meanwhile, the closed-form solution for each driving joint variables was derived by employing the proposed method with Paden-Kahan sub-problem method. A mapping relationship between the plücker coordinates of the object and the location information of end-effector was derived through an algebraic method according to the principle of minimum displacement and its operating mode in which the gripper of end-effector should reach the position of the trunk with two labels detected by the robot vision system and be perpendicular to the orientation of the trunk. In addition, the problem of multiple solutions in the inverse kinematics analysis for the harvesting manipulator was solved according to the range of joint variables. Finally, the real-world experiment was performed under laboratory environment. In order to vertify the correctness and obtain the accuracy of the method proposed in this paper. A wooden stick with two markers was placed in the kinematics test platform as the object, which consisted of a laser tracker and a harvesting manipulator. Then, the values of each joint variable could be calculated via the proposed method according to the plücker coordinate data of the markers measured in the object. The results showed that the forest-fruit harvesting manipulator was driven by the solution of inverse kinematics to the position on the stick that its end-effect reached and normal to the stick, which meant this method could meet the requirements of the operating mode. Then ten sets of joint variable values were randomly generated where the positions were measured and the manipulator was sequentially driven by that. The joint variable values were calculated according to the positions through the method proposed in this paper. At last, the calculated results were re-inputted into the controller to drive the manipulator to the new positions. The two measure results on different positions driven by joint variable values generated and calculated were used to obtain the error. The results showed that the maximum position error of end-effector was 6.597 mm, far less than the open size of its gripper, 200 mm, and no more than 3.30%, with the maximum orientation error of 0.975°. The method in this paper was not limited by the specific structure, therefore it is versatile.

robots; harvesters; kinematics; screw theory; inverse kinematics; Paden-Kahan sub-problem; hybrid manipulator; forest-fruit harvest

2018-10-28

2019-02-12

湖南省科技重大專項（2017NK1010）；國家自然科學(xué)基金（51475483）

李立君，湖南寧鄉(xiāng)人，教授，博士生導(dǎo)師，主要從事智能林業(yè)技術(shù)裝備的研究。Email：junlili1122@163.com

10.11975/j.issn.1002-6819.2019.08.009

TP242

A

1002-6819(2019)-08-0075-08

李立君，劉 濤，高自成，廖 凱，李禹卓，許世斌.基于旋量理論的六自由度林果采摘混聯(lián)機(jī)械臂運(yùn)動學(xué)逆解[J]. 農(nóng)業(yè)工程學(xué)報，2019，35(8)：75－82. doi：10.11975/j.issn.1002-6819.2019.08.009 http://www.tcsae.org

Li Lijun, Liu Tao, Gao Zicheng, Liao Kai, Li Yuzhuo, Xu Shibin.Inverse kinematics of 6-DOF hybrid manipulator for forest-fruit harvest based on screw theory[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(8): 75－82. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2019.08.009 http://www.tcsae.org