雙足機(jī)器人跟蹤誤差在線(xiàn)修正方法

張世龍，劉國(guó)棟

(江南大學(xué)物聯(lián)網(wǎng)工程學(xué)院，江蘇 無(wú)錫 214122)

1 概述

雙足機(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)的偏差。

2 雙足機(jī)器人模型及步行模式的生成

2.1 雙足機(jī)器人模型

通過(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)化模型

2.2 步行模式的生成

在給定機(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)。

3 基于SSP和DSP的機(jī)器人在線(xiàn)修正

大多數(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)定性和步行速度的目的。

3.1 單腿支撐階段的在線(xiàn)全身關(guān)節(jié)修正

在雙足機(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可由下式求得：

3.2 雙腿支撐階段的在線(xiàn)全身關(guān)節(jié)修正

相對(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。

4 實(shí)驗(yàn)結(jié)果與分析

本文實(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é)果

5 結(jié)束語(yǔ)

雙足機(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.