基于人工勢場法的局部路徑規(guī)劃改進(jìn)

張 軍 許靖宜 于士坤

（1.安徽理工大學(xué)人工智能學(xué)院；2.安徽理工大學(xué)機(jī)械工程學(xué)院 安徽淮南 232001）

路徑規(guī)劃技術(shù)是機(jī)器人移動機(jī)器人領(lǐng)域的關(guān)鍵技術(shù)之一，其含義為在包含障礙物的二維或三維環(huán)境中，按照一定規(guī)則和標(biāo)準(zhǔn)，規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)的滿足機(jī)器人自身約束要求的無碰撞路徑[1]。如圖1 所示，為移動機(jī)器人在未知環(huán)境下完成路徑規(guī)劃，到達(dá)預(yù)設(shè)地點(diǎn)的示意圖，其中曲線為移動機(jī)器人運(yùn)動路徑。路徑規(guī)劃技術(shù)廣泛應(yīng)用于多種領(lǐng)域，如移動機(jī)器人的自主避障行動，巡航導(dǎo)彈在避開雷達(dá)搜索情況下的最優(yōu)飛行路線，工廠車間中物流機(jī)器人的自主行動等[2]。在移動機(jī)器人路徑規(guī)劃技術(shù)中，除了確保機(jī)器人能夠抵達(dá)目標(biāo)點(diǎn)外，還應(yīng)考慮兩個目標(biāo)：一是要令所規(guī)劃出的路徑長度短，轉(zhuǎn)折點(diǎn)少，以減少成本開銷，提高工作效率；二 是要充分考慮機(jī)器人移動路線與障礙物的相對位置關(guān)系以及路徑所經(jīng)區(qū)域的障礙物分布情況，規(guī)避行駛風(fēng)險，確保安全性[3]。

圖1 移動機(jī)器人路徑規(guī)劃示意圖

在移動機(jī)器人進(jìn)行路徑規(guī)劃時，將機(jī)器人在地圖環(huán)境中的運(yùn)動抽象成一種人造引力場中的運(yùn)動，構(gòu)建人工勢場模型[4]。勢場包含兩個部分，一部分是由目標(biāo)點(diǎn)產(chǎn)生的引力場，一部分是由障礙物產(chǎn)生的斥力場，其中，引力場發(fā)揮全局作用，機(jī)器人在引力的作用下進(jìn)行趨向目標(biāo)點(diǎn)的運(yùn)動，引力的大小與機(jī)器人和目標(biāo)點(diǎn)之間的距離成正比；斥力場在一定范圍內(nèi)發(fā)揮作用，在障礙物的影響范圍內(nèi)，機(jī)器人在斥力作用下進(jìn)行遠(yuǎn)離障礙物的運(yùn)動，斥力與機(jī)器人和障礙物之間的距離成正比，所受斥力為多個障礙物斥力的矢量疊加。機(jī)器人在地圖環(huán)境中所受的合力即為引力和所有斥力的矢量和，始終保持在合力的方向上進(jìn)行移動，直至合力為零或抵達(dá)目標(biāo)點(diǎn)[5,6]。

本次研究通過對當(dāng)前技術(shù)手段所存在的傳統(tǒng)人工勢場法路徑規(guī)劃存在的問題進(jìn)行分析，針對性地提出對傳統(tǒng)人工勢場法的改進(jìn)方法，并用MATLAB 進(jìn)行仿真實(shí)驗(yàn)，通過實(shí)驗(yàn)驗(yàn)證改進(jìn)后算法的可行性與準(zhǔn)確性。

一、傳統(tǒng)人工勢場法路徑規(guī)劃存在的問題

人工勢場法是通過利用地圖環(huán)境信息建立勢場模型，在機(jī)器人路徑規(guī)劃中具有比較成熟的應(yīng)用并取得了理想的效果。易先軍[7]等人提出了一種模糊改進(jìn)人工勢場法，針對人工勢場法存在的目標(biāo)不可達(dá)等問題，建立斥力勢場梯度等級解決目標(biāo)不可達(dá)問題，同時引入基于模糊算法推算的逃逸力，避免局部最優(yōu)；為克服局部極小值問題，Hongqiang Sang[8]等人提出了一種多目標(biāo)人工勢場算法，將地圖環(huán)境劃分為多個子目標(biāo)勢場分段實(shí)現(xiàn)路徑規(guī)劃，該算法在不同環(huán)境中均能有效發(fā)揮作用；Ulises Orozco-Rosas[9]等人提出了一種膜進(jìn)化人工勢場法，將膜計(jì)算與遺傳算法和人工勢場法結(jié)合，優(yōu)化參數(shù)生成安全可行的路徑。但由于勢場函數(shù)定義方式的局限性，使得算法會在某些場景下因?yàn)闄C(jī)器人、目標(biāo)點(diǎn)和障礙物之間的相對位置不同而存在目標(biāo)不可達(dá)、局部極小值、U型障礙物導(dǎo)致的局部死區(qū)等問題[10]，導(dǎo)致機(jī)器人無法到達(dá)目標(biāo)點(diǎn)，下面就具體問題進(jìn)行說明。

