張世俊 邢 琰,2 胡 勇,2

1.北京控制工程研究所，北京 100190 2. 空間智能控制技術重點實驗室，北京 100190

與常見的輪式移動機器人相比，足式機器人具有更好的地形適應能力，針對不平整地形下的足式機器人步態(tài)規(guī)劃與控制，已有許多研究成果[1-3]，主要思路為根據(jù)周圍的地形信息，選擇合適的落足點，進而規(guī)劃運動軌跡。然而該方法存在以下限制，落足點的選取依賴于機器人對周圍地形感知的準確度，并且由于計算量較大，需要在步態(tài)中加入靜止站立階段用于機器人運動規(guī)劃。然而對于崎嶇程度相對不大的地形，步態(tài)規(guī)劃算法并不需要在每一步根據(jù)地形信息選取擺動腿的落足點，可以通過主動柔順控制方案使得機器人在不需要地形信息的情況下，對外部環(huán)境表現(xiàn)出柔順特性，從而提高機器人對不平整地形的適應能力，使得機器人能夠快速通過不平整的地形。

迄今，針對足式機器人的主動柔順控制研究已取得了許多成果，阻抗控制[4-8]是一種常見的主動柔順控制方法，它基于機器人足端與地面環(huán)境的接觸力模型，通過反饋調(diào)整足端位置達到控制接觸力的目的，該方法相當于將接觸力誤差通過一個二階濾波器得到足端位置調(diào)整量，與參考足端軌跡相加后通過逆運動學模型生成關節(jié)角指令，這種基于足端工作空間的修正方法并沒有給出關節(jié)角速度的修正方法，一定程度上影響關節(jié)PD控制的關節(jié)角度跟蹤效果；Zucker和Winkler等[9-10]利用逆動力學控制和力反饋控制，結(jié)合低增益系數(shù)的關節(jié)PD控制，實現(xiàn)了機器人關節(jié)空間內(nèi)的主動柔順性，并成功將其應用于“Little dog”機器人，該方法通過逆動力學模型預測接觸力，力反饋控制直接將接觸力誤差轉(zhuǎn)換成關節(jié)力矩施加到關節(jié)上，這需要精確的動力學模型和地形信息，同時需要機器人關節(jié)能夠?qū)崿F(xiàn)高精度力矩控制。

針對上述分析，針對未知不平整地形提出一種主動柔順控制方法，包括基于關節(jié)空間的阻抗控制、姿態(tài)柔順控制和關節(jié)PD控制，其中基于關節(jié)空間的阻抗控制方法將力反饋信號轉(zhuǎn)換到關節(jié)空間，通過阻抗濾波器同時生成關節(jié)角度修正量和關節(jié)角速度修正量，再將修正后的關節(jié)角度和角速度指令輸出給關節(jié)PD控制器；姿態(tài)柔順控制通過機身姿態(tài)角誤差計算支撐腿長度調(diào)整量，進而通過阻抗濾波器得到腿長修正指令，綜合設計關節(jié)柔順控制和姿態(tài)柔順控制的濾波器參數(shù)，使機器人腿部關節(jié)和機身姿態(tài)對不平整地形表現(xiàn)出柔順特性，通過不平整地形時更加平穩(wěn)。最后本文的主動柔順控制方案在四足機器人中得到了仿真驗證，實現(xiàn)了機器人在未知不平整地形下的穩(wěn)定行走。

1 整體步態(tài)規(guī)劃與控制框架

以四足機器人為例設計主動柔順控制方案，機器人構型如圖1所示，機器人包含4條腿，每條腿具有3個關節(jié)，分別為根關節(jié)(偏航自由度)、髖關節(jié)(俯仰自由度)和膝關節(jié)(俯仰自由度)。定義固連于機器人身體的機體坐標系{B}，原點位于機器人身體中心，x軸指向機器人右方，y軸指向機器人正前方，z軸垂直于身體向上。

圖1 四足機器人結(jié)構示意圖

圖2 整體步態(tài)規(guī)劃與控制框架

