雙足步行機器人在線步態生成與偏航控制策略

丁加濤，肖曉暉，王楊

（武漢大學 動力與機械學院，湖北 武漢，430072）

雙足步行機器人在線步態生成與偏航控制策略

丁加濤，肖曉暉，王楊

（武漢大學 動力與機械學院，湖北 武漢，430072）

針對雙足步行機器人的偏航行走需求，提出一種步態生成與偏航行走控制策略，實現步態模式的在線生成與自主偏航行走。其步驟為：首先，基于零力矩點（ZMP）穩定判據，采用線性倒立擺模型和正-逆運動學循環求解算法，建立離線步態生成器；其次，采用預觀控制理論，建立用于軌跡跟蹤的預觀控制器，實現雙足步態的在線生成；然后，引入角度系數，運用矢量合成法，根據目標偏航角度，通過回放-糾正方法確定合適的固定系數；最后，采用MATLAB和Adams組成的聯合仿真平臺，驗證方法的有效性。研究結果表明：該方法能利用離線生成的角度，有效減少質心側向軌跡跟蹤誤差；通過矢量合成和回放糾正，選取合適的角度系數，實現無偏航步態的在線生成；不需要實時規劃參考ZMP軌跡，在保證穩定的前提下，實現15°和30°偏航行走。

雙足步行機器人；步態生成；偏航控制；矢量合成；回放-糾正

雙足步行機器人具有高度的活性、適應性和與人的親和力，在生產服務、醫療康復等方面有著廣闊的應用前景［1］。步態模式生成和自主偏航控制是實現雙足靈活運動關鍵。雙足步態生成策略可分為離線生成和在線生成2類。1972年，VUKOBRATOVIC等［1-2］提出零力矩點（zero moment point，ZMP）概念并給出了相應的步行穩定性判據?；诖?，HUANG等［3］通過規劃足部軌跡和髖關節軌跡，離線獲得了高穩定裕度的平穩步態。KAGAMI等［4］研究動態平衡條件下ZMP與質心的關心，實現了步態的離線快速生成。但是，離線步態生成策略未能考慮環境的多變性，適應性不強。為了增強適應性，研究者提出了多種控制策略。MITOBE等［5］提出角動量控制法來協調機器人整體姿態；KOMURA等［6］提出1種反饋控制器來抵消行走過程的沖擊。但是，一方面，僅僅使用一種在線控制策略也很難獲得較高的適應性，有時需要多種控制方式協調進行［7］。另一方面，這些控制策略往往依賴于離線規劃的步態，效果受到限制。因此，越來越多的研究者將注意力轉向在線步態生成。HARADA等［8］提出數值求解法，實現了步態的實時生成。KAJITA等［9］使用預觀控制器進行在線步態跟蹤，生成了用于螺旋階梯行走的步態。DIEDAM等［10］使用模型預測控制器實現足部位置自適應調整。由于具有良好的跟蹤性，預觀控制在步態在線生成與步行控制方面得到了廣泛的運用［11-12］。偏航行走有直線跟蹤行走和主動偏航行走2種需求。一方面，為了提高步行速度，在環境條件允許時需要避免偏航。 HIRABAYASHI等［13］通過調整擺動腿軌跡減少了偏航力矩；CISNEROS等［14］利用機器人不工作的自由度對步行姿態進行補償，減少了偏航位移。另一方面，在某些環境和特殊任務要求下（避障等），應該采取主動偏航行走策略。STROM等［15］利用預觀控制器，通過4個子工作框架的信息交換，實現了機器人的偏航與換向。YEON等［16］利用機器人在單足相期間產生的偏航，實現了快速的偏轉。但以上2類策略將實現無偏航直線行走和主動偏航區分考慮，難以滿足實際任務需求。TZUU-HSENG等［17］以拉格朗日插值法為基礎，編制了步態數據庫，結合強化梯度學習和模糊邏輯控制方法，實現了對曲線軌跡的跟蹤，但編制步態數據庫需要大量時間。KAGAMI等［18］提出基于視覺系統的回放方法，用于汽車行駛時跟蹤彎曲路徑。MERI?LI等［19］將回放法與錯誤糾正相結合，實現了雙足步行機器人對人體步行數據的學習。基于此，本文作者針對雙足步行機器人下肢結構，提出一種在線步態生成和偏航控制策略。在步態生成方面，采用ZMP 穩定判據，利用時域離散法，通過正-逆運動學的循環求解離線獲得一組步態角度。利用預觀控制理論構造預觀控制器，將離線生成的關節角度作為在線逆運動學求解的初始角度，實現步態模式的在線生成與輸出。在偏航控制方面，將離線生成的角度和在線生成的角度作為偏航控制算法的 2組輸入量；引入角度系數，將這2組角度結合起來。利用矢量合成的平行四邊形法則確定目標軌跡需要的模糊角度系數；通過回放-糾正法確定準確的系數，以實現滿足目標偏航角度的步態模式的實時生成。

