應(yīng)用人工勢場算法的智能車路徑規(guī)劃

汪 波，張建勛，侯之旭

(重慶理工大學計算機科學與工程學院，重慶 400054)

基于時間最優(yōu)或路徑最短等準則，在已知或未知環(huán)境中規(guī)劃出一條從起點到終點的無碰撞路徑等路徑規(guī)劃問題是移動機器人研究領(lǐng)域的重要課題。Khatib提出了人工勢場算法，將機器人的運行空間模擬成抽象勢場模型，定義引力和斥力函數(shù)模型，共同引導機器人向終點運動。當引力和斥力達到平衡時，機器人會陷入局部運動，導致目標不可達。針對此類問題，許多學者對算法做了大量的改進性研究。石為人等［1］在保持引力場函數(shù)不變的情況下，在斥力場函數(shù)中引入終點與機器人的相對位置，避免勢場中出現(xiàn)合力為零的情況，從而使算法適用于復(fù)雜室內(nèi)環(huán)境中。姚婧婧等［2］在包含有大型障礙物的場景中，將大型障礙物劃分成質(zhì)點的集合，對質(zhì)點使用人工勢場法，當機器人、障礙物、終點位置由于處于同一條直線時易陷入目標不可達時，添加虛擬子目標，加大引力的作用，使機器人迅速擺脫合力為零的狀況，達到避障的效果。王萌等［3］通過分析勢場函數(shù)的局部最小點和該處產(chǎn)生的局部極小值，提出設(shè)置適當大小的控制力使機器人盡快跳出局部極小點，從而達到避障效果。羅乾又等［4］通過改進斥力場函數(shù)，考慮機器人和目標間的相對距離，在遇到局部較小值時，在原目標點處增加虛擬目標點，通過二者共同的引力幫助機器人擺脫局部極小點向著目標處前進。盧恩超等［5］引入目標與機器人的相對距離，在人工勢場法中的斥力函數(shù)模型中增加距離因子，利用工作空間的幾何信息對機器人的狀態(tài)進行微調(diào)，應(yīng)用邊緣探測法引導機器人繞著距障礙物安全距離之外的區(qū)域邊緣運動，該算法具有較強規(guī)劃能力。針對復(fù)雜環(huán)境以及動態(tài)不確定環(huán)境，學者也應(yīng)用該算法進行路徑規(guī)劃，并基于傳統(tǒng)人工勢場法進行了改進及驗證試驗［6－10］，人工勢場法結(jié)合粒子群等智能算法［11－13］已被廣泛應(yīng)用。上述算法對解決此類路徑規(guī)劃問題較為有效，但實現(xiàn)起來較為復(fù)雜。本研究應(yīng)用人工勢場算法，并引入一種新的避障策略，同樣可以規(guī)劃出有效的避障路徑。

1 傳統(tǒng)人工勢場算法

1.1 勢場模型

人工勢場算法中將機器人、障礙物及目標點視為質(zhì)點，通過建立引力勢場和斥力勢場模型共同引導機器人在抽象勢場中的運動，運動方向與勢場下降的方向一致。運動過程中，障礙物對機器人產(chǎn)生斥力作用，目標對機器人產(chǎn)生引力作用，作用力的大小取決與機器人的的運動位置。對機器人的工作空間進行建模，斥力作用隨著機器人與障礙物的不斷靠近逐漸變大，

引力作用隨著機器人與目標的偏離而增大。機器人運行在二維空間中，通過獲取機器人、目標兩點的坐標向量建立引力勢函數(shù)模型:

式(1)中:Uatt為目標對機器人的引力場;katt為大于零的引力勢場增益系數(shù);X是機器人在工作空間中的位置向量;Xg是目標處的位置向量。利用該模型定義出引力大小的計算方法。引力大小是引力勢場函數(shù)的負梯度:

利用障礙物、機器人二者的坐標向量，建立斥力函數(shù)模型

