陽(yáng)涵疆，李立君，高自成

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

基于旋量理論的混聯(lián)采摘機(jī)器人正運(yùn)動(dòng)學(xué)分析與試驗(yàn)

陽(yáng)涵疆，李立君※，高自成

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

為滿足油茶果機(jī)械化、自動(dòng)化采摘的要求，避免利用傳統(tǒng)的Denavit-Hartenberg(D-H)參數(shù)法對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)分析時(shí)的缺陷，提出了一種基于旋量理論構(gòu)建混聯(lián)采摘機(jī)器人運(yùn)動(dòng)學(xué)方程的方法。根據(jù)混聯(lián)采摘機(jī)器人機(jī)械臂的結(jié)構(gòu)特點(diǎn)進(jìn)行簡(jiǎn)化；基于所提出的方法建立了機(jī)器人正運(yùn)動(dòng)學(xué)方程，獲得末端執(zhí)行器的位置正解；隨機(jī)選取5組關(guān)節(jié)變量值，得出末端執(zhí)行器在基礎(chǔ)坐標(biāo)系各坐標(biāo)軸上的最大絕對(duì)位置誤差為10.4 mm，遠(yuǎn)小于末端執(zhí)行器200 mm的開(kāi)度，滿足該機(jī)器人末端執(zhí)行器的采摘工作要求，驗(yàn)證了通過(guò)文中所提出的方法建立混聯(lián)采摘機(jī)器人運(yùn)動(dòng)學(xué)正解方程的可行性及方程的正確性。該研究可為后續(xù)開(kāi)展混聯(lián)采摘機(jī)器人控制方法和軌跡規(guī)劃研究提供參考。

機(jī)器人；運(yùn)動(dòng)學(xué)；模型；旋量理論；機(jī)械臂；混聯(lián)機(jī)器人

0 引言

近年來(lái)隨著中國(guó)油茶種植產(chǎn)業(yè)的快速發(fā)展，油茶果采摘機(jī)械化程度低、人工采摘油茶果效率低下的問(wèn)題日益凸顯，因此研制能夠提高生產(chǎn)效率、降低勞動(dòng)強(qiáng)度的油茶果自動(dòng)化采收裝備成為中國(guó)未來(lái)油茶產(chǎn)業(yè)發(fā)展的基本趨勢(shì)。因此本項(xiàng)目組針對(duì)油茶果、果樹(shù)的生長(zhǎng)特性，研究開(kāi)發(fā)了一種油茶果混聯(lián)采摘機(jī)器人。根據(jù)給定的各關(guān)節(jié)轉(zhuǎn)角或位移，確定機(jī)器人末端執(zhí)行器位置和姿態(tài)的機(jī)器人正運(yùn)動(dòng)學(xué)，是進(jìn)一步開(kāi)展機(jī)器人研究的基礎(chǔ)，對(duì)于機(jī)器人工作空間分析，軌跡規(guī)劃，運(yùn)動(dòng)控制及誤差補(bǔ)償?shù)确矫婢哂袠O重要作用。

對(duì)機(jī)器人開(kāi)展正運(yùn)動(dòng)學(xué)研究的傳統(tǒng)方法是Denavit-Hartenberg(D-H)參數(shù)法[1]，但該方法需要為每個(gè)關(guān)節(jié)建立局部坐標(biāo)系，各坐標(biāo)系的姿態(tài)根據(jù)關(guān)節(jié)類型的不同而有所區(qū)別，并且在改變機(jī)器人構(gòu)型后，需要重新建立坐標(biāo)系，建模過(guò)程復(fù)雜繁瑣，幾何意義不明顯[2-6]。而利用基于旋量理論的指數(shù)積公式（product of exponential，POE）進(jìn)行正運(yùn)動(dòng)分析時(shí)只需要建立基礎(chǔ)坐標(biāo)系S和末端工具坐標(biāo)系T，坐標(biāo)系建立過(guò)程簡(jiǎn)單易懂，充分了利用機(jī)器人的幾何特性，使得指數(shù)積公式（旋量法）成為D-H參數(shù)法的最佳替代[7]。

目前國(guó)內(nèi)外學(xué)者對(duì)旋量法在串、并聯(lián)機(jī)器人領(lǐng)域的應(yīng)用做了大量研究，并獲得了一批研究成果[8-13]。Jaime 等[14]采用基于旋量理論的指數(shù)積運(yùn)動(dòng)學(xué)建模方法對(duì)Delta機(jī)器人進(jìn)行了正運(yùn)動(dòng)學(xué)分析，求解了動(dòng)平臺(tái)速度、加速度輸入輸出方程，有利于后續(xù)根據(jù)并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)性能指標(biāo)進(jìn)行參數(shù)設(shè)計(jì)；Kong等[15]基于螺旋理論，提出了一種對(duì)三自由度移動(dòng)機(jī)構(gòu)進(jìn)行分析的方法，并對(duì)該類機(jī)構(gòu)主動(dòng)關(guān)節(jié)的有效性條件進(jìn)行了分析；陳偉海等[16]基于旋量理論對(duì)一種模塊化冗余度機(jī)器人進(jìn)行了運(yùn)動(dòng)學(xué)分析，簡(jiǎn)化了具有任意自由度和任意構(gòu)型的模塊化機(jī)器人的運(yùn)動(dòng)學(xué)建模過(guò)程；莊未等[17]建立了基于螺旋理論的四足機(jī)器人運(yùn)動(dòng)學(xué)模型，給出了單腿串聯(lián)和軀體并聯(lián)運(yùn)動(dòng)學(xué)方程，并通過(guò)仿真驗(yàn)證了模型的正確性；張付祥等[18]利用旋量理論對(duì)一種閉鏈級(jí)聯(lián)式機(jī)器人進(jìn)行了正運(yùn)動(dòng)學(xué)分析，并在此基礎(chǔ)上提出了一種基于旋量理論建立閉鏈級(jí)聯(lián)式機(jī)器人運(yùn)動(dòng)學(xué)方程的方法。作為本文研究對(duì)象的混聯(lián)采摘機(jī)器人是在并聯(lián)機(jī)器人運(yùn)動(dòng)臺(tái)上擴(kuò)展串聯(lián)結(jié)構(gòu)得到的，其運(yùn)動(dòng)學(xué)分析方法與單一的串聯(lián)或并聯(lián)機(jī)器人有所不同，而由上面可知，旋量理論在混聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)領(lǐng)域的研究和應(yīng)用還不多，也未歸納出具有通用性的方法。

