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

汪 波，張建勛，侯之旭

(重慶理工大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院，重慶 400054)

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

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

1.1 勢場模型

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

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

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

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

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

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

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

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

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

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

1.3 局部極小值處理

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

圖2 邊緣探測法

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

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

2 幾何方法

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

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

圖4 單障礙物

圖5 幾何切線法

幾何學(xué)中，該情形分為2種:

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖6 改進(jìn)方法實(shí)驗(yàn)結(jié)果

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

表1 對比結(jié)果

4 結(jié)束語

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

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

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

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

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

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

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

［7］楊放瓊，譚青，彭高明.不確定環(huán)境下集礦機(jī)環(huán)境感知與實(shí)時避障研究［J］.中山大學(xué)學(xué)報:自然科學(xué)版，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］楊柳，張洪，高忠國.基于人工勢場法的移動機(jī)器人路徑規(guī)劃研究［J］.機(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.