張金柱，金振林,2※，陳廣廣

（1. 燕山大學(xué)機(jī)械工程學(xué)院，秦皇島 066004； 2. 上海交通大學(xué)機(jī)械系統(tǒng)與振動(dòng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，上海 200240）

六足步行機(jī)器人腿部機(jī)構(gòu)運(yùn)動(dòng)學(xué)分析

張金柱1，金振林1,2※，陳廣廣1

（1. 燕山大學(xué)機(jī)械工程學(xué)院，秦皇島 066004； 2. 上海交通大學(xué)機(jī)械系統(tǒng)與振動(dòng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，上海 200240）

為了提高農(nóng)業(yè)自動(dòng)化程度，拓寬農(nóng)業(yè)機(jī)器人的應(yīng)用范圍，提高農(nóng)業(yè)機(jī)器人對(duì)工作環(huán)境的適應(yīng)性及工作的靈活性，該文介紹了一種六足步行機(jī)器人三自由度腿部機(jī)構(gòu)。該機(jī)構(gòu)由并聯(lián)驅(qū)動(dòng)機(jī)構(gòu)和行走機(jī)構(gòu)組成，既具有并聯(lián)機(jī)構(gòu)的特點(diǎn)，又具有很好的防護(hù)性。該文建立了驅(qū)動(dòng)機(jī)構(gòu)動(dòng)平臺(tái)線速度與角速度之間的關(guān)系矩陣和該腿部機(jī)構(gòu)全雅可比矩陣，繪制了全雅可比矩陣條件數(shù)分布圖，建立了并聯(lián)驅(qū)動(dòng)機(jī)構(gòu)和腿部行走機(jī)構(gòu)顯式3×3×3形式Hessian矩陣。在滿足步矩為300 mm、越障高度為200 mm的條件下，利用組合多項(xiàng)式的方法，對(duì)該腿部足端進(jìn)行軌跡規(guī)劃，并求出了足端軌跡函數(shù)。將該軌跡函數(shù)作為足端輸入，分別繪制了機(jī)構(gòu)驅(qū)動(dòng)關(guān)節(jié)在擺動(dòng)相的角速度、角加速度理論曲線和虛擬樣機(jī)仿真曲線。分析曲線中的數(shù)據(jù)可得角速度、角加速度的理論與仿真結(jié)果相近度均可達(dá)到10-3mm，從而驗(yàn)證了理論分析的正確性。該研究為六足機(jī)器人的開發(fā)和控制提供了參考。

機(jī)器人；計(jì)算機(jī)仿真；運(yùn)動(dòng)學(xué)；雅可比矩陣；軌跡規(guī)劃

0 引言

近年來(lái)在農(nóng)業(yè)自動(dòng)化領(lǐng)域，農(nóng)業(yè)機(jī)器人的研究得到了眾多學(xué)者的重視[1-5]，但專門用于農(nóng)業(yè)領(lǐng)域的機(jī)器人并不多見(jiàn)[6]。據(jù)中國(guó)農(nóng)業(yè)主管部門調(diào)查發(fā)現(xiàn)，隨著中國(guó)現(xiàn)代農(nóng)業(yè)生產(chǎn)方式從粗放型向集約型的不斷轉(zhuǎn)變[7]，農(nóng)業(yè)從業(yè)人員迫切希望在更多的農(nóng)業(yè)作業(yè)領(lǐng)域應(yīng)用機(jī)器人。因此，農(nóng)業(yè)機(jī)器人的多樣化是農(nóng)業(yè)機(jī)器人發(fā)展的趨勢(shì)。

六足步行機(jī)器人作為農(nóng)業(yè)機(jī)器人的一種，擁有穩(wěn)定性好、運(yùn)動(dòng)靈活性好、占地面積小、對(duì)地面適應(yīng)性好等特點(diǎn)[8]，可作為對(duì)履帶式和導(dǎo)軌式農(nóng)業(yè)移動(dòng)平臺(tái)的補(bǔ)充，大大的擴(kuò)展了機(jī)器人在農(nóng)業(yè)領(lǐng)域的作業(yè)場(chǎng)合。并聯(lián)結(jié)構(gòu)腿作為六足機(jī)器人腿的一種，具有承載能力強(qiáng)、結(jié)構(gòu)緊湊、驅(qū)動(dòng)或電機(jī)靠近基座或者在基座上等特點(diǎn)，可使其作為步行機(jī)器人腿部機(jī)構(gòu)。在并聯(lián)腿的研究領(lǐng)域，相關(guān)學(xué)者[9-13]做了大量研究工作，并取得了豐碩的成果，上海交通大學(xué)高峰團(tuán)隊(duì)研制的“章魚機(jī)器人”也采用并聯(lián)結(jié)構(gòu)腿。然而，上述機(jī)器人的腿部驅(qū)動(dòng)或電機(jī)隨腿部一起運(yùn)動(dòng)，很難進(jìn)行腿部防護(hù)，使該類機(jī)器人易受雨天、雷電、風(fēng)沙及意外撞擊的影響，制約了其在農(nóng)業(yè)領(lǐng)域的廣泛應(yīng)用。本文介紹的六足機(jī)器人采用的腿部機(jī)構(gòu)為三自由度2RUS+RU+FD機(jī)構(gòu)(其中R、U、S分別表示轉(zhuǎn)動(dòng)副、虎克鉸、球副；FD表示平行四邊形放大機(jī)構(gòu))，并將該機(jī)構(gòu)3個(gè)R副作為驅(qū)動(dòng)副，以期提高六足機(jī)器人的防護(hù)特性，拓寬其應(yīng)用領(lǐng)域。

低損傷、精細(xì)操作[14]是現(xiàn)代農(nóng)業(yè)對(duì)農(nóng)業(yè)機(jī)器人的要求。雅可比矩陣和Hessian矩陣是農(nóng)業(yè)機(jī)器人實(shí)現(xiàn)上述工作指標(biāo)的理論基礎(chǔ)。目前，針對(duì)少自由度并聯(lián)機(jī)構(gòu)的雅可比矩陣和Hessian矩陣，國(guó)內(nèi)外學(xué)者做了大量的研究工作[15-19]，然而專門針對(duì)并聯(lián)驅(qū)動(dòng)的串并混聯(lián)機(jī)構(gòu)的全雅可比矩陣和Hessian矩陣的研究鮮有報(bào)道。本文在考慮動(dòng)物的行走過(guò)程本質(zhì)[20]的前提下，建立了2RUS+RU驅(qū)動(dòng)機(jī)構(gòu)動(dòng)平臺(tái)的線速度與角速度之間的關(guān)系矩陣、2RUS+RU驅(qū)動(dòng)機(jī)構(gòu)的統(tǒng)一量綱雅可比矩陣和腿部機(jī)構(gòu)的全雅可比矩陣以及并聯(lián)驅(qū)動(dòng)機(jī)構(gòu)和行走機(jī)構(gòu)二者各自的3×3×3的Hessian矩陣，以期為六足步行機(jī)器人的腿部設(shè)計(jì)與精確控制提供參考。

1 腿部機(jī)構(gòu)描述

本文所研究的腿部機(jī)構(gòu)為一種六足步行機(jī)器人腿部機(jī)構(gòu)，該機(jī)器人采用六條相同的機(jī)械腿，圖1所示為該六足步行機(jī)器人整體結(jié)構(gòu)示意圖，圖中所示行走方向?yàn)橹鬟\(yùn)動(dòng)方向，在該方向上機(jī)器人的基本運(yùn)動(dòng)要求為：步距300 mm，越障高度200 mm。此外，為了提高機(jī)器人腿部的靈活性，利用半球形足使其與地面以點(diǎn)接觸的形式接觸。

圖1 六足步行機(jī)器人整體結(jié)構(gòu)圖Fig.1 Overall structure of six-legged warking robot

圖2所示為該機(jī)器人腿部機(jī)構(gòu)簡(jiǎn)圖，由圖2可知該腿部機(jī)構(gòu)由驅(qū)動(dòng)機(jī)構(gòu)和行走機(jī)構(gòu)串聯(lián)而成，其中，驅(qū)動(dòng)機(jī)構(gòu)為2RUS+RU并聯(lián)機(jī)構(gòu)，行走機(jī)構(gòu)為平行四邊形機(jī)構(gòu)（即FD機(jī)構(gòu)），二者通過(guò)一個(gè)中間球副S連接。在2RUS+RU中，2條RUS支鏈相對(duì)于RU支鏈所在的平面對(duì)稱布置，在初始狀態(tài)下，RU支鏈和FD機(jī)構(gòu)布置于同一豎直平面內(nèi)。設(shè)機(jī)構(gòu)的主要幾何參數(shù)為RDiUi=mi(i=1,2,3)，UiSi=li(i=1,2)，UiG=l3，SiG=a，R4R1=l4，R1R3=l5，R1R2=l6，R1B=c，SB=lg，R5C=l7，GS=l33，RD1RD2=d，O0RD3=h1。定義：∥表示平行，⊥表示垂直。則機(jī)構(gòu)中桿件和運(yùn)動(dòng)副的幾何關(guān)系有l(wèi)4∥l5，l5∥l7，l6∥R3R5，l4⊥S1S2，lg⊥l6。

圖2 六足步行機(jī)器人腿部機(jī)構(gòu)簡(jiǎn)圖Fig.2 Sketch map of leg mechanism of six-legged walking robot

坐標(biāo)系建立方面,如圖2所示，以RD1和RD2連線的中點(diǎn)O0為原點(diǎn)建立固定坐標(biāo)系O0-x0y0z0，其中x0,y0,z0為坐標(biāo)軸，x0∥RD1RD2，y0⊥RD1RD2，z0由右手螺旋定則確定；以G為坐標(biāo)原點(diǎn)建立動(dòng)坐標(biāo)系G-xdydzd，xd,yd,zd為坐標(biāo)軸，xd∥S1S2,yd⊥S1S2,zd∥l3；為方便各支鏈連桿姿態(tài)描述，以Ui為坐標(biāo)原點(diǎn)建立各支鏈參考坐標(biāo)系U-xiyizi(i=1,2,3)，其中xi,yi,zi為坐標(biāo)軸，yi∥mi,zi⊥mi且zi在豎直平面內(nèi)方向向上，xi由右手螺旋定則確定；同時(shí)，以Ui為坐標(biāo)原點(diǎn)建立各支鏈連桿連體坐標(biāo)系Ui-eifigi(i=1,2,3)，其中ei，fi，gi為坐標(biāo)軸，fi與相應(yīng)支鏈虎克鉸遠(yuǎn)離杠桿的轉(zhuǎn)動(dòng)軸線重合，gi∥li，ei由右手螺旋定則確定；以H為原點(diǎn)建立參考坐標(biāo)系H-x4y4z4和動(dòng)參考坐標(biāo)系H-x5y5z5，有x4∥x0，x5⊥l4且x5⊥l6，y4∥y5∥y0，z4和z5由右手螺旋定則確定。

2 機(jī)構(gòu)的速度分析

2.1 2RUS+RU驅(qū)動(dòng)機(jī)構(gòu)速度分析

2.1.1 2RUS+RU驅(qū)動(dòng)機(jī)構(gòu)速度伴隨性分析

利用旋轉(zhuǎn)變換矩陣T來(lái)表示動(dòng)坐標(biāo)系G-xdydzd相對(duì)于固定坐標(biāo)系O0-x0y0z0的位姿變化。由圖2可知，動(dòng)坐標(biāo)系經(jīng)過(guò)如下變化

式中TG為動(dòng)坐標(biāo)系相對(duì)定坐標(biāo)系的齊次變換矩陣；xdt, ydt, zdt為在變化過(guò)程中動(dòng)坐標(biāo)系的x, y和z軸。θp3表示轉(zhuǎn)動(dòng)副RD3相對(duì)初始位置的轉(zhuǎn)角，α1, β1分別為虎克鉸U3的2個(gè)轉(zhuǎn)軸相對(duì)于豎直位置的轉(zhuǎn)角，rad；h1為O0RD3的豎直距離，mm；l3為連桿U3G的長(zhǎng)度，mm。且有如下關(guān)系

由機(jī)構(gòu)自由度組成可以看出，α，β為并聯(lián)驅(qū)動(dòng)機(jī)構(gòu)動(dòng)平臺(tái)XY歐拉角（即動(dòng)坐標(biāo)系先處于如圖2中初始位置，然后繞動(dòng)坐標(biāo)系的x軸旋轉(zhuǎn)α角，再繞動(dòng)坐標(biāo)系的y軸旋轉(zhuǎn)β），rad。對(duì)于G點(diǎn)位置矢量，由坐標(biāo)變換得

式中G=[x0y0z0]T為動(dòng)平臺(tái)參考點(diǎn)G在固定坐標(biāo)系中的位置矢量。

由式（1）～式（3）位置關(guān)系得

式中m3為杠桿RD3U3的長(zhǎng)度，mm；x0，y0，z0分別為G點(diǎn)的三維橫縱坐標(biāo)，mm；θp3表示轉(zhuǎn)動(dòng)副RD3相對(duì)初始位置的轉(zhuǎn)角，rad；為書寫方便，文中規(guī)定sβ=sinβ，cβ=cosβ，sα=sinα，cα=cosα，cθp3=cosθp3，sθp3=sinθp3。由式（4）可以看出，姿態(tài)和位置并不獨(dú)立，它們之間存在一定的伴隨關(guān)系。

對(duì)式（4）求導(dǎo)，整理得式中v為動(dòng)平臺(tái)末端線速度，mm/s；γ并聯(lián)機(jī)構(gòu)繞z軸的歐拉角，rad。

由于機(jī)構(gòu)的角速度與歐拉角角速度之間的關(guān)系[21]為

式中J01為動(dòng)平臺(tái)線速度與歐拉角角速度之間的關(guān)系矩陣；w3為動(dòng)平臺(tái)末端角速度矢量，wx、wy、wz分別為w3的3個(gè)坐標(biāo)分量，rad/s；Rα、Rβ、Rγ分別為歐拉角α、β、γ的轉(zhuǎn)軸。

由此得動(dòng)平臺(tái)末端線速度v和角速度w3之間的關(guān)系為

式中J02? J01為2RUS+RU驅(qū)動(dòng)機(jī)構(gòu)動(dòng)平臺(tái)線速度與角速度之間的關(guān)系矩陣。

2.1.2 2RUS+RU驅(qū)動(dòng)機(jī)構(gòu)雅可比矩陣的建立

根據(jù)幾何關(guān)系，建立矢量方程為

式中G為G點(diǎn)在固定坐標(biāo)系中的位置矢量；ui，RDi和gi(i=1,2,3)分別表示支鏈i的虎克鉸中心點(diǎn)Ui、轉(zhuǎn)動(dòng)副中心點(diǎn)RDi(i=1,2,3)在固定坐標(biāo)系O0-x0y0z0中的位置矢量和連桿方向矢量；Li(i=1,2,3)為i支鏈連桿在固定坐標(biāo)系O0-x0y0z0中的長(zhǎng)度矢量；li為i支鏈連桿桿長(zhǎng)，mm；ni=[0 cθpisθpi]T(i=1,2,3)為驅(qū)動(dòng)杠桿的單位方向矢量；θpi為轉(zhuǎn)動(dòng)副RDi(i=1,2,3)相對(duì)于初始位置的轉(zhuǎn)角，rad；mi(i=1,2,3)為杠桿i的桿長(zhǎng)，mm；ai(i=1,2)分別為桿SiG的長(zhǎng)度矢量。

對(duì)式（8）、式（9）兩式分別對(duì)時(shí)間求導(dǎo)，整理得

聯(lián)合式（7）～式（11）得并聯(lián)驅(qū)動(dòng)機(jī)構(gòu)雅克比矩陣為

式中J0為并聯(lián)驅(qū)動(dòng)機(jī)構(gòu)的速度雅克比矩陣；nit=[0 cθp3sθp3]T為ni求導(dǎo)所得。

又由于點(diǎn)S和點(diǎn)G之間的速度關(guān)系為

式中l(wèi)33為桿GS在固定坐標(biāo)系中的長(zhǎng)度矢量，l33為桿GS的長(zhǎng)度，mm；vsp為點(diǎn)S在固定坐標(biāo)系中的速度矢量；I 為3×3單位矩陣；3?g表示g3的反對(duì)稱矩陣。

由式（13）得

式中J1為點(diǎn)S和點(diǎn)G之間速度映射矩陣。

2.2 行走機(jī)構(gòu)雅可比矩陣求解

根據(jù)圖2中點(diǎn)C和S在各個(gè)坐標(biāo)系中的幾何關(guān)系得

式中Cp，Sp，Cc，Sc，S0，C0分別表示C和S點(diǎn)在固定坐標(biāo)系、參考坐標(biāo)系、動(dòng)參考坐標(biāo)系中的位置矢量；B0為B點(diǎn)在動(dòng)參考坐標(biāo)系中的位置矢量；R4=Rot(y4, θ3), H=[0 lylz]T；y4為參考坐標(biāo)系H-x4y4z4的坐標(biāo)軸；θ3為行走機(jī)構(gòu)相對(duì)初始位置繞y4軸轉(zhuǎn)過(guò)的角度，rad；ly、lz分別為H點(diǎn)在固定坐標(biāo)系中沿y軸和z軸方向坐標(biāo)，mm。

在動(dòng)參考坐標(biāo)系中，得點(diǎn)C和θ1，θ2之間的位置關(guān)系

式中θ1，θ2分別為桿件R4R1和R1R2相對(duì)于動(dòng)參考坐標(biāo)系H-x5y5z5中的y5軸轉(zhuǎn)過(guò)的角度，rad；l4、l5、l6、l7分別為桿件R4R1、R1R3、R1R2、R5C的長(zhǎng)度，mm；xc0、yc0、zc0分別為點(diǎn)C在動(dòng)參考坐標(biāo)系中的坐標(biāo)，mm。

在動(dòng)參考坐標(biāo)系中，得θ1,θ2和點(diǎn)B之間的位置關(guān)系

式中c為桿件R1B 的桿長(zhǎng)，mm；xb0、yb0、zb0分別為點(diǎn)B在動(dòng)參考坐標(biāo)系中的坐標(biāo)，mm。

在動(dòng)參考坐標(biāo)系中，得點(diǎn)S和點(diǎn)B之間的位置關(guān)系

式中xs0、ys0、zs0分別為點(diǎn)S在動(dòng)參考坐標(biāo)系中的坐標(biāo)，mm；lg為桿件SB的桿長(zhǎng)，mm。

式（15）兩端分別對(duì)時(shí)間求導(dǎo)得

式中vcp, vsp, vcc, vsc分別為點(diǎn)C和點(diǎn)S在固定坐標(biāo)系和參考坐標(biāo)系中的速度矢量。

為了方便vcc提取，由式（16）得

式中xcc、zcc為速度矢量vcc的x軸和y軸方向分量，mm。

式（16）～式（20）兩端分別對(duì)時(shí)間求導(dǎo)，構(gòu)造得

式中vyc0、vzc0、vys0、vzs0分別表示點(diǎn)C和點(diǎn)S在動(dòng)參考坐標(biāo)系中速度矢量沿著y軸和z軸方向的分量，mm/s；1θ˙、2θ˙、3θ˙分別為角θ1、θ2和θ3對(duì)時(shí)間的導(dǎo)數(shù)，rad/s。

則由式（21）～式（23）得行走機(jī)構(gòu)的速度關(guān)系為

2.3 腿部機(jī)構(gòu)全雅可比矩陣及其條件數(shù)

腿部機(jī)構(gòu)全雅可比矩陣為輸入轉(zhuǎn)動(dòng)副RDi的轉(zhuǎn)動(dòng)角速度與足端點(diǎn)C在固定坐標(biāo)系中的速度vcp之間的映射關(guān)系。由式（12）～式（14）和式（24）得腿部機(jī)構(gòu)全雅可比矩陣為

式中J為腿部機(jī)構(gòu)全雅克比矩陣。

依據(jù)文獻(xiàn)[22]中雅克比條件數(shù)的定義，可知雅克比矩陣條件數(shù)隨足端點(diǎn)C的位置變化而變化。為了說(shuō)明雅可比矩陣在評(píng)價(jià)運(yùn)動(dòng)學(xué)性能方面的有效性[23]，給定一組機(jī)構(gòu)的幾何參數(shù)如下（單位為mm）：d=276，h1=70，a=100，l1= l2=200，l3=250，l33=120，m1=m2=200，m3=370，c=370，l4=l5=200，l6=300，l7=450，ly=190，lz=55，lg=46.5?；谏鲜鋈趴杀染仃嘕，參照腿部越障和步矩要求，給定轉(zhuǎn)動(dòng)副的轉(zhuǎn)動(dòng)角度在[-45°，45°]范圍內(nèi)轉(zhuǎn)動(dòng)時(shí)，全雅可比矩陣J的條件數(shù)k(J)在足端點(diǎn)C的z坐標(biāo)（相對(duì)于固定坐標(biāo)系）為?700 mm時(shí)隨足端點(diǎn)C的x坐標(biāo)和y坐標(biāo)變化的分布規(guī)律如圖3所示，此外，足端點(diǎn)C的z坐標(biāo)為?600 、?800 mm時(shí)腿部全雅可比矩陣J的條件數(shù)k(J)隨x坐標(biāo)和y坐標(biāo)變化的分布規(guī)律與圖3類似。由此可知，腿部速度全雅可比矩陣條件數(shù)k(J)在工作空間內(nèi)連續(xù)分布，在工作空間的中心區(qū)域條件數(shù)k(J)的值較小，且變化平緩，說(shuō)明在此區(qū)域機(jī)構(gòu)的靈活性較好，滿足機(jī)器人步距300 mm，越障高度200 mm的主體運(yùn)動(dòng)要求。

圖3 足端點(diǎn)的z坐標(biāo)為?700 mm時(shí)的全雅可比矩陣條件數(shù)分布圖Fig.3 Distribution diagram of conditional number of entire jacobian matrix (z coordinate of foot endpoint is ?700 mm)

3 腿部機(jī)構(gòu)加速度分析

3.1 并聯(lián)驅(qū)動(dòng)機(jī)構(gòu)加速度分析

對(duì)式（10）和式（11）兩端分別對(duì)時(shí)間求導(dǎo)得

由式（8）、式（9）中矢量關(guān)系有

由式（29）兩端對(duì)時(shí)間求導(dǎo)得

則由式（26）～式（30）得

將式（31）變形，寫為

其中

為并聯(lián)驅(qū)動(dòng)機(jī)構(gòu)的3×3×3形式的Hessian矩陣，式中的各項(xiàng)因子可由式（31）求得。

3.2 腿部行走機(jī)構(gòu)加速度分析

對(duì)于行走機(jī)構(gòu)，對(duì)式（23）的兩端對(duì)時(shí)間求導(dǎo)得

將式（33）變形，寫為

利用HXij表示上式中HXi(i=1～4)的第j(j=1～3)個(gè)元素，該元素為三維列矢的3×3矩陣，式中各項(xiàng)因子可由式（33）求得。結(jié)合設(shè)計(jì)過(guò)程可知，以上速度和加速度模型為控制模型的建立和控制系統(tǒng)的搭建提供了參考。

4 腿部機(jī)構(gòu)運(yùn)動(dòng)學(xué)仿真驗(yàn)證

4.1 單腿足端軌跡規(guī)劃

多足機(jī)器人的步態(tài)規(guī)劃對(duì)機(jī)器人運(yùn)動(dòng)的靈活性和平穩(wěn)性有很大影響[24-25]，文中以六足機(jī)器人為例。根據(jù)對(duì)六足昆蟲行走方式的研究[26-27]，發(fā)現(xiàn)六足昆蟲的行走方式大都為三角步態(tài)，在此步態(tài)下，腿部的主要運(yùn)動(dòng)（前進(jìn)后退、下蹲起立）及腿部載重和快速奔跑時(shí)主要承載力都分布在腿部的矢狀面，腿部冠狀面的運(yùn)動(dòng)主要為平衡、轉(zhuǎn)彎、受沖擊時(shí)的左右移動(dòng)[28]。因此，本文以機(jī)器人直線行走（矢狀面內(nèi)的運(yùn)動(dòng)）為例，針對(duì)有障礙物的地面，為減少機(jī)器人運(yùn)動(dòng)過(guò)程中機(jī)身震動(dòng)，提高運(yùn)動(dòng)穩(wěn)定性，使其重心始終保持水平運(yùn)動(dòng)，以軌跡平順、落地平穩(wěn)及滿足越障要求為目標(biāo)，對(duì)腿部足端進(jìn)行軌跡規(guī)劃；考慮設(shè)計(jì)要求為越障高度H=200 mm,步距S=300 mm的條件下，為了更加方便的描述軌跡曲線，建立軌跡坐標(biāo)系A(chǔ)-xyz如圖所示,其中坐標(biāo)原點(diǎn)與A點(diǎn)重合，y軸方向由A指向D，z軸豎直向上，x軸由右手螺旋定則確定。圖4為直線行走時(shí)的腿部位姿及足端軌跡示意圖。

為了讓機(jī)器人在行走的過(guò)程碰撞少,平順性好，把軌跡擺動(dòng)相分為3段，分別為線段AEB，BC，CFD。其中AEB可以避免在起步的過(guò)程中與足端近前方的突起相碰撞，BC段為勻速直線運(yùn)動(dòng)，提高了電機(jī)輸入的穩(wěn)定性,CFD段可以增加落地的平穩(wěn)性。

圖4 腿部行走位姿及足端軌跡示意圖Fig.4 Schematic diagram of walking pose of leg and trajectory of foot

綜合考慮該腿部機(jī)構(gòu)運(yùn)動(dòng)性能和機(jī)器人的行走速度要求，設(shè)定該機(jī)器人單腿行走周期T=4 s，其中擺動(dòng)相時(shí)間為TswinD=2 s，支撐相時(shí)間為Tstance=2 s。為了確保機(jī)器人在運(yùn)動(dòng)過(guò)程中平穩(wěn)無(wú)沖擊，且具有較好的起落特性，設(shè)定足端點(diǎn)在擺動(dòng)相軌跡上各點(diǎn)的狀態(tài)量如表1所示。

表1 足端點(diǎn)在擺動(dòng)相軌跡上各點(diǎn)的狀態(tài)量Table 1 State quantity of foot endpoint in trajectories of swing phase

設(shè)線段AEB、CFD的多項(xiàng)式表達(dá)式均為

式中g(shù)(t)=[x(t) y(t) z(t)]T，ai=[aixaiyaiz]T。

由上述設(shè)定狀態(tài)量得AEB和CFD段的曲線都為6次多項(xiàng)式曲線，為了表達(dá)簡(jiǎn)潔，規(guī)定多項(xiàng)式系數(shù)向量組成為

式中a為軌跡多項(xiàng)系數(shù)組成的向量。

將足端點(diǎn)在擺動(dòng)相軌跡上各點(diǎn)的狀態(tài)量帶入表達(dá)式（34）中得各段多項(xiàng)式的系數(shù)向量為

BC段為勻速運(yùn)動(dòng)，則得軌跡表達(dá)式為

式中x(t)，y(t)，z(t)分別為足端點(diǎn)的3個(gè)坐標(biāo)分量。

4.2 基于上述軌跡的逆運(yùn)動(dòng)學(xué)仿真

利用表1中提供的幾何參數(shù)，將上述擺動(dòng)相的軌跡函數(shù)作為理論速度模型和仿真模型的足端輸入，得到腿部的驅(qū)動(dòng)轉(zhuǎn)動(dòng)副的角速度、角加速度隨時(shí)間的變化曲線如圖5所示。

經(jīng)過(guò)對(duì)圖5的分析表明，當(dāng)該腿部機(jī)構(gòu)的足端按給定的運(yùn)動(dòng)規(guī)律運(yùn)動(dòng)時(shí)，利用上述建立的速度、加速度模型計(jì)算出的驅(qū)動(dòng)關(guān)節(jié)的角速度、角加速度與利用ADAMS教學(xué)版軟件中的函數(shù)控制法（即將上述所得軌跡函數(shù)作為足端的控制輸入函數(shù)）進(jìn)行運(yùn)動(dòng)仿真的結(jié)果吻合且精確度可達(dá)到10-3，從而說(shuō)明了上述腿部速度、加速度模型的正確性。同時(shí)從仿真結(jié)果可知，驅(qū)動(dòng)關(guān)節(jié)的角速度最大值小于60 °/s（約0.33π rad/s）、角加速度的最大值小于220 °/s2（約1.22π rad/s2），表明該腿部機(jī)構(gòu)在按照所規(guī)劃軌跡運(yùn)動(dòng)過(guò)程中所需驅(qū)動(dòng)轉(zhuǎn)速和轉(zhuǎn)矩較小，對(duì)驅(qū)動(dòng)是有利的，由此表明了軌跡規(guī)劃的合理性。上述研究結(jié)果為控制和驅(qū)動(dòng)電機(jī)的選取提供了參考。

圖5 角速度及角加速度理論、仿真結(jié)果Fig.5 Theory and simulation results of Angular velocity and angular acceleration

5 結(jié)論

1）建立了2RUS+RU機(jī)構(gòu)的速度關(guān)系矩陣、腿部機(jī)構(gòu)3×3形式的全雅可比矩陣以及腿部各組成機(jī)構(gòu)顯式3×3×3形式Hessian矩陣，繪制了雅克比矩陣條件數(shù)分布規(guī)律。該分布規(guī)律表明：2RUS+ RU+FD在工作空間內(nèi)的運(yùn)動(dòng)靈活性較好，滿足腿部越障高度為200 mm、步距為300 mm的主體運(yùn)動(dòng)要求。

2）理論計(jì)算結(jié)果和仿真結(jié)果具有很好的一致性且精確度可達(dá)到10-3mm，從而驗(yàn)證了理論模型的正確性。同時(shí)，從仿真結(jié)果中的驅(qū)動(dòng)關(guān)節(jié)角速度和角加速度變化曲線可以看出，驅(qū)動(dòng)關(guān)節(jié)的角速度最大值小于0.33π rad/s、角加速度的最大值小于1.22π rad/s2，所需驅(qū)動(dòng)轉(zhuǎn)速和轉(zhuǎn)矩較小表明了軌跡規(guī)劃的合理性。

本文研究的成果為該腿部機(jī)構(gòu)的進(jìn)一步研究提供了參考，為拓展農(nóng)業(yè)機(jī)器人的應(yīng)用范圍及擴(kuò)大六足機(jī)器人的應(yīng)用領(lǐng)域提供了參考。

[1] Shigehiko H, Kenta S. Evaluation of a strawberry-harvesting robot in a field test[J]. Bio Systems Engineering, 2010, 105(2): 160－171.

[2] Kanae T, Tateshi F, Akira A, et al. Cherry-harvesting robot[J]. Compute Electron Agriculture, 2011, 63(1): 65－72.

[3] 紀(jì)超，馮青春，袁挺，等. 溫室黃瓜采摘機(jī)器人系統(tǒng)研制及性能分析田[J]. 機(jī)器人，2011，33(6)：726－730. Ji Chao, Feng Qingchun, Yuan Ting, et al. Development and performance analysis on cucumber harvesting robot system in greenhouse[J]. Robot, 2011, 33(6): 726－730. (in Chinese with English abstract)

[4] 牛雪梅，高國(guó)琴，劉辛軍，等. 三自由度驅(qū)動(dòng)冗余并聯(lián)機(jī)構(gòu)動(dòng)力學(xué)建模與試驗(yàn)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2013，29(16)：31－41. Niu Xuemei, Gao Guoqin, Liu Xinjun, et al. Dynamics modeling and experiments of 3-DOF parallel mechanism with actuation redundancy[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2013, 29(16): 31－41. (in Chinese with English abstract)

[5] Tang Jinglei, Jing Xu, He Dongjian, et al. Visual navigation control for agricultural robot using serial BP neural network[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2011, 27(2): 194－198.唐晶磊，景旭，何東健，等. 基于串行BP網(wǎng)絡(luò)的農(nóng)業(yè)機(jī)器人視覺(jué)導(dǎo)航控制[J]. 農(nóng)業(yè)工程學(xué)報(bào), 2011, 27(2): 194－198. (in English with Chinese abstract)

[6] 崔冰艷，金振林. 農(nóng)業(yè)機(jī)器人新型肘關(guān)節(jié)的靜力學(xué)性能分析[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2011，27(3)：122－125. Cui Bingyan, Jin Zhenlin. Analysis of statics performance for a novel elbow joint of agricultural robot[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2011, 27(3): 122－125. (in Chinese with English abstract)

[7] 楊邦杰，裴志遠(yuǎn)，周清波，等. 我國(guó)農(nóng)情遙感監(jiān)測(cè)關(guān)鍵技術(shù)研究進(jìn)展[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2002，18(3)：191－194. Yang Bangjie, Pei Zhiyuan, Zhou Qingbo, et al. Key technologies of crop monitoring using remote sensing at a national scale: Progress and problems[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2002, 18(3): 191－194. (in Chinese with English abstract)

[8] 榮譽(yù)，金振林，曲夢(mèng)可. 三自由度并聯(lián)機(jī)械腿靜力學(xué)分析與優(yōu)化[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2012，28(20)：41－49. Rong Yu, Jin Zhenlin, Qu Mengke. Statics analysis and optimal design of 3-DOF parallel mechanical leg[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2012, 28(20): 41－49. (in Chinese with English abstract)

[9] Sugahara Y, Carbone G, Hashimoto K, et al. Experimental stiffness measurement of WL-16RⅡbiped walking vehicle during walking operation[J]. Journal of Robotics and Mechatronics, 2007, 19(3): 272－280.

[10] 王洪波，徐桂玲，胡星，等. 四足并聯(lián)腿步行機(jī)器人動(dòng)力學(xué)[J]. 機(jī)械工程學(xué)報(bào)，2012，48(23)：76－82. Wang Hongbo, Xu Guiling, Hu Xing, et al. Dynamics of quadruped walking robot with parallel leg mechanism[J]. Journal of Mechanical Engineering, 2012, 48(23): 76－82. (in Chinese with English abstract)

[11] 崔冰艷. 仿生機(jī)器人并聯(lián)關(guān)節(jié)/運(yùn)動(dòng)單元的性能分析與設(shè)計(jì)[D]. 秦皇島：燕山大學(xué)，2011：83－125. Cui Bingyan. Performance Analysis and Design for Parallel Joint/kinematical Unit of Bionic Robot[D]. Qinhuangdao: Yanshan University, 2011: 83－125. (in Chinese with English abstract)

[12] 程剛. 并聯(lián)式仿生機(jī)械腿結(jié)構(gòu)設(shè)計(jì)及動(dòng)力學(xué)研究[D]. 北京：中國(guó)礦業(yè)大學(xué)，2008. Chen Gang. Study on Structure Design and Dynamic Performance of the Parallel Bionic Robot Leg[D]. Beijing: China University of Mining and Technology, 2008. (in Chinese with English abstract)

[13] 榮譽(yù)，金振林，崔冰艷. 六足農(nóng)業(yè)機(jī)器人并聯(lián)腿構(gòu)型分析與結(jié)構(gòu)參數(shù)設(shè)計(jì)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2012，28(15)：9－14. Rong Yu, Jin Zhenlin, Cui Bingyan. Configuration analysis and structure parameter design of six-leg agricultural robot with parallel-leg mechanisms[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2012, 28(15): 9－14. (in Chinese with English abstract)

[14] 張鐵中，楊麗，陳兵旗，等. 農(nóng)業(yè)機(jī)器人技術(shù)研究進(jìn)展[J].中國(guó)科學(xué)，2010，40(增刊)：71－87. Zhang Tiezhong, Yang Li, Chen Bingqi, et al. Research progress of agricultural robot technology[J]. Science China, 2010,40(Supp.): 71－87. (in Chinese with English abstract)

[15] 李劍鋒，費(fèi)仁元，范金紅，等. 具有大位置空間的3自由度并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)性能分析[J]. 機(jī)械工程學(xué)報(bào)，2007，43(8)：53－59. Li Jianfeng, Fei Renyuan, Fan Jinhong, et al. Kinematic performance analysis of 3-DOF parallel mechanism with large positional workspace[J]. Chinese Journal of Mechanical Engineering, 2007, 43(8): 53－59. (in Chinese with English abstract)

[16] Joshi S, Tsai L W. Jacobian analysis of limited-DOF parallel manipulators[J]. ASME Journal of Mechanical Design, 2002, 124(2): 254－258.

[17] Kim S G, Ryu J. New dimensionally homogeneous Jacobian matrix formulation by three end-effector points for optimal design of parallel manipulators[J]. IEEE Transactions on Robotics and Automation, 2003, 19(4): 731－736.

[18] 劉海濤. 少自由度機(jī)器人機(jī)構(gòu)一體化建模理論、方法及工程應(yīng)用[D]. 天津：天津大學(xué)，2010：39－58. Liu Haitao. Unified Parameter Modeling of Lower Mobility Robotic Manipulators: Theory, Methodology and Application[D]. Tianjin: University of Warwiek, 2010: 39－58. (in Chinese with English abstract)

[19] 胡波. 基于約束力/矩的少自由度并聯(lián)機(jī)構(gòu)和串并聯(lián)機(jī)構(gòu)理論研究[D]. 秦皇島：燕山大學(xué)，2010：16－41. Hu Bo. Theoritical study of Limited-DOF Parallel Manipulators and Serial-parallel Manipulators Based on Constrained Force/torque[D]. Qinghuangdao: Yanshan University, 2010: 16－41. (in Chinese with English abstract)

[20] 尹文英，宋大祥，楊星科，等. 六足動(dòng)物（昆蟲）系統(tǒng)發(fā)生的研究[M]. 北京：科學(xué)出版社，2008：1－77.

[21] 胡波，宋春曉，張慶玲，等. 2(2-UPR+SPR)串并聯(lián)機(jī)構(gòu)雅可比矩陣的建立[J]. 中國(guó)機(jī)械工程，2015，26(7)：853－858. Hu Bo, Song Chunxiao, Zhang Qingling, et al. Jacobian matrix establishment of 2(2-UPR+SPR) serial-parallel manipulator[J]. Chinese Journal of Mechanical Engineering, 2015, 26(7): 853－858. (in Chinese with English abstract)

[22] 熊有倫，丁漢，劉恩滄. 機(jī)器人學(xué)[M]. 北京：機(jī)械工業(yè)出版社，1993：97－114.

[23] 張艷偉，韋斌，王南，等. 空間轉(zhuǎn)動(dòng) 3-SPS-S 并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)學(xué)性能分析[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2012，43(4)：212－215，207. Zhang Yanwei, Wei Bin,Wang Nan, et al. Kinematic performance analysis of 3-SPS-S spatial[J]. Transactions of the Chinese Society of Agricultural Mechinery, 2012, 43(4): 212－215, 207. (in Chinese with English abstract)

[24] 張赫. 具有力感知功能的六足機(jī)器人及其崎嶇地形步行控制研究[D]. 哈爾濱：哈爾濱工業(yè)大學(xué)，2013：49－62. Zhang He. Research on Force Sensing Based Hexapod Robot and Control for Walking on Uneven Terrain[D]. Harbin: Harbin Institute of Technology, 2013: 49－62. (in Chinese with English abstract)

[25] 陳甫. 六足仿生機(jī)器人的研制及其運(yùn)動(dòng)規(guī)劃研究[D]. 哈爾濱：哈爾濱工業(yè)大學(xué)，2009：65－80. Chen Fu. Development of Hexapod Biomimetic Robot and Research on Its Motion Planning Issue[D]. Harbin: Harbin Institute of Technology, 2009: 65－80. (in Chinese with English abstract)

[26] 尹文英. 有關(guān)六足動(dòng)物（昆蟲）系統(tǒng)分類中的爭(zhēng)論熱點(diǎn)[J].生命科學(xué)，2001，13(2)：49－53. Yin Wenying. On the hotly debated points in phylogeny of Hexapoda[J]. Chinese Bulletin of Life Seienees, 2001, 13(2): 49－53. (in Chinese with English abstract)

[27] 李斌，何斌，蘇永清. 濕吸型仿生六足機(jī)器人的設(shè)計(jì)[J].華中科技大學(xué)學(xué)報(bào)：自然科學(xué)版，2008，36(增刊1)：210－212. Li Bin, He Bin, Su Yongqing. Design of hexapod bionic robot on wet adhesion[J]. Huazhong Univ. of Sci. &Tech.: Natural Science Edition. 2008, 36(Supp.l): 210－212. (in Chinese with English abstract)

[28] 田興華，高峰，陳先寶，等. 四足仿生機(jī)器人混聯(lián)腿構(gòu)型設(shè)計(jì)及比較[J]. 機(jī)械工程學(xué)報(bào)，2013，49(6)：81－88. Tian Xinghua, Gao Feng, Chen Xianbao, et al. Mechanism design and comparison for quadruped robot with parallel-serial leg[J]. Journal of Mechanical Engineering, 2013, 49(6): 81－88. (in Chinese with English abstract)

Kinematic analysis of leg mechanism of six-legged walking robot

Zhang Jinzhu1, Jin Zhenlin1,2※, Chen Guangguang1
(1. College of Mechanical Engineering, Yanshan University, Qinhuangdao 066004, China; 2. State Key Laboratory of Mechanical System and Vibration, Shanghai Jiao Tong University, Shanghai 200240, China)

In order to increase the automation level of agricultural operations, broaden the application scope of agricultural robot, and improve the ability of adapting to the different working environment and flexible work, a novel three-degree-of-freedom leg mechanism used in the six-legged walking robot is introduced. This leg mechanism comprised a drive mechanism based on 2RUS+RU parallel manipulator and a traveling mechanism based on parallelogram mechanism. The motor of drive mechanism is fixed on body frame. This leg mechanism has not only the advantage of parallel mechanism, but also a good protectiveness. In this paper, kinematic analysis and simulation of leg mechanism of six-legged walking robot is accomplished. Firstly, based on the intrinsic relation between the angular velocity and the angular velocity of Euler angles of the dynamic platform, the relationship matrix between linear velocity and angular velocity of driving mechanism is established. Based on that, the entire Jacbian matrix in the 3×3 form of the leg mechanism is deduced by using the relationship matrix derivative method, and the explicit Hessian matrix in the 3×3×3 form of the parallel drive mechanism and the leg walking mechanism is obtained, which also adopts the method of derivative matrix. Secondly, with the rationed rotation angle of the revolute joint ranging in [-45°, 45°], a distribution diagram of condition number of the integral Jacobian matrix is drawn. The condition number of integral Jacobian matrix is changed slowly and smaller in the central region of the workspace in this diagram, so that the mechanism flexibility is good in this area and can meet the requirements of the robot movement. Lastly, under the conditions that were step increment of 300 mm and crossing obstacle height of 200 mm, the trajectory planning of the foot end is accomplished and the track function of the foot end is presented based on the method of combined polynomial, which can make the robot stable and free from impact and have a good landing performance in the process of motion. Under the condition of the geometrical parameters of the leg mechanism, the simulation model of the robot's leg is established and the track function of the foot end is presented when the six-legged walking robot walks straight. The function of the trajectory is as input. The velocity and acceleration curves of driver deputy based on the analytical solutions and virtual prototype are described. Through the data analysis in curves, the accuracy of angular velocity and angular acceleration based on the theory and simulation results both can reach 10-3mm, which indicates that the theoretic analysis is correct and feasible. In addition, the simulation results show that the maximum of angular velocity of drive joint is less than 0.33π rad/s and the maximum of angular acceleration of drive joint is less than 1.22π rad/s. Therefore, the driving speed and torque of leg mechanism are both smaller in the course of the planned trajectory movement, which is favorable to drive. Accordingly, the rationality of the trajectory planning is confirmed. The results can provide the theoretical reference for the development and control of the hexapod robot.

robots; computer simulation; kinematics; Jacobian matrix; trajectory planning

10.11975/j.issn.1002-6819.2016.09.007

TP242

A

1002-6819(2016)-09-0045-08

張金柱，金振林，陳廣廣. 六足步行機(jī)器人腿部機(jī)構(gòu)運(yùn)動(dòng)學(xué)分析[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2016，32(9)：45－52.

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

Zhang Jinzhu, Jin Zhenlin, Chen Guangguang. Kinematic analysis of leg mechanism of six-legged walking robot[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(9): 45－52. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2016.09.007 http://www.tcsae.org

2015-09-06

2016-03-10

機(jī)械系統(tǒng)與振動(dòng)國(guó)家重點(diǎn)實(shí)驗(yàn)室課題資助項(xiàng)目（MSV201506）；河北省高等學(xué)?？茖W(xué)技術(shù)研究項(xiàng)目（QN2015185）

張金柱，男，博士生，主要研究方向?yàn)槎嘧悴叫袡C(jī)器人技術(shù)及其應(yīng)用。秦皇島 燕山大學(xué)機(jī)械工程學(xué)院，066004。Email：ysuzhangjz@126.com

※通信作者：金振林，男，教授、博士研究生導(dǎo)師，主要研究方向?yàn)椴⒙?lián)機(jī)器人技術(shù)及應(yīng)用。秦皇島 燕山大學(xué)機(jī)械工程學(xué)院，066004。Email：zljin@ysu.edu.cn