基于ZMP的四足仿生機器人反應式行為控制策略研究

閆 曈， 許 威， 蘇 波

(中國北方車輛研究所，北京 100072)

現(xiàn)今，智能移動平臺正朝著高機動性、高通過性和高穩(wěn)定性方向發(fā)展，因此移動機器人對復雜環(huán)境的適應隨之成為運動控制領域的研究熱點，足式機器人也因其結構復雜而對控制系統(tǒng)提出了更高的要求.一方面，足式機器人移動平臺在未知的崎嶇地形行走時會受到足端與地面非預期碰撞而產(chǎn)生的沖擊力，影響平臺穩(wěn)定；另一方面，足式機器人也會由于機身與外界碰撞或搭載載荷而受到外力擾動，影響平臺位姿.因此減少外力沖擊對四足機器人穩(wěn)定行走的影響十分重要.

為實現(xiàn)四足機器人的柔順步態(tài)行走，一般在腿部設計被動柔順機構或采用合適的主動柔順控制策略.被動柔順結構對四足機器人穩(wěn)定性的提高有限，主動柔順控制主要通過對四足機器人模型施加力控制算法來實現(xiàn).上世紀80年代，Raibert使用彈簧負載倒立擺模型(SLIP)[1]實現(xiàn)單足機器人Hopper的穩(wěn)定控制，后續(xù)很多四足機器人的控制策略包括著名的BigDog[2]都是在該模型的基礎上發(fā)展而來的.

虛擬模型控制(Virtual Model Control，VMC)是一種描述足式機器人與周圍環(huán)境交互的力行為語言.首先確定系統(tǒng)需要控制的自由度，然后在該自由度上構建恰當?shù)奶摂M構件以產(chǎn)生合適的虛擬力，建立整體質量-彈簧-阻尼虛擬模型，通過雅可比矩陣計算期望的關節(jié)扭矩.將機器人的移動軌跡用作用于機體質心的虛擬力去表征，并將質心虛擬力通過足力分配策略轉換為關節(jié)扭矩，從而把四足機器人整機的移動映射為關節(jié)的扭矩.通過實時在線調(diào)整虛擬力，提高移動平臺在復雜環(huán)境下的適應能力.HyQ[3]、StarlETH[4]等機器人均采用了虛擬模型控制.文獻[5]設計了一種基于虛擬模型的四足機器人對角小跑步態(tài)控制器，在仿真環(huán)境中驗證了四足機器人應對不平坦地形能力.文獻[6]擴展了VMC，實現(xiàn)了圍繞支撐對角線機體旋轉的控制.文獻[7]提出了一種CPG(Central Pattern Generator)結合VMC的生物啟發(fā)運動控制器，實現(xiàn)四足機器人在仿真環(huán)境下的崎嶇地面行走.日本的Matsuoka研究了生物神經(jīng)細胞的適應現(xiàn)象，構成了Matsuoka神經(jīng)元振蕩器模型[8].日本的大多數(shù)研究者在四足機器人控制中采用的都是Matsuoka模型.

零力矩點ZMP(Zero Moment Point)由國外學者Vukobratovic提出，是地面上機器人的反應力矩與凈慣性矩之和為零的點.當ZMP落在支撐面內(nèi)，可保證機器人運動過程的穩(wěn)定性，所以ZMP可以作為機器人的動態(tài)穩(wěn)定判據(jù).Kurazume等人基于ZMP方法提出了軀干的三維擺動曲線，在TITAN-VIII機器人上實現(xiàn)了穩(wěn)定的對角小跑運動[9].文獻[10]將ZMP理論與CPG控制系統(tǒng)結合在了一起，提出了一種用于仿真機器人的混合CPG-ZMP控制器.但目前ZMP方法下機器人運動速度偏慢，且抗擾性較差.本研究針對這一問題設計了一種基于ZMP判據(jù)的反應式行為策略，該策略采用一種基于非線性干擾觀測器的滑模角度跟蹤控制策略，增大了四足機器人在受到外力突變時的抗干擾能力.

1 基于ZMP判據(jù)的反應式行為策略設計

從運動學角度來看，負載突變主要影響四足機器人機身的高度和機身在空間中的姿態(tài).若從動力學角度看，負載突變作用于機身可以分解為擾動力和力矩.很多傳統(tǒng)的ZMP判據(jù)并沒有引入機身動力學信息，將機身動力學信息引入ZMP判據(jù)中來，結合動力學模型，機器人在負載突變下的機身動力學同樣可以描述為

(1)

式中：m代表機身的質量；Θ是機身的慣性張量；g是重力加速度；Fext和Text是負載突變下作用在質心處的合力和合力矩；a和ω分別是質心的線性加速度和角加速度。上標變量I指慣性坐標系，上標變量B指機身坐標系.

