一種不平整地形下足式機(jī)器人的主動(dòng)柔順控制方法*

張世俊 邢 琰,2 胡 勇,2

1.北京控制工程研究所，北京 100190 2. 空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京 100190

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

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

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

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

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

圖1 四足機(jī)器人結(jié)構(gòu)示意圖

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

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

(1)

(2)

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

2 主動(dòng)柔順控制

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

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

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

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

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

(3)

(4)

(5)

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

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

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

定義機(jī)身水平坐標(biāo)系{L}，原點(diǎn)與機(jī)體坐標(biāo)系{B}相同，x軸水平向右，y軸水平向前，z軸垂直向上。設(shè)機(jī)體坐標(biāo)系{B}相對(duì)于機(jī)身水平坐標(biāo)系{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ù)測(cè)得的關(guān)節(jié)角度和身體姿態(tài)，有

(6)

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

(7)

(8)

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

(9)

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

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

(10)

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

3 四足機(jī)器人仿真

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

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

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

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

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

3.1 不平整地形1仿真

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

圖5 采用主動(dòng)柔順控制仿真截圖

圖6 機(jī)器人前進(jìn)方向位移和速度曲線

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

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

3.2 不平整地形2仿真

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

圖8 采用主動(dòng)柔順控制仿真截圖

圖9 機(jī)器人機(jī)身運(yùn)動(dòng)軌跡

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

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

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

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

圖11 機(jī)器人機(jī)身運(yùn)動(dòng)軌跡

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

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

4 結(jié)論

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