面向未知地圖的六足機(jī)器人路徑規(guī)劃算法

楊 洋，童東兵*，陳巧玉

(1.上海工程技術(shù)大學(xué)電子電氣工程學(xué)院，上海201620; 2.上海立信會(huì)計(jì)金融學(xué)院統(tǒng)計(jì)與數(shù)學(xué)學(xué)院，上海201620)

(*通信作者電子郵箱tongdongbing@163.com)

0 引言

近年來，仿生機(jī)器人的研究逐漸成為機(jī)器人領(lǐng)域的熱門話題，六足機(jī)器人作為其中的典型對(duì)象，有著豐富的步態(tài)和良好的地形適應(yīng)能力，在排爆、探礦、檢測(cè)等方面有廣泛的應(yīng)用。在實(shí)際的應(yīng)用環(huán)境中，由于環(huán)境的復(fù)雜性和多樣性，使用傳統(tǒng)的全局路徑規(guī)劃難以適應(yīng)需求，局部路徑規(guī)劃由于對(duì)未知環(huán)境具有較強(qiáng)適應(yīng)性和實(shí)時(shí)性，逐漸取代了全局路徑規(guī)劃。

文獻(xiàn)［1］提出了一種具有三個(gè)路徑點(diǎn)的多項(xiàng)式插值算法，減少了沖擊，保證了機(jī)器人運(yùn)動(dòng)的平穩(wěn)性，并減少了轉(zhuǎn)矩突變帶來的影響。文獻(xiàn)［2］中，針對(duì)機(jī)器人在視覺導(dǎo)航系統(tǒng)中的魯棒性受到運(yùn)動(dòng)模糊制約的問題，提出一種基于運(yùn)動(dòng)模糊特征的實(shí)時(shí)性異常探測(cè)方法。文獻(xiàn)［3－5］采用人工勢(shì)場法對(duì)機(jī)器人的路徑進(jìn)行規(guī)劃，文獻(xiàn)［3，5］通過對(duì)斥力函數(shù)進(jìn)行修改、加入中間目標(biāo)點(diǎn)等方法，優(yōu)化了機(jī)器人的路徑，減少了行走路徑震蕩;文獻(xiàn)［4］通過修改引力函數(shù)和斥力函數(shù)，引入“逃脫力”的方法來解決路徑規(guī)劃中的局部最小問題。文獻(xiàn)［6］利用人工勢(shì)場法求解多機(jī)器人路徑規(guī)劃，機(jī)器人之間的軌跡相互排斥，得到最優(yōu)路徑。在文獻(xiàn)［7－11］中，使用自反饋生物激勵(lì)神經(jīng)網(wǎng)絡(luò)、自適應(yīng)模糊神經(jīng)網(wǎng)絡(luò)和徑向基神經(jīng)網(wǎng)絡(luò)來解決路徑規(guī)劃問題。在文獻(xiàn)［12］中，提出了一種改進(jìn)人工勢(shì)場法，該方法是在引力函數(shù)中增加了一個(gè)指數(shù)項(xiàng)，用于消除奇異值點(diǎn)，避免了行走路徑震蕩現(xiàn)象的發(fā)生，同時(shí)，在斥力函數(shù)中引入敏感度這個(gè)概念，用于克服傳統(tǒng)的人工勢(shì)場法中目標(biāo)點(diǎn)與障礙物過近，導(dǎo)致無法到達(dá)的現(xiàn)象;但是僅僅通過修改或者添加力的手段是很難做到適應(yīng)大部分地圖的，而且人為痕跡明顯。文獻(xiàn)［13］針對(duì)蟻群算法收斂速度慢和人工勢(shì)場法容易落入陷阱的問題，提出了一種基于柵格地圖的局部搜索尋優(yōu)算法，減少了蟻群算法的計(jì)算量，提高了機(jī)器人對(duì)障礙物的規(guī)避能力。在文獻(xiàn)［14］中，使用飛行機(jī)器人來對(duì)地面數(shù)據(jù)進(jìn)行掃描，得到地形和海拔等數(shù)據(jù)，根據(jù)數(shù)據(jù)建立3D模型，在3D模型的基礎(chǔ)上進(jìn)行路徑規(guī)劃。文獻(xiàn)［15］提出了一種用于機(jī)器人的角速度約束的非線性跟蹤控制算法，利用模糊規(guī)劃，得到了實(shí)際避障的路徑。在文獻(xiàn)［16－19］中，使用粒子群算法對(duì)機(jī)器人的路徑進(jìn)行規(guī)劃。文獻(xiàn)［20］提出了一種基于演示的最優(yōu)路徑規(guī)劃方法，用于重型設(shè)備的組裝。盡管以上方法可以實(shí)現(xiàn)機(jī)器人的路徑規(guī)劃，但主要側(cè)重于機(jī)器人在已知地圖上的路徑規(guī)劃，對(duì)于未知地圖，往往依賴于全局地形檢測(cè)手段，而且這類手段可能無法得到適用于當(dāng)前機(jī)器人的地形數(shù)據(jù)。本文使用測(cè)距與模糊規(guī)則，進(jìn)行局部地圖的隨動(dòng)構(gòu)建，由此通過采用一種新的人工勢(shì)場函數(shù)和路徑鈍化操作來對(duì)機(jī)器人的路徑進(jìn)行規(guī)劃，避免了障礙物周圍的抖動(dòng)問題，提高了機(jī)器人對(duì)環(huán)境的實(shí)時(shí)響應(yīng)能力。