本文介紹了一種2P4R6自由度油茶果混聯(lián)采摘機(jī)器人，并以該機(jī)器人為研究對(duì)象，分析其機(jī)械結(jié)構(gòu)，提出一種利用旋量理論開(kāi)展混聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)研究的一般性方法，利用該方法構(gòu)建機(jī)器人運(yùn)動(dòng)學(xué)模型，通過(guò)試驗(yàn)驗(yàn)證基于旋量理論構(gòu)建混聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)模型的可行性及該模型的正確性，以期為后續(xù)研究混聯(lián)機(jī)器人控制方法和軌跡規(guī)劃提供參考。

1 基于旋量理論的運(yùn)動(dòng)學(xué)模型

1.1 旋量理論基礎(chǔ)

剛體從一個(gè)位置到另一個(gè)位置的運(yùn)動(dòng)可以通過(guò)繞某直線的轉(zhuǎn)動(dòng)加上沿平行于該直線的移動(dòng)得到，這種轉(zhuǎn)動(dòng)與移動(dòng)的組合稱為旋量運(yùn)動(dòng)[7]。

若設(shè)與剛體固連的動(dòng)坐標(biāo)系為B，慣性坐標(biāo)系為A，那么剛體在A上的位姿變換集合可表示為

式中SE(3)為三維空間中剛體變換集合的特殊歐式群；R ∈SO(3)為B系相對(duì)于A系的姿態(tài)矩陣；p∈R3為B系相對(duì)于A系的位置矢量；SO(3)是一個(gè)以單位矩陣I作為單位元素的群；R3是以3維列向量作為單位元素的群。

若剛體以單位速度繞ω=(ωx，ωy，ωz)T∈R3旋轉(zhuǎn)軸純轉(zhuǎn)動(dòng)θ角度，則剛體從初始位置到最終位置坐標(biāo)之間的變換可以用矩陣指數(shù)表示為

通常采用下面的Rodriguez公式計(jì)算矩陣指數(shù)

式中I為3×3的單位矩陣。

當(dāng)剛體繞ω=(ωx，ωy，ωz)T∈R3旋轉(zhuǎn)軸做旋量運(yùn)動(dòng)時(shí)，如圖1所示，由Chasles定理[7]知，可由既繞ω=(ωx，ωy，ωz)T∈R3旋轉(zhuǎn)軸轉(zhuǎn)動(dòng)θ角又沿平行于ω軸方向直線平移v的復(fù)合運(yùn)動(dòng)來(lái)實(shí)現(xiàn)，則相應(yīng)的剛體位姿變換g用矩陣指數(shù)表示為

圖1 剛體的旋量運(yùn)動(dòng)Fig.1 Rotation of rigid body

根據(jù)式（3），可以將式（4）展開(kāi)成下面的形式

如上所述，運(yùn)動(dòng)旋量的指數(shù)可以表示剛體的相對(duì)運(yùn)動(dòng)，即剛體上經(jīng)過(guò)旋量運(yùn)動(dòng)后，與剛體固連的B系相對(duì)于固定的A系的瞬時(shí)位姿變換為

式中g(shù)ab(0)為初始位姿時(shí)B系與A系之間的剛體位姿換；gab(θ)為B系相對(duì)A系的最終位姿變換。

1.2 指數(shù)積方程

對(duì)于n自由度的開(kāi)鏈機(jī)器人，末端執(zhí)行器位姿是由各關(guān)節(jié)復(fù)合運(yùn)動(dòng)構(gòu)成的。當(dāng)給定一組關(guān)節(jié)轉(zhuǎn)角，希望確定末端工具坐標(biāo)系T相對(duì)于基礎(chǔ)坐標(biāo)系S的位形。對(duì)于第i（i=1, 2, …, n）個(gè)關(guān)節(jié)可以構(gòu)造一個(gè)運(yùn)動(dòng)旋量，對(duì)應(yīng)于除該關(guān)節(jié)以外其他關(guān)節(jié)均固定于零位時(shí)的旋量運(yùn)動(dòng)。對(duì)于轉(zhuǎn)動(dòng)關(guān)節(jié)，運(yùn)動(dòng)旋量的坐標(biāo)表示形式為

對(duì)于移動(dòng)關(guān)節(jié)，運(yùn)動(dòng)旋量?iξ的坐標(biāo)表示形式為