在一個平整的路面上，零力矩點(ZMP點)被定義為由重力和慣性力的合力投影到地面上的點。假設nI為地面平面的法向量，結合式(1)，可以得到式(2)關系式.

.

(2)

式中：RIB是從慣性坐標系到機身坐標系的歐拉旋轉矩陣，r和rZMP表示質心和ZMP的位置.

使慣性坐標系的z軸和平面法向量的方向重合，式(2)中ZMP在平面上的分量為：

,

(3)

其中，xy(·)指一個量在xy平面上的分量，xyL由式(4)給出.

.

(4)

式中：zB是rI在z軸上的分量；RIB，i指RIB的第i行.

當機器人處于行走步態(tài)時，至少有3條腿接觸地面構成支撐面.此時只有當ZMP落入到支撐面上才能保證行走的穩(wěn)定性.但是當四足機器人處于一種邁腿頻率更快的對角小跑(trot)步態(tài)時，同時只有兩條腿接觸地面，構成支撐線.只有當ZMP落在這條支撐線上，才可以認為此時機器人處于行走穩(wěn)定狀態(tài).

將機器人的左前、右前、左后、右后4條腿足端與地面接觸點Pi的位置表示為(xi，yi，zi)，i=1，…，4.假設下一次足端落地點Pi′坐標為(xi+δx，yi，zi)，即Pi沿著慣性坐標系x軸平移了δx，那么變化后的足端支撐線在xy平面可以表示為式(5).

(5)

根據(jù)trot步態(tài)的相位關系可知，i=1時j=4，或i=2時j=3.

為了使零力矩點落在機器人的支撐線上，根據(jù)點與直線的判斷條件，將ZMP的坐標式(6)代入支撐線方程式(5)，得到式(7).

(6)

(7)

如圖1(a)所示，計算出δx的值以后，就可以使ZMP落在調(diào)整后的支撐線上.但上述過程明顯存在不足，那就是只調(diào)節(jié)落地點x軸方向上的位置.實際上，trot步態(tài)是一種不穩(wěn)定步態(tài)，若受到外力擾動時，從x和y方向一起來調(diào)節(jié)足端落地點會使機器人反應更加迅速，提升機器人的抗擾性能，使機器人更快地恢復平穩(wěn).

假設足端落地點Pi′(xi+δx1，yi+δy1，zi)為上一周期的足端觸地點Pi沿著機身坐標系x軸方向平移了δx1，又沿著y軸方向平移了δy1后得到，如圖1(b)所示，此時相當于把上一周期足端支撐線l平移到了l′的位置，并且使支撐線經(jīng)過ZMP.圖1中θ是足端支撐線和機器人前進方向的夾角，x軸方向是機器人前進方向，y軸方向是機器人側向方向.

由圖1的幾何關系可以得出：

δx1=δxsin2θ，δy1=δxsinθcosθ.

(8)

圖1 兩種調(diào)整支撐線的方法示意圖

假設規(guī)劃出的足端落地點都能符合機器人腿部本身的運動學約束，那么只要通過規(guī)劃出足端下一次的落地點Pi′的位置(xi+δx1，yi+δy1，zi)，就能使足端落地后，機器人的ZMP落在支撐線上.若足端的調(diào)整位置超出了足端工作空間，則應在滿足機器人運動學約束的情況下，盡可能地落在足端工作空間邊界上使足端調(diào)整量達到最大.由機器人設計的尺寸和其正常行走實驗可知θ<45°，此時滿足δx1+δy1<δx.從x軸和y軸方向一起來調(diào)節(jié)足端落地點，其調(diào)整量總和比在單一的在x軸方向上調(diào)整的調(diào)整量減小，從而使修正后足端位置最大可能地滿足足端工作空間的限制，進而提升足端位置調(diào)整后的抗擾性能并使機器人能夠應對更大幅度的外力擾動.

四足機器人的trot步態(tài)對角線上的兩條腿步態(tài)一致.假設T是運動周期，S是單腿的步距，H為髖部到足端最低點的距離，h為髖部到足端最高點的距離，δx1和δy1是基于ZMP的步態(tài)調(diào)整量，則經(jīng)過改進后的單腿在3個方向上的足端軌跡如式(9)～式(11)所示.

(9)

(10)

.

(11)

足端落地點調(diào)整量還需要滿足工作空間約束：

0≤S+δx1≤Xmax,h≤Ymax

.

(12)

式中：Xmax和Ymax為機器人足端在x軸和y軸方向上可以到達的極限距離.

4條腿的軌跡只要根據(jù)trot步態(tài)不同的相位就可以得到.只要保證機器人足端沿著基于ZMP規(guī)劃的步態(tài)行走，就能保證機器人的穩(wěn)定行走.通過機器人逆運動學就能求出期望的關節(jié)角度、角速度和角加速度.