主步態(tài)規(guī)劃模塊包括重心運動軌跡規(guī)劃、擺動腿軌跡規(guī)劃和腿部逆運動學模塊。四足機器人的典型步態(tài)有慢走步態(tài)(walk)、對角步態(tài)(trot)、溜蹄步態(tài)(pace)和疾跑步態(tài)(gallop)等，與其他幾種步態(tài)相比，對角步態(tài)具有較高的能效比和較大的速度適應范圍，對稱性較好，易于保持機器人運動的穩(wěn)定性，是一種具有較高實用性的動態(tài)步行方式[11]。因此，本文采用對角步態(tài)作為四足機器人在不平整地形下行走的主步態(tài)。機器人擺動腿軌跡采用如下具有復合擺線形式的軌跡[12]：

(1)

(2)

其中，s為跨距；H為抬腿高度；tw為擺動相持續(xù)時間。該足端軌跡滿足位置、速度和加速度連續(xù)無突變，擺動相與支撐相切換平滑。腿部逆運動學模塊將足端軌跡轉(zhuǎn)換到關節(jié)空間內(nèi)的關節(jié)角軌跡，將其作為控制系統(tǒng)的參考指令，從而驅(qū)動各個關節(jié)實現(xiàn)四足機器人的前進步態(tài)。

2 主動柔順控制

2.1 基于關節(jié)空間的阻抗控制

基于關節(jié)空間的阻抗控制框圖如圖3所示，由位置控制內(nèi)環(huán)和力控制外環(huán)組成，主要思想為由外環(huán)控制器將足端的接觸力誤差映射到關節(jié)空間內(nèi)，再通過阻抗濾波器和積分器生成關節(jié)角和關節(jié)角速度修正指令，用于修正步態(tài)規(guī)劃器給出的關節(jié)角和角速度參考值，然后將生成的關節(jié)角和角速度指令輸出給內(nèi)環(huán)位置控制器，通過反饋調(diào)整足端位置達到控制接觸力的目的。采用基于關節(jié)空間的阻抗控制使得機器人腿部關節(jié)直接響應不平整地形造成的接觸力突變，從而實現(xiàn)機器人的主動柔順性。

圖3 基于關節(jié)空間的阻抗控制框圖

根據(jù)步態(tài)規(guī)劃中給出的參考軌跡，每條腿關節(jié)空間內(nèi)的力矩誤差可按下式計算[13-14]：

Terr=JT(qref)Fref-JT(q)F

(3)

(4)

(5)

2.2 姿態(tài)柔順控制

機器人在通過不平整地形時，四條腿的落足點高度不一致，會導致機身姿態(tài)發(fā)生偏轉(zhuǎn)，從而影響機器人步態(tài)的穩(wěn)定性，因此需要加入機身姿態(tài)反饋控制保證機器人身體姿態(tài)保持穩(wěn)定。機身姿態(tài)反饋控制利用支撐腿長度的調(diào)整使機身實際姿態(tài)與期望姿態(tài)重合，通過調(diào)整左右腿長度差改變機身滾轉(zhuǎn)角，通過調(diào)整前后腿長度差改變機身俯仰角。

圖4 姿態(tài)調(diào)整圖

定義機身水平坐標系{L}，原點與機體坐標系{B}相同，x軸水平向右，y軸水平向前，z軸垂直向上。設機體坐標系{B}相對于機身水平坐標系{L}的歐拉角按照1-2-3轉(zhuǎn)序定義：俯仰角α，滾轉(zhuǎn)角φ和偏航角ψ，則姿態(tài)轉(zhuǎn)換矩陣可表示為：

其中,s1=sin(α)；c1=cos(α)；s2=sin(φ)；c2=cos(φ)；s3=sin(ψ)；c3=cos(ψ)。則根據(jù)測得的關節(jié)角度和身體姿態(tài)，有

(6)