1　步態生成

首先研究離線步態生成的基本理論，然后實現步態模式的在線生成。

1.1離線步態生成

以 ZMP穩定判據作為依據，規劃出合適的ZMP（zero-moment point）軌跡和足部運動軌跡，通過逆運動學計算生成步態模式。

1.1.1ZMP 穩定判據

ZMP是指機器人在重力和慣性力的作用下，地面反力的傾覆力矩等于0 N·m的點。ZMP穩定判據指出：雙足步行機器人實現穩定行走的前提是 ZMP應該位于足部支撐多邊形的內部［1］。本文采用此穩定判據，指導ZMP軌跡和足部軌跡的離線規劃。ZMP軌跡和足部軌跡示意圖如圖1所示，其中，x軸指向前進方向，y軸指向側行方向，規劃的參考ZMP軌跡始終位于足部支撐多邊形（圖（1）中虛點線與足部圍成的區域）的內部。

圖1　ZMP軌跡和足部軌跡示意圖Fig. 1　Diagram of ZMP trajectory and foot trajectory

1.1.2線性倒立擺模型

采用線性倒立擺模型。該模型假定機器人腿部無質量；將機器人簡化成1個質量集中于腰部的倒立擺。根據該模型，得ZMP和質心位置的關系式［9］：

式中：Zc為模型質心高度；xcom和ycom分別為質心前向、側向位移；px和 py分別為質心ZMP前向、側向位移；，和g分別為質心前向加速度、側向加速度以及重力加速度。本文預先規劃 ZMP軌跡，再求解質心軌跡，采用桌子-小車模型［9］，將雙足步行機器人近似為1個不計質量的桌子及在其上水平運動的小車。得ZMP和質心關系式：

1.1.3離線步態求解流程

離線步態生成算法流程如圖2所示，流程如下。

1） 規劃ZMP軌跡，求解對應的質心和足部運動軌跡。

2） 利用桿式模型，將質心位姿信息和足部位姿信息分別賦給腰部桿和足部桿。

3） 從運動鏈首端（腰部桿）到運動鏈末端（足部桿）進行正-逆運動學循環計算，得到機器人10個關節的角度-時間序列即步態模式。

圖2　離線步態求解流程Fig. 2　Calculation proceedings of offlinebiped gait generation

1.2預觀控制理論

利用未來的信息進行控制的方法叫預觀控制［9， 20］。由于使用未來一段時間的步態信息預觀控制器對目標軌跡具有良好的跟蹤性，本文采用預觀控制器作為在線步態發生器。將式（3）和（4）統一為

以此為輸入，由式（5）得狀態方程：

以采樣時間Δt對連續系統方程（7）和（8）進行離散化處理：

為了減小跟蹤誤差，采用增量形式：

以上是一個最優跟蹤問題。為了使系統的輸出pk盡可能精確地跟蹤目標，并且能夠實現過程控制，采用積分型性能指標J：

為了極小化 J，據現代控制論和預觀控制理論，確定控制輸入的表達式：

控制增益 Ks和Kx，預觀增益fj和跟蹤步長N由預觀控制理論計算，Q 和 R為正的權系數。

在線步態求解流程見圖3。未來目標ZMP值儲存于 FIFO寄存器，其輸出值作為當前的參考值。預觀控制器用FIFO緩沖器中的ZMP參考值和小車的狀態計算控制輸入。其中，小車狀態包括質心位移和速度，可根據桌子-小車模式生成。