2 關節(jié)角魯棒跟蹤控制策略

圖2為控制系統(tǒng)結構框圖.控制系統(tǒng)由外環(huán)和內(nèi)環(huán)構成.外環(huán)基于ZMP規(guī)劃出了四足機器人在負載突變下的穩(wěn)定步態(tài)，并通過逆運動學計算出期望關節(jié)角信息.內(nèi)環(huán)則為了實現(xiàn)期望關節(jié)角的跟蹤.本研究采用了一種基于非線性干擾觀測器的滑模角度跟蹤控制策略.首先根據(jù)實際關節(jié)角度與給定的期望關節(jié)角度的誤差，建立了一個滑模面；然后通過非線性干擾觀測器盡可能地估計關節(jié)上存在的擾動，通過滑模補償，來抵消擾動或者盡可能降低擾動帶來的影響，從而提升關節(jié)角跟蹤性能.

非線性干擾觀測器設計為

.

(13)

為了克服觀測器的不足之處，取

L(θ)=X-1M-1(θ)

(14)

.

(15)

式中：X為可逆矩陣，可通過線性矩陣不等式來求.

由式(14)、式(15)可求得

.

(16)

式(13)、式(14)和式(15)構成了非線性干擾觀測器.

設計Lyapunov函數(shù)為

(17)

式中：M(θ)為對稱正定矩陣.

對式(17)求導，可得

.

(18)

.

(19)

.

(20)

將式(14)代入式(20)，可得

(21)

將式(21)代入式(18)，可得

.

(22)

構造不等式：

.

(23)

式中：Γ為對稱正定陣.

于是

.

(24)

可見，干擾觀測器指數(shù)收斂，收斂速度取決于λΓmin值.λΓmin為Γ的最小特征值，λΓmin值越大，收斂速度越快.

由不等式(23)可見，式中含有非線性項，必須轉化為線性矩陣不等式才能求解.令Y=X-1，將YT=(X-1)T和Y=X-1分別乘以式(23)的左右兩邊，得

(25)

即

(26)

YT+Y-YTΓY-I>ζI

.

(27)

.

(28)

通過Matlab下的YALMIP工具箱求解式(28)，便可以得到Y，從而得到X.能否求解該不等式取決于ζ和Γ值.ζ越小、Γ越小，越容易得到可行解.

采用觀測器式(18)觀測干擾d.在滑?？刂浦袑Ω蓴_進行補償，可有效地降低切換增益，從而有效地降低抖振.

(29)

對式(29)求導，可得

(30)

設計控制器為

(31)

將式(31)代入式(30)，并整理可得

(32)

(33)

(34)

(35)

由式(35)可得

(36)

V(t)a≤e-μ(t-t0)V(t0)

.

(37)

3 實驗仿真

仿真先對比常規(guī)滑?？刂啤в蟹蔷€性干擾觀測器的滑?？刂坪突赯MP的帶有非線性干擾觀測器的滑模控制3種機制下，受到負載突變擾動的機器人機身姿態(tài)角的調(diào)整曲線；然后給出了基于ZMP的帶有非線性干擾觀測器的滑?？刂撇呗宰饔孟?，機器人腿部關節(jié)角的跟蹤曲線，最后給出了Adams虛擬樣機仿真動畫截圖.

由于需要同時對多個參數(shù)變量進行實時檢測、計算和調(diào)整并且需要模擬實際系統(tǒng)的動力學特性，控制過程相對復雜，因此選擇使用Matlab2016a和Adams2017聯(lián)合仿真技術來進行算法仿真，仿真環(huán)境設置如下：

第一步，在Adams里建立四足機器人的模型，四足機器人有4條結構一樣的腿，每條腿有3個自由度.仿真中關節(jié)1代表髖關節(jié)側擺關節(jié)，關節(jié)2代表髖關節(jié)前擺關節(jié)，關節(jié)3代表膝關節(jié)前擺關節(jié).使4條腿關節(jié)1的初始位置為0°，關節(jié)2的初始位置為-30°，關節(jié)3的初始位置為60°，在每個關節(jié)處添加旋轉副，在足端設置與地面的接觸.

第二步，設置虛擬樣機環(huán)境與時間觸發(fā)函數(shù).機器人0～1 s的時候處于四腿著地的穩(wěn)定靜止狀態(tài)，1～8 s處于對角小跑步態(tài)(trot)正常行走.在距離機身右上方約0.5 m高的地方放置一個長方體重物，在第4 s的時候，觸發(fā)重物釋放函數(shù)，重物會呈自由落體砸到機身上.