其中,BPi為支撐腿i的足端在坐標系{B}中的坐標；LPi=[pixpiypiz]T為支撐腿i的足端在坐標系{L}中的坐標，則可以估計出調(diào)整俯仰角和滾轉(zhuǎn)角所需的腿長修正量：

(7)

(8)

其中,plx為左支撐腿足端x軸坐標；prx為右支撐腿足端x軸坐標；pfy為前支撐腿足端z軸坐標；pbz為后支撐腿足端z軸坐標；zep和zer分別為對應調(diào)整俯仰角和滾轉(zhuǎn)角的腿長修正量；φ0和α0為滾轉(zhuǎn)角和俯仰角閾值，可根據(jù)平地行走身體波動狀況設定，當姿態(tài)角超過一定閾值時，姿態(tài)控制器開始發(fā)揮作用。將上述計算得到的支撐腿長度調(diào)節(jié)量通過一階阻抗濾波器后生成支撐腿長度修正指令，濾波器傳遞函數(shù)如下：

(9)

其中,(kp,bp)和(kr,br)分別為對應俯仰角和滾轉(zhuǎn)角的阻抗濾波器參數(shù)。

令通過濾波器后的俯仰角和滾轉(zhuǎn)角的支撐腿長度修正指令分別為zcp和zcr，再將該腿長度調(diào)節(jié)量按式(10)分別分配至四條腿：

(10)

設計姿態(tài)阻抗濾波器的原因主要有以下2點，1)使機器人機身姿態(tài)調(diào)整對不平整地形表現(xiàn)出柔順特性，延緩姿態(tài)反饋的響應速度，使姿態(tài)調(diào)整與足端接觸力反饋相比表現(xiàn)為慢周期特性；2)機器人阻抗控制存在正反饋的現(xiàn)象，例如當機器人姿態(tài)向后傾斜時，后支撐腿的支撐力大于期望支撐力，此時根據(jù)關節(jié)阻抗控制需要收縮后支撐腿以期減小接觸力，從而加劇機器人姿態(tài)的偏轉(zhuǎn)，因此通過引入姿態(tài)阻抗濾波器，綜合設計關節(jié)柔順控制和姿態(tài)柔順控制的濾波器參數(shù)使機器人在通過不平整地形時保持平穩(wěn)。

3 四足機器人仿真

在ADAMS中搭建四足機器人虛擬樣機模型，采用ADAMS和SIMULINK聯(lián)合仿真技術進行虛擬樣機仿真[15]。機器人總質(zhì)量200kg，大腿長度為0.4m，小腿長度為0.3m。

為驗證本文提出的控制方法的有效性和機器人通過不平整地形的能力，本文進行如下兩種地形仿真：

不平整地形1：機器人通過一段左右錯落的不平整地面，地形高度落差最大值為6cm；

不平整地形2：機器人通過上下坡的地形，地形高度落差為16cm，坡度為10°。

針對每種地形情況分別做2組仿真進行對比，2組仿真中步態(tài)規(guī)劃算法相同，其中一組采用本文提出的柔順控制和姿態(tài)控制方法，另一組只采用關節(jié)PD控制。整個仿真過程中，機器人沒有任何地形信息，僅依靠自身的關節(jié)角、角速度和姿態(tài)角測量信息，實現(xiàn)在不平整地形下的穩(wěn)定行走。

3.1 不平整地形1仿真

仿真中機器人通過一段左右錯落的不平整地面，采用主動柔順控制時仿真截圖如圖5所示，仿真中機器人在2～8s期間通過不平整地形，機器人前進距離和速度曲線如圖6所示，可知2種方法均能使機器人通過不平整地形，采用柔順控制時機器人前進速度波動更小，對地形適應能力更強。

圖5 采用主動柔順控制仿真截圖

圖6 機器人前進方向位移和速度曲線

機身俯仰角和滾轉(zhuǎn)角曲線如圖7所示，采用PD控制時，機身俯仰角在±4°內(nèi)波動，滾轉(zhuǎn)角在-6°～4°內(nèi)波動，而采用柔順控制時機身俯仰角和滾轉(zhuǎn)角的波動范圍均控制在±2°左右。

