雙足機器人跟蹤誤差在線修正方法

張世龍，劉國棟

(江南大學物聯網工程學院，江蘇 無錫 214122)

1 概述

雙足機器人是一門由仿生學、機械設計、加工和計算機控制理論以及人工智能等多種學科形成的交叉學科。然而，步態規劃是研究雙足機器人控制的難點和熱點，步行過程必須保證雙足機器人的穩定性[1]。雙足機器人控制多采用靜態步態規劃得到一步或多步參考軌跡，由關節控制器伺服跟蹤期望軌跡。但由于機器人自身的機械誤差，模型誤差以及地面摩擦力等環境因素的影響，機器人的實際步態必然與期望步態之間會產生一定的誤差，這些誤差的積累很可能會使機器人不穩定而摔倒[2]。為了減小或消除這些誤差，必須對機器人的步行模式進行在線修正，以實現機器人的穩定行走。

為解決此問題，學者們曾提出過對機器人進行在線全身關節實時修正的方法，但由于機器人在行走時會存在慣性力的作用，在進行不適當的實時修正時，會導致機器人的零力矩點(Zero Moment Point, ZMP)位置波動，出現不穩定現象，從而導致使機器人的步行速度受到限制[3]。

本文提出一種雙足機器人跟蹤誤差在線修正方法。利用雙足機器人傳感器反饋，實時檢測機器人質心(Centre of Mass, COM)的偏差與零力矩點的偏差，根據偏差計算得出步態修正時間點與修正量，在線修正雙足機器人實際狀態與目標狀態的偏差。

2 雙足機器人模型及步行模式的生成

2.1 雙足機器人模型

通過分析機器人的運動特性可知，機器人腳桿的運動速度快于質心的運動速度，并大幅變化。因此，從能量角度來看機器人的質量應該盡量集中于上半身。另外，在行走的過程中，轉動集中于質量較小的下半身，行走時轉動能量要比平動能量小很多，轉動能量可以忽略不計。因此，可以將機器人所有連桿的質量集中到機器人上半身的質心上，只考慮質心質量，并忽略各腳桿的轉動慣量[4]。三維線性倒立擺簡化模型如圖1所示。

圖1可以使用ZMP的定義描述如下：

通常情況下機器人的質心高度保持不變，即約束面水平(kx=ky= 0)。

將式(2)代入式(1)得到如下與 Z軸運動相獨立的運動方程[6]：

由式(3)可知，根據機器人的ZMP點，可以求得機器人的質心運動。

圖1 三維線性倒立擺簡化模型

2.2 步行模式的生成

在給定機器人的步行參數(步長、步寬、步行周期)、起點和終點的情況下，可以規劃出機器人的 ZMP軌跡，如圖2所示。

圖2 機器人的ZMP規劃軌跡

由式(3)可以得到機器人的質心軌跡，如圖3所示。

圖3 機器人的質心軌跡

基于ZMP步行模式生成的方法為：由機器人的多連桿模型求得機器人的質心位置，機器人的質心高度 zc保持不變，質心在XOY平面上的軌跡可由圖3得到，機器人足部軌跡可由圖2的ZMP軌跡得到[7]。一旦機器人質心和足部軌跡確定，則可以根據機器人的逆運動學得到腿部各關節的角度，從而由電機驅動機器人運動。

3 基于SSP和DSP的機器人在線修正

大多數雙足機器人研究都將步行過程視為由連續單腿支撐相和瞬態雙足支撐相組成的周期運動，然而，在人類步行運動中，DSP約占整個步態周期的0.15%～0.25%，僅把 DSP作為瞬態過程考慮是不合理的[8]。同時，由于雙足機器人在行走過程中，要保持不傾倒就必須使得機器人的ZMP點在穩定區域內[9]。機器人的穩定支撐區域如圖 4所示。

圖4 機器人的穩定支撐區域

在圖4中，SSP階段穩定區域為支撐腳；DSP階段穩定區域為兩腳之間的部分和支撐腳。從圖 4可以看出，單腿支撐階段ZMP的穩定區域相對于雙腿支撐階段ZMP的穩定區域比較小，即ZMP的穩定裕度不同[10]。這就導致了SSP和DSP的穩定性不同。