式中ωi∈R3為第i個(gè)關(guān)節(jié)軸線上的單位矢量；qi∈R3為第i個(gè)關(guān)節(jié)軸線上的任意一點(diǎn)坐標(biāo)；vi為第i個(gè)關(guān)節(jié)移動(dòng)方向上的單位矢量；ξi為運(yùn)動(dòng)旋量?iξ的坐標(biāo)表示形式。

將各關(guān)節(jié)運(yùn)動(dòng)加以組合，即得到機(jī)器人運(yùn)動(dòng)學(xué)正解的指數(shù)積方程

式中g(shù)st(θ)為T系相對(duì)S系的最終位姿；gst(0)為初始位形時(shí)T系與S系之間的剛體位姿變換；為第n個(gè)關(guān)節(jié)的運(yùn)動(dòng)旋量；θn為第n個(gè)關(guān)節(jié)的轉(zhuǎn)動(dòng)量；為θn的矩陣指數(shù)。

2 混聯(lián)采摘機(jī)器人機(jī)械臂結(jié)構(gòu)分析

圖2為2P4R6自由度混聯(lián)采摘機(jī)器人機(jī)械臂的三維模型。該機(jī)器人主要由腰部、手臂、腕部組成。腰部基座1為整個(gè)機(jī)器人的基礎(chǔ)部分，與手臂結(jié)構(gòu)通過(guò)旋轉(zhuǎn)關(guān)節(jié)連接，可驅(qū)動(dòng)除腰部以外的其余部分繞腰部關(guān)節(jié)旋轉(zhuǎn)；手臂部分主要包括旋轉(zhuǎn)座2、前臂4、后臂7、后小臂8、下小臂10、上小臂11、三角形連接件9，旋轉(zhuǎn)座為整個(gè)手臂提供支撐，上面安裝有水平滑塊3和豎直滑塊6，其中水平滑塊與前臂下端通過(guò)鉸鏈連接，豎直滑塊與后臂下端通過(guò)鉸鏈連接，下小臂和上小臂的末端與腕部基座12鉸接；腕部則由3個(gè)串聯(lián)的旋轉(zhuǎn)關(guān)節(jié)組成。

圖2 混聯(lián)采摘機(jī)器人機(jī)械臂參數(shù)化三維模型Fig.2 Parameterized manipulator model of hybrid harvesting robot

手臂結(jié)構(gòu)中前臂和后臂下端通過(guò)主臂連桿5連接，前臂與后臂相互平行，主臂連桿與下小臂相互平行，四者構(gòu)成一個(gè)平行四邊形機(jī)構(gòu)，不僅增加了手臂結(jié)構(gòu)部分的剛度，而且可以通過(guò)較小的驅(qū)動(dòng)行程獲得末端執(zhí)行器較大的工作行程，從而滿足了采摘作業(yè)對(duì)機(jī)器人工作空間的性能要求。機(jī)器人通過(guò)控制腰部和腕部關(guān)節(jié)的轉(zhuǎn)動(dòng)實(shí)現(xiàn)末端執(zhí)行器姿態(tài)調(diào)整，通過(guò)控制腰部旋轉(zhuǎn)、手臂的關(guān)節(jié)移動(dòng)實(shí)現(xiàn)對(duì)末端執(zhí)行器的位置調(diào)整，這樣設(shè)計(jì)使得機(jī)器人腕部減少了2個(gè)自由度，減輕了手臂末端運(yùn)動(dòng)負(fù)荷，使機(jī)器人在保證同樣自由度的情況下，讓驅(qū)動(dòng)電機(jī)靠近基座，減少末端執(zhí)行器產(chǎn)生的高頻振動(dòng)對(duì)驅(qū)動(dòng)電機(jī)及關(guān)節(jié)的不利影響，適合于振動(dòng)采摘機(jī)器人的作業(yè)環(huán)境。

3 機(jī)械臂正運(yùn)動(dòng)學(xué)分析

3.1 基于旋量理論的混聯(lián)機(jī)器人的運(yùn)動(dòng)學(xué)分析方法

由于混聯(lián)機(jī)器人通常都是在并聯(lián)機(jī)器人運(yùn)動(dòng)臺(tái)上擴(kuò)展串聯(lián)結(jié)構(gòu)得到的，因此總是能從混聯(lián)機(jī)器人中找到一條主運(yùn)動(dòng)鏈，這條主運(yùn)動(dòng)鏈可以看成一個(gè)串聯(lián)機(jī)器人，主運(yùn)動(dòng)鏈中的部分構(gòu)件又從屬于并聯(lián)機(jī)器人，可以把混聯(lián)機(jī)器人看成是由并聯(lián)機(jī)器人驅(qū)動(dòng)關(guān)節(jié)運(yùn)動(dòng)的串聯(lián)機(jī)器人。針對(duì)混聯(lián)機(jī)器人的這一特點(diǎn)，綜合旋量理論進(jìn)行串、并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)分析的方法，提出基于旋量理論的混聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)方程構(gòu)建方法：首先確定混聯(lián)機(jī)器人主運(yùn)動(dòng)鏈和副運(yùn)動(dòng)鏈，構(gòu)建兩條運(yùn)動(dòng)鏈在參考位形時(shí)的結(jié)構(gòu)方程等式，通過(guò)化簡(jiǎn)等式獲得主運(yùn)動(dòng)鏈中被動(dòng)關(guān)節(jié)與主、副運(yùn)動(dòng)鏈中主動(dòng)關(guān)節(jié)的位姿映射關(guān)系；然后運(yùn)用指數(shù)積公式對(duì)主運(yùn)動(dòng)鏈進(jìn)行分析，得到主動(dòng)鏈各關(guān)節(jié)與末端執(zhí)行器的位姿映射關(guān)系，此時(shí)該位姿映射關(guān)系包含了被動(dòng)關(guān)節(jié)參數(shù)；最后綜合上面所求得的2種位姿映射關(guān)系式，即得到機(jī)器人主動(dòng)關(guān)節(jié)與末端執(zhí)行器只包含有主動(dòng)關(guān)節(jié)參數(shù)的位姿映射關(guān)系，完成混聯(lián)機(jī)器人的運(yùn)動(dòng)分析。