式(3)中:Ur為障礙物對機器人的斥力場;kr是非負的斥力場常量;ρ(X)函數(shù)描述的是障礙物與機器人的最短空間距離;ρO表示單個障礙物對機器人作用的最大空間距離。當機器人處在最大空間距離之外時，障礙物對機器人未產(chǎn)生任何斥力作用。斥力大小Fr是斥力場負梯度:

工作空間中，引力場、斥力場的疊加產(chǎn)生合力場，引導小車朝著目標行進，因此，人工勢場及作用力定義為:

1.2 傳統(tǒng)人工勢場算法的缺陷

人工勢場算法易適用于解決機器人路徑規(guī)劃問題，但在機器人行進的過程中，斥力和引力的大小隨著距離的變化發(fā)生改變，且方向相反。通常會出現(xiàn)以下3種情形:① 當機器人、障礙物、目標處于同一條直線上時，障礙物位于機器人、目標中間，接近目標時，引力減小，斥力增大，出現(xiàn)合力為零的情況;②當機器人、障礙物、目標處于同一條直線上時，目標處于中間，但機器人受障礙物的斥力影響，會出現(xiàn)引力、斥力平衡;③ 在多障礙物環(huán)境中，多個障礙物所施加的斥力疊加和與機器人所受目標處的引力大小一致，但方向相反。這3種局部極值問題會導致機器人無法避開障礙物到達目標處。在大小為10 m×10 m的仿真環(huán)境中，隨機分布著7個大小不一的障礙物，使用傳統(tǒng)人工勢場算法規(guī)劃出一條路徑，仿真實驗結(jié)果如圖1所示。

圖1 傳統(tǒng)人工勢場算法的仿真效果

圖1中，起點坐標為(0，0)，終點坐標為(10，10)。由于引力和斥力的作用，小車從起點沿著勢場下降的方向運動，當接近目標處時，陷入局部極值點，再往前行進會進入障礙物的影響范圍，導致發(fā)生碰撞。實際上，終點處應(yīng)該為勢場最低值，許多專家學者為解決該問題，對算法做了進一步的改進研究。

1.3 局部極小值處理

為解決人工勢場算法的缺陷，盧恩超等［5］采用了邊緣探測法。邊緣探測法通過引入沿邊行走的思想，將障礙物的體積進行膨脹化處理，擴大成一個圓形，機器人相對于該圓形可視為質(zhì)點，利用圓形的幾何知識對機器人姿態(tài)進行微調(diào)。當探測到機器人運動到的下一個位置處于膨脹圓形內(nèi)時，停止運行，調(diào)整方向，使機器人繞著據(jù)障礙物安全距離之外的圓弧運動，直到機器人遠離危險區(qū)域。圖2中當機器人到達p1位置時，預(yù)測下一個位置到達p2處，p2位于障礙物的影響范圍內(nèi)，若按照此路徑繼續(xù)行駛則發(fā)生碰撞，不符合設(shè)計要求，使用探測法到達p3處，成功避障。不斷激活該方法之后使用人工勢場算法，最終行駛路徑是繞著圓形區(qū)域行走。在多障礙物環(huán)境中的仿真結(jié)果如圖3所示。

圖2 邊緣探測法

圖3 人工勢場結(jié)合邊緣探測法仿真結(jié)果

圖2中的沿圓弧行走行為雖然引導機器人避免了碰撞，但圓弧在一定程度上延長了機器人的行駛距離，且經(jīng)過多次轉(zhuǎn)向調(diào)整，使規(guī)劃出的路徑并不是當前最優(yōu)。當遇到局部極小值問題時可保存該位置，從該點重新規(guī)劃路徑。本文將障礙物處理成圓形，以局部極小值點為起點，處理起點與目標點間的障礙物，使機器人沿著圓形切線前進，少走圓弧，減少路徑距離。

2 幾何方法