1 基于測(cè)距和模糊規(guī)則的聲源識(shí)別算法

1.1 機(jī)器人視窗

設(shè)定基于超聲測(cè)距儀的機(jī)器人視窗，每個(gè)視窗由一組測(cè)距儀提供，共6個(gè)視窗。如圖1所示，使用超聲波傳感器，測(cè)距儀感應(yīng)角15°。

在前進(jìn)方向上，設(shè)定水平方向測(cè)距儀一個(gè)，斜向下測(cè)距儀一個(gè)，如圖2所示。

圖2 機(jī)器人傳感器位置Fig.2 Sensor position of robot

對(duì)于水平測(cè)距儀組，在起始時(shí)刻測(cè)得機(jī)器人到障礙物的距離w，隨著機(jī)器人的前進(jìn)，測(cè)得下一時(shí)刻機(jī)器人到障礙物的距離w'，兩個(gè)傳感器之間固定距離為l，那么，起始時(shí)刻夾角θ、下一時(shí)刻夾角θ'、障礙物與預(yù)定路線距離s、剩余距離l'與機(jī)器人前進(jìn)方向之間的關(guān)系如圖3所示。

圖3 水平測(cè)距儀組測(cè)距Fig.3 Ranging of horizontal range finder group

于是有如下的關(guān)系:

解上述方程，可以得到需要的參數(shù):

對(duì)相近的離散時(shí)刻t1和t2，計(jì)算障礙物與路徑的距離與這段時(shí)間內(nèi)前進(jìn)的距離d之比g=Δs/d=(s1－s2)/d為障礙物形狀變化率。

同樣的，斜向下測(cè)距儀起始時(shí)刻夾角γ、下一時(shí)刻夾角γ'、剩余預(yù)定路線l″和障礙物與預(yù)定路線高度差h″、前進(jìn)方向之間的關(guān)系如圖4所示。

圖4 斜向下測(cè)距儀組測(cè)距Fig.4 Ranging of oblique down range finder group

有如下的關(guān)系:

同樣的，解方程:

則障礙物離地高度為h=h'－h(huán)″。

對(duì)不同時(shí)刻t1和t2，計(jì)算障礙物離地高度差與前進(jìn)距離d之比f=Δh/d=(h1－h(huán)2)/d，近似作為地形高度變化的梯度數(shù)據(jù)。

1.2 障礙物模糊判定

模糊控制器的輸入為1.1節(jié)算得的形狀變化率、近似梯度值，左1與右1測(cè)得的距離值，對(duì)于前下測(cè)距儀的近似梯度值f，記DPD為向下陡坡，HP為緩坡，DPU為向上陡坡，其隸屬度函數(shù)圖像如圖5所示。