3.2 確定主運(yùn)動(dòng)鏈關(guān)節(jié)與主動(dòng)關(guān)節(jié)映射關(guān)系

圖3為機(jī)械臂結(jié)構(gòu)簡(jiǎn)圖及參考位形，從腰部旋轉(zhuǎn)座到腕部基座bFF′的構(gòu)件組成了機(jī)械臂的手臂（并聯(lián)結(jié)構(gòu)部分），腰部旋轉(zhuǎn)座為該并聯(lián)結(jié)構(gòu)的旋轉(zhuǎn)座、腕部基座bFF′為動(dòng)平臺(tái)[19]，豎直滑塊、后臂lAD、下小臂lDF和水平滑塊、后小臂lC′E″、三角形連接件bEE′E″、上小臂lE′F′分別組成了并聯(lián)結(jié)構(gòu)的2條串聯(lián)運(yùn)動(dòng)鏈。本文中選擇垂直導(dǎo)軌滑塊所在的運(yùn)動(dòng)鏈為主運(yùn)動(dòng)鏈，水平導(dǎo)軌滑塊所在的運(yùn)動(dòng)鏈為副運(yùn)動(dòng)鏈。

圖3 機(jī)械臂結(jié)構(gòu)簡(jiǎn)圖及參考位形Fig.3 Structure diagram and reference position of manipulator

圖3所示的機(jī)器人位形為θi=0時(shí)的參考位形，一般位形的結(jié)構(gòu)方程為

整理式（10），可得

由圖3可知各關(guān)節(jié)軸單位矢量為

根據(jù)機(jī)器人結(jié)構(gòu)參數(shù)取各軸線上的點(diǎn)為

式中a為圖3中A、B之間的距離，mm；其余同理。

由式（8）可求解機(jī)器人水平滑塊、豎直滑塊的運(yùn)動(dòng)旋量坐標(biāo)，由式（7）可求得其余關(guān)節(jié)的運(yùn)動(dòng)旋量坐標(biāo)。

前面對(duì)機(jī)器人結(jié)構(gòu)分析可知，水平滑塊和豎直滑塊為機(jī)械臂中并聯(lián)結(jié)構(gòu)中的主動(dòng)件，即β=[θ2，θ9]T為主動(dòng)關(guān)節(jié)，α=[θ3，θ4，θ5，θ10，θ11，θ12，θ13]T為被動(dòng)關(guān)節(jié)，則可將并聯(lián)部分的結(jié)構(gòu)方程（11）調(diào)整為

當(dāng)給定主動(dòng)關(guān)節(jié)位置β時(shí)，即可解出α。那么被動(dòng)關(guān)節(jié)旋轉(zhuǎn)角度θ3、θ5與主動(dòng)關(guān)節(jié)之間的映射關(guān)系為

式中x為水平滑塊的位移量，在圖3中所對(duì)應(yīng)的運(yùn)動(dòng)旋量坐標(biāo)為ξ9，mm；z為豎直滑塊的位移量，在圖3中所對(duì)應(yīng)的運(yùn)動(dòng)旋量坐標(biāo)為ξ2，mm。

3.3 建立主運(yùn)動(dòng)鏈運(yùn)動(dòng)學(xué)正解方程

當(dāng)θi=0時(shí)T系與S系的初始位姿變換為

式中f為圖3中鉸接點(diǎn)F與運(yùn)動(dòng)旋量ξ6之間的距離，mm；其余同理。

由圖3可知主動(dòng)鏈中未求解的各關(guān)節(jié)軸單位矢量為

根據(jù)機(jī)器人結(jié)構(gòu)參數(shù)取各軸線上的點(diǎn)為

由式（9）可求得主動(dòng)鏈中尚未求解的各關(guān)節(jié)運(yùn)動(dòng)旋量坐標(biāo)。由于選擇垂直滑塊所在的運(yùn)動(dòng)鏈為主動(dòng)鏈，則主運(yùn)動(dòng)鏈運(yùn)動(dòng)學(xué)正解方程為

展開(kāi)可得

其中ci=cosθi，si=sinθi。

3.4 機(jī)械臂末端執(zhí)行器運(yùn)動(dòng)學(xué)正解方程

機(jī)器人運(yùn)動(dòng)學(xué)正解方程只包含有主動(dòng)關(guān)節(jié)參數(shù)，而上面p(θ)包含了被動(dòng)關(guān)節(jié)參數(shù)θ3、θ5。為消去主運(yùn)動(dòng)鏈運(yùn)動(dòng)學(xué)正解方程中的被動(dòng)關(guān)節(jié)參數(shù)，將主運(yùn)動(dòng)鏈關(guān)節(jié)與主動(dòng)關(guān)節(jié)映射關(guān)系式（15）、式（16）代入到p(θ)中，得到消去了被動(dòng)關(guān)節(jié)參數(shù)θ3、θ5的pe(θ)。獲得了混聯(lián)采摘機(jī)器人機(jī)械臂運(yùn)動(dòng)學(xué)正解方程（18）。對(duì)p(θ)求關(guān)于時(shí)間的一階導(dǎo)后整理即可得到機(jī)械臂速度正解方程，求二階導(dǎo)整理可得到機(jī)械臂加速度正解方程。