圖3　基于預觀控制的在線步態求解流程Fig. 3　Calculation proceeding of online biped gaitgeneration based on preview control

2　偏航控制

2.1偏航與角度系數

機器人在行走過程中，擺動腿的加速和減速產生了繞支撐腿的偏航力矩。若偏航力矩大于地面對支撐腳的摩擦力矩，則會發生偏轉［13］。機器人偏航反映在位姿上就是質心（center of mass，COM）軌跡（側前-側向位移軌跡，y-x曲線）實現偏航與轉向。不考慮質心高度變化，可利用質心軌跡在水平面上的投影研究機器人的偏航（簡稱質心軌跡）。

離線步態生成算法只能生成穩定行走的步態。要使機器人產生特定角度的偏航，則需事先規劃好相應的ZMP軌跡。但一方面，離線規劃ZMP軌跡容易導致轉彎處的關節角突變，從而使機器人失穩；另一方面，由于沒有在線調整策略，不能保證規劃 ZMP的跟蹤效果。預觀控制強調跟蹤規劃的 ZMP 軌跡，但難以確定合適的權重系數Q，R和跟蹤步長N。在仿真中發現，離線生成的軌跡和預觀控制在線生成的軌跡往往是不重合的2條曲線。經分析知，這種方向的不同是離線生成的角度-時間序列和在線生成的角度-時間序列不同所致。由機器人正運動學理論，一組角度-時間序列對應于1條運動軌跡即1條質心軌跡。采用拉格朗日插值法，引入角度系數（Kwei），利用離線計算的角度（θofflne）和在線生成的角度（θonlne），得到計算式：

為了生成目標偏航角度的質心軌跡，只需要確定合適的角度系數即可。

2.2矢量合成法確定目標角度系數模糊值

規劃的ZMP軌跡是與前進方向夾角為0°的直線，在理想情況下，相應的質心軌跡與前進方向夾角也應為0°，但實際上并非如此。本文將離線和在線生成的軌跡近似為與前進方向呈一定夾角的直線。記離線生成的軌跡方向矢量記為V1，預觀控制生成的軌跡方向矢量記為V2，目標偏航軌跡方向矢量記為V。在笛卡爾坐標系中，由V1，V2合成 V的關系可以由平行四邊形法則確定，如圖4所示。

圖4　目標偏航軌跡合成示意圖Fig. 4　Vectors synthesis for target trajectory with specific angle yaw

將方向矢量的橫坐標歸一化，得：V1=（1，K1），V2=（1，K2），V=（1，K）。根據矢量合成的平行四邊形法則，結合式（13），有

目標角度系數由下式計算：

其中，K1≠K2，符合實際情況。當目標軌跡向左偏轉時，K取正值；當目標軌跡向右偏轉時，K取負值。Kwei由式（15）決定。

2.3回放-糾正確定目標角度系數準確值

利用矢量合成法確定目標角度系數時，假設質心軌跡為1條方向不變的直線，但實際上往往不是1條直線。當周期T為3 s，步長Fs為290 mm 時，離線和在線生成的質心軌跡如圖5所示，其中，Kwei=0和Kwei=1.00分別代表離線、預觀控制生成的質心軌跡。

圖5　T=3 s，Fs=290 mm時的質心軌跡Fig. 5　Lateral trajectories of center of mass when T=3 s and Fs=290 mm

如圖5所示，離線生成的質心軌跡一直向右偏轉，前進2.0 m后偏航明顯加劇。預觀控制生成的軌跡略微向右偏轉，最后向左反向偏轉，這給V1和V2的確定帶來了誤差，因此，矢量合成法計算的角度系數往往與目標要求有偏差，需要進一步修正。

回放-糾正法作為一種學習方法，操作簡單靈活，能夠快速找到目標解，在機器人導航、示教等領域應用廣泛。本文使用該方法的簡化形式來確定目標角度系數，流程如下：

1） 離線生成步態，記錄質心軌跡；

2） 在線生成步態，記錄質心軌跡；

3） 矢量合成，確定滿足目標偏航角度的角度系數模糊值；

4） 使用模糊角度系數，生成實際質心軌跡，比較該軌跡方向和目標軌跡方向的偏差，重新修正角度系數；

