基于旋量理論的六自由度林果采摘混聯機械臂運動學逆解

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

?

基于旋量理論的六自由度林果采摘混聯機械臂運動學逆解

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

（中南林業科技大學機電工程學院，長沙 410000）

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

機器人；收獲機；運動學；旋量理論；逆運動學；Paden-Kahan子問題；混聯機械臂；林果采摘

0 引 言

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

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

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

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

1 林果采摘機械臂及旋量運動模型

1.1 旋量理論與剛體運動

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

式中為6×1旋量坐標，為旋量軸線的方向余弦，￠為旋量軸線上任選一點￠的位置矢量，為旋距。

當關節為轉動副時，=0；當關節為移動副時，趨于無窮大。在這2種極端情況下，旋量坐標對應李代數se(3)的標準4×4矩陣形式表示為

旋量坐標可以通過如下步驟確定：

1）選擇一個合適的結構參考位形。

2）確定此位形下每個關節軸線的方向余弦矢量及軸線上任選點￠的位置矢量￠。

3）根據關節運動類型，由式（2）計算旋量坐標。

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

式中為4×4單位矩陣，為關節變量值，為展開階數。

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

林果采摘機械臂結構簡圖與參考位形如圖1所示，對機械臂主運動鏈使用指數積公式可以得到其末端位姿為[23]

本文所用正運動學模型建立在文獻[23]研究基礎之上，具體正運動學建模過程可參考文獻[23]。

1.2 機械臂結構分析

如式（5）所示，機械臂主運動鏈有8個活動關節，而機械臂自由度為6。為簡化求解過程，可將其并聯移動結構等效為串聯轉動結構，因此需通過分析其幾何特征獲得主動移動關節與被動轉動關節之間關系。機械臂結構參數如表1所示：

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

表1 機械臂結構參數

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

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

結合圖1可知，、￠￠及￠￠為3個平行四邊形。對于平行四邊形結構，其連桿上4個關節所轉過角度相同，于是有：

表2 機械臂在參考位形下的旋量參數

依次選取點和點為結點，由指數積公式（4）得到結構方程：

前3個方程組相互獨立，解得：

2 逆運動學求解

2.1 求解θ1、θ2與θ9

逆運動分析解決的問題是根據末端執行器的期望位姿來獲得一組或若干組關節變量，從而控制末端執行器到達該位姿。因此末端執行器位姿矩陣是已知的：

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

根據旋量代數的基本性質，當旋矩=0時，若在關節軸線上取一點，旋轉作用對該點無效，即：

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

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

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

根據表1所示關節變量取值范圍及連桿尺寸

聯立（9）、（10）、（16）與（17）可得到方程組（16）的解：

2.2 求解θ6和θ7

(0)根據圖1得到：

根據引入的中間點可以將上述子問題2分解為2個子問題1：

由此解得關節變量6和7的值：

2.3 求解θ8

8通過構造子問題1求解，選取不在8上的點1，從圖1中可以得到關系：

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

求解可得8：

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

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

3.1 目標姿態與末端執行器姿態映射建立

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

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

式中為樹干的普呂克坐標，為樹干上夾持點的位置矢量，為樹干的方向余弦。

根據定義可計算出與：

于是將檢測裝置所獲取到的樹干位姿信息轉化為末端執行器期望位姿：首先，為使末端執行器到達樹干所處位置，末端執行器應該滿足：

其次，為使末端執行器能順利夾持樹干，末端執行器坐標系的軸應與樹干方向余弦平行，軸與垂直，即：

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

可解得位置矢量：

于是：

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

最后，根據右手坐標系建立規則，得到：

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

3.2 可行解分析

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

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

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

4 試驗與結果分析

4.1 試驗設計及誤差測量

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

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

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

然后通過以下步驟進行誤差測量試驗：

1）隨機選取整數關節變量值并輸入至樣機之中。

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

3）根據測量數據求解關節變量并將其輸入至樣機之中，重復步驟2）得到新的位姿與。

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

e=|–| （38）

再根據文獻[29]所提方法先計算旋轉誤差矩陣，再化為四元數計算姿態誤差：

將其轉化為四元數得到[20]：

式中是對應的四元數，|為所求姿態誤差。

4.2 結果與分析

4.2.1 可行性試驗與分析

進行第一階段試驗，通過激光追蹤儀獲得標記點的位置(mm)：

根據4.1節所述方法，計算得到末端執行器期望位姿為

再求得關節變量解為：

將關節變量值輸入至樣機中，結果如圖2b與圖2c所示，末端執行器到達目標所在位置且其夾持器已呈現利于夾緊目標的姿態，從而驗證物體坐標到機械臂關節變量映射關系的可行性。

1. 夾持目標 2. 標記點Pt1 3. 末端執行器坐標系 4. 標記點Pt2 5. 基礎坐標系 6. 林果采摘機械臂 7. 激光追蹤儀

4.2.2 精度試驗與分析

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

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

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

圖3 末端執行器位置誤差

表3 末端執行器姿態誤差

5 結 論

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

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

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

[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] 孫恒輝，趙愛武，李達，等. 基于新旋量子問題改進一類6R串聯機器人逆解算法[J]. 機械工程學報，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] 趙杰，劉玉斌，蔡鶴皋. 一種運動旋量逆解子問題的求解及其應用[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]. 機械工程學報，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] 呂世增，張大衛，劉海年. 基于吳方法的6R機器人逆運動學旋量方程求解[J]. 機械工程學報，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結式法的6自由度機器人逆運動學求解分析[J]. 農業工程學報，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] 陳慶誠，朱世強，王宣銀，等. 基于旋量理論的串聯機器人逆解子問題求解算法[J]. 浙江大學學報：工學版，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] 陽涵疆，李立君，高自成. 基于旋量理論的混聯采摘機器人正運動學分析與試驗[J]. 農業工程學報，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] 王朋輝，李立君，高自成，等. 擺動式林果采摘頭設計與分析[J]. 西北林學院學報，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] 黃真，趙永生，趙鐵石. 高等空間機構學[M]. 北京：高等教育出版社，2014：2－26.

[26] 李國利，姬長英，顧寶興，等. 多末端蘋果采摘機器人機械手運動學分析與試驗[J]. 農業機械學報，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] 梅東棋. 六自由度工業機器人全姿態精度補償方法[D]. 南京：南京航空航天大學，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] 王先奎. 機械制造工藝學[M]. 北京：機械工業出版社，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）；國家自然科學基金（51475483）

李立君，湖南寧鄉人，教授，博士生導師，主要從事智能林業技術裝備的研究。Email：junlili1122@163.com

10.11975/j.issn.1002-6819.2019.08.009

TP242

A

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

李立君，劉 濤，高自成，廖 凱，李禹卓，許世斌.基于旋量理論的六自由度林果采摘混聯機械臂運動學逆解[J]. 農業工程學報，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