式中pe(θ)=[pe1pe2pe3]T為只包含有主動(dòng)關(guān)節(jié)參數(shù)的機(jī)器人末端執(zhí)行器位置矢量。

4 運(yùn)動(dòng)學(xué)試驗(yàn)與分析

為驗(yàn)證本文所提出的算法及基于該算法所構(gòu)建的機(jī)器人運(yùn)動(dòng)學(xué)正解方程的正確性，利用采摘機(jī)器人實(shí)體樣機(jī)所搭建的運(yùn)動(dòng)學(xué)試驗(yàn)平臺(tái)進(jìn)行了運(yùn)動(dòng)學(xué)試驗(yàn)[20-21]。圖4為運(yùn)動(dòng)學(xué)試驗(yàn)平臺(tái)，該平臺(tái)左側(cè)為三維坐標(biāo)測(cè)量?jī)x，右側(cè)為采摘機(jī)器人。采摘機(jī)器人以腰部旋轉(zhuǎn)基座的中心軸線為基礎(chǔ)坐標(biāo)系S的Z軸，以機(jī)器人腰部旋轉(zhuǎn)基座的底面為XOY平面。測(cè)量?jī)x基礎(chǔ)坐標(biāo)系S′在采摘機(jī)器人基礎(chǔ)坐標(biāo)系S中的位置坐標(biāo)為（1 500，0，0），其各坐標(biāo)軸方向與S系一致。試驗(yàn)過(guò)程中，通過(guò)三維坐標(biāo)測(cè)量?jī)x直接測(cè)得采摘機(jī)械臂末端執(zhí)行器上的標(biāo)記點(diǎn)相對(duì)測(cè)量?jī)x基礎(chǔ)坐標(biāo)系S′的坐標(biāo)，然后通過(guò)簡(jiǎn)單換算求得末端執(zhí)行器標(biāo)記點(diǎn)在機(jī)器人基礎(chǔ)坐標(biāo)系中的位置坐標(biāo)。

圖4 運(yùn)動(dòng)學(xué)試驗(yàn)平臺(tái)Fig.4 Test platform of kinematics

設(shè)定ω1=-5t，ω6=10t，ω7=-10t，ω8=-10t，νx=15t，νz=20t，其中ωi為第i個(gè)關(guān)節(jié)的角速度，rad/s；νx為水平滑塊速度，mm/s；νz為豎直滑塊移動(dòng)速度，mm/s；t為時(shí)間變量，s。選取1、2、3、4、5 s時(shí)的5組關(guān)節(jié)變量值，并將這5組數(shù)值和表1中的機(jī)械臂結(jié)構(gòu)參數(shù)代入到機(jī)械臂運(yùn)動(dòng)學(xué)正解方程（22）中，利用Matlab軟件編寫程序計(jì)算得出機(jī)械臂末端執(zhí)行器的理論位置坐標(biāo)值，然后將5組關(guān)節(jié)變量值輸入至機(jī)械臂控制軟件中，控制機(jī)械臂各個(gè)關(guān)節(jié)運(yùn)動(dòng)，使機(jī)械臂末端執(zhí)行器達(dá)到各組關(guān)節(jié)變量所對(duì)應(yīng)的最終位置，通過(guò)三維坐標(biāo)儀測(cè)量獲得機(jī)械臂末端執(zhí)行器的實(shí)際位置坐標(biāo)值。

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

根據(jù)試驗(yàn)結(jié)果整理可以得到表2所示的機(jī)器人末端執(zhí)行器在X、Y、Z軸上的位置誤差。由表2可知，末端執(zhí)行器在X、Y、Z軸上的最大絕對(duì)位置誤差為10.4 mm，可以通過(guò)提高機(jī)器人幾何參數(shù)和關(guān)節(jié)伺服定位的精度來(lái)降低該誤差。本文所提出的油茶果混聯(lián)采摘機(jī)器人是以油茶果樹(shù)干作為目標(biāo)夾持對(duì)象，其10.4 mm的末端執(zhí)行器最大絕對(duì)位置誤差遠(yuǎn)小于200 mm的最大開(kāi)度，滿足該機(jī)器人末端執(zhí)行器夾持樹(shù)干的作業(yè)要求，證明了本文所提出的基于旋量理論的混聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)分析方法的可行性和正確性。

表2 末端執(zhí)行器位置誤差Table 2 Position error of end effector

5 討論