5） 不斷重復第4）步，直到獲得目標偏航角度的質心軌跡為止。

3　結果與分析

為了驗證算法的有效性， 采用 MATLAB 和Adams 組成的聯合仿真平臺。在每組仿真工況下，分別記錄離線算法、預觀控制算法、本文算法生成的質心軌跡。

10自由度樣機模型見圖6，相應的物理參數和仿真參數見表1。表1中，軀干高度、大腿長度、小腿長度、總體質量、軀干質量、腿部質量、髖關節寬度、髖關節寬度和足部長、寬均為實際樣機的物理參數；Zc為規劃的機器人穩定行走時的質心高度。

圖6　機器人模型Fig. 6　Robot models

表1　模型參數與仿真參數Table 1 Model parameters and simulation parameters

3.1質心側向軌跡跟蹤誤差分析

為了驗證該算法降低質心側向軌跡跟蹤誤差（Ey）的作用，采用桿式模型在MATLAB中進行數值仿真。

步行周期 T=4 s，步長 Fs=300 mm，角度系數Kwei=0.5。將這3種方法的質心側向軌跡與規劃的參考質心側向軌跡求差，得到相應的跟蹤誤差，見圖 7。這3種方法產生的跟蹤誤差差別不大，但橢圓區域內峰值相差很大。

與離線生成模式相比，使用角度系數的方法使質心側向軌跡的跟蹤誤差降低至少 44.9%（由12.16 mm降到 6.69 mm，區域Ⅷ）。與預觀控制器相比，本文提出的方法在大部分區域都能有效降低誤差絕對極大值（從133.91 mm降到-53.38 mm區域Ⅴ）。

3.2無偏航直線行走

圖7　質心軌跡和誤差Fig. 7　COM trajectory and tracking error

針對多組步態參數，規劃相應的直線行走參考ZMP軌跡，并利用本文提出的方法減少偏航。在不同的步態模式中，給出機器人實際質心在地面的投影軌跡（y-x）來判斷是否達到目標。為了便于分析，對實際軌跡進行最小二乘擬合。

由表1可知：髖關節的寬度為133 mm，當機器人質心側向位移在連續3個周期內的絕對最大值不超過150 mm時，則認為機器人無偏航。由于在本算法中，起步階段只采用離線生成的角度，故軌跡曲線的方向矢量要從起步階段完成之后才開始計算。

T=2 s，Fs=260 mm的步態模式對應的質心軌跡見圖8。其中，圖8（a）中Kwei為0，1.00和0.74時對應曲線分別代表離線、預觀控制、角度系數為0.74時生成的質心軌跡曲線，圖8（b）所示為其相應的最小二乘擬合軌跡曲線。

圖8　T=2 s，Fs=260 mm時的質心軌跡Fig. 8　COM trajectory when T=2 s and Fs=260 mm

圖8中，離線生成的質心位移矢量不是直線，取方向矢量中的縱坐標 K1=1.00；預觀控制生成的軌跡曲線可取方向矢量縱坐標K2=-0.25，由式（15）計算0°偏航軌跡直線對應的模糊角度系數Kwei= 0.80。

通過回放-糾正，確定最終的系數為0.74，見圖8 中Kwei=0.74對應的軌跡曲線：前3.00 m中最大側向偏移為-0.130 m，基本實現了無偏航行走。

圖9所示為步行周期3 s、步長變化時3種方法生成的質心軌跡，最大偏航位移見表2。

圖9表明：在不同步態參數下，離線算法和預觀控制算法生成的軌跡往往有較大偏轉，而提出的矢量合成法有效抑制了偏航。表2表明：與離線步態相比，

本方法生成的軌跡偏航數值最小減少65%（從0.334 m下降到0.115 m），最大降幅達到85.4%（從-1.298 m降到-0.190 m），基本實現無偏航行走。

3.3目標角度偏航行走

表2　T=3 s時軌跡偏轉極大值Table 2 Maximal yaw values when T=3 s mm

圖9　不同步態參數下無偏航行走軌跡Fig. 9　Trajectory without yaws under different parameters

