面向未知地圖的六足機器人路徑規劃算法

楊 洋，童東兵*，陳巧玉

(1.上海工程技術大學電子電氣工程學院，上海201620; 2.上海立信會計金融學院統計與數學學院，上海201620)

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

0 引言

近年來，仿生機器人的研究逐漸成為機器人領域的熱門話題，六足機器人作為其中的典型對象，有著豐富的步態和良好的地形適應能力，在排爆、探礦、檢測等方面有廣泛的應用。在實際的應用環境中，由于環境的復雜性和多樣性，使用傳統的全局路徑規劃難以適應需求，局部路徑規劃由于對未知環境具有較強適應性和實時性，逐漸取代了全局路徑規劃。

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

1 基于測距和模糊規則的聲源識別算法

1.1 機器人視窗

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

在前進方向上，設定水平方向測距儀一個，斜向下測距儀一個，如圖2所示。

圖2 機器人傳感器位置Fig.2 Sensor position of robot

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

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

于是有如下的關系:

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

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

同樣的，斜向下測距儀起始時刻夾角γ、下一時刻夾角γ'、剩余預定路線l″和障礙物與預定路線高度差h″、前進方向之間的關系如圖4所示。

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

有如下的關系:

同樣的，解方程:

則障礙物離地高度為h=h'－h″。

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

1.2 障礙物模糊判定

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

圖5 前下測距儀隸屬度函數圖像Fig.5 Membership function image of front down range finder

對于前測距儀的形狀變化率g，記Convergence為收斂，也就是向靠近預定路線的方向彎曲;MD為近似平直;Divergence為發散，也就是向遠離預定路線的方向彎曲。其隸屬度函數如圖6所示。

圖6 前測距儀隸屬度函數圖像Fig.6 Membership function image of front range finder

對于左1和右1測距儀，記N為近距離，M為中等距離，F為遠距離。其隸屬度函數如圖7所示。

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

采用Madami規則對障礙物形狀進行判定，其中U為楔形走廊，D為喇叭形走廊，Flat為平直走廊，Obstacle為障礙物。

模糊規則共19條:R=Ri，i為規則序號。

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

這里例舉其中Obstacle的部分關系:

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)。

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

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

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

1.3 構建局部地圖

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

如圖8所示，sd為測定點到目標點的距離，sr為測定點到機器人的距離，測量測定點到機器人與測定點到目標點的夾角為β，顯然，可以得到機器人到目標點與測定點到機器人之間的夾角α與距離。

圖8 目標點構建示意圖Fig.8 Schematic diagram of target point construction

記錄從開始到當前的時間內，機器人測得的地形數據，構建局部地圖，依照局部地圖規劃路徑逐漸逼近全局路徑。

為了方便計算，取機器人形心進行計算，設立測量點為坐標原點，機器人初始位置在橫坐標軸上，如圖9所示。

圖9 障礙檢測示意圖Fig.9 Schematic diagram of obstacle detection

對于第一個檢測到的障礙物，其坐標為:

其中:Sr1為初始時刻機器人到測定點之間的距離，在無阻擋的條件下;α1為初始時刻機器人到目標點與機器人到測定點之間的夾角;θ1為第一個障礙物與預定路線之間的夾角;w'1為第一個障礙物與機器人之間的距離。

對于第二個障礙物，此時機器人的坐標為:

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

此時障礙物的坐標為:

其中，δ2=α1+∑ηi為當前機器人水平方向夾角。

由此，可以推出第m個障礙物的坐標:

隨著機器人的運動，上述算法不停地錄入地形數據，一步一步地構建局部地圖，從而實現從局部地圖到全局地圖的構建。

2 人工勢場法路徑規劃

人工勢場法，假設空間中存在著虛擬的力場，機器人在力場中受力運動。這里的力場包括:源自目標點的引力場，即目標點對機器人產生引力，引導機器人向目標點運動;源自障礙物的斥力場，即障礙物對機器人產生斥力，避免機器人與障礙物發生碰撞。

人工勢場法特點是機器人的運動由其當前受到的合力決定，相比其他路徑算法計算更加簡單，實時性更強。但是一般的勢場法存在一定的問題，如容易落入陷阱、靠近障礙物發生路徑震蕩、存在局部最小等。

為了應對上述問題，這里使用一種改進的斥力函數:

其中:Frep1是從障礙物指向機器人的斥力;Frep2從機器人指向目標點的斥力;ρ為斥力影響距離;P為機器人的位置;Pobs為障礙物的位置;Pgoal為目標點的位置;ΔPobs為機器人到障礙物的距離;ΔPgoal為機器人到目標點的距離;krep為斥力增益系數;a為斥力影響因子。

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

機器人受到的引力Fatt為珔Fatt=katt|P－Pgoal|，方向從機器人指向目標點，katt為引力增益系數。

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

圖10 機器人所受合力Fig.10 Resultant force of robot

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

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

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

首先進行的是初始位置檢測，以建立坐標系;在設定完目標點之后，機器人通過聲源檢測來構建局部地圖，在此基礎上進行局部的路徑規劃并前進一段路程，然后進行局部路徑規劃，如此往復，直到到達目標點

3 實驗仿真與分析

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

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

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

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

4 結語

本文針對未知地圖情況下六足機器人的路徑規劃，提出一種聲源識別算法，用于構建局部地圖;使用改進的人工勢場法，實現六足機器人的路徑規劃。本文主要工作如下:

1)考慮六足機器人的行動能力和對地形的適應能力，通過構建機器人視窗，設計測距儀組，結合模糊算法，解決了對于普通移動機器來說是障礙物，對六足機器人而言不是障礙物的問題。

2)基于1)構建適用于六足機器人的局部地圖，隨著機器人的前進，不停錄入數據，提高了機器人對地形變化的反應能力和實時性。

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

未來將考慮對于路徑鈍化的延遲時間和引力函數的優化。同時，由于測距儀不可避免地會存在精度問題，需要設計算法以應對探測盲區。