張 軍 許靖宜 于士坤

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

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

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

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

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

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

人工勢(shì)場(chǎng)法是通過利用地圖環(huán)境信息建立勢(shì)場(chǎng)模型，在機(jī)器人路徑規(guī)劃中具有比較成熟的應(yīng)用并取得了理想的效果。易先軍[7]等人提出了一種模糊改進(jìn)人工勢(shì)場(chǎng)法，針對(duì)人工勢(shì)場(chǎng)法存在的目標(biāo)不可達(dá)等問題，建立斥力勢(shì)場(chǎng)梯度等級(jí)解決目標(biāo)不可達(dá)問題，同時(shí)引入基于模糊算法推算的逃逸力，避免局部最優(yōu)；為克服局部極小值問題，Hongqiang Sang[8]等人提出了一種多目標(biāo)人工勢(shì)場(chǎng)算法，將地圖環(huán)境劃分為多個(gè)子目標(biāo)勢(shì)場(chǎng)分段實(shí)現(xiàn)路徑規(guī)劃，該算法在不同環(huán)境中均能有效發(fā)揮作用；Ulises Orozco-Rosas[9]等人提出了一種膜進(jìn)化人工勢(shì)場(chǎng)法，將膜計(jì)算與遺傳算法和人工勢(shì)場(chǎng)法結(jié)合，優(yōu)化參數(shù)生成安全可行的路徑。但由于勢(shì)場(chǎng)函數(shù)定義方式的局限性，使得算法會(huì)在某些場(chǎng)景下因?yàn)闄C(jī)器人、目標(biāo)點(diǎn)和障礙物之間的相對(duì)位置不同而存在目標(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)的周圍存在一個(gè)及以上障礙物且目標(biāo)點(diǎn)在障礙物的輻射范圍內(nèi)時(shí)，當(dāng)機(jī)器人向目標(biāo)點(diǎn)和障礙物所在區(qū)域移動(dòng)時(shí)，機(jī)器人所受引力逐漸減小而斥力逐漸加大，從而可能會(huì)出現(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)人工勢(shì)場(chǎng)法中導(dǎo)致路徑規(guī)劃失敗的主要問題。在人工勢(shì)場(chǎng)法中，當(dāng)引力和斥力位于同一直線上，且絕對(duì)值相同、方向相反時(shí)機(jī)器人所受合力為零，但此時(shí)機(jī)器人所處位置并非全局勢(shì)場(chǎng)最低點(diǎn)[12]，從而導(dǎo)致局部極小值問題。

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

圖3 局部極小值問題

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

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

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

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

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

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

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

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

Fr1和Fr2為改進(jìn)后斥力的兩個(gè)分量，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)移動(dòng)。

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

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

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

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

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

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

（二）引入粒子群算法優(yōu)化勢(shì)場(chǎng)參數(shù)克服局部最優(yōu)。針對(duì)局部極小值問題，提出引入粒子群算法優(yōu)化勢(shì)場(chǎng)參數(shù)克服局部最優(yōu)的改進(jìn)方法。人當(dāng)機(jī)器人遇到合力為零的局部極小值問題時(shí)，可以通過調(diào)整參數(shù)，使合力始終大于零，從而確保機(jī)器人總是朝指向目標(biāo)點(diǎn)位置的方向運(yùn)動(dòng)[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ī)器人和障礙物之間時(shí)，m表示障礙物和目標(biāo)點(diǎn)之間的距離，且滿足。

令

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

其中，

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

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

（1）n＜2,此時(shí)

（2）n= 2，此時(shí)

（3）n＞2，此時(shí)

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

（三）引入切向力。針對(duì)U型障礙物導(dǎo)致的局部死區(qū)問題，提出引入切向力的改進(jìn)方法。針對(duì)人工勢(shì)場(chǎng)法在局部受困從而導(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)有兩個(gè)，所以要進(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.在人工虛擬力場(chǎng)中引入切向力。為擺脫局部極小點(diǎn)，在傳統(tǒng)勢(shì)場(chǎng)的基礎(chǔ)上加入切向力。在確定切點(diǎn)后，從當(dāng)前點(diǎn)到切點(diǎn)的方向即為切向力的方向，此時(shí)合力為：

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

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

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

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

為驗(yàn)證改進(jìn)后算法的有效性，使用MATLAB軟件并編寫相關(guān)程序進(jìn)行仿真驗(yàn)證。針對(duì)障礙物影響范圍內(nèi)的目標(biāo)不可達(dá)問題、局部極小值問題以及U 型障礙物問題進(jìn)行仿真，相關(guān)參數(shù)設(shè)置為斥力增益系數(shù)η=5，引力增益系數(shù)ξ=2，障礙物影響半徑為ρo=2，步長(zhǎng)為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)和障礙物附近的范圍時(shí)，由于障礙物斥力的干擾作用，機(jī)器人將在合力為零點(diǎn)處停止運(yùn)動(dòng)。使用改進(jìn)后人工勢(shì)場(chǎng)法時(shí)，由于在斥力公式中加入了目標(biāo)點(diǎn)因素，使得機(jī)器人在干擾區(qū)域(即障礙物作用范圍)內(nèi)合力始終不為零且指向目標(biāo)點(diǎn)，最終機(jī)器人成功到達(dá)目標(biāo)點(diǎn)位置，結(jié)果如圖9所示

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

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

在圖10中，目標(biāo)點(diǎn)位于兩個(gè)障礙物連線的垂線上，當(dāng)機(jī)器人沿該垂線向目標(biāo)點(diǎn)行進(jìn)時(shí)，如圖-所示，會(huì)在障礙物斥力的合力與目標(biāo)點(diǎn)引力數(shù)值相等點(diǎn)處陷入局部極小值停止運(yùn)動(dòng)。使用改進(jìn)后人工勢(shì)場(chǎng)法，通過優(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)人工勢(shì)場(chǎng)法時(shí)，機(jī)器人將進(jìn)入位于障礙物內(nèi)部的死區(qū)范圍，導(dǎo)致局部震蕩或運(yùn)動(dòng)終止。改進(jìn)后通過使用引入切向力的方法，機(jī)器人在包含切向力的合力作用向下脫離局部極小值，成功抵達(dá)目標(biāo)點(diǎn)，結(jié)果如圖12所示。

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

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

四、結(jié)論

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

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

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

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