六足機(jī)器人不規(guī)則地形下的運(yùn)動規(guī)劃算法研究

(1.天津商業(yè)大學(xué) 機(jī)械工程學(xué)院，天津 300134； 2.北京理工大學(xué) 機(jī)電學(xué)院，北京 100081)

0 引言

與輪式或履帶式機(jī)器人相比，足式機(jī)器人的落足點(diǎn)是離散的，因而足式機(jī)器人能在足尖點(diǎn)可達(dá)域范圍內(nèi)靈活調(diào)整行走姿態(tài)，并合理選擇支撐點(diǎn)，具有更強(qiáng)的不規(guī)則地形適應(yīng)能力。但如何才能為足式機(jī)器人設(shè)計(jì)出可適應(yīng)不規(guī)則地形環(huán)境的步行控制系統(tǒng)，成為足式機(jī)器人研發(fā)過程中面臨的最大挑戰(zhàn)[1-2]。眾所周知，自然界中的各種生物通過物競天擇和長期進(jìn)化，已對外界環(huán)境產(chǎn)生了極強(qiáng)的適應(yīng)性，在能量轉(zhuǎn)換、運(yùn)動控制、姿態(tài)調(diào)節(jié)、信息處理和方位辨別等方面還表現(xiàn)出了高度的合理性，已日益成為人類開發(fā)先進(jìn)技術(shù)裝備的參照物。為了研制出合適的足式機(jī)器人步行控制系統(tǒng)，國內(nèi)外的機(jī)器人學(xué)專家們正密切關(guān)注仿生設(shè)計(jì)，力圖從動物適應(yīng)不規(guī)則地形的突出能力中尋找靈感與方法[3]。昆蟲具有良好的運(yùn)動和環(huán)境適應(yīng)能力，是足式機(jī)器人理想的仿生對象，大部分的六足機(jī)器人均采用了螞蟻等昆蟲的結(jié)構(gòu)形式[4-6]。除了在結(jié)構(gòu)方式外,模仿動物的運(yùn)動控制方式也是重要的仿生研究內(nèi)容。通過對昆蟲神經(jīng)系統(tǒng)的研究，基于中樞模式發(fā)生(CPG)開始廣泛應(yīng)用于足式機(jī)器人的節(jié)律運(yùn)動控制之中[7-9]。已經(jīng)提出的CPG網(wǎng)絡(luò)均具有較強(qiáng)的耦合性，當(dāng)足式機(jī)器人在不規(guī)則地形下運(yùn)動時(shí)，由于參數(shù)整定的困難，運(yùn)動效果較差[10]。通過觀察動物的運(yùn)動來獲得運(yùn)動控制靈感是一種簡單有效的仿生手段，文獻(xiàn)[11]通過對螞蟻的觀察實(shí)驗(yàn)提出了一種轉(zhuǎn)向運(yùn)動偏差的修正算法，提供了六足機(jī)器人轉(zhuǎn)向運(yùn)動的穩(wěn)定性。本文將采用類似的方法，通過對螞蟻運(yùn)動的觀察實(shí)驗(yàn)，提出一種六足機(jī)器人在不規(guī)則地形下的運(yùn)動控制算法，并通過樣機(jī)試驗(yàn)對所提算法進(jìn)行驗(yàn)證。

1 螞蟻運(yùn)動觀測實(shí)驗(yàn)與分析

作為昆蟲綱的典型物種，螞蟻的運(yùn)動能力極其優(yōu)秀，具有很高的仿生研究價(jià)值，而其軀、腿分明的體型特點(diǎn)亦使深入觀測其運(yùn)動特征較為方便。本文采用與文獻(xiàn)[11]類似的實(shí)驗(yàn)思路、儀器、環(huán)境和方法，重點(diǎn)模擬了兩種障礙物情況(平放障礙柱及堆疊障礙柱)，通過觀察螞蟻經(jīng)過這兩種理想、典型的障礙物時(shí)的姿態(tài)特征，尋找螞蟻在不規(guī)則地形環(huán)境中的運(yùn)動應(yīng)重點(diǎn)模擬了兩種障礙物情況(平放障礙柱及堆疊障礙柱)，通過觀察螞蟻經(jīng)過這兩種理想、典型的障礙物時(shí)的姿態(tài)特征，尋找螞蟻在不規(guī)則地形環(huán)境中的運(yùn)動應(yīng)變策略。選取樣本蟻的運(yùn)動截圖如圖1所示。

圖1 螞蟻跨越障礙柱和障礙柱堆過程截圖