第三步，建立Adams和Simulink交互的數(shù)據(jù)，Adams的輸入就是Simulink的輸出，Adams的輸出就是Simulink的輸入.設置Adams的輸入為12個關節(jié)的力矩，Adams的輸出為12個關節(jié)的角度和角速度、機身的姿態(tài)角和姿態(tài)角速度、機身的高度和在慣性坐標系下x軸和y軸方向的加速度.

第四步，設置仿真步長為0.001 s，仿真求解器為ode15s，動畫模式為交互式，仿真模型為離散型，仿真時間為8 s.

控制器參數(shù)選取如下：Λ1=3I12×12，KD=5I12×12，ζ=2.6，即X=0.385.

為了突出基于ZMP的步態(tài)重規(guī)劃和引入非線性干擾觀測器對抵抗負載突變擾動的有效性，圖3給出了負載突變下的機器人在3種不同控制機制下橫滾角和俯仰角的調(diào)整曲線.仿真開始后，機器人正常行走，常規(guī)滑?？刂圃谧藨B(tài)角調(diào)整性能上略差于另外兩種控制策略.第4 s負載落下，4.3 s落到機身，可以看出橫滾角和俯仰角在4.3 s之后產(chǎn)生明顯抖變，機器人發(fā)生傾覆，僅使用常規(guī)滑?？刂撇呗詿o法保證機器人姿態(tài)穩(wěn)定.而在另外兩種控制策略作用下，機器人遇到負載突變擾動后都能保持較為穩(wěn)定的行走.

圖3(a)中，對比曲線2和曲線3，可以發(fā)現(xiàn)曲線3距離平衡位置的最大偏離值為3°，而曲線2距離平衡位置的最大偏移值有7.5°；曲線3的振蕩幅值在4.3 s負載突變以后并不會有太大變化，而曲線2的振蕩幅值在負載突變后振蕩幅值變大，也即機身不穩(wěn)定程度加深.對比可知曲線3所對應的控制策略更優(yōu)，此仿真結果證明基于ZMP規(guī)劃的步態(tài)使機器人在負載突變后姿態(tài)穩(wěn)定性增強.

圖3 3種控制策略下機器人的橫滾角俯仰角曲線對比

圖4給出了機器人在基于ZMP的四足仿生機器人反應式行為控制策略作用下左前腿三關節(jié)的期望角度和實際跟蹤角度.每個關節(jié)的期望角度是通過基于ZMP理論規(guī)劃出的步態(tài)和逆運動學求解出的.關節(jié)跟蹤控制器為帶有非線性干擾觀測器的滑?？刂破?，可以看出該控制策略下關節(jié)角跟蹤性能較好，在第4.3 s機器人負載突變時，由于負載突變給關節(jié)3帶來急劇增大的壓力，使得關節(jié)3的角度跟蹤誤差較大，誤差達到0.1 rad，但之后實際關節(jié)角又能迅速跟蹤上期望關節(jié)角信號，這表明所提出的控制策略是有效的.

在每個關節(jié)處施加一個幅值為5 N·m、周期為5 s的方波信號，對比在有無擾動觀測器時的角度跟蹤情況.實際得出的結論是，具有干擾觀測器比沒有干擾觀測器的跟蹤效果精度要高很多.

圖4 機器人關節(jié)期望角度和實際跟蹤曲線

圖5給出了四足機器人Adams虛擬樣機的仿真過程圖，墜落負載的質量為11.7 kg，墜落高度為0.3 m，負載墜落位置在機器人機身質心前方.

從圖5可以明顯看出機器人正常行走的時候姿態(tài)平穩(wěn)，在遭到負載撞擊后姿態(tài)相對于正常行走時有小幅變化.通過觀察足端軌跡曲線圖，可明顯看出機器人負載突變造成了四條腿的位置不對稱.但0.5 s以內(nèi)機器人便能恢復平穩(wěn)，背負負載繼續(xù)正常行走.

上述仿真結果表明了所提出的基于ZMP的步態(tài)重規(guī)劃和關節(jié)角魯棒控制策略在四足機器人應對負載突變的有效性，證明了組合控制策略魯棒性優(yōu)于任何一種單一的控制策略.

圖5 負載突變下的四足機器人虛擬樣機抗擾仿真過程圖

4 結 論

針對四足機器人行進間負載突變下的穩(wěn)定性問題，設計了一種基于ZMP穩(wěn)定判據(jù)的步態(tài)自適應規(guī)劃和底層帶有非線性干擾觀測器的關節(jié)角魯棒滑模控制的反應式控制策略.Adams和Matlab聯(lián)合仿真結果表明該方案的有效性.后續(xù)會在本方法的基礎上，考慮研究加速減速等其他特殊工況下受到較大側向干擾力等多應用場景下的四足機器人抗干擾能力規(guī)劃與控制策略設計.