圖5 前下測(cè)距儀隸屬度函數(shù)圖像Fig.5 Membership function image of front down range finder

對(duì)于前測(cè)距儀的形狀變化率g，記Convergence為收斂，也就是向靠近預(yù)定路線的方向彎曲;MD為近似平直;Divergence為發(fā)散，也就是向遠(yuǎn)離預(yù)定路線的方向彎曲。其隸屬度函數(shù)如圖6所示。

圖6 前測(cè)距儀隸屬度函數(shù)圖像Fig.6 Membership function image of front range finder

對(duì)于左1和右1測(cè)距儀，記N為近距離，M為中等距離，F(xiàn)為遠(yuǎn)距離。其隸屬度函數(shù)如圖7所示。

圖7 左1、右1測(cè)距儀隸屬度函數(shù)圖像Fig.7 Membership function image of left 1 and right 1 range finders

采用Madami規(guī)則對(duì)障礙物形狀進(jìn)行判定，其中U為楔形走廊，D為喇叭形走廊，F(xiàn)lat為平直走廊，Obstacle為障礙物。

模糊規(guī)則共19條:R=Ri，i為規(guī)則序號(hào)。

由于規(guī)則條目較多，很難通過一兩條式子表示總模糊關(guān)系，這里直接使用邏輯語句來表示，在程序內(nèi)部也直接表示為條件語句。

這里例舉其中Obstacle的部分關(guān)系:

1)If(Down is DP_D)then(OP is Obstacle)。

2)If(Down is DP_U)then(OP is Obstacle)。

3)If(Down is HP)and(front is Convergence)and(left 1 is N)and(right 1 is N)then(OP is Obstacle)。

簡而言之，對(duì)于陡坡，無論下降與否，都認(rèn)為是無法翻越的障礙物。對(duì)于同方向的障礙物，認(rèn)為是平直，對(duì)于方向相反的障礙物，依據(jù)其方向的不同分為楔形走廊或者喇叭形走廊。

根據(jù)近似梯度對(duì)障礙物進(jìn)行判斷，依據(jù)其離地高度h和機(jī)器人腿長len。若h＜1.3len，則認(rèn)為滿足跨越條件;若h≥1.3len，則認(rèn)為不能跨越，需要繞行。

兩次判斷取交集，即可得到在二維地圖上的障礙物與非障礙物。

1.3 構(gòu)建局部地圖

在測(cè)定點(diǎn)設(shè)置激光發(fā)生器和相應(yīng)的舵機(jī)，機(jī)器人上設(shè)置接收器(光敏傳感器組)。在初始的時(shí)刻需要將激光束對(duì)準(zhǔn)在傳感器組中央。測(cè)量時(shí)先從測(cè)定點(diǎn)射出脈沖激光束，接著光敏傳感器組接收到脈沖激光束。通過測(cè)量接收激光束的傳感器與傳感器組中央的傳感器之間的距離，來判斷舵機(jī)轉(zhuǎn)角的大小。若接收激光束的傳感器在傳感器組中央的傳感器的左邊，則舵機(jī)向右轉(zhuǎn);反之，向左轉(zhuǎn)。

如圖8所示，sd為測(cè)定點(diǎn)到目標(biāo)點(diǎn)的距離，sr為測(cè)定點(diǎn)到機(jī)器人的距離，測(cè)量測(cè)定點(diǎn)到機(jī)器人與測(cè)定點(diǎn)到目標(biāo)點(diǎn)的夾角為β，顯然，可以得到機(jī)器人到目標(biāo)點(diǎn)與測(cè)定點(diǎn)到機(jī)器人之間的夾角α與距離。

圖8 目標(biāo)點(diǎn)構(gòu)建示意圖Fig.8 Schematic diagram of target point construction

記錄從開始到當(dāng)前的時(shí)間內(nèi)，機(jī)器人測(cè)得的地形數(shù)據(jù)，構(gòu)建局部地圖，依照局部地圖規(guī)劃路徑逐漸逼近全局路徑。