針對不同的步態參數，驗證算法對機器人左、右各15°和30°偏航的控制效果，軌跡分別如圖10和圖11所示。在圖10（a）中，以向左偏航15°為例，機器人以T=2 s，Fs=260 mm行走（見圖10 （a）中曲線4），在前0.26 m處于起步和偏航過渡階段；在0.26～4.00 m階段，軌跡過點（0.26，0.02）和（3.50，1.00），斜率K=0.302，對應角度為16.8°，與15°時的偏差為12%，基本滿足目標軌跡要求。按此計算方法，統計不同步態參數下實際偏航角度，結果見表3。

圖10　左、右15°偏航軌跡Fig. 10 Trajectories with right and left 15° yaw

圖11　左、右30°偏航軌跡Fig. 11 Trajectories with right and left 30° yaw

表3　軌跡偏航角度Table 3 Trajectories yaw angles

表3表明：對于目標15°偏航軌跡，當 T=2 s，Fs=260 mm時，最大角度偏差為34%；當T=3 s，Fs= 280 mm時，最大偏差為14%。對于目標30°偏航軌跡，當T=2 s，Fs=260 mm和T=3 s，Fs=280 mm時最大角度偏差不超過15%。這說明該算法能在機器人實際行走過程中有效地控制機器人偏航角度。

4　結論

1） 建立了基于ZMP穩定判據的離線步態生成器和基于預觀控制的在線步態生成器；提出角度系數概念，運用矢量合成法和回放-糾正法獲得了滿足目標偏航角度的角度系數。所提出的方法可實現不同周期、步長的雙足步態模式的在線生成，能有效減少質心側向軌跡跟蹤誤差，實現不同周期、步長參數下無偏航步態的在線生成，不需要實時規劃參考 ZMP軌跡，實現15°和30°偏航。

2） 機器人在起步階段可能發生的偏轉會影響實際軌跡，使之與目標軌跡有一定的偏差。下一步將進一步研究該問題，將本文的算法用于實際樣機的行走控制中。

［1］ VUKOBRATOVI? M， STEPANENKO J. On the stability of anthropomorphic systems［J］. Mathematical Biosciences， 1972，15（1）： 1-37.

［2］ VUKOBRATOVI? M， BOROVAC B. Zero-moment point-thirty five years of its life［J］. International Journal of Humanoid Robotics， 2004， 1（1）： 157-173.

［3］ HUANG Q， KAJITA S， KOYACHI N， et al. A high stability，smooth walking pattern for a biped robot［C］//Proceedings of 1999 IEEE International Conference on IEEE. Detroit， USA，1999： 65-71.

［4］ KAGAMI S， KITAGAWA T， NISHIWAKI K， et al. A fast dynamically equilibrated walking trajectory generation method of humanoid robot［J］. Autonomous Robots， 2002， 12（1）： 71-82.

［5］ MITOBE K， CAPI G， NASU Y. A new control method for walking robots based on angular momentum［J］. Mechatronics，2004， 14（2）： 163-174.

［6］ KOMURA T， LEUNG H， KUDOH S， et al. A feedback controller for biped humanoids that can counteract large perturbations during gait［C］//Proceedings of the 2005 IEEE International Conference on Barcelona. Spain： IEEE， 2005：1989-1995.

［7］ KIM J Y， PARK I W， OH J H. Experimental realization of dynamic walking of the biped humanoid robot KHR-2 using zero moment point feedback and inertial measurement［J］. Advanced Robotics， 2006， 20（6）： 707-736.

［8］ HARADA K， KAJITA S， KANEKO K， et al. An analytical method for real-time gait planning for humanoid robots［J］. International Journal of Humanoid Robotics， 2006， 3（1）： 1-19.

［9］ KAJITA S， KANEHIRO F， KANEKO K， et al. Biped walking pattern generation by using preview control of zero-moment point［C］//Robotics and Automation， Proceedings， 2003： 1620-1626.

［10］ DIEDAM H， DIMITROV D， WIEBER P B， et al. Online walking gait generation with adaptive foot positioning through linear model predictive control［C］//Intelligent Robots and Systems. IEEE/RSJ International Conference on IEEE， 2008：1121-1126.

