基于測地線的3D-LIPM機(jī)器人軌跡規(guī)劃方法

張博聞，宋雪萍，張連東，平原，陳勇

(大連交通大學(xué) 機(jī)械工程學(xué)院，遼寧 大連 116028)*

?

基于測地線的3D-LIPM機(jī)器人軌跡規(guī)劃方法

張博聞，宋雪萍，張連東，平原，陳勇

(大連交通大學(xué) 機(jī)械工程學(xué)院，遼寧 大連 116028)*

提出一種基于測地線的3D-LIPM(3D Linear Inverted Pendulum model)機(jī)器人軌跡規(guī)劃方法.以3D-LIPM為研究對(duì)象，構(gòu)建使系統(tǒng)能量(動(dòng)能)為最優(yōu)的黎曼度量，建立3D-LIPM的測地線方程.采用數(shù)值分析方法對(duì)測地線微分方程組進(jìn)行求解，得出3D-LIPM機(jī)器人的軌跡變化規(guī)律：當(dāng)?shù)沽[與水平面的夾角θ位于0～π/2和π/2～π之間時(shí)，其投影與X軸的夾角ρ會(huì)隨之增大且曲率半徑也隨之增大；當(dāng)θ為π/2時(shí)，在一定范圍內(nèi)，曲率半徑無窮大，曲線隨之出現(xiàn)轉(zhuǎn)折點(diǎn)且呈下降趨勢；當(dāng)ρ改變時(shí)，角位移θ、線位移l與ρ的曲線變化關(guān)系不變，只是隨著ρ值的改變圖像呈平移關(guān)系；當(dāng)l改變時(shí)，角位移θ、角位移ρ與線位移l曲線變化關(guān)系近似不變，只是隨著l值的改變圖像呈現(xiàn)近似平移關(guān)系.

機(jī)器人軌跡規(guī)劃；測地線；能量最優(yōu)

0 引言

機(jī)器人軌跡規(guī)劃對(duì)研究機(jī)器人來說非常重要，機(jī)器人的軌跡規(guī)劃合理，可以準(zhǔn)確和快速地對(duì)機(jī)器人進(jìn)行一系列的控制.軌跡規(guī)劃包括關(guān)節(jié)空間的軌跡規(guī)劃和笛卡爾空間的軌跡規(guī)劃，前者是通過關(guān)節(jié)之間的變化來描述機(jī)器人軌跡的，后者是通過一種時(shí)間函數(shù)來描述機(jī)器人手部的位姿、速度和加速度[1].國內(nèi)外許多學(xué)者針對(duì)測地線方法進(jìn)行了相關(guān)研究，Milos Zefran等[2]應(yīng)用測地線研究了單自由度剛體的平滑軌跡；Park[3]將黎曼度量的概念用于機(jī)器人機(jī)構(gòu)的設(shè)計(jì)；范國良等[4]基于幾何方法對(duì)多智能體群體剛性運(yùn)動(dòng)的路徑規(guī)劃進(jìn)行了研究；Rodnay與Rimon[5]用測地線定性分析了二自由度機(jī)器人的動(dòng)力學(xué)性質(zhì).但并未給出一般條件下的測地線的確定方法.張連東等[6-8]采用微分幾何中活動(dòng)標(biāo)架方法，對(duì)平面二自由度機(jī)器人的相關(guān)問題運(yùn)用測地線方法方法進(jìn)行了研究以系統(tǒng)動(dòng)能作為黎曼度量， 使兩節(jié)點(diǎn)之間動(dòng)能最優(yōu)，將軌跡進(jìn)行優(yōu)化.

目前將測地線應(yīng)用于3D-LIPM機(jī)器人的最優(yōu)軌跡規(guī)劃方面的研究比較少.本文從系統(tǒng)模型3D-LIPM機(jī)器人的動(dòng)能出發(fā)，通過構(gòu)建測地線黎曼度量的方法，得出動(dòng)能最優(yōu)的軌跡，此分析方法在一定程度上減小了計(jì)算量，如果用雙腿12個(gè)自由度把系統(tǒng)的動(dòng)能表達(dá)出來，用測地線求其計(jì)算量龐大.該方法同樣適用于三維以上的機(jī)器人使用.

1 建立3D-LIMP機(jī)器人測地線方程

1.1 測地線

測地線是n維曲面上兩點(diǎn)之間的最短距離.黎曼度量是黎曼曲面上兩點(diǎn)之間距離的度量方式，不同的黎曼曲面對(duì)應(yīng)著不同的黎曼度量，不同的黎曼度量對(duì)應(yīng)著不同的測地線方程，在實(shí)際方程將黎曼度量中的系數(shù)帶入到測地線微分方程即可對(duì)其求解算出系統(tǒng)最優(yōu)軌跡.

1.2 構(gòu)建系統(tǒng)能量最優(yōu)的黎曼度量

把仿人機(jī)器人看成三維倒立擺模型，機(jī)器人在X-Y-Z坐標(biāo)中運(yùn)動(dòng)學(xué)正解(見圖1)：

(1)

圖1 關(guān)節(jié)空間坐標(biāo)中的機(jī)器人倒立擺

已知機(jī)器人端執(zhí)行器的角速度ωi，弧長s平方的黎曼度量表達(dá)式為：

(2)

式中,i的取值范圍為1～3.

弧長平方的黎曼度量表達(dá)式可寫成黎曼度量系數(shù)矩陣的形式為：

(3)

式中，矩陣G為黎曼度量的度量系數(shù)矩陣，gij為黎曼度量系數(shù).

根據(jù)上述已知可以得到機(jī)器人軌跡弧長的黎曼度量為：

ds2=dX2+dY2+dZ2

(4)

其中， s為機(jī)器人軌跡的弧長；ds2為弧長的黎曼度量.

將機(jī)器人的速度式中的X、Y、Z變量轉(zhuǎn)換成為極坐標(biāo)形式，從而得到弧長的黎曼度量為：

ds2=l2dθ2+l2cos2θdθ2+dl2

(5)

將式(5)寫成黎曼度量系數(shù)矩陣的形式為:

(6)

1.3 求Christoffel符號(hào)

黎曼度量系數(shù)已求出即式(6)，已知Christoffel符號(hào)公式：

(7)

為第一類克里斯托弗符號(hào)(Christoffel)，所有指標(biāo)i，j，k，m的取值范圍為1～n，gij為度量系數(shù)矩陣的逆矩陣所對(duì)應(yīng)的各元素.

1.4 求3D-LIMP機(jī)器人測地線微分方程

已知測地線的微分方程為：

(8)

已知測地線方程組為：

(9)

(10)

式(10)為最終的測地線微分方程組.

2 求解測地線微分方程

2.1 測地線微分方程初始條件

給定機(jī)器人關(guān)節(jié)空間坐標(biāo)內(nèi)的初值，隨意選一組解：

l=1.0; l=0.6

2.2 解測地線的微分方程

根據(jù)解出的測地線微分方程，在給定的初始條件下，求解微分方程組，得到該機(jī)器人的軌跡規(guī)劃結(jié)果，如圖2所示.

(a) 弧長-角位移θ

(b) 弧長-角速度

(c) 弧長-角位移ρ

(e) 弧長-線位移

(f) 弧長-線速度

3 對(duì)測地線方程進(jìn)行軌跡規(guī)劃

3.1 給定若干θ的值時(shí)的軌跡規(guī)劃圖

當(dāng)θ為某一定值時(shí)，得出角位移ρ和線位移l與角位移θ之間變化關(guān)系曲線如圖3～ 5所示.

由圖3可得：

(1)當(dāng)θ從0-π/2，角位移ρ隨著θ的增加而增加，而在θ=π/4，ρ=2.5時(shí)，曲線呈水平線，角位移θ最大值為1.25，這幾種情況表示機(jī)器人質(zhì)心軌跡呈弧線擺動(dòng)；θ=π/3時(shí)，曲線先增加后下降(轉(zhuǎn)折點(diǎn)在ρ=3處)，此時(shí)機(jī)器人質(zhì)心軌跡擺到一定角度后會(huì)往回運(yùn)動(dòng)，對(duì)應(yīng)的3D-LIPM不斷調(diào)整自己的姿勢以保持平衡.而θ=π/2時(shí)，當(dāng)角位移ρ增加，角位移θ呈水平線，當(dāng)ρ=3.8時(shí)圖像會(huì)垂直下降；機(jī)器人質(zhì)心會(huì)繞著z軸進(jìn)行旋轉(zhuǎn)，θ角始終不變，在轉(zhuǎn)折點(diǎn)處質(zhì)心會(huì)上升但桿在平面上的投影角度不會(huì)改變，只是投影長度變化；

(2)當(dāng)θ=2π/3和θ=3π/4時(shí)，曲線為凹曲線且線長較短.θ=2π/3時(shí)，曲線增長速率較快；θ=3π/4時(shí)，曲線增長速率較慢，表示倒立擺活動(dòng)范圍變小.

圖3 角位移ρ與角位移θ的關(guān)系圖

圖4 角位移ρ與線位移l的關(guān)系圖

由圖4可得：

(1)當(dāng)θ=π/8、π/6、π/4、π/3、2π/3、3π/4時(shí)，曲線為凹曲線增長，隨著θ增加，角位移ρ和線位移l都增加，但曲線增長率會(huì)逐漸降低.對(duì)應(yīng)的3D-LIPM旋轉(zhuǎn)的弧度半徑增大;

(2)當(dāng)θ=π/2，圖像先呈水平線增長，然后會(huì)垂直上升.此時(shí)的3D-LIPM與地面傾角最大，質(zhì)心點(diǎn)離地面高度不變，處于平穩(wěn)的過程，當(dāng)達(dá)到最平穩(wěn)的時(shí)候，3D-LIPM質(zhì)心與地面距離達(dá)到最大.

由圖5可得：

(1)當(dāng)θ=π/2時(shí)，線位移隨著角位移增大而減小.對(duì)應(yīng)的3D-LIPM與地面傾角會(huì)逐漸減小，但3D-LIPM質(zhì)心高度會(huì)增加.當(dāng)θ為0.8(角度值為45°)時(shí)，質(zhì)心高度不變；此時(shí)如果角度再減小的話，3D-LIPM機(jī)器人就會(huì)有不穩(wěn)定，產(chǎn)生摔倒趨勢;

(2)而對(duì)于其他情況，線位移隨著角位移增大而增大.對(duì)應(yīng)的3D-LIPM與地面傾角增大，3D-LIPM質(zhì)心高度會(huì)增加.

圖5 角位移θ與線位移l的關(guān)系圖

當(dāng)θ位于0～π/2之間以及π/2～π之間時(shí)，隨著θ的增加，ρ會(huì)隨之增大且曲率半徑增大，3D-LIPM機(jī)器人不斷進(jìn)行調(diào)整以保持平穩(wěn)；當(dāng)為π/2時(shí)，在一定范圍內(nèi)，曲率半徑無窮大，隨之出現(xiàn)轉(zhuǎn)折點(diǎn)且曲線呈下降趨勢，3D-LIPM機(jī)器人此時(shí)最平穩(wěn).

3.2 給定若干ρ的值時(shí)的軌跡規(guī)劃圖

當(dāng)ρ為某一定值時(shí)，得出角位移和線位移l與角位移ρ之間變化關(guān)系曲線如圖6～ 7所示.

圖6 角位移ρ與角位移θ的關(guān)系圖

由圖6可得：角位移隨著角位移ρ的增加而先增加后下降.表示此時(shí)3D-LIPM與地面傾角先增加后減小，3D-LIPM遠(yuǎn)離x軸，將向下一個(gè)位置邁出，機(jī)器人質(zhì)心軌跡相同.

由圖7可得：圖像呈曲線增長，隨著橫坐標(biāo)增加曲線斜率會(huì)增加，表示3D-LIPM機(jī)器人的質(zhì)心點(diǎn)離地面距離增加，質(zhì)心軌跡相同.

圖7 角位移ρ與線位移l的關(guān)系圖

由圖6和圖7可以判斷出角位移與線位移l的關(guān)系圖，圖中會(huì)出現(xiàn)多條線重疊于一條線，說明3D-LIPM與地面傾角先增加后減小，3D-LIPM機(jī)器人質(zhì)心高度增加.當(dāng)ρ增大時(shí)，曲線形狀趨于相同，圖像呈平移關(guān)系.3D-LIPM與地面傾角先增加后減小，3D-LIPM機(jī)器人質(zhì)心點(diǎn)離地面距離增加，以便向下一步邁出做好準(zhǔn)備.

3.3 給定若干l的值時(shí)的軌跡規(guī)劃圖

當(dāng)l為某一定值時(shí)，得出角位移、角位移ρ與線位移l之間變化關(guān)系曲線如圖8所示.

圖8 角位移ρ與角位移θ、線位移l的關(guān)系圖

由圖8角位移ρ與角位移的關(guān)系可得：當(dāng)桿長改變的時(shí)候，角位移θ與角位移ρ的關(guān)系變化不大，基本重疊于同一條曲線.表明桿長的改變對(duì)角位移和角位移ρ的影響不大.由圖8角位移ρ與線位移l的關(guān)系、角位移與線位移l的關(guān)系表明：桿長隨著角位移ρ的增加而增大，當(dāng)達(dá)到一定值時(shí)，變化加劇；當(dāng)l增大時(shí)，曲線形狀近似趨于相同，呈平移關(guān)系.

4 結(jié)論

(1)本文應(yīng)用測地線方法對(duì)仿人機(jī)器人的倒立擺模型進(jìn)行了分析，通過求解測地線方程得到了3D-LIPM倒立擺的測地線模型；

(2)分析了各參數(shù)之間的變化規(guī)律，可知當(dāng)位于0～π/2之間以及π/2～π之間時(shí)，隨著的增加，ρ會(huì)隨之增大且曲率半徑增大；當(dāng)為π/2時(shí)，在一定范圍內(nèi)，曲率半徑無窮大，隨之出現(xiàn)轉(zhuǎn)折點(diǎn)且曲線呈下降趨勢；當(dāng)分別給定角位移ρ和線位移l時(shí)，曲線關(guān)系變化不大，近似呈平移關(guān)系；

(3)文中的分析對(duì)象是三維模型，如果探討多維情況下的，測地線也是一種可行的研究方法.

[1]蔡自興,謝斌.機(jī)器人學(xué)[M]. 3版，北京:清華大學(xué)出版社，2015.

[2]ZEFRAN M, KUMAR V, CROKE B C.On the generation of smooth three-dimensional rigid body motions[J]. IEEE Transactions on Robotics and Automation, 1998, 14(4): 576-589.

[3]PARK FC. Distance Metrics on the Rigid Body Motions with Applications to Mechanism Design[J].ASME Journal of Mechanical Design, 1995,117(3):48-54.

[4]范國梁,王云寬.基于幾何方法的多智能體群體剛性運(yùn)動(dòng)的路徑規(guī)劃[J].機(jī)器人，2005,27(4):362-366.

[5]RODNAY G, RIMON E. Isometric visualization of configuration spaces of two degrees of freedom mechanisms[J]. Mechanism and Machine Theory, 2001, 36(4): 523-545.

[6]張連東,王德倫.一種基于測地線的機(jī)器人軌跡規(guī)劃方法[J].機(jī)器人, 2004, 26(1): 83-86.

[7]張連東.基于微分幾何學(xué)的機(jī)器人操作性能的研究[D].大連,大連理工大學(xué), 2004.

[8]ZHANG LIANDONG,ZHOU CHANGJIU.Robot optimal trajectory planning based on geodesics[C]//2007 IEEE International Conference on Control and Automation .Guangzhou, 2007: 2433-2436.

3D-LIPM Robot Trajectory Planning Method based On Geodesic

ZHANG Bowen, SONG Xueping, ZHANG Liandong, PING Yuan, CHEN Yong

(School of Mechanical Engineering, Dalian Jiaotong University, Dalian 116028, China)

A geodesic based trajectory planning method for 3D-LIPM(3D Linear Inverted Pendulum model) robots is proposed. Taking 3D-LIPM as the research object, the system energy (kinetic energy) is constructed as the optimal Riemann metric, and the geodesic equation of 3D-LIPM is established. Numerical analysis method is used to solve the differential equations of the geodesic line, and the change law of the trajectory of the 3D-LIPM robot is obtained. When the inverted pendulum and the horizontal plane angle θ is located in between 0-π/2 and π/2-π, the angle ρ between the projection and the X axis will subsequently increases, and the radius of curvature increases. When θ is π/2, the radius of curvature is infinity in a certain range, then the curve has turned point and downward trend. When changing ρ, invariant curve variation of θ angle displacement and angular displacement ρ and line displacement l, the image show a translation relation with the ρ value change. When changing l, invariant curve variation of θ angle displacement and angular displacement ρ and line displacement l, the image show the approximate relation shift with value l change.

robot trajectory planning; geodesics; energy optimal

1673- 9590(2016)06- 0089- 06

2016-06-23

國家自然科學(xué)基金資助項(xiàng)目(51275065，51305055)

張博聞(1992-)，男，碩士研究生；張連東(1971-)，男，副教授，博士，主要從事機(jī)器人軌跡規(guī)劃方面的研究E- mail:2285084880@qq.com.

A