在智能導航研究領(lǐng)域，基于圖論原理的機器人最短路徑算法通常要經(jīng)過以下3個步驟實現(xiàn)［14］:① 利用圖像處理的方法識別障礙物，并選取一定的形狀(如凸多邊形、圓形、橢圓、三角形)來近似描述;②利用采集的信息，建立小車工作環(huán)境模型，如柵格法;③利用相關(guān)算法在工作環(huán)境中尋找最優(yōu)路徑，如Dijkstra算法。

理想狀況時，機器人沿著起點S到終點T的行走路徑上沒有任何障礙物，那么沿著直線ST行駛路徑最優(yōu)且平滑。在單障礙物環(huán)境中，路徑ST上有一個半徑大小為r的圓形障礙物，如圖4所示。從幾何學角度出發(fā)，以S，T為起點，針對該圓可規(guī)劃出4條外切線 SA1，SA2，TE，TF，如圖5所示。沿著4條路徑中的任意一條都可以到達終點T處。利用數(shù)學知識可知，沿著SA1，弧A1E，ET和SA2，弧A2F，F(xiàn)T這兩條路徑所經(jīng)過的路徑長度是不同的。

圖4 單障礙物

圖5 幾何切線法

幾何學中，該情形分為2種:

1)連接S，T兩點，直線ST過圓心O時，那么選擇從 SA1，圓弧 A1E，ET到達目標處和選擇從SA2，A2F，F(xiàn)T到達目標處的路徑長度是相等的;

2)連接S，T兩點的直線未過圓點O，如圖4所示。從圓心O點做直線ST的垂線，垂點為D，且d(OD)＜r，根據(jù)數(shù)學知識可證明選擇SA1，圓弧A1E，ET路徑長度較短。

式(7)中:SA，SO 是向量;θ=arcsin(OA/SO)，θ決定著從起點出發(fā)是選擇A1還是A2位置;P±θ是旋轉(zhuǎn)算子。連接起點和終點的直線SF可以稱作是基線，A1，A2兩點離基線的距離越小，就優(yōu)先選擇［14］。

外切線SA1，TE必然相交于一點P，以該點為起始點，連接PT，從圓心O點做直線PT的垂線并求取最短距離，當最短距離大于r時，小車跳出障礙物的影響范圍和局部極小值。再應(yīng)用人工勢場算法，繼續(xù)行進能安全到達目標處。

3 仿真實驗及分析

人工勢場算法結(jié)合幾何方法的實驗步驟:

1)建立工作空間，并設(shè)立小車的起始點、終點位置，引力、斥力的增益系數(shù)，障礙物的個數(shù)n，障礙物半徑s，小車行駛的步長l。

2)根據(jù)引力、斥力模型計算力的大小。

3)向下一個位置移動并判斷是否陷入局部極值，進入障礙物的影響距離內(nèi)，若是則調(diào)到步驟4);否則調(diào)到步驟5)。

4)調(diào)用幾何方法進行路徑規(guī)劃。

5)偏離局部極值處繼續(xù)使用人工勢場算法。

6)在合力作用下前進并記錄行進位置、迭代次數(shù)。若到達目標處則停止路徑規(guī)劃;否則，重復(fù)步驟2)～5)。

應(yīng)用人工勢場算法結(jié)合邊緣探測法解決路徑規(guī)劃問題時，要不斷地激活沿邊行走行為，并計算小車要旋轉(zhuǎn)的角度用于調(diào)整位姿，結(jié)果會帶來一定的路徑及時間冗余。利用上述實驗步驟，基于酷睿2.0GHz CPU，2Gbit內(nèi)存的筆記本，在 Matlab 7.11實驗平臺上進行仿真實驗，驗證以上算法的有效性。