（一）障礙物影響范圍內(nèi)的目標(biāo)不可達(dá)問題。當(dāng)目標(biāo)點(diǎn)的周圍存在一個及以上障礙物且目標(biāo)點(diǎn)在障礙物的輻射范圍內(nèi)時，當(dāng)機(jī)器人向目標(biāo)點(diǎn)和障礙物所在區(qū)域移動時，機(jī)器人所受引力逐漸減小而斥力逐漸加大，從而可能會出現(xiàn)斥力大于引力的情況，導(dǎo)致機(jī)器人無法到達(dá)目標(biāo)點(diǎn)。如圖1所示，其中Fatt為目標(biāo)點(diǎn)產(chǎn)生的引力，F(xiàn)req為障礙物產(chǎn)生的斥力，F(xiàn)res為二者合力。

圖2 障礙物影響范圍內(nèi)的目標(biāo)不可達(dá)問題

（二）局部極小值問題。局部極小值問題[11]是傳統(tǒng)人工勢場法中導(dǎo)致路徑規(guī)劃失敗的主要問題。在人工勢場法中，當(dāng)引力和斥力位于同一直線上，且絕對值相同、方向相反時機(jī)器人所受合力為零，但此時機(jī)器人所處位置并非全局勢場最低點(diǎn)[12]，從而導(dǎo)致局部極小值問題。

如圖3所示，障礙物的數(shù)量為兩個及以上，機(jī)器人、目標(biāo)點(diǎn)及障礙物不在同一直線上，障礙物產(chǎn)生的斥力的合力與目標(biāo)點(diǎn)產(chǎn)生的引力大小相同、方向相反。

圖3 局部極小值問題

（三）U型障礙物導(dǎo)致的局部死區(qū)問題。如圖4所示，當(dāng)移動機(jī)機(jī)器人的運(yùn)動方向上存在U型障礙物時，隨著機(jī)器人與障礙物距離的逐步減小，由障礙物所產(chǎn)生的斥力會越來越大，當(dāng)機(jī)器人進(jìn)入U型區(qū)域內(nèi)時，來自各個方向上的斥力的合力逐漸抵消目標(biāo)點(diǎn)的引力作用，當(dāng)斥力的合力與引力值大小相等時，機(jī)器人運(yùn)動在末端震蕩后陷入停滯，即陷入U型死區(qū)[13]。

圖4 移動機(jī)器人進(jìn)入U型死區(qū)導(dǎo)致路徑規(guī)劃失敗

二、針對傳統(tǒng)人工勢場法進(jìn)行改進(jìn)

針對障礙物影響范圍內(nèi)的目標(biāo)不可達(dá)問題，提出通過考慮目標(biāo)點(diǎn)因素改進(jìn)斥力函數(shù)的改進(jìn)方法；針對局部極小值問題，提出引入粒子群算法優(yōu)化勢場參數(shù)克服局部最優(yōu)的改進(jìn)方法；針對U型障礙物導(dǎo)致的局部死區(qū)問題，提出引入切向力改進(jìn)力的組成結(jié)構(gòu)的改進(jìn)方法。

（一）通過考慮目標(biāo)點(diǎn)因素改進(jìn)斥力函數(shù)。針對障礙物影響范圍內(nèi)的目標(biāo)不可達(dá)問題，提出通過考慮目標(biāo)點(diǎn)因素改進(jìn)斥力函數(shù)的改進(jìn)方法。機(jī)器人所受斥力是關(guān)于機(jī)器人與障礙物之間距離倒數(shù)的二次函數(shù)，當(dāng)機(jī)器人進(jìn)入障礙物影響范圍時，隨著機(jī)器人逐漸接近障礙物其斥力會迅速變大。機(jī)器人所受引力是關(guān)于機(jī)器人與目標(biāo)點(diǎn)之間距離的二次函數(shù)，隨著機(jī)器人逐漸接近目標(biāo)點(diǎn)其引力會迅速減小。因此當(dāng)目標(biāo)點(diǎn)位于障礙物影響范圍內(nèi)時，在機(jī)器人逐漸接近目標(biāo)點(diǎn)的過程中，會發(fā)生引力急劇減小而斥力急劇變大的情況，從而導(dǎo)致機(jī)器人無法到達(dá)目標(biāo)點(diǎn)。為解決上述問題，提出改進(jìn)的斥力函數(shù)，即在斥力函數(shù)中加入目標(biāo)點(diǎn)因素，如式（1）所示：

