劉海清，劉玉斌，張 赫，陳 杰，趙 杰

(哈爾濱工業(yè)大學(xué)機(jī)器人技術(shù)與系統(tǒng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，哈爾濱150001)

六足機(jī)器人具有豐富的步態(tài)和較強(qiáng)的地形適應(yīng)能力，能夠在失去若干肢體的情況下繼續(xù)行走，具有較高的可靠性和穩(wěn)定性.六足機(jī)器人的穩(wěn)定性主要取決于其自身的位姿，通過(guò)對(duì)位姿的實(shí)時(shí)調(diào)整，可提高機(jī)器人的步行穩(wěn)定性.

六足機(jī)器人行走時(shí)為實(shí)時(shí)變化的串并聯(lián)機(jī)構(gòu)，運(yùn)動(dòng)學(xué)分析復(fù)雜，采用位置控制方法對(duì)機(jī)器人進(jìn)行位姿調(diào)整難以滿足實(shí)時(shí)性，且運(yùn)算量大[1]，因此位姿調(diào)整通常采用力控制的方法.目前位姿調(diào)整采取的方法有懸浮控制[2－4]、力/位混合控制[5]以及力/力矩分配[6－7]等.力/位混合控制是為了使機(jī)器人腿從擺動(dòng)空間平穩(wěn)過(guò)渡到約束空間，力/力矩分配則是對(duì)機(jī)器人關(guān)節(jié)受力進(jìn)行均衡和優(yōu)化，懸浮控制則是對(duì)機(jī)器人軀干的位姿偏差進(jìn)行修復(fù).這三種方法都只是從一個(gè)側(cè)面對(duì)位姿進(jìn)行平衡和修復(fù)，缺乏對(duì)位姿的主動(dòng)調(diào)整.文獻(xiàn)[3]對(duì)四足機(jī)器人的足力控制進(jìn)行了研究，六足機(jī)器人的足力控制則更加復(fù)雜，目前尚無(wú)相關(guān)研究工作.文獻(xiàn)[8]提出通過(guò)調(diào)整足式機(jī)器人的重心位置到支撐多邊形的幾何中心來(lái)提高機(jī)器人的穩(wěn)定性，因此可以考慮將機(jī)器人重心調(diào)整引入到位姿調(diào)整中，來(lái)對(duì)機(jī)器人進(jìn)行主動(dòng)位姿調(diào)整.

本文提出了基于足力分布的位姿調(diào)整策略，通過(guò)力學(xué)分析建立了機(jī)器人在任意步態(tài)模式下的足力分布模型，求得足底受力的平衡關(guān)系;采用重心位置調(diào)整來(lái)進(jìn)行位姿優(yōu)化，提高機(jī)器人行走時(shí)的穩(wěn)定裕度，并獲得期望的足力分布;然后引入虛擬懸掛模型來(lái)計(jì)算足力補(bǔ)償，修復(fù)外界干擾帶來(lái)的軀干位姿偏差.最后通過(guò)Adams仿真分析驗(yàn)證了算法的有效性.

1 六足機(jī)器人的足力分布模型

建立足力分布模型的目的是為了實(shí)現(xiàn)機(jī)器人足底反力的合理分布.六足機(jī)器人在任何時(shí)刻都需要至少三條腿來(lái)支撐，因此需要分析機(jī)器人三足到六足支撐時(shí)的足力分布，以建立任意足步態(tài)情況下的足力分布模型.

1.1 機(jī)器人的力學(xué)模型

六足機(jī)器人的力學(xué)模型如圖1所示.全局坐標(biāo)系為O－XYZ，機(jī)器人重心坐標(biāo)為(xG，yG，zG);機(jī)體坐標(biāo)系為[XGG，YGG，ZGG]，機(jī)器人機(jī)體位于平面XGGYGG內(nèi);足端與地面接觸點(diǎn)為Pi=[xi，yi，zi](i為六足編號(hào));機(jī)器人受外力F=[Fx，F(xiàn)y，F(xiàn)z]和外力矩M=[Mx，My，Mz]作用;足端受力為fi=[fix，fiy，fiz].

圖1 仿生六足機(jī)器人力學(xué)模型

從位姿平衡角度考慮，機(jī)器人在豎直方向上的波動(dòng)以及沿俯仰、側(cè)翻方向的轉(zhuǎn)動(dòng)是機(jī)器人位姿的主要影響因素，這些因素與足端分布力的豎直分量fiz直接相關(guān).于是得到下列平衡方程式:

其中，腿i為機(jī)器人的支撐腿.

1.2 三足支撐時(shí)的足力分布

當(dāng)機(jī)器人為三足支撐時(shí)，設(shè)三支撐腿為A、B和C，解方程組(1)可得機(jī)器人的足力分布為:

1.3 四足支撐時(shí)的足力分布

機(jī)器人四足支撐時(shí)，力學(xué)平衡方程式(1)包含四個(gè)未知數(shù)，卻只有三個(gè)獨(dú)立方程，因而沒(méi)有惟一解，系統(tǒng)不確定.針對(duì)這樣的情況，需要引入額外的約束條件.通常情況下，當(dāng)支撐腿均勻承擔(dān)機(jī)器人的重力時(shí)，機(jī)器人的位姿穩(wěn)定，承載能力大，并且能耗小[9].因此，設(shè)定足力的方差最小作為目標(biāo)函數(shù):

其中:μ=(1/n)∑ifiz=(Fz+mg)/n，n為機(jī)器人支撐腿數(shù)目.由式(1)和(2)可求出四足支撐時(shí)的足力分布.

1.4 五足支撐時(shí)的足力分布

五足支撐時(shí)，將式(1)代入式(2)中，通過(guò)化解消元，目標(biāo)函數(shù)D變?yōu)橐粋€(gè)二元二次多項(xiàng)式，不妨設(shè)函數(shù):

