張世龍,劉國(guó)棟
(江南大學(xué)物聯(lián)網(wǎng)工程學(xué)院,江蘇 無(wú)錫 214122)
雙足機(jī)器人是一門(mén)由仿生學(xué)、機(jī)械設(shè)計(jì)、加工和計(jì)算機(jī)控制理論以及人工智能等多種學(xué)科形成的交叉學(xué)科。然而,步態(tài)規(guī)劃是研究雙足機(jī)器人控制的難點(diǎn)和熱點(diǎn),步行過(guò)程必須保證雙足機(jī)器人的穩(wěn)定性[1]。雙足機(jī)器人控制多采用靜態(tài)步態(tài)規(guī)劃得到一步或多步參考軌跡,由關(guān)節(jié)控制器伺服跟蹤期望軌跡。但由于機(jī)器人自身的機(jī)械誤差,模型誤差以及地面摩擦力等環(huán)境因素的影響,機(jī)器人的實(shí)際步態(tài)必然與期望步態(tài)之間會(huì)產(chǎn)生一定的誤差,這些誤差的積累很可能會(huì)使機(jī)器人不穩(wěn)定而摔倒[2]。為了減小或消除這些誤差,必須對(duì)機(jī)器人的步行模式進(jìn)行在線(xiàn)修正,以實(shí)現(xiàn)機(jī)器人的穩(wěn)定行走。
為解決此問(wèn)題,學(xué)者們?cè)岢鲞^(guò)對(duì)機(jī)器人進(jìn)行在線(xiàn)全身關(guān)節(jié)實(shí)時(shí)修正的方法,但由于機(jī)器人在行走時(shí)會(huì)存在慣性力的作用,在進(jìn)行不適當(dāng)?shù)膶?shí)時(shí)修正時(shí),會(huì)導(dǎo)致機(jī)器人的零力矩點(diǎn)(Zero Moment Point, ZMP)位置波動(dòng),出現(xiàn)不穩(wěn)定現(xiàn)象,從而導(dǎo)致使機(jī)器人的步行速度受到限制[3]。
本文提出一種雙足機(jī)器人跟蹤誤差在線(xiàn)修正方法。利用雙足機(jī)器人傳感器反饋,實(shí)時(shí)檢測(cè)機(jī)器人質(zhì)心(Centre of Mass, COM)的偏差與零力矩點(diǎn)的偏差,根據(jù)偏差計(jì)算得出步態(tài)修正時(shí)間點(diǎn)與修正量,在線(xiàn)修正雙足機(jī)器人實(shí)際狀態(tài)與目標(biāo)狀態(tài)的偏差。
通過(guò)分析機(jī)器人的運(yùn)動(dòng)特性可知,機(jī)器人腳桿的運(yùn)動(dòng)速度快于質(zhì)心的運(yùn)動(dòng)速度,并大幅變化。因此,從能量角度來(lái)看機(jī)器人的質(zhì)量應(yīng)該盡量集中于上半身。另外,在行走的過(guò)程中,轉(zhuǎn)動(dòng)集中于質(zhì)量較小的下半身,行走時(shí)轉(zhuǎn)動(dòng)能量要比平動(dòng)能量小很多,轉(zhuǎn)動(dòng)能量可以忽略不計(jì)。因此,可以將機(jī)器人所有連桿的質(zhì)量集中到機(jī)器人上半身的質(zhì)心上,只考慮質(zhì)心質(zhì)量,并忽略各腳桿的轉(zhuǎn)動(dòng)慣量[4]。三維線(xiàn)性倒立擺簡(jiǎn)化模型如圖1所示。
圖1可以使用ZMP的定義描述如下:
通常情況下機(jī)器人的質(zhì)心高度保持不變,即約束面水平(kx=ky= 0)。
將式(2)代入式(1)得到如下與 Z軸運(yùn)動(dòng)相獨(dú)立的運(yùn)動(dòng)方程[6]:
由式(3)可知,根據(jù)機(jī)器人的ZMP點(diǎn),可以求得機(jī)器人的質(zhì)心運(yùn)動(dòng)。
圖1 三維線(xiàn)性倒立擺簡(jiǎn)化模型
在給定機(jī)器人的步行參數(shù)(步長(zhǎng)、步寬、步行周期)、起點(diǎn)和終點(diǎn)的情況下,可以規(guī)劃出機(jī)器人的 ZMP軌跡,如圖2所示。
圖2 機(jī)器人的ZMP規(guī)劃軌跡
由式(3)可以得到機(jī)器人的質(zhì)心軌跡,如圖3所示。
圖3 機(jī)器人的質(zhì)心軌跡
基于ZMP步行模式生成的方法為:由機(jī)器人的多連桿模型求得機(jī)器人的質(zhì)心位置,機(jī)器人的質(zhì)心高度 zc保持不變,質(zhì)心在XOY平面上的軌跡可由圖3得到,機(jī)器人足部軌跡可由圖2的ZMP軌跡得到[7]。一旦機(jī)器人質(zhì)心和足部軌跡確定,則可以根據(jù)機(jī)器人的逆運(yùn)動(dòng)學(xué)得到腿部各關(guān)節(jié)的角度,從而由電機(jī)驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)。
大多數(shù)雙足機(jī)器人研究都將步行過(guò)程視為由連續(xù)單腿支撐相和瞬態(tài)雙足支撐相組成的周期運(yùn)動(dòng),然而,在人類(lèi)步行運(yùn)動(dòng)中,DSP約占整個(gè)步態(tài)周期的0.15%~0.25%,僅把 DSP作為瞬態(tài)過(guò)程考慮是不合理的[8]。同時(shí),由于雙足機(jī)器人在行走過(guò)程中,要保持不傾倒就必須使得機(jī)器人的ZMP點(diǎn)在穩(wěn)定區(qū)域內(nèi)[9]。機(jī)器人的穩(wěn)定支撐區(qū)域如圖 4所示。
圖4 機(jī)器人的穩(wěn)定支撐區(qū)域
在圖4中,SSP階段穩(wěn)定區(qū)域?yàn)橹文_;DSP階段穩(wěn)定區(qū)域?yàn)閮赡_之間的部分和支撐腳。從圖 4可以看出,單腿支撐階段ZMP的穩(wěn)定區(qū)域相對(duì)于雙腿支撐階段ZMP的穩(wěn)定區(qū)域比較小,即ZMP的穩(wěn)定裕度不同[10]。這就導(dǎo)致了SSP和DSP的穩(wěn)定性不同。
從模仿人類(lèi)的角度出發(fā),由于DSP和SSP的穩(wěn)定性差異很大,因此應(yīng)將在線(xiàn)實(shí)時(shí)修正分為SSP和DSP進(jìn)行考慮。在SSP階段,通過(guò)引入誤差裕量函數(shù)得到在線(xiàn)修正的時(shí)間點(diǎn);在DSP階段,通過(guò)設(shè)置誤差限值尋找修正時(shí)間點(diǎn)。在機(jī)器人實(shí)際行走的過(guò)程中,通過(guò)SSP階段修正方法可以補(bǔ)償DSP階段的不足,DSP階段也可以補(bǔ)償SSP階段的不足,通過(guò) 2種方法的相互配合,從而達(dá)到提高機(jī)器人穩(wěn)定性和步行速度的目的。
在雙足機(jī)器人步行過(guò)程中,單腿支撐階段在步行周期中所占的比例是非常大的,而且三維線(xiàn)性倒立擺模型主要針對(duì)單腿支撐階段進(jìn)行了分析[11]。由此可以看出SSP在機(jī)器人行走過(guò)程中的重要性。
根據(jù)步行模式的生成原理,質(zhì)心的軌跡可以由ZMP軌跡根據(jù)三維倒立擺模型求得。但此時(shí)的質(zhì)心軌跡是離線(xiàn)規(guī)劃好的,而實(shí)際的質(zhì)心軌跡由于外部環(huán)境的影響一定會(huì)與期望的質(zhì)心軌跡有偏差。此時(shí),如果不進(jìn)行質(zhì)心位置修正,勢(shì)必會(huì)影響ZMP的位置,從而導(dǎo)致機(jī)器人行走不穩(wěn)定。然而,機(jī)器人在單腿支撐階段的穩(wěn)定裕度是比較小的,如果對(duì)機(jī)器人的質(zhì)心進(jìn)行實(shí)時(shí)修正,由于修正過(guò)程中需要對(duì)機(jī)器人全身關(guān)節(jié)進(jìn)行補(bǔ)償,這會(huì)引起ZMP位置的波動(dòng),進(jìn)而導(dǎo)致機(jī)器人處于不穩(wěn)定狀態(tài)。為了解決此問(wèn)題,本文提出了一種設(shè)置誤差裕量函數(shù)的方法。函數(shù)如下:
其中,坐標(biāo)向量ZMPact是實(shí)際得到的ZMP位置;坐標(biāo)向量ZMPref是離線(xiàn)規(guī)劃的ZMP位置;EerZMP為ZMP位置誤差向量;|·|符號(hào)表示對(duì)相應(yīng)的向量的各分量取絕對(duì)值;而參數(shù)K需根據(jù)實(shí)際環(huán)境確定。
通過(guò)此函數(shù)與質(zhì)心誤差函數(shù)進(jìn)行比較。質(zhì)心誤差函數(shù)如下:
其中,COMact是實(shí)際檢測(cè)并計(jì)算的質(zhì)心位置向量;COMref是離線(xiàn)規(guī)劃的質(zhì)心位置向量;EerCOM 為質(zhì)心位置誤差向量。
在進(jìn)行質(zhì)心位置修正時(shí),通過(guò)檢測(cè)ZMP與COM的實(shí)際位置計(jì)算出EerZMP與EerCOM的值。其中,||·||符號(hào)表示對(duì)相應(yīng)變量取模。
當(dāng)||EerCOM||<||EerZMP||時(shí),此時(shí)可以認(rèn)為實(shí)際質(zhì)心軌跡能夠跟蹤期望的質(zhì)心軌跡,即機(jī)器人是穩(wěn)定的,不需要對(duì)質(zhì)心位置進(jìn)行在線(xiàn)修正。
當(dāng)||EerCOM||≥||EerZMP||時(shí),此時(shí)實(shí)際質(zhì)心軌跡已嚴(yán)重偏離期望的質(zhì)心軌跡,即機(jī)器人處于不穩(wěn)定狀態(tài),需要對(duì)質(zhì)心位置進(jìn)行在線(xiàn)修正。
通過(guò)此方法可以解決在單腿支撐階段質(zhì)心實(shí)時(shí)修正與ZMP位置波動(dòng)之間的沖突,即質(zhì)心的跟蹤誤差是有一定裕量的,并且裕量的大小是根據(jù)實(shí)時(shí)檢測(cè)到 ZMP的值來(lái)確定的。
經(jīng)轉(zhuǎn)染PRKCI表達(dá)載體的SCC-15細(xì)胞,與對(duì)照組相比,PRKCI基因的表達(dá)量顯著增加且明顯高于未轉(zhuǎn)染PRKCI表達(dá)載體的對(duì)照組細(xì)胞(圖2A,2B)。
關(guān)節(jié)修正的數(shù)值解法如下[12]:
(1)給定期望的質(zhì)心位置和ZMP位置。
(2)檢測(cè)并計(jì)算實(shí)際的質(zhì)心位置和ZMP位置。
(3)計(jì)算質(zhì)心位置誤差 EerCOM 和 ZMP位置誤差EerZMP。
(4)當(dāng)||EerCOM||≥||EerZMP||時(shí),執(zhí)行步驟(5),當(dāng)||EerCOM||<||EerZMP||時(shí),返回步驟(1)。
(5)計(jì)算關(guān)節(jié)角的修正向量δq。
為了實(shí)現(xiàn)上述算法的步驟(5),需要計(jì)算出修正向量δq以便減少機(jī)器人質(zhì)心位置的誤差。假設(shè)從當(dāng)前狀態(tài)的關(guān)節(jié)角向量q減去一個(gè)δq,考慮質(zhì)心位置的微小變化向量δp,δp=Fp(δ q ),當(dāng)δq很小時(shí),可以通過(guò)雅克比矩陣描述和計(jì)算,改寫(xiě)成:
其中,J為雅克比矩陣,它取決于機(jī)器人當(dāng)前的位姿,一旦式(6)確定之后,由于δP可由下式求得:
相對(duì)于單腿支撐階段,雙腿支撐階段的穩(wěn)定性好很多。因此,在對(duì)雙腿支撐階段進(jìn)行質(zhì)心修正時(shí),可以不必考慮質(zhì)心實(shí)時(shí)修正與ZMP位置波動(dòng)間的沖突問(wèn)題。
本文對(duì)質(zhì)心位置進(jìn)行實(shí)時(shí)修正,即全身關(guān)節(jié)在線(xiàn)補(bǔ)償。但是,在實(shí)際環(huán)境中。實(shí)際的質(zhì)心軌跡完全跟蹤期望的質(zhì)心軌跡,而達(dá)到零誤差是不可能的。因此,在此階段,設(shè)置誤差限值EM,可以根據(jù)實(shí)際環(huán)境確定。當(dāng)||EerCOM||≥EM時(shí),繼續(xù)進(jìn)行質(zhì)心在線(xiàn)修正;當(dāng)||EerCOM||<EM時(shí),實(shí)際的質(zhì)心能夠跟蹤期望的質(zhì)心。
關(guān)節(jié)修正數(shù)值解法如下:
(1)給定期望的質(zhì)心位置和ZMP位置。
(2)檢測(cè)并計(jì)算實(shí)際的質(zhì)心位置和ZMP位置。
(3)計(jì)算質(zhì)心位置誤差EerCOM。
(4)當(dāng)||EerCOM||≥EM 時(shí),執(zhí)行步驟(5),當(dāng)||EerCOM||<EM時(shí),返回步驟(1)。
(5)計(jì)算關(guān)節(jié)角的修正向量δq。
本文實(shí)驗(yàn)的目的在于驗(yàn)證雙足機(jī)器人步行模式分階段處理的控制性能,完成完整步行周期的實(shí)驗(yàn)。首先,利用Matlab平臺(tái)建立機(jī)器人模型。通過(guò)仿真得到了基于三維線(xiàn)性倒立擺模型的機(jī)器人步行軌跡,如圖 5所示。機(jī)器人的步行參數(shù)為:步長(zhǎng)0.1 m,步寬0.1 m,步行周期為0.92 s。
圖5 基于三維倒立擺模型的機(jī)器人步行軌跡
將本文方法應(yīng)用于實(shí)體機(jī)器人,通過(guò) AFU-10進(jìn)行試驗(yàn),圖6顯示了實(shí)際步行過(guò)程中期望的ZMP軌跡和實(shí)際檢測(cè)到的 ZMP軌跡對(duì)比,此過(guò)程中沒(méi)有采用本文方法。從圖 6可以看出,機(jī)器人的跟蹤誤差比較大,一旦出現(xiàn)外界干擾就很容易跌倒,并且步行周期已達(dá)到所能承受的最小值。
圖7為采用本文方法后得到的ZMP軌跡和質(zhì)心軌跡。設(shè)定EM=0.0108,參數(shù)K=0.01。可以看出,采用本文方法后機(jī)器人的穩(wěn)定性明顯提高,步行周期由1.3 s左右提高到了0.92 s,從而步行速度上限提高了25%左右。并且雙足機(jī)器人步行時(shí)對(duì)外部環(huán)境具有了一定的抗干擾能力。
圖6 未使用本文方法的ZMP軌跡和質(zhì)心軌跡
圖7 使用本文方法的ZMP軌跡和質(zhì)心軌跡
將本文方法應(yīng)用于RoboCup(3D)項(xiàng)目比賽,結(jié)果如圖8所示。應(yīng)用本文方法后,雙足機(jī)器人行走起來(lái)更加穩(wěn)定,行走速度明顯提高,并且在比賽中榮獲一等獎(jiǎng)的好成績(jī)。
圖8 RoboCup項(xiàng)目實(shí)驗(yàn)結(jié)果
雙足機(jī)器人的步行穩(wěn)定性一直是機(jī)器人研究的熱點(diǎn)及難點(diǎn)。本文提出一種雙足機(jī)器人跟蹤誤差在線(xiàn)修正方法。解決了機(jī)器人在線(xiàn)修正過(guò)程中質(zhì)心實(shí)時(shí)修正與ZMP位置波動(dòng)之間的沖突問(wèn)題。實(shí)驗(yàn)結(jié)果表明了該方法有效性。但該方法僅局限于在特定環(huán)境下的穩(wěn)定行走,將其應(yīng)用于機(jī)器人在其他環(huán)境下的穩(wěn)定行走是下一步的研究方向。
[1]Morimoto J, Endo G, Nakanishi J, etal.A Biologically Inspired Biped Locomotion Strategy for Humanoid Robots:Modulation of Sinusoidal Patterns by a Coupled Oscillator Model[J].IEEE Transactions on Robotics, 2008, 24(1):185-191.
[2]劉 飛, 陳小平.使用零力矩點(diǎn)軌跡規(guī)劃的四足機(jī)器人步態(tài)進(jìn)化方法[J].機(jī)器人, 2010, 32(3): 398-404.
[3]Park J H, Kim E S.Foot and Body Control of Robots to Walk on Irregularly Protruded Uneven Surfaces[J].IEEE Transactions on Systems, Man, and Cybernetics, 2009, 39(1): 289-297.
[4]Liu Hsin-Yu, Wang Wen-June, Wang Rong-Jyue.A Course in Simulation and Demonstration of Humanoid Robot Motion[J].IEEE Transactions on Education, 2011, 54(2): 255-262.
[5]Shimmyo S, Tomoya S, Ohnishi K.Biped Walking Pattern Generation by Using Preview Control with Virtual Plane Method[C]//Proc.of the 11th IEEE International Workshop on Advanced Motion Control.Nagaoka, Japan: IEEE Conference Publications, 2010.
[6]Shimmyo S, Ohnishi K.Nested Preview Control by Utilizing Virtual Plane for Biped Walking Pattern Generation Including COG Up-down Motion[C]//Proc.of the 36th Annual Conference on IEEE Industrial Electronics Society.Glendale,USA: IEEE Conference Publications, 2010.
[7]梶田秀司.仿人機(jī)器人[M].管貽生, 譯.北京: 清華大學(xué)出版社, 2007.
[8]朱志斌, 王 巖, 陳興林.一種雙足機(jī)器人實(shí)時(shí)步態(tài)規(guī)劃方法及相關(guān)數(shù)值算法[J].機(jī)器人, 2008, 30(6): 521-527.
[9]伊 強(qiáng), 陳 懇, 劉 莉, 等.考慮綜合步行約束的仿人機(jī)器人參數(shù)化3D步態(tài)規(guī)劃方法[J].機(jī)器人, 2009, 31(4): 342-350.
[10]Jimmny O.A Hybrid CPG-ZMP Controller for the Real-time Balance of a Simulated Flexible Spine Humanoid Robot[J].IEEE Transactions on Systems, Man, and Cybernetics, 2009,39(5): 547-561.
[11]湯 卿, 熊 蓉, 褚 健.給予最優(yōu)化線(xiàn)性搜索的穩(wěn)定步態(tài)規(guī)劃方法[J].控制理論與應(yīng)用, 2008, 25(4): 661-664.
[12]黃元林, 付成龍, 王健美, 等.雙足跑步機(jī)器人控制方法研究概述[J].機(jī)器人, 2009, 31(4): 370-377.
[13]敬成林, 李祖樞, 薛方正.雙足機(jī)器人逆運(yùn)動(dòng)學(xué)的模糊自適應(yīng)算法[J].機(jī)器人, 2010, 32(4): 534-539.