已知小車行駛的起始點和終點，障礙物之間不存在重疊，選取引力增益系數(shù)k=15，斥力增益系數(shù)m=5，障礙物個數(shù)n=7，小車行進步長l=0.2，最大迭代次數(shù)J=200，仿真實驗結(jié)果如圖6所示。從實驗結(jié)果中可以看出，在局部極值點處，沿著圓的切線行進到兩條切線的交點時跳出局部極小點區(qū)域，繼續(xù)使用人工勢場算法，可順利避過障礙物到達指定目標處。

圖6 改進方法實驗結(jié)果

在柵格大小為10×10的仿真環(huán)境下，設(shè)置相同參數(shù)，對比邊緣探測法和改進算法，結(jié)果如表1所示。從表1中不難看出，改進算法在較短時間、較少的迭代次數(shù)中即規(guī)劃出避障路徑。

表1 對比結(jié)果

4 結(jié)束語

對多障礙物環(huán)境進行建模，使用人工勢場算法結(jié)合邊緣探測法，解決了局部極值問題并使機器人順利到達目標處，但繞著較長的圓弧行走路線帶來了路徑冗余。在局部極值處通過引進幾何切線的方法，能夠較快地擺脫局部極小點，可跳出障礙物影響范圍并避免繞著較長圓弧行進，結(jié)合人工勢場算法可順利完成路徑規(guī)劃。仿真實驗結(jié)果表明:該方法可有效規(guī)劃出一條較優(yōu)避障路徑。

［1］石為人，黃興華，周偉.基于改進人工勢場法的移動機器人路徑規(guī)劃［J］.計算機應(yīng)用，2010，30(8):2021－2023.

［2］姚婧婧，邱于兵，敖俊宇.移動機器人避障路徑規(guī)劃改進人工勢場法［J］.科學技術(shù)與工程，2011，13(11):2953－2956.

［3］王萌，王曉榮，李春貴，等.改進人工勢場法的移動機器人路徑規(guī)劃研究［J］.計算機工程與設(shè)計，2008，29(6):1504－1506.

［4］羅乾又，張華，解興哲.改進人工勢場法在機器人路徑規(guī)劃中的應(yīng)用［J］.計算機工程與設(shè)計，2011，32(4):1411－1415.

［5］盧恩超，張鄧斕，寧雅男，等.改進人工勢場法的機器人路徑規(guī)劃［J］.西北大學學報:自然科學版，2012，42(5):735－738.

［6］連曉峰，劉載文，左敏.移動機器人動態(tài)人工勢場路徑規(guī)劃方法研究［J］.計算機仿真，2011，28(1):27－31.

［7］楊放瓊，譚青，彭高明.不確定環(huán)境下集礦機環(huán)境感知與實時避障研究［J］.中山大學學報:自然科學版，2010，49(4):58－63.

［8］DONG HUN KUM，SEIICHI SHIN.Local path planning using a new artificial potential function compositon and its analytical design guidelines［J］.Advanced Robotics，2006，20(1):115－135.

［9］Rimon E，Koditschek D E.Exact robot navigation using artificial potential functions［J］.IEEE Trans.Robotics Automat，2010(8):501－518.

［10］Vadakkepat P，Tan K C，Wang M.Evolutionary artificial potential fields and their application in real time robot path planning［C］//Proc.Congr.of Evolutionary Computation.San Diego，CA:［s.n.］，2000:256－263.

［11］楊柳，張洪，高忠國.基于人工勢場法的移動機器人路徑規(guī)劃研究［J］.機床與液壓，2011，39(9):68－70.

［12］Ge S S，Cui Y J.New Potential Functions for Mobile Robot Path Planning［J］.IEEE Transactions on Robotics and Automation.2009，16:615－620.

［13］Lavalle S M.Planning algorithms.Cambridge［M］.MA:Cambridge University Press，2006.

［14］Gasilov N，Dogan M.Two-stage Shortest Path Algorithm for Solve Optimal Obstacle Avoidance Problem［J］.IETE JOURNAL OF RESEARCH，2011，3(57):278－285.