由上述典型樣本可以發(fā)現(xiàn)，螞蟻在應(yīng)對兩種障礙時(shí)采取了不同的運(yùn)動策略：當(dāng)面對高度小于其軀體離地高度的障礙柱時(shí)，螞蟻始終保持軀體與地面平行，沒有出現(xiàn)側(cè)傾、俯仰或翻轉(zhuǎn)現(xiàn)象，與文獻(xiàn)介紹的螞蟻在理想環(huán)境下運(yùn)動的姿態(tài)類似；而當(dāng)面對高度大于其軀體離地高度的障礙柱堆時(shí)，螞蟻則調(diào)整位姿以保持軀體與障礙物斜面平行。經(jīng)過進(jìn)一步的實(shí)驗(yàn)觀察發(fā)現(xiàn)，幾乎在任何時(shí)刻，螞蟻軀體都平行于支撐相核心三足所確定的平面(螞蟻可能會有超過三只足處于支撐相，而此處的“核心”三足是指處于軀體一側(cè)的前足、后足以及對側(cè)的中足)。

2 仿生六足機(jī)器人的運(yùn)動學(xué)模型

圖2所示為普通18關(guān)節(jié)六足機(jī)器人的機(jī)構(gòu)簡圖。在任意腿著地情況下該機(jī)器人都具有6個(gè)自由度。在通常的運(yùn)動學(xué)建模過程中，一般通過D-H方法為機(jī)器人的單腿建立三自由度的D-H模型，然后根據(jù)支撐相或擺動相的相位情況分別討論機(jī)器人整體的運(yùn)動學(xué)模型。該方法固然可以建立機(jī)器人的精確模型，但也存在明顯缺點(diǎn)：首先是運(yùn)動學(xué)模型較為復(fù)雜，不便于計(jì)算機(jī)程序編寫；其次是將機(jī)器人的六足嚴(yán)格劃分為一半屬于支撐相和一半屬于擺動相，忽視了六足中分別處于支撐相和擺動相的腿數(shù)并非完全相等的事實(shí)；最后是在探討機(jī)器人支撐相腿時(shí)將機(jī)器人機(jī)構(gòu)系統(tǒng)視為并聯(lián)系統(tǒng)，在進(jìn)行相應(yīng)運(yùn)動學(xué)解算時(shí)需對非線性方程組進(jìn)行繁瑣求解(而一旦超過三足著地則為冗余方程，求解更為困難)，而求解非線性方程組正是計(jì)算機(jī)的弱項(xiàng)，所以該模型在計(jì)算機(jī)上的復(fù)用性較差。

圖2 六足機(jī)器人機(jī)構(gòu)圖

圖3 六足機(jī)器人單腿D-H模型圖

針對以上問題，本文提出建立D-H模型的另一種思路，即將地面與足端的球鉸副反映到機(jī)器人的單腿D-H模型中，球鉸副所代表的3自由度(在后文被稱為被動關(guān)節(jié))與機(jī)器人腿部3驅(qū)動關(guān)節(jié)(在后文被稱為主動關(guān)節(jié))正好分為兩部分，不僅便于運(yùn)動學(xué)模型解算，而且對于不需計(jì)算被動關(guān)節(jié)的情況可以不予計(jì)算，從而更符合接觸點(diǎn)地形復(fù)雜的現(xiàn)實(shí)情況。六足機(jī)器人單腿模型只涉及地面、足端與軀體重心這三部分(該模型不需要區(qū)分支撐相與擺動相，將擺動相足端視為虛擬著地)，根據(jù)單腿模型6自由度即可確定機(jī)器人軀體位姿，根據(jù)軀體位姿又可確定機(jī)器人其他腿的位姿，這時(shí)可將并聯(lián)系統(tǒng)理解成串連系統(tǒng)，從而大大降低了非線性方程組的求解難度。另外，整個(gè)系統(tǒng)幾乎可以濃縮為單腿模型，并且各腿的模型完全相同，可以復(fù)用，這樣不僅簡化了系統(tǒng)的運(yùn)動學(xué)模型，而且便于計(jì)算機(jī)編程以實(shí)現(xiàn)六足機(jī)器人的在線運(yùn)動控制。

按照上述思路，建立六足機(jī)器人單腿D-H模型如圖3所示，D-H參數(shù)見表1，圖中虛線代表的構(gòu)件長度為零，所有虛線構(gòu)件代表地面與足端接觸的球鉸副。根據(jù)上述參數(shù)可以方便地獲得:

(1)

由全局坐標(biāo)系{G}與各足足端著地點(diǎn)坐標(biāo)系的固定關(guān)系可得0GTi，再由軀體坐標(biāo)系{B}(坐標(biāo)系方向如圖4所示)與各足的D-H模型中{6}的固定關(guān)系可得BGTi，從而可以方便地求解機(jī)器人軀體位姿與各主動自由度的關(guān)系。