從模仿人類的角度出發，由于DSP和SSP的穩定性差異很大，因此應將在線實時修正分為SSP和DSP進行考慮。在SSP階段，通過引入誤差裕量函數得到在線修正的時間點；在DSP階段，通過設置誤差限值尋找修正時間點。在機器人實際行走的過程中，通過SSP階段修正方法可以補償DSP階段的不足，DSP階段也可以補償SSP階段的不足，通過 2種方法的相互配合，從而達到提高機器人穩定性和步行速度的目的。

3.1 單腿支撐階段的在線全身關節修正

在雙足機器人步行過程中，單腿支撐階段在步行周期中所占的比例是非常大的，而且三維線性倒立擺模型主要針對單腿支撐階段進行了分析[11]。由此可以看出SSP在機器人行走過程中的重要性。

根據步行模式的生成原理，質心的軌跡可以由ZMP軌跡根據三維倒立擺模型求得。但此時的質心軌跡是離線規劃好的，而實際的質心軌跡由于外部環境的影響一定會與期望的質心軌跡有偏差。此時，如果不進行質心位置修正，勢必會影響ZMP的位置，從而導致機器人行走不穩定。然而，機器人在單腿支撐階段的穩定裕度是比較小的，如果對機器人的質心進行實時修正，由于修正過程中需要對機器人全身關節進行補償，這會引起ZMP位置的波動，進而導致機器人處于不穩定狀態。為了解決此問題，本文提出了一種設置誤差裕量函數的方法。函數如下：

其中，坐標向量ZMPact是實際得到的ZMP位置；坐標向量ZMPref是離線規劃的ZMP位置；EerZMP為ZMP位置誤差向量；|·|符號表示對相應的向量的各分量取絕對值；而參數K需根據實際環境確定。

通過此函數與質心誤差函數進行比較。質心誤差函數如下：

其中，COMact是實際檢測并計算的質心位置向量；COMref是離線規劃的質心位置向量；EerCOM 為質心位置誤差向量。

在進行質心位置修正時，通過檢測ZMP與COM的實際位置計算出EerZMP與EerCOM的值。其中，||·||符號表示對相應變量取模。

當||EerCOM||＜||EerZMP||時，此時可以認為實際質心軌跡能夠跟蹤期望的質心軌跡，即機器人是穩定的，不需要對質心位置進行在線修正。

當||EerCOM||≥||EerZMP||時，此時實際質心軌跡已嚴重偏離期望的質心軌跡，即機器人處于不穩定狀態，需要對質心位置進行在線修正。

通過此方法可以解決在單腿支撐階段質心實時修正與ZMP位置波動之間的沖突，即質心的跟蹤誤差是有一定裕量的，并且裕量的大小是根據實時檢測到 ZMP的值來確定的。

經轉染PRKCI表達載體的SCC-15細胞，與對照組相比，PRKCI基因的表達量顯著增加且明顯高于未轉染PRKCI表達載體的對照組細胞(圖2A，2B)。

關節修正的數值解法如下[12]：

(1)給定期望的質心位置和ZMP位置。

(2)檢測并計算實際的質心位置和ZMP位置。

(3)計算質心位置誤差 EerCOM 和 ZMP位置誤差EerZMP。

(4)當||EerCOM||≥||EerZMP||時，執行步驟(5)，當||EerCOM||＜||EerZMP||時，返回步驟(1)。

(5)計算關節角的修正向量δq。

為了實現上述算法的步驟(5)，需要計算出修正向量δq以便減少機器人質心位置的誤差。假設從當前狀態的關節角向量q減去一個δq，考慮質心位置的微小變化向量δp，δp=Fp(δ q )，當δq很小時，可以通過雅克比矩陣描述和計算，改寫成：

其中，J為雅克比矩陣，它取決于機器人當前的位姿，一旦式(6)確定之后，由于δP可由下式求得：