Urep(q)表示斥力勢能函數(shù)，ρo代表每個障礙物的影響半徑；

ρ(q,qgoal)表示機(jī)器人和目標(biāo)點(diǎn)之間的距離；n為斥力調(diào)節(jié)因子，其取值為正實(shí)數(shù)。

斥力Frep(q)為斥力場的負(fù)梯度，則

Fr1和Fr2為改進(jìn)后斥力的兩個分量，F(xiàn)r1方向?yàn)閺恼系K物點(diǎn)指向機(jī)器人所在位置，F(xiàn)r2方向?yàn)閺臋C(jī)器人所在位置指向目標(biāo)點(diǎn)。改進(jìn)前斥力的斥力變化只與機(jī)器人和障礙物之間的距離有關(guān)，而改進(jìn)之后的斥力還受機(jī)器人和目標(biāo)點(diǎn)之間的距離影響。Fr1的作用為使機(jī)器人遠(yuǎn)離障礙物，F(xiàn)r2的作用為使機(jī)器人向目標(biāo)點(diǎn)移動。

根據(jù)斥力調(diào)節(jié)因子n的取值不同，改進(jìn)斥力也會表現(xiàn)出不同的特性，下面分情況展開討論：

（1）n= 0，F(xiàn)r2= 0，此時

（2）0 ＜n＜1，改進(jìn)后的斥力勢場函數(shù)為非連續(xù)函數(shù)，在目標(biāo)點(diǎn)位置處不可微，當(dāng)機(jī)器人與障礙物之間的距離條件滿足ρ(q,qobs)＜ρo且ρ(q,qobs)≠0時，改進(jìn)后斥力的兩個分量為：

（3）n= 1，當(dāng)機(jī)器人與障礙物之間的距離條件滿足ρ(q,qobs)＜ρo且ρ(q,qobs)≠0時，改進(jìn)后斥力的兩個分量為：

（4）n＞1，斥力勢場函數(shù)處處可微。

改進(jìn)后的斥力函數(shù)的優(yōu)勢在于額外提供一個從機(jī)器人指向目標(biāo)點(diǎn)的力，使機(jī)器人能夠克服障礙物的驅(qū)離作用，從而到達(dá)目標(biāo)點(diǎn)。

（二）引入粒子群算法優(yōu)化勢場參數(shù)克服局部最優(yōu)。針對局部極小值問題，提出引入粒子群算法優(yōu)化勢場參數(shù)克服局部最優(yōu)的改進(jìn)方法。人當(dāng)機(jī)器人遇到合力為零的局部極小值問題時，可以通過調(diào)整參數(shù)，使合力始終大于零，從而確保機(jī)器人總是朝指向目標(biāo)點(diǎn)位置的方向運(yùn)動[14]。在改進(jìn)斥力函數(shù)后，機(jī)器人所受合力為：

考慮力的方向，使機(jī)器人所受合力大于零，有

其中，ρrg=ρ(q,qgoal)，表示機(jī)器人和目標(biāo)點(diǎn)之間的距離；ρro=ρ(q,qobs)，表示機(jī)器人和障礙物之間的距離。

考慮引力增益系數(shù)ξ和斥力增益系數(shù)η為正實(shí)數(shù)，式（10）可以改寫為：

其中m=ρro-ρrg。當(dāng)機(jī)器人、障礙物和目標(biāo)點(diǎn)在同一直線上且目標(biāo)點(diǎn)位于機(jī)器人和障礙物之間時，m表示障礙物和目標(biāo)點(diǎn)之間的距離，且滿足。

令

根據(jù)前文所述，得

其中，

則式(2-11)可以改寫為

設(shè)式（16）右側(cè)的上界為

（1）n＜2,此時

（2）n= 2，此時

（3）n＞2，此時

通過引入粒子群算法對相關(guān)參數(shù)進(jìn)行優(yōu)化，使算法在動態(tài)調(diào)整參數(shù)的過程中逐漸改變力的結(jié)構(gòu)，從而擺脫局部困局。在人工勢場法運(yùn)行過程中，將參數(shù)ξ和η實(shí)時反饋給粒子群算法，粒子群算法基于式（18）、（19）、（20）的約束條件對這兩個參數(shù)進(jìn)行優(yōu)化，直至合力不為零，即擺脫局部極小值。