將其轉(zhuǎn)化為求解函數(shù)f(x，y)的最小值問(wèn)題:

目標(biāo)函數(shù)(3)的求解過(guò)程如下:

1)求函數(shù)在定義域內(nèi)所有駐點(diǎn)處的函數(shù)值，駐點(diǎn)處函數(shù)的偏導(dǎo)數(shù)為零

2)求函數(shù)在定義域邊界上的最小值

3)比較函數(shù)值的大小，找出最小值

由于函數(shù)定義域?yàn)殚_(kāi)區(qū)間(0，mg+Fz)，又f(x，y)為二元二次函數(shù)，則方程組(4)為二元一次方程組，存在惟一解(x0，y0)，使得函數(shù)在該點(diǎn)取得最小值.x0、y0也就為足力fiz其中的兩個(gè)力.再代入式(1)即可求得其余的三個(gè)力，從而得到五足支撐時(shí)的足力分布.

1.5 六足支撐時(shí)的足力分布

六足支撐時(shí)，求足力分布的方法和五足支撐的解法類(lèi)似，此時(shí)目標(biāo)函數(shù)轉(zhuǎn)化為求三元二次函數(shù)D(fiz)=f(x，y，z)的最小值，函數(shù)駐點(diǎn)處的偏導(dǎo)數(shù)為零:

方程組(5)為三元一次方程組，存在惟一解(x0，y0，z0)，使函數(shù)f在該點(diǎn)取得最小值.x0、y0、z0也就為fix其中的三個(gè)力，代入式(1)即可求得其余的三個(gè)力.

2 機(jī)器人重心位置調(diào)整

六足機(jī)器人要實(shí)現(xiàn)其非結(jié)構(gòu)化地形的穩(wěn)定行走，僅僅依靠足力分配是不夠的，還需要配合機(jī)器人重心位置的調(diào)整，以提高其步態(tài)穩(wěn)定性.六足機(jī)器人在行走時(shí)的穩(wěn)定裕度越大則越穩(wěn)定，在進(jìn)行機(jī)器人步態(tài)設(shè)計(jì)時(shí)，對(duì)步態(tài)周期的每一分步都試圖使其穩(wěn)定裕度達(dá)到最大.為此，需要研究機(jī)器人在各種支撐情況下獲取最大穩(wěn)定裕度的條件.

2.1 六足機(jī)器人的最大穩(wěn)定裕度算法

機(jī)器人三足支撐時(shí)，由穩(wěn)定裕度的定義可知，當(dāng)機(jī)器人重心的投影落在三角形內(nèi)切圓心上時(shí)，穩(wěn)定裕度為最大.

受此啟發(fā)，當(dāng)機(jī)器人在四足、五足和六足支撐時(shí)，支撐多邊形最大內(nèi)接圓的圓心即為穩(wěn)定裕度最大的點(diǎn).這可以通過(guò)反證法簡(jiǎn)單證明:若該圓圓心O不是穩(wěn)定裕度最大的點(diǎn)，根據(jù)穩(wěn)定裕度的定義，即存在某點(diǎn)O1到多邊形各邊的最短距離(記為R1)大于該圓半徑，那么以點(diǎn)O1為圓心，以R1為半徑的圓是多邊形內(nèi)接圓.顯然，圓O1比O圓大，這與圓O是最大內(nèi)接圓相矛盾.