為了方便計(jì)算，取機(jī)器人形心進(jìn)行計(jì)算，設(shè)立測(cè)量點(diǎn)為坐標(biāo)原點(diǎn)，機(jī)器人初始位置在橫坐標(biāo)軸上，如圖9所示。

圖9 障礙檢測(cè)示意圖Fig.9 Schematic diagram of obstacle detection

對(duì)于第一個(gè)檢測(cè)到的障礙物，其坐標(biāo)為:

其中:Sr1為初始時(shí)刻機(jī)器人到測(cè)定點(diǎn)之間的距離，在無阻擋的條件下;α1為初始時(shí)刻機(jī)器人到目標(biāo)點(diǎn)與機(jī)器人到測(cè)定點(diǎn)之間的夾角;θ1為第一個(gè)障礙物與預(yù)定路線之間的夾角;w'1為第一個(gè)障礙物與機(jī)器人之間的距離。

對(duì)于第二個(gè)障礙物，此時(shí)機(jī)器人的坐標(biāo)為:

其中:ηi為截止第二次轉(zhuǎn)向前的第i次轉(zhuǎn)向角度;li為截止第二次轉(zhuǎn)向前的第i次轉(zhuǎn)向走過的路程。

此時(shí)障礙物的坐標(biāo)為:

其中，δ2=α1+∑ηi為當(dāng)前機(jī)器人水平方向夾角。

由此，可以推出第m個(gè)障礙物的坐標(biāo):

隨著機(jī)器人的運(yùn)動(dòng)，上述算法不停地錄入地形數(shù)據(jù)，一步一步地構(gòu)建局部地圖，從而實(shí)現(xiàn)從局部地圖到全局地圖的構(gòu)建。

2 人工勢(shì)場法路徑規(guī)劃

人工勢(shì)場法，假設(shè)空間中存在著虛擬的力場，機(jī)器人在力場中受力運(yùn)動(dòng)。這里的力場包括:源自目標(biāo)點(diǎn)的引力場，即目標(biāo)點(diǎn)對(duì)機(jī)器人產(chǎn)生引力，引導(dǎo)機(jī)器人向目標(biāo)點(diǎn)運(yùn)動(dòng);源自障礙物的斥力場，即障礙物對(duì)機(jī)器人產(chǎn)生斥力，避免機(jī)器人與障礙物發(fā)生碰撞。

人工勢(shì)場法特點(diǎn)是機(jī)器人的運(yùn)動(dòng)由其當(dāng)前受到的合力決定，相比其他路徑算法計(jì)算更加簡單，實(shí)時(shí)性更強(qiáng)。但是一般的勢(shì)場法存在一定的問題，如容易落入陷阱、靠近障礙物發(fā)生路徑震蕩、存在局部最小等。

為了應(yīng)對(duì)上述問題，這里使用一種改進(jìn)的斥力函數(shù):

其中:Frep1是從障礙物指向機(jī)器人的斥力;Frep2從機(jī)器人指向目標(biāo)點(diǎn)的斥力;ρ為斥力影響距離;P為機(jī)器人的位置;Pobs為障礙物的位置;Pgoal為目標(biāo)點(diǎn)的位置;ΔPobs為機(jī)器人到障礙物的距離;ΔPgoal為機(jī)器人到目標(biāo)點(diǎn)的距離;krep為斥力增益系數(shù);a為斥力影響因子。

那么，機(jī)器人受到的總斥力Frep為Frep=Frep1+Frep2。

機(jī)器人受到的引力Fatt為珔Fatt=katt|P－Pgoal|，方向從機(jī)器人指向目標(biāo)點(diǎn)，katt為引力增益系數(shù)。

于是，機(jī)器人受到的合力Ftotal為Ftotal=Frep+Fatt，受力情況如圖10所示。

圖10 機(jī)器人所受合力Fig.10 Resultant force of robot