圖7 機身俯仰角和滾轉(zhuǎn)角曲線

3.2 不平整地形2仿真

仿真中機器人通過一段上下坡的地形，采用主動柔順控制時仿真截圖如圖8所示，機器人機身運動軌跡曲線如圖9所示，采用柔順控制時，機器人能夠順利通過上坡和下坡地形，仿真中機器人在3～7s期間上坡，9～13s期間下坡；而采用PD控制時，機器人無法通過該地形，在6s左右機器人向坡下滑落。

圖8 采用主動柔順控制仿真截圖

圖9 機器人機身運動軌跡

機身俯仰角和滾轉(zhuǎn)角曲線如圖10所示，采用柔順控制時，機器人在上下坡期間機身俯仰角控制在-6°～8°以內(nèi)，滾轉(zhuǎn)角控制在±5°以內(nèi)；而采用PD控制時，機器人上坡時機身姿態(tài)波動越來越大，在6s左右向坡下滑落。

圖10 機身俯仰角和滾轉(zhuǎn)角曲線

為進一步驗證本文所提的主動柔順控制方法中姿態(tài)柔順控制的必要性，繼續(xù)做下面2組仿真對比，其中一組采用本文提出的主動柔順控制方案，其中姿態(tài)控制為本文提出的姿態(tài)柔順控制，稱為柔順控制Ⅰ；另一組同樣采用柔順控制方法，稱為柔順控制Ⅱ，與柔順控制Ⅰ相比有2點不同，一是關節(jié)空間的阻抗控制中不加入自適應參數(shù)，二是姿態(tài)控制的腿長調(diào)節(jié)量不通過阻抗濾波器直接生成支撐腿長度的修正指令，此時姿態(tài)控制沒有柔順性。

機器人機身運動軌跡曲線如圖11所示，采用柔順控制Ⅰ時，機器人能夠通過上坡和下坡地形，不平整地形對機器人前進速度影響較小，仿真中機器人在3～7s期間上坡，9～13s期間下坡；而采用柔順控制Ⅱ時，機器人雖然能通過該不平整地形，在機器人前進速度受地形影響較大，前進距離明顯小于采用柔順控制Ⅰ時的前進距離，主要體現(xiàn)在爬坡階段，仿真中機器人在3～10s期間上坡，爬坡時間接近采用柔順控制Ⅰ時的2倍。

圖11 機器人機身運動軌跡

機身俯仰角和滾轉(zhuǎn)角曲線如圖12所示，由圖可知，采用柔順控制Ⅰ時機器人爬坡階段身體姿態(tài)波動小于采用柔順控制Ⅱ的情況。

圖12 機身俯仰角和滾轉(zhuǎn)角曲線

4 結(jié)論

提出了一種未知不平整地形下的足式機器人主動柔順控制方法，使機器人在不需要地形信息的情況下，對不平整地形具有一定的適應能力。該方法融合了基于關節(jié)空間的阻抗控制、關節(jié)PD控制和姿態(tài)柔順控制3部分，通過綜合設計關節(jié)柔順控制和姿態(tài)柔順控制的濾波器參數(shù)，結(jié)合自適應調(diào)節(jié)參數(shù)，改善了傳統(tǒng)阻抗控制的正反饋現(xiàn)象，使機器人在通過不同地形時保持姿態(tài)平穩(wěn)。最后基于虛擬樣機仿真平臺，分別針對2種地形環(huán)境進行對比仿真。對于地形高度落差最大值為6cm的不平整地形，采用本文提出的控制方法后，機身姿態(tài)波動的最大值減小50%；對于坡度為10°的上下坡地形，與PD控制和傳統(tǒng)的阻抗控制相比，采用本文提出的控制方法后機器人通過不平整地形的平穩(wěn)性和快速性均有提高。2組仿真結(jié)果表明，在不平整地面下機器人可以快速平穩(wěn)行走，從而驗證了提出方法對不平整地形的適應性。