（三）引入切向力。針對U型障礙物導(dǎo)致的局部死區(qū)問題，提出引入切向力的改進(jìn)方法。針對人工勢場法在局部受困從而導(dǎo)致路徑規(guī)劃失敗的問題，許多研究者通過添加輔助力的方式進(jìn)行解決[15]。通過引入逃逸力打破局部力的平衡，使機(jī)器人逃離受困點(diǎn)。本文提出引入切向力輔助機(jī)器人避開障礙的策略。

1.導(dǎo)航線的基本定義。如圖5，定義線段Lot為起始點(diǎn)Po到目標(biāo)點(diǎn)Pt的連接線，命名為導(dǎo)航線；定義Dp為機(jī)器人與障礙物切點(diǎn)Pp到導(dǎo)航線Lot的距離：

其中，Sotp表示由起始點(diǎn)Po、目標(biāo)點(diǎn)Pt和障礙物切點(diǎn)Pp組成的三角形。

定義σ為路徑效率因子：

2.判斷當(dāng)前障礙物的影響。提出一種新的障礙物規(guī)避策略，該策略主要依賴當(dāng)前位置到障礙物的切點(diǎn)實(shí)現(xiàn)。借助切點(diǎn)的避障策略的基本原理為首先畫出導(dǎo)航線Lot如果Lot沒有與障礙物相交，那么Lot就是目標(biāo)路徑，否則，找到當(dāng)前位置到當(dāng)前障礙物的切點(diǎn)(將障礙物擬化為圓形)，將切點(diǎn)設(shè)置為當(dāng)前目標(biāo)，并進(jìn)行后續(xù)運(yùn)算。

3.借助地圖信息反饋選擇切點(diǎn)。因?yàn)辄c(diǎn)到圓的切點(diǎn)有兩個，所以要進(jìn)行切點(diǎn)選擇。提出利用地圖反饋信息來選擇切點(diǎn)的方法，定義T為比較因子來輔助選擇切點(diǎn)：

其中，Ep為路徑效率系數(shù)，σp為路徑效率因子(與p在同一側(cè)的分支點(diǎn)到當(dāng)前位置的路徑規(guī)劃效率)，w1和w2分別為Dp和Ep的權(quán)重。分別計(jì)算導(dǎo)航線兩側(cè)的平均效率，效率越低表示相關(guān)區(qū)域中的障礙物越多，找到更優(yōu)路徑的可能性越低。

4.在人工虛擬力場中引入切向力。為擺脫局部極小點(diǎn)，在傳統(tǒng)勢場的基礎(chǔ)上加入切向力。在確定切點(diǎn)后，從當(dāng)前點(diǎn)到切點(diǎn)的方向即為切向力的方向，此時合力為：

圖6和圖7分別表示切向力的作用位置和包含切向力的合力結(jié)構(gòu)。其中，F(xiàn)A和Ftan分別表示傳統(tǒng)勢場合力和障礙物產(chǎn)生的切向力，wA和wT分別表示FA和Ftan所占的比重。當(dāng)機(jī)器人位于障礙物作用范圍之外時，只有切向力發(fā)揮作用，此時機(jī)器人只在切向力驅(qū)動下運(yùn)動；當(dāng)機(jī)器人進(jìn)入障礙物的作用范圍內(nèi)時，機(jī)器人在障礙物所產(chǎn)生的斥力和切向力以及目標(biāo)點(diǎn)產(chǎn)生的引力的共同作用下運(yùn)動，直至離開障礙物作用范圍。針對U型障礙物問題，可以將其視作圓形障礙物，通過結(jié)合切向力的作用避障。

圖6 U型障礙物切向力的作用位置

圖7 包含切向力的合力結(jié)構(gòu)圖

三、仿真實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證改進(jìn)后算法的有效性，使用MATLAB軟件并編寫相關(guān)程序進(jìn)行仿真驗(yàn)證。針對障礙物影響范圍內(nèi)的目標(biāo)不可達(dá)問題、局部極小值問題以及U 型障礙物問題進(jìn)行仿真，相關(guān)參數(shù)設(shè)置為斥力增益系數(shù)η=5，引力增益系數(shù)ξ=2，障礙物影響半徑為ρo=2，步長為stepSize=0.1，斥力調(diào)節(jié)因子n=2，切向力相關(guān)參數(shù)w1=0.6，w2=0.4，wA=0.5，wB=0.5，迭代次數(shù)為Itemax=200。