3.2 雙腿支撐階段的在線全身關節修正

相對于單腿支撐階段，雙腿支撐階段的穩定性好很多。因此，在對雙腿支撐階段進行質心修正時，可以不必考慮質心實時修正與ZMP位置波動間的沖突問題。

本文對質心位置進行實時修正，即全身關節在線補償。但是，在實際環境中。實際的質心軌跡完全跟蹤期望的質心軌跡，而達到零誤差是不可能的。因此，在此階段，設置誤差限值EM，可以根據實際環境確定。當||EerCOM||≥EM時，繼續進行質心在線修正；當||EerCOM||＜EM時，實際的質心能夠跟蹤期望的質心。

關節修正數值解法如下：

(1)給定期望的質心位置和ZMP位置。

(2)檢測并計算實際的質心位置和ZMP位置。

(3)計算質心位置誤差EerCOM。

(4)當||EerCOM||≥EM 時，執行步驟(5)，當||EerCOM||＜EM時，返回步驟(1)。

(5)計算關節角的修正向量δq。

4 實驗結果與分析

本文實驗的目的在于驗證雙足機器人步行模式分階段處理的控制性能，完成完整步行周期的實驗。首先，利用Matlab平臺建立機器人模型。通過仿真得到了基于三維線性倒立擺模型的機器人步行軌跡，如圖 5所示。機器人的步行參數為：步長0.1 m，步寬0.1 m，步行周期為0.92 s。

圖5 基于三維倒立擺模型的機器人步行軌跡

將本文方法應用于實體機器人，通過 AFU-10進行試驗，圖6顯示了實際步行過程中期望的ZMP軌跡和實際檢測到的 ZMP軌跡對比，此過程中沒有采用本文方法。從圖 6可以看出，機器人的跟蹤誤差比較大，一旦出現外界干擾就很容易跌倒，并且步行周期已達到所能承受的最小值。

圖7為采用本文方法后得到的ZMP軌跡和質心軌跡。設定EM=0.0108，參數K=0.01。可以看出，采用本文方法后機器人的穩定性明顯提高，步行周期由1.3 s左右提高到了0.92 s，從而步行速度上限提高了25%左右。并且雙足機器人步行時對外部環境具有了一定的抗干擾能力。

圖6 未使用本文方法的ZMP軌跡和質心軌跡

圖7 使用本文方法的ZMP軌跡和質心軌跡

將本文方法應用于RoboCup(3D)項目比賽，結果如圖8所示。應用本文方法后，雙足機器人行走起來更加穩定，行走速度明顯提高，并且在比賽中榮獲一等獎的好成績。

圖8 RoboCup項目實驗結果

5 結束語

雙足機器人的步行穩定性一直是機器人研究的熱點及難點。本文提出一種雙足機器人跟蹤誤差在線修正方法。解決了機器人在線修正過程中質心實時修正與ZMP位置波動之間的沖突問題。實驗結果表明了該方法有效性。但該方法僅局限于在特定環境下的穩定行走，將其應用于機器人在其他環境下的穩定行走是下一步的研究方向。

[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]劉 飛, 陳小平.使用零力矩點軌跡規劃的四足機器人步態進化方法[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]梶田秀司.仿人機器人[M].管貽生, 譯.北京: 清華大學出版社, 2007.

[8]朱志斌, 王 巖, 陳興林.一種雙足機器人實時步態規劃方法及相關數值算法[J].機器人, 2008, 30(6): 521-527.

[9]伊 強, 陳 懇, 劉 莉, 等.考慮綜合步行約束的仿人機器人參數化3D步態規劃方法[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]湯 卿, 熊 蓉, 褚 健.給予最優化線性搜索的穩定步態規劃方法[J].控制理論與應用, 2008, 25(4): 661-664.

[12]黃元林, 付成龍, 王健美, 等.雙足跑步機器人控制方法研究概述[J].機器人, 2009, 31(4): 370-377.

[13]敬成林, 李祖樞, 薛方正.雙足機器人逆運動學的模糊自適應算法[J].機器人, 2010, 32(4): 534-539.