實(shí)際上，由于局部地圖存在障礙物探測(cè)不全、障礙物探測(cè)有誤的局限性和在一些情況下路徑存在一定的震蕩等問題，這里采用“鈍化”操作，是指機(jī)器人不需要嚴(yán)格依照規(guī)劃的路徑運(yùn)動(dòng)。在已經(jīng)規(guī)劃完畢的路徑上，依照規(guī)劃路徑前進(jìn)延遲時(shí)間為:Tload(Tload＜Lmax/(8v))。其中:Lmax為最大探測(cè)距離;v為機(jī)器人前進(jìn)速度，依照新探測(cè)到的地圖重新進(jìn)行路徑規(guī)劃，再前進(jìn)Tload，如此往復(fù)，直至到達(dá)目標(biāo)點(diǎn)。

本文提出的算法流程如圖11所示。

圖11 本文算法流程Fig.11 Flow chart of proposed algorithm

首先進(jìn)行的是初始位置檢測(cè)，以建立坐標(biāo)系;在設(shè)定完目標(biāo)點(diǎn)之后，機(jī)器人通過聲源檢測(cè)來構(gòu)建局部地圖，在此基礎(chǔ)上進(jìn)行局部的路徑規(guī)劃并前進(jìn)一段路程，然后進(jìn)行局部路徑規(guī)劃，如此往復(fù)，直到到達(dá)目標(biāo)點(diǎn)

3 實(shí)驗(yàn)仿真與分析

設(shè)定引力增益系數(shù)為15，斥力增益系數(shù)為5，斥力影響因子為0.5，斥力影響距離為4，延遲時(shí)間為3，機(jī)器人初始位置為(－10，0)，使用兩幅地圖進(jìn)行仿真，結(jié)果如圖12所示。

圖12(b)中虛線為采用了鈍化操作后的隨機(jī)障礙物下機(jī)器人的路徑。將圖12(a)與圖12(b)相對(duì)比可以看到，改進(jìn)型勢(shì)場法與傳統(tǒng)的勢(shì)場法相比，不會(huì)出現(xiàn)與障礙物相碰撞的情況。

圖12(c)描述的是在存在凹凸障礙物的情況下，使用改進(jìn)型勢(shì)場法，機(jī)器人的行走路徑。機(jī)器人首先需要經(jīng)過兩個(gè)圓形障礙物，隨后接近凹凸障礙物，受到障礙物上邊沿的斥力，路徑將稍許下偏，進(jìn)一步前進(jìn)時(shí)受到來自障礙物底部的斥力，隨后在引力的作用下沿著墻壁前進(jìn)，最后繞過障礙物，繞行過程中存在一定路徑震蕩，采用鈍化操作，以消除大部分路徑震蕩的影響。

圖12 不同障礙物時(shí)使用不同方法機(jī)器人的路徑Fig.12 Robot's path of different methods with different obstacles

4 結(jié)語

本文針對(duì)未知地圖情況下六足機(jī)器人的路徑規(guī)劃，提出一種聲源識(shí)別算法，用于構(gòu)建局部地圖;使用改進(jìn)的人工勢(shì)場法，實(shí)現(xiàn)六足機(jī)器人的路徑規(guī)劃。本文主要工作如下:

1)考慮六足機(jī)器人的行動(dòng)能力和對(duì)地形的適應(yīng)能力，通過構(gòu)建機(jī)器人視窗，設(shè)計(jì)測(cè)距儀組，結(jié)合模糊算法，解決了對(duì)于普通移動(dòng)機(jī)器來說是障礙物，對(duì)六足機(jī)器人而言不是障礙物的問題。

2)基于1)構(gòu)建適用于六足機(jī)器人的局部地圖，隨著機(jī)器人的前進(jìn)，不停錄入數(shù)據(jù)，提高了機(jī)器人對(duì)地形變化的反應(yīng)能力和實(shí)時(shí)性。

3)在人工勢(shì)場法中，使用新的斥力函數(shù)，通過結(jié)合鈍化操作，有效解決了傳統(tǒng)的勢(shì)場函數(shù)在障礙物附近的路徑震蕩問題，對(duì)障礙物的兼容性更好。

未來將考慮對(duì)于路徑鈍化的延遲時(shí)間和引力函數(shù)的優(yōu)化。同時(shí)，由于測(cè)距儀不可避免地會(huì)存在精度問題，需要設(shè)計(jì)算法以應(yīng)對(duì)探測(cè)盲區(qū)。