本文針對(duì)利用D-H參數(shù)法在建立混聯(lián)采摘機(jī)器人正運(yùn)動(dòng)學(xué)方程時(shí)過(guò)于復(fù)雜的問(wèn)題，提出了一種基于旋量理論構(gòu)建混聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)正解方程的一般性方法。但機(jī)器人實(shí)際采摘過(guò)程中，往往是先由視覺(jué)系統(tǒng)對(duì)目標(biāo)樹(shù)干進(jìn)行定位檢測(cè)后，利用該目標(biāo)樹(shù)干位姿信息推導(dǎo)期望的機(jī)器人末端執(zhí)行器位姿，再反向求解對(duì)應(yīng)的各個(gè)關(guān)節(jié)轉(zhuǎn)角或位移，這種通過(guò)末端位姿反向求解各關(guān)節(jié)的過(guò)程稱為逆運(yùn)動(dòng)學(xué)分析。為發(fā)揮旋量理論在進(jìn)行機(jī)器人運(yùn)動(dòng)學(xué)分析的優(yōu)勢(shì)以及為機(jī)器人系統(tǒng)的運(yùn)動(dòng)規(guī)劃等問(wèn)題的研究提供基礎(chǔ),后續(xù)擬基于旋量理論對(duì)該混聯(lián)采摘機(jī)器人進(jìn)行逆運(yùn)動(dòng)學(xué)分析。

本文中所進(jìn)行的試驗(yàn)是在實(shí)驗(yàn)室無(wú)枝葉、果實(shí)遮擋的理想環(huán)境中完成的。實(shí)際的采摘過(guò)程中，末端執(zhí)行器為避讓枝葉、果實(shí)，順利夾取樹(shù)干，不僅要滿足位置要求，還需滿足姿態(tài)要求。試驗(yàn)得到的機(jī)器人末端執(zhí)行器的位置絕對(duì)誤差最大為10.4 mm，雖然滿足機(jī)器人末端執(zhí)行器對(duì)于夾取樹(shù)干的要求，但是誤差絕對(duì)值較大，會(huì)造成對(duì)機(jī)器人其他系統(tǒng)（如視覺(jué)識(shí)別系統(tǒng)）提出較高的精度要求。因此為提高末端執(zhí)行器位姿精度，降低對(duì)機(jī)器人其他系統(tǒng)的精度要求，項(xiàng)目組后續(xù)擬開(kāi)展該機(jī)器人樣機(jī)位姿誤差分析以及機(jī)器人位姿誤差補(bǔ)償?shù)妊芯抗ぷ鳌?/p>

6 結(jié)論

1）本文中所提出的油茶果混聯(lián)采摘機(jī)器人采用平行四邊形構(gòu)型作為手臂結(jié)構(gòu)，不僅可增加整個(gè)手臂的剛度，而且可以通過(guò)較小的驅(qū)動(dòng)行程獲得末端執(zhí)行器較大的工作行程，從而滿足了振動(dòng)采摘作業(yè)對(duì)機(jī)器人結(jié)構(gòu)強(qiáng)度以及末端執(zhí)行器較大工作空間的性能要求。

2）本文提出了基于旋量理論構(gòu)建混聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)正解方程的方法，該方法首先對(duì)混聯(lián)機(jī)器人運(yùn)動(dòng)鏈進(jìn)行分解，由此將混聯(lián)機(jī)器人正運(yùn)動(dòng)學(xué)問(wèn)題轉(zhuǎn)化成單開(kāi)鏈機(jī)器人以及單開(kāi)鏈中被動(dòng)關(guān)節(jié)與機(jī)器人主動(dòng)關(guān)節(jié)映射關(guān)系的運(yùn)動(dòng)學(xué)子問(wèn)題，然后利用指數(shù)積公式和結(jié)構(gòu)方程等數(shù)學(xué)工具求解上述子問(wèn)題，最后綜合各子問(wèn)題結(jié)果就獲得了混聯(lián)機(jī)器人的運(yùn)動(dòng)學(xué)正解方程。

3）本文對(duì)混聯(lián)采摘機(jī)器人進(jìn)行了結(jié)構(gòu)分析，利用所提出的運(yùn)動(dòng)學(xué)分析方法，建立了該機(jī)器人機(jī)械臂運(yùn)動(dòng)學(xué)正解方程。利用該方程和機(jī)器人運(yùn)動(dòng)學(xué)試驗(yàn)平臺(tái)分析得到末端工具坐標(biāo)系位置理論值和實(shí)際值之間最大絕對(duì)誤差為10.4 mm，遠(yuǎn)小于末端執(zhí)行器200 mm的開(kāi)度，滿足末端執(zhí)行器的夾持要求。由此驗(yàn)證了本文所提出方法對(duì)構(gòu)建混聯(lián)采摘機(jī)器人運(yùn)動(dòng)學(xué)正解方程的可行性及正確性。

[1] Santolaria J, Juan-José A, José-Antonio Y, et al. Kinematic parameter estimation technique for calibration and repeatability improvement of articulated arm coordinate measuring machines[J]. Precision Engineering, 2008, 32(4): 251－268.

[2] 張志強(qiáng). 混聯(lián)碼垛機(jī)器人運(yùn)動(dòng)學(xué)分析及仿真[J]. 機(jī)械設(shè)計(jì)，2010，27(11)：47－51. Zhang Zhiqiang. Kinematics analysis and simulation of series-parallel palletizing robot[J]. Journal of Machine Design, 2010, 27(11): 47－51. (in Chinese with English abstract)

[3] 孫祥溪. 工業(yè)碼垛機(jī)器人運(yùn)動(dòng)學(xué)仿真[J]. 計(jì)算機(jī)仿真，2013，30(3)：303－306. Sun Xiangxi. Kinematics simulation of industrial palletizing robot[J]. Computer Simulation, 2013, 30(3): 303－306. (in Chinese with English abstract)