［11］ CZARNETZKI S， KERNER S， URBANN O. Observer-based dynamic walking control for biped robots［J］. Robotics and Autonomous Systems， 2009， 57（8）： 839-845.

［12］ SHIMMYO S， SATO T， OHNISHI K. Biped walking pattern generation by using preview control based on three-mass model［J］. IEEE Transactions on Industrial Electronics， 2013，60（11）： 5137-5147.

［13］ HIRABAYASHI T， UGURLU B， KAWAMURA A， et al. Yaw moment compensation of biped fast walking using 3D inverted pendulum［C］//2008 10th IEEE International Workshop on Advanced Motion Control. Trento， Italy； IEEE， 2008： 296-300.

［14］ CISNEROS R， YOKOI K， YOSHIDA E. Yaw moment compensation by using full body motion［C］//2014 IEEE International Conference on Mechatronics and Automation （ICMA）. Tianjin， China： IEEE， 2014： 119-125.

［15］ STROM J， SLAVOV G， CHOWN E. Omnidirectional walking using zmp and preview control for the nao humanoid robot［M］//RoboCup 2009： robot soccer world cup XIII. Heidelberg： Springer， 2010： 378-389.

［16］ YEON J S， PARK J H. A Fast Turning Method for Biped Robots With Foot Slip During Single-Support Phase［J］. IEEE/ASME Transactions on Mechatronics， 2014， 19（6）： 1847-1858.

［17］ TZUU-HSENG S L， YU-TE S， SHAO-WEI L， et al. Walking motion generation， synthesis， and control for biped robot by using PGRL， LPI， and fuzzy logic［J］. Transactions of Systems，Man， and Cybernetics， Part B： Cybernetics， 2012， 41（3）：736-748.

［18］ KAGAMI Y， EMURA T， HIYAMA M. Vision-based playback method of wheeled mobile robots［J］. Robotica， 2000， 18（3）：281-286.

［19］ MERI?LI ?， VELOSO M. Biped Walk Learning Through Playback and Corrective Demonstration［C］//AAAI 2010：Twenty-fourth Conference on Artificial Intelligence. Atlanta， GA，United States： AAAI， 2010： 1594-1599.

［20］ KATAYAMA T， OHKI T， INOUE T， et al. Design of an optimal controller for a discrete time system subject to previewable demand［J］. Int J Control， 1985， 41（3）： 677-699.

（編輯 陳燦華）

Strategy for biped gait robot online generation and yaw control

DING Jiatao， XIAO Xiaohui， WANG Yang

（School of Power and Mechanical Engineering， Wuhan University， Wuhan 430072， China）

To meet the task requirements of autonomous yaw for biped robot， a combination strategy for biped gait robot generation and yaw control was proposed， thus the online biped gait generation was realized and the autonomous yaw was obtained. The procedures were as follows. Firstly， based on the zero moment point （ZMP） stabilization criterion，using the linear inverted pendulum model and forward-inverse kinematics cyclic solution algorithm， the offline biped gait generator was built. Secondly， adopting the preview control theory， the online generator was established to realize real-time generation and ZMP trajectory tracking. Then， combining the two generators， the concept of angle coefficient was proposed. After that， the method of displacement vector synthesis was adopted. Through the playback-correction scheme， the angle coefficient was acquired to satisfy the purposed yaw angle. Finally， the effectiveness was verified on the co-simulation platform using the MATLAB and Adams softwares. The results show that the proposed method is effective to utilize the offline generated angle and reduce the tracking error of the center of mass （COM） lateral trajectory. Through vector synthesis and playback-correction， the suitable angle coefficient can be achieved and no-yaw walking pattern can be obtained. Without requiring real-time ZMP trajectory design， stable biped gait with 15°and 30° yaw is realized.

bipedal walking robot； walking pattern generation； yaw control； vector synthesis； playback-correction

TP242

A

1672-7207（2016）04-1136-08

10.11817/j.issn.1672-7207.2016.04.008

2015-07-17；

2015-09-30

國家自然科學基金資助項目（51175383）（Project （51175383） supported by the National Natural Science Foundation of China）

肖曉暉，博士，教授，從事特種機器人技術與機器人動力學研究；E-mail：xhxiao@whu.edu.cn