為求得該點(diǎn)坐標(biāo)，只需對(duì)多邊形的任意三條邊(及其延長(zhǎng)線)構(gòu)成的三角形計(jì)算其內(nèi)切圓，判斷該圓是否在多邊形內(nèi)部，窮舉所有情況即可，步驟如下:

1)任取多邊形三條邊X、Y、Z組成三角形;

2)求出此三角形的內(nèi)切圓Q;

3)檢查多邊形的邊與圓Q是否相交.

如果檢查通過(guò)，那圓Q即為多邊形內(nèi)最大內(nèi)接圓.以四足支撐為例，如圖2所示，A、B、C、D為機(jī)器人落足點(diǎn)，則情形I中的圓為支撐四邊形內(nèi)最大內(nèi)接圓.

當(dāng)機(jī)器人的落足點(diǎn)不在同一水平面時(shí)，可將各落足點(diǎn)投影到同一水平面上，再利用上述算法近似求得穩(wěn)定裕度最大的坐標(biāo)點(diǎn).

圖2 四足支撐時(shí)的最大穩(wěn)定裕度

2.2 重心調(diào)整后的足力分布

機(jī)器人重心調(diào)整后，其足力分布也隨之改變.將上節(jié)求得的重心投影坐標(biāo)(xg，yg)代入力學(xué)平衡方程(1)得:

與之前的解法類(lèi)似，當(dāng)機(jī)器人三足支撐時(shí)，由式(6)可求得足力惟一解，對(duì)于其他支撐情況，結(jié)合目標(biāo)函數(shù)D(fiz)→min即可求出各足力值.該足力大小即為機(jī)器人運(yùn)動(dòng)過(guò)程中期望的足力分布值，此時(shí)機(jī)器人具有最大的穩(wěn)定裕度.

3 基于虛擬懸掛模型的足力補(bǔ)償

通過(guò)建立機(jī)器人的虛擬懸掛模型，采用足力補(bǔ)償策略來(lái)修復(fù)外界干擾帶來(lái)的偏差，維持機(jī)器人的位姿穩(wěn)定.

3.1 虛擬懸掛模型

如圖3所示，在機(jī)器人軀干的豎直方向、俯仰和側(cè)翻方向建立三個(gè)虛擬彈簧阻尼器，并規(guī)定軀干的高度和姿態(tài)變化為θp、θr和Δz，修復(fù)偏差所需要的力矩和力為ΔMp、ΔMr和ΔFz，則有下列方程:

其中:Kz、Kp、Kr為虛擬彈簧的剛度(角剛度)系數(shù);Cz、Cp、Cr為虛擬阻尼的阻尼(角阻尼)系數(shù);m為機(jī)器人的質(zhì)量，Ip、Ir分別為沿俯仰、側(cè)翻方向的轉(zhuǎn)動(dòng)慣量;¨z、¨θp、¨θr分別為沿著豎直、俯仰、側(cè)翻方向外界的干擾量.

圖3 仿生六足機(jī)器人虛擬懸掛模型

3.2 足力補(bǔ)償

由式(7)得系統(tǒng)的特征方程為:

為了提高系統(tǒng)的響應(yīng)速度并減小超調(diào)，阻尼系數(shù)ζ應(yīng)在0.7～1.0之間選擇.同時(shí)，為避免外界干擾給系統(tǒng)給帶來(lái)的共振等影響，應(yīng)盡可能地增大自振頻率.據(jù)此，選取自然角頻率和阻尼系數(shù)為:

機(jī)器人的質(zhì)量m=4.926 kg，俯仰方向的轉(zhuǎn)動(dòng)慣量Ip=8.978×104kg·mm2，俯仰方向的轉(zhuǎn)動(dòng)慣量Ir=1.157×105kg·mm2.代入上式則得

通過(guò)機(jī)器人軀干上安裝的姿態(tài)傳感器檢測(cè)出其位姿變化，由式(7)計(jì)算出修復(fù)偏差所需的力/力矩;然后代入平衡方程(1)中，取代原來(lái)的力/力矩，并結(jié)合目標(biāo)函數(shù)D(fiz)→min，即可求出各足力補(bǔ)償，將該補(bǔ)償值與原來(lái)的足力一起用于機(jī)器人的控制中.實(shí)際上位姿調(diào)整是一個(gè)動(dòng)態(tài)過(guò)程，隨著位姿的恢復(fù)，足力補(bǔ)償值也逐漸趨于零.

4 仿真分析

為了驗(yàn)證調(diào)整算法的有效性，在Pro/e環(huán)境下建立了六足機(jī)器人和彈性路面模型，然后通過(guò)MECH/Pro接口導(dǎo)入到Adams環(huán)境下.機(jī)器人以三角步態(tài)行走，取占空系數(shù)為0.7，分別采用位置控制和位姿控制進(jìn)行實(shí)驗(yàn)，仿真結(jié)果如圖4～7所示.