[4] 張翔. 基于D－H法油茶果采摘機(jī)器人正運(yùn)動(dòng)學(xué)分析[J].農(nóng)機(jī)化研究，2014，4(4)：25－28. Zhang Xiang. Forward kinematics analysis of the camellia oleifera picked robot based on the D-H method[J]. Journal of Agricultural Mechanization Research, 2014, 4(4): 25－28. (in Chinese with English abstract)

[5] 張金玲. 新型可控碼垛機(jī)器人機(jī)構(gòu)運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)研究[D].南寧：廣西大學(xué)，2013. Zhang Jinling. Research on Kinematic and Dynamic of a Novel Palletizing Robot with Controllable Mechanism[D]. Nanning: Guangxi University, 2013. (in Chinese with English abstract)

[6] 劉揚(yáng). 混聯(lián)碼垛機(jī)器人運(yùn)動(dòng)學(xué)分析與仿真[J]. 機(jī)械與電子，2010，3：57－60. Liu Yang. Kinematics analysis and simulation of the hybrid stacking robot[J]. Machinery & Electronics, 2010, 3: 57－60. (in Chinese with English abstract)

[7] 理查德·摩雷，李澤湘，夏恩卡·薩思特里．機(jī)器人操作的數(shù)學(xué)導(dǎo)論[M]. 徐衛(wèi)良，錢瑞明，譯. 北京：機(jī)械工業(yè)出版社，1998：11－80.

[8] 盧宏琴. 基于旋量理論的機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)研究及其應(yīng)用[D]. 南京：南京航空航天大學(xué)，2007. Lu Hongqin. Kinematics and Dynamics Research of Robot Manipulators and Its Application Based on Screw Theory[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2007. (in Chinese with English abstract)

[9] 王小榮. 仿人機(jī)器人基于旋量理論的運(yùn)動(dòng)學(xué)分析方法[J].蘭州交通大學(xué)學(xué)報(bào)，2010，29(1)：73－75. Wang Xiaorong. Screw theory based method of kinematic analysis on humanoid[J]. Journal of Lanzhou Jiaotong University, 2010, 29(1): 73－75. (in Chinese with English abstract)

[10] 田海波. 串聯(lián)機(jī)器人機(jī)械臂工作空間與結(jié)構(gòu)參數(shù)研究[J].農(nóng)業(yè)機(jī)械學(xué)報(bào)，2013，44(4)：196－201. Tian Haibo. Workspace and structural parameters analysis for manipulator of serial robot[J]. Transactions of the Chinese Society for Agricultural Machinery, 2013, 44(4): 196－201. (in Chinese with English abstract)

[11] 朱大昌. 基于螺旋理論的3-RPS型并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)分析[J].機(jī)械設(shè)計(jì)與制造，2011，7：149－150. Zhu Dachang. Kinematic analysis of 3-RPS type parallel robot based on screw theory[J]. Machinery Design & Manufacture, 2011, 7: 149－150. (in Chinese with English abstract)

[12] 宮金良，黃風(fēng)安，張彥斐. 基于指數(shù)積的Delta機(jī)器人運(yùn)動(dòng)學(xué)正解建模[J]. 北京理工大學(xué)學(xué)報(bào)，2013，33(6)：581－585. Gong Jinliang, Huang Feng’an, Zhang Yanfei. Direct kinematic analysis of delta robot based on exponential product[J]. Transactions of Beijing Institute of Technology, 2013, 33(6): 581－585. (in Chinese with English abstract)

[13] 張?jiān)茓? 基于旋量理論的3-US并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)學(xué)[J]. 機(jī)械設(shè)計(jì)與研究，2014，30(2)：8－11. Zhang Yunjiao. Kinematic analysis of a 3-US parallel mechanism using screw theory[J]. Machine Design and Research, 2014, 30(2): 8－11. (in Chinese with English abstract)

[14] Jaime G. An application of screw theory to the kinematic analysis of a delta-type robot[J]. Journal of Mechanical Science and Technology, 2014, 28(9): 3785－3792.

[15] Kong X W, Lin C M. Type synthesis of 3-dof translational parallel manipulators based on screw theory[J]. Journal of Mechanical Design, 2004, 126(1): 83－92.

[16] 陳偉海. 基于螺旋理論模塊化機(jī)器人運(yùn)動(dòng)學(xué)分析與仿真[J].北京航空航天大學(xué)學(xué)報(bào)，2005，31(7)：814－818. Chen Weihai. Kinematic analysis and simulation for modular manipulators based on screw theory[J]. Journal of Beijing University of Aeronautics and Astronautics, 2005, 31(7): 814－818. (in Chinese with English abstract)

[17] 莊未. 基于螺旋理論的冗余液壓驅(qū)動(dòng)四足機(jī)器人運(yùn)動(dòng)學(xué)分析[J]. 機(jī)械設(shè)計(jì)，2011，28(11)：15－21. Zhuang Wei. Kinematic analysis of redundant hydraulic actuated quadruped robot based on screw theory[J]. Journal of Machine Design, 2011, 28(11): 15－21. (in Chinese with English abstract)

[18] 張付祥. 閉鏈級(jí)聯(lián)式機(jī)器人基于旋量理論的運(yùn)動(dòng)學(xué)分析方法[J]. 機(jī)械工程學(xué)報(bào)，2006，42(4)：112－117. Zhang Fuxiang. Screw theory based method of kinematic analysis on tandem robots of closed chains[J]. Chinese Journal of Mechanical Engineering, 2006, 42(4): 112－117. (in Chinese with English abstract)