在圖8中，障礙物位于接近目標(biāo)點(diǎn)的位置，目標(biāo)點(diǎn)位于障礙物的影響范圍內(nèi)。當(dāng)機(jī)器人在到達(dá)目標(biāo)點(diǎn)和障礙物附近的范圍時，由于障礙物斥力的干擾作用，機(jī)器人將在合力為零點(diǎn)處停止運(yùn)動。使用改進(jìn)后人工勢場法時，由于在斥力公式中加入了目標(biāo)點(diǎn)因素，使得機(jī)器人在干擾區(qū)域(即障礙物作用范圍)內(nèi)合力始終不為零且指向目標(biāo)點(diǎn)，最終機(jī)器人成功到達(dá)目標(biāo)點(diǎn)位置，結(jié)果如圖9所示

圖8 傳統(tǒng)人工勢場法路徑規(guī)劃失敗

圖9 引入斥力參數(shù)后路徑規(guī)劃成功

在圖10中，目標(biāo)點(diǎn)位于兩個障礙物連線的垂線上，當(dāng)機(jī)器人沿該垂線向目標(biāo)點(diǎn)行進(jìn)時，如圖-所示，會在障礙物斥力的合力與目標(biāo)點(diǎn)引力數(shù)值相等點(diǎn)處陷入局部極小值停止運(yùn)動。使用改進(jìn)后人工勢場法，通過優(yōu)化參數(shù)使機(jī)器人所受合力不為零且方向指向目標(biāo)點(diǎn)，成功擺脫局部極小值點(diǎn)，抵達(dá)目標(biāo)點(diǎn)，結(jié)果如圖11所示。

圖11 引入粒子群算法后路徑規(guī)劃成功

在圖4中，地圖環(huán)境中的障礙物為U形障礙物，機(jī)器人的前進(jìn)方向存在U 型障礙物，當(dāng)使用傳統(tǒng)人工勢場法時，機(jī)器人將進(jìn)入位于障礙物內(nèi)部的死區(qū)范圍，導(dǎo)致局部震蕩或運(yùn)動終止。改進(jìn)后通過使用引入切向力的方法，機(jī)器人在包含切向力的合力作用向下脫離局部極小值，成功抵達(dá)目標(biāo)點(diǎn)，結(jié)果如圖12所示。

圖12 引入切向力后路徑規(guī)劃成功

通過實(shí)驗(yàn)仿真結(jié)果分析可知：傳統(tǒng)人工勢場法在目標(biāo)不可達(dá)、局部極小值和U 型死區(qū)的情況下，移動機(jī)器人路徑規(guī)劃失敗；而在對傳統(tǒng)人工勢場法使用了引入斥力參數(shù)、引入粒子群算法和引入切向力的算法改進(jìn)，移動機(jī)器人則能夠成功完成局部路徑規(guī)劃。

四、結(jié)論

通過分析傳統(tǒng)人工勢場法及其存在的問題，針對具體情況提出了相應(yīng)的改進(jìn)方法，并用MATLAB 軟件進(jìn)行仿真驗(yàn)證，仿真結(jié)果表明：改進(jìn)后的算法能更好地實(shí)現(xiàn)局部路徑規(guī)劃。針對目標(biāo)不可達(dá)情況，提出在斥力計(jì)算公式中引入斥力參數(shù)的改進(jìn)方法；針對局部極小值問題，提出對相關(guān)參數(shù)進(jìn)行優(yōu)化并進(jìn)行相應(yīng)合力計(jì)算的改進(jìn)方法；針對U型障礙物導(dǎo)致機(jī)器人陷入局部死區(qū)的情況，提出引入切向力的改進(jìn)方法。通過MATLAB軟件進(jìn)行了仿真實(shí)驗(yàn)，所提出的方法均能夠有效解決相應(yīng)的路徑規(guī)劃問題，表明改進(jìn)后的算法能夠有效完成局部路徑規(guī)劃。

所提的改進(jìn)人工勢場法算法總體上能夠在二維環(huán)境中實(shí)現(xiàn)預(yù)期目標(biāo)，但仍存在一定局限性，后續(xù)應(yīng)從以下兩個方面繼續(xù)開展研究工作：

（一）算法只在考慮二維環(huán)境因素，對環(huán)境信息的考慮比較單一，后續(xù)應(yīng)綜合考慮立體環(huán)境信息進(jìn)行三維建圖，在三維環(huán)境下測試算法的性能并進(jìn)一步改進(jìn)，增強(qiáng)算法對環(huán)境的適應(yīng)性。

（二）算法只考慮了單目標(biāo)的情況，在多目標(biāo)條件下的規(guī)劃效果有待檢驗(yàn)，后續(xù)應(yīng)考慮基于多目標(biāo)情況進(jìn)行試驗(yàn)和改進(jìn)。