表1 機(jī)器人單足D-H參數(shù)一覽表

應(yīng)當(dāng)指出，機(jī)器人腿部必需兩種傳感器：一是位置傳感器，用來感知各主動關(guān)節(jié)的運(yùn)動位置；二是足端的力傳感器，用來感知腿與地面的接觸力。位置傳感器可以保證腿準(zhǔn)確地按規(guī)劃運(yùn)動，而力傳感器可以保證足端很好地適應(yīng)陌生地形。

圖4 六足機(jī)器人中的坐標(biāo)系

3 仿生六足機(jī)器人運(yùn)動規(guī)劃算法研究

仿生六足機(jī)器人在任何時(shí)候都擁有六自由度，這意味著該機(jī)器人在運(yùn)動中的位姿有多種選擇，其足的運(yùn)動位姿也有多種選擇(理論上是無數(shù)種解)，而理想情況下機(jī)器人需要且只需要一種“解”，以期獲得高效執(zhí)行。另外，哪種運(yùn)動策略更高效、更節(jié)能、更穩(wěn)定，也是一個(gè)需要深入探討的問題。而螞蟻應(yīng)對不規(guī)則地形的運(yùn)動規(guī)律為仿生六足機(jī)器人在不規(guī)則地形條件下的運(yùn)動提供了很好的規(guī)劃方法。首先可以將六足機(jī)器人的運(yùn)動地形劃分為兩種情況：淺度不平地形(H>h，其中H為機(jī)器人離地高度，h為地面不平幅度)和深度不平地形(H

3.1 淺度不平地形上的運(yùn)動策略

當(dāng)六足機(jī)器人在淺度不平地形上運(yùn)動時(shí)，可建立初始狀態(tài)下機(jī)器人與軀體坐標(biāo)系{B}方向相同但原點(diǎn)不同的全局坐標(biāo)系{G}，使得{B}原點(diǎn)在{G}上投影即為{G}原點(diǎn)。根據(jù)軀體重心坐標(biāo)pB以及軀體繞{G}x,y,z軸旋轉(zhuǎn)角度α,β,γ可獲得BGTi，將螞蟻應(yīng)對障礙柱的運(yùn)動規(guī)律轉(zhuǎn)化為數(shù)學(xué)語言，即:

pBz(t)≡H,α(t)≡β(t)≡0

(2)

式中,pBz代表pB的z軸向分量。

由上式可知，通過pBx(t),pBy(t),γ(t)可確定唯一一條機(jī)器人軀體運(yùn)動軌跡，該軌跡可保證機(jī)器人運(yùn)動過程沒有高度起伏、傾側(cè)或俯仰現(xiàn)象出現(xiàn)。

在獲知機(jī)器人軀體重心運(yùn)動軌跡之后，即可按文獻(xiàn)[13]所述方法規(guī)劃機(jī)器人足端運(yùn)動軌跡，根據(jù)前述D-H模型可獲得機(jī)器人相應(yīng)的關(guān)節(jié)運(yùn)動軌跡。對于淺度不平地形，足端與未知地面的適應(yīng)是一個(gè)難點(diǎn)，文獻(xiàn)[14]所述二次規(guī)劃以進(jìn)行避障的方法以及機(jī)器人腿部力傳感器的現(xiàn)實(shí)保證能很好地解決這一難題。當(dāng)一次規(guī)劃軌跡完成后，若機(jī)器人足端仍未感應(yīng)到合適的著地點(diǎn)，則繼續(xù)規(guī)劃運(yùn)動直至力傳感器感知到合適的著地點(diǎn)，同時(shí)通過位置傳感器獲得的位置以及相應(yīng)時(shí)刻的機(jī)器人位姿，確定相應(yīng)足的著地點(diǎn)在{G}中的坐標(biāo)，以便進(jìn)行下一步運(yùn)動規(guī)劃。

3.2 深度不平地形上的運(yùn)動策略

六足機(jī)器人在深度不平地形中的運(yùn)動更為復(fù)雜，不僅機(jī)器人足部要適應(yīng)未知地形，而且軀體位姿也要隨地形的變化而變化。通過研究發(fā)現(xiàn)，不論六足昆蟲采用何種步態(tài)，為了保證穩(wěn)定性，六足每隔半個(gè)周期會同時(shí)著地，之后擺動相的核心三足陸續(xù)切向支撐相以支撐軀體穩(wěn)定移動，而支撐相的核心三足陸續(xù)切向擺動相以探測新的著地點(diǎn)。借鑒這一特點(diǎn)，針對六足機(jī)器人不采用一成不變的全局坐標(biāo)系，而是每隔半個(gè)運(yùn)動周期即進(jìn)行一次坐標(biāo)系迭代。在機(jī)器人六足同時(shí)著地時(shí)刻，由支撐相即將切向擺動相的核心三足來確定全局坐標(biāo)系{G}，那么各足位置在{G}坐標(biāo)系下均為已知；然后以擺動相即將切向支撐相的核心三足著地點(diǎn)A,B,C構(gòu)造新的全局坐標(biāo)系{G’}，根據(jù)前述螞蟻在應(yīng)對障礙柱堆時(shí)的運(yùn)動規(guī)律確定{G’}的構(gòu)造條件：①{G’}的原點(diǎn)為△ABC重心；②{G’}z軸垂直于△ABC；③{G’}y軸與{G}yoz平面夾角反應(yīng)機(jī)器人運(yùn)動狀況(直線前進(jìn)時(shí)該夾角為零，轉(zhuǎn)向時(shí)該夾角為相應(yīng)角度)。根據(jù)條件①有：

(3)

式中,GpA,GpB,GpC,GpG'分別代表點(diǎn)A,B,C,G′在{G}中矢量坐標(biāo)。

根據(jù)條件②有：

(4)

其中:

A=(GpCy-GpBy)(GpAz-GpBz)-(GpCz-GpBz)(GpAy-GpBy)

B=(GpCz-GpBz)(GpAx-GpBx)-(GpAz-GpBz)(GpCx-GpBx)

C= (GpAy-GpBy)(GpCx-GpBx)-(GpCy-GpBy)(GpAx-GpBx)

完成全局坐標(biāo)系迭代之后，需要確定機(jī)器人軀體運(yùn)動軌跡。首先確定軀體重心目標(biāo)位姿，以直線前進(jìn)為例，根據(jù)前述螞蟻在障礙柱堆的位姿確定軀體目標(biāo)位姿為：

(5)

式中,L為預(yù)期前進(jìn)距離，H為設(shè)定離地高度。

然后通過六次多項(xiàng)式擬合，確定軀體運(yùn)動軌跡。最后，類同淺度不平地形下單足運(yùn)動規(guī)劃方法，進(jìn)行即將由支撐相切向擺動相的核心三足的運(yùn)動規(guī)劃，在該半個(gè)周期結(jié)束時(shí)探測、確定一個(gè)新的三角形進(jìn)行下一次迭代。此后，可依次循環(huán)上述步驟。

4 樣機(jī)試驗(yàn)

為了對本文所提算法進(jìn)行研究，分別對仿生六足機(jī)器人樣機(jī)在臺階和斜坡這兩種非結(jié)構(gòu)地形的運(yùn)動進(jìn)行試驗(yàn)驗(yàn)證，試驗(yàn)過程的截圖分別如圖5和圖6所示。圖5中的臺階高度為130 mm，而機(jī)器人離地高度為200 mm，此時(shí)H>h，可以認(rèn)為該地形屬于淺度不平地面，因而采用本文3.1節(jié)所提出的算法進(jìn)行規(guī)劃。從圖5中可以看出六足機(jī)器人成功登上了臺階，并在此過程中保持機(jī)體處于水平狀態(tài)。圖6中的斜坡角度為15°，由于斜坡較長，其最高處已經(jīng)超過了機(jī)器人離地高度，此時(shí)H

圖5 六足機(jī)器人臺階運(yùn)動試驗(yàn)

圖6 六足機(jī)器人斜坡運(yùn)動試驗(yàn)

圖7 臺階試驗(yàn)中機(jī)器人左側(cè)髖關(guān)節(jié)角度

圖8 斜坡試驗(yàn)中機(jī)器人左側(cè)髖關(guān)節(jié)角度

5 結(jié)論

本文通過對螞蟻爬越障礙柱和障礙柱堆的兩種運(yùn)動過程的觀察和分析，總結(jié)了螞蟻在應(yīng)對不規(guī)則地形時(shí)的運(yùn)動策略，建立了更為合理的六足機(jī)器人的運(yùn)動學(xué)數(shù)學(xué)模型。模仿螞蟻在應(yīng)對不規(guī)則地形時(shí)的運(yùn)動策略，提出在進(jìn)行運(yùn)動規(guī)劃時(shí)，將地形劃分為淺度不平地形和深度不平地形兩種情況，進(jìn)而針對兩種地形情況提出了六足機(jī)器人在不規(guī)則地形條件下的運(yùn)動規(guī)劃算法并分別給出了合理的數(shù)學(xué)算法。最后通過六足機(jī)器人樣機(jī)在臺階和斜坡上的試驗(yàn)驗(yàn)證了本文所提算法的有效性。