[19] 黃真. 并聯(lián)機(jī)器人機(jī)構(gòu)學(xué)理論及控制[M]. 機(jī)械工業(yè)出版社，1997.

[20] 權(quán)龍哲. 三臂多功能棚室農(nóng)業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)分析及試驗(yàn)[J].農(nóng)業(yè)工程學(xué)報(bào)，2015，31(13)：32－38. Quan Longzhe. Kinematics analysis and experiment of multifunctional agricultural robot in greenhouse with three arms[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(13): 32－38. (in Chinese with English abstract)

[21] 肖名濤. 雙平行多桿栽植機(jī)構(gòu)運(yùn)動(dòng)學(xué)分析與試驗(yàn)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2014，30(17)：25－33. Xiao Mingtao. Kinematic analysis and experiment of dual parallelogram multi-pole planting mechanism[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2014, 30(17): 25－33. (in Chinese with English abstract)

Forward kinematics analysis and experiment of hybrid harvesting robot based on screw theory

Yang Hanjiang, Li Lijun※, Gao Zicheng
(School of Mechanical and Electrical Engineer, Central South University of Forestry and Technology, Changsha 410000, China)

In this paper, the research progress of the camellia oleifera fruit harvesting equipment was introduced. This paper simplified the structure of the 2P4R hybrid camellia oleifera fruit harvesting robot, which included waist part, arm part and wrist part. The manipulator could accomplish 6 kinds of movements including waist revolution, translational motion of vertical slider and horizontal slider, and 3 kinds of revolute motions of the wrist part. In arm part, the fore-arm is linked with back-arm by 2 components; one is link-bar below and the other is lower-arm above. Fore-arm is paralleled with back-arm and link-bar is paralleled with lower-arm. It means these 4 components form a parallel quadrilateral mechanism, which not only increases the stiffness of the arm part, but also can obtain larger end effector working space through a smaller drive stroke, and thus, the harvesting robot meets the requirements of large end effector working space when it clamps the camellia oleifera trunk. Robot can adjust the posture and position of the end effector by controlling the rotation of waist part and wrist part and the translation of arm part respectively. It makes the wrist part decrease by 2 degrees of freedom, lightens the burden of the arm, and also reduces the adverse effect of high-frequency vibration when the robot harvests camellia oleifera fruit. The screw theory and a kind of kinematics analysis method for hybrid robot were introduced. With the proposed method, firstly, the open chain from the waist part to the end effector of the manipulator was defined, which contained the vertical slider as major chain, and the kinematic analysis problem of hybrid robot was turned to the sub-problems of the kinematic analysis of an open major chain and a single closed chain; secondly, the kinematics equation of the closed chain and major chain was established, and then the conversion formula between the driving joint variable in closed chain and the passive joint variable in major chain was obtained by figuring out the equation of the closed chain; thirdly, the position of the end effector was got from the kinematics analysis of the closed chain by using the Lie group, Lie algebra, screw theory and product-of-exponential formula; and finally the positive kinematics equation for the position of the end effector was obtained from the synthesized results of the kinematic analysis of the major chain and closed chain. The first-order and second-order derivative of the position equation were the velocity equation and acceleration equation of the manipulator respectively. In order to verify the feasibility of the proposed method and the correctness of the kinematics equation, select a group of joint variable values, and then figure out the theoretical position coordinate of the end effector by putting the values into the kinematical equation in Matlab. A test platform for kinematics experiment was built, which consists of a three-dimensional position measuring instrument and a camellia oleifera fruit hybrid harvesting robot. The three-dimensional position measuring instrument was used to measure the actual position coordinates of the end effector directly, which was driven by the selected values. From the comparison between the theoretical and actual results, it was found that the maximum position error between kinematical equation resolution and actual position coordinate of the end effector was 10.4 mm, which was significantly smaller than 200 mm, the open size of the end effector. From the above experiment results, the correctness of the kinematics equation of the manipulator based on the proposed method was verified. Therefore, the application of the proposed method based on screw theory in kinematics analysis is beneficial for establishing the control method and trajectory planning.

robots; kinematics; models; screw theory; manipulator; hybrid robot

10.11975/j.issn.1002-6819.2016.09.008

TP24

A

1002-6819(2016)-09-0053-07

陽(yáng)涵疆，李立君，高自成. 基于旋量理論的混聯(lián)采摘機(jī)器人正運(yùn)動(dòng)學(xué)分析與試驗(yàn)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2016，32(9)：53－59.

10.11975/j.issn.1002-6819.2016.09.008 http://www.tcsae.org

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) doi：10.11975/j.issn.1002-6819.2016.09.008 http://www.tcsae.org

2015-10-08

2016-02-26

國(guó)家林業(yè)公益性項(xiàng)目（201104090）；湖南省高?？萍紕?chuàng)新團(tuán)隊(duì)支持計(jì)劃（2014207）

陽(yáng)涵疆，男，湖南益陽(yáng)人，主要從事機(jī)器人運(yùn)動(dòng)控制研究。長(zhǎng)沙中南林業(yè)科技大學(xué)機(jī)電工程學(xué)院，410000。Email：yanghanjiang@hotmail.com※通信作者：李立君，女，湖南寧鄉(xiāng)人，教授，博士，主要從事現(xiàn)代林業(yè)裝備研究。長(zhǎng)沙 中南林業(yè)科技大學(xué)機(jī)電工程學(xué)院，410000。

Email：junlili1122@163.com