六足機(jī)器人前進(jìn)時(shí)，重心的位置變化是俯仰角上下波動(dòng)的主要原因，通過(guò)重心位置的實(shí)時(shí)調(diào)整，使波動(dòng)得到抑制.圖4中，位置控制下，機(jī)器人起伏波動(dòng)明顯，而采用位姿調(diào)整后，俯仰角的波動(dòng)得到明顯的抑制.而翻滾角的變化則與支撐足的足力分布密切相關(guān)，通過(guò)力學(xué)模型計(jì)算出足力的合理分布，并由虛擬懸掛模型對(duì)足力進(jìn)行實(shí)時(shí)補(bǔ)償，修復(fù)位姿偏差.圖5中，位姿控制相比位置控制的翻滾角的變化幅度更小，并且圖中的斜線為位姿控制下的足力實(shí)時(shí)補(bǔ)償.圖6表明位姿調(diào)整提高了機(jī)器人行走時(shí)的穩(wěn)定裕度，這是通過(guò)實(shí)時(shí)調(diào)整機(jī)器人重心

圖6 重心穩(wěn)定裕度的變化曲線

位置來(lái)實(shí)現(xiàn)的.圖7為重心的高度變化曲線，由于位置控制在腿向前擺動(dòng)的同時(shí)移動(dòng)支撐腿，此時(shí)機(jī)器人六條腿都處于運(yùn)動(dòng)狀態(tài)，因而重心起伏較大.而位姿控制由于重心調(diào)整環(huán)節(jié)的存在，擺動(dòng)腿運(yùn)動(dòng)和支撐腿運(yùn)動(dòng)不是同時(shí)進(jìn)行，因而重心波動(dòng)小，運(yùn)動(dòng)平穩(wěn).

圖7 重心高度的變化曲線

5 結(jié)語(yǔ)

基于建立的仿生六足機(jī)器人的力學(xué)模型，提出了基于足力分布的位姿調(diào)整算法.通過(guò)基于重心調(diào)整策略的位姿調(diào)整提高了機(jī)器人步行的穩(wěn)定性，并且結(jié)合阻力補(bǔ)償策略有效的抑制了外界的擾動(dòng)，提高機(jī)器人非結(jié)構(gòu)化地形的步行能力.仿真結(jié)果表明，應(yīng)用該位姿調(diào)整算法提高了六足機(jī)器人行走時(shí)的穩(wěn)定裕度，減小了軀干的波動(dòng)，使機(jī)器人能夠在非結(jié)構(gòu)化地形中平穩(wěn)地行走.

[1]NILSON G M，QUINN R D.Posture Control of a Cockroachlike Robot[C]//IEEE International Conference on Robotics and Automation，[S.l]:[s.n]，1998:9－14.

[2]SONG SM，WALDRON K J.Machines that walk:the adaptive suspension vehicle[M].Massachusetts London:MIT Press Cambridge，1989.

[3]KAN Y，HIROYUKI I，SHIGEO.Sky－Hook Suspension Control of a Quadruped Walking Vehicle[J].The Journal of Robotics Society of Japan，1994，12(7):1066－1071.

[4]HUANG Q J，YASUYUKI F.Posture and Vibration Control Based on Virtual Suspension Model Using Sliding Mode Control for Six－Legged Walking Robot[C]//Proceedings of the IEEE/RSJ.，[S.l]:[s.n]，2006:5232－5237.

[5]HUANG Q J，KENZO N.Neuro－Based Position and Force Hybrid Control of Six－Legged Walking Robot，Special Issue on Modern Trends on Mobile Robotics[J].Journal of Robotics and Mechatronics，2002，14(4):534－543.

[6]CHEN X D，KEIGOW，KAZUO K，et al.Optimal Force Distribution for the Legs of a Quadruped Robot[J].Machine Intelligence＆Robotic Control，1999，1(2):87－94.

[7]ERDEN M S，LEBLEBICIOGLU K.Torque Distribution in a Six－Legged Robot[C]//IEEE Transactions on Robotics，[S.l]:[s.n]，2007，23(1):179－186.

[8]CELAYA E，PORTA JM.A Control Structure for the Locomotion of a Legged Robot on Difficult Terrain[C]//IEEE Robotics＆Automation，[S.l]:[s.n]，1998:43－51.

[9]GORINEVSKY D M，SHNEIDER A Y.Force Control in Locomotion of Legged Vehicles over Rigid and Soft Surfaces[J].Int.J.Robotics Research.1990，9(2):4－23.