基于改進(jìn)人工勢場法的移動(dòng)機(jī)器人路徑規(guī)劃

徐小強(qiáng)，王明勇，冒 燕

（武漢理工大學(xué)自動(dòng)化學(xué)院，武漢430070）

（?通信作者電子郵箱maoyan@whut.edu.cn）

0 引言

路徑規(guī)劃是指存在障礙物的環(huán)境下，按照一定的性能指標(biāo)，機(jī)器人如何從所處的環(huán)境中搜索到一條從初始位置到終點(diǎn)的安全的、無碰撞的最優(yōu)或次優(yōu)路徑［1］。常用的算法有蟻群算法［2-3］、Dijkstra算法［4-5］、A*算法［6-7］、粒子群法［8］、遺傳算法［9-10］、人工勢場法（Artificial Potential Field method，APF）［11-14］等算法。蟻群算法、Dijkstra 算法、A*算法、遺傳算法等算法存在運(yùn)算時(shí)間長、迭代次數(shù)多，在未知環(huán)境下規(guī)劃的實(shí)時(shí)性不好等問題。人工勢場法在進(jìn)行路徑規(guī)劃時(shí)，不需要提前生成完整路徑，只需要根據(jù)當(dāng)前姿態(tài)以及周邊障礙物來確定下一步的運(yùn)動(dòng)，算法反應(yīng)速度快、計(jì)算量小，便于實(shí)時(shí)性控制，且規(guī)劃出的路徑平滑安全［15］。但是傳統(tǒng)人工勢場法容易陷入局部極小點(diǎn)，且當(dāng)障礙物環(huán)繞時(shí)，容易陷入陷阱區(qū)域，當(dāng)路徑中存在局部極小點(diǎn)或者陷阱區(qū)域時(shí)，傳統(tǒng)人工勢場法無法規(guī)劃出完整的路徑。

為此，文獻(xiàn)［16］通過“沿邊走”的方法來解決局部極小點(diǎn)和復(fù)雜障礙物環(huán)境下的路徑規(guī)劃問題，但規(guī)劃出的路徑太長。文獻(xiàn)［17］對障礙物連通性進(jìn)行了分析，并設(shè)立臨時(shí)目標(biāo)點(diǎn)來幫助機(jī)器人脫離局部極小點(diǎn)，該算法雖然可以減少搜索空間，提高搜索效率，但沒有考慮U 型陷阱的情況。文獻(xiàn)［18］提出了一種基于電勢場的路徑規(guī)劃算法，可以避免陷阱問題，但其建模過程復(fù)雜、計(jì)算量大。文獻(xiàn)［19］提出在扇形分區(qū)設(shè)置臨時(shí)障礙物的方法，打破局部極小點(diǎn)的平衡，幫助機(jī)器人脫離局部極小狀態(tài)，但是臨時(shí)障礙物的位置選取過于粗略，規(guī)劃出的路徑不夠簡短。文獻(xiàn)［20］通過引入方向向量和虛擬目標(biāo)點(diǎn)的方法，幫助機(jī)器人逃離局部極小點(diǎn)和陷阱區(qū)域，但是在虛擬目標(biāo)點(diǎn)的選取上不太合理，導(dǎo)致規(guī)劃出的路徑較長且存在拐角過大情況。

針對上述算法存在的問題，本文通過引入安全距離，對障礙物進(jìn)行一定的篩選，減少規(guī)劃出的路徑長度；引入預(yù)測距離，通過提前改變前進(jìn)方向，避免陷入陷阱區(qū)域、局部極小點(diǎn)或者發(fā)生碰撞；通過設(shè)置虛擬目標(biāo)點(diǎn)，引導(dǎo)機(jī)器人避開以上情況，安全到達(dá)終點(diǎn)。最后利用Matlab 對本文算法——提前預(yù)測人工勢場法（Artificial Potential Field Method with Advance Prediction）進(jìn)行了可行性驗(yàn)證。

1 人工勢場法存在的問題

根據(jù)文獻(xiàn)［11-14］可知，傳統(tǒng)人工勢場法規(guī)劃路徑時(shí)會(huì)存在以下問題：

1）當(dāng)路徑中障礙物產(chǎn)生的斥力與目標(biāo)點(diǎn)產(chǎn)生的引力大小相等、方向相反時(shí)，機(jī)器人將陷入局部極小點(diǎn)，無法完成整個(gè)路徑的規(guī)劃，如圖1（a）。

2）當(dāng)目標(biāo)點(diǎn)附近有障礙物時(shí)，由于靠近目標(biāo)點(diǎn)，目標(biāo)點(diǎn)對機(jī)器人的引力很小，此時(shí)障礙物的斥力大于目標(biāo)點(diǎn)的引力，會(huì)使機(jī)器人在目標(biāo)點(diǎn)附近抖動(dòng)，無法達(dá)到目標(biāo)點(diǎn)，如圖1（b）。

3）當(dāng)存在復(fù)雜障礙物環(huán)境，如U型障礙物時(shí)，機(jī)器人被障礙物環(huán)繞，無法逃出，從而導(dǎo)致路徑規(guī)劃失敗，如圖1（c）。

圖1 局部極小點(diǎn)和U型障礙物Fig.1 Local minimum and U-shaped obstacle

針對傳統(tǒng)人工勢場法容易出現(xiàn)圖1 所示的問題，提出了一種改進(jìn)算法——提前預(yù)測人工勢場法，可以很好地解決上述問題。該算法對斥力的生成機(jī)制做出調(diào)整，在一定條件下對前進(jìn)方向做出了改進(jìn)，通過設(shè)置虛擬目標(biāo)點(diǎn)引導(dǎo)機(jī)器人到達(dá)終點(diǎn)。

2 改進(jìn)算法

2.1 障礙物處理

為了便于實(shí)際路徑規(guī)劃，對環(huán)境中的障礙物進(jìn)行一定的處理，將所有障礙物分割成若干個(gè)圓形障礙物，圓的半徑為Ro，要求圓形障礙物將原障礙物全覆蓋且兩個(gè)鄰近的圓形障礙物圓心之間的距離小于2Ro，如圖2 所示。為了安全，對圓形障礙物進(jìn)行膨脹處理，處理后的半徑為Rc，要求機(jī)器人到圓形障礙物的圓心距離即安全距離rs大于Rc。

圖2 障礙物的處理Fig.2 Processing of obstacles

2.2 安全距離rs

傳統(tǒng)人工勢場法路徑規(guī)劃過程中，機(jī)器人從當(dāng)前位置本可以以直線前進(jìn)安全到達(dá)目標(biāo)點(diǎn)，但是卻由于直線路徑上的一些點(diǎn)在障礙物的影響范圍內(nèi)，導(dǎo)致機(jī)器人在斥力的作用下走了彎路，這樣規(guī)劃出來的路徑長度就有所增長，導(dǎo)致規(guī)劃出的路徑不是最優(yōu)路徑。針對這一問題，在計(jì)算斥力時(shí)引入安全距離，使機(jī)器人避免不必要的斥力作用。

在路徑規(guī)劃的過程中，計(jì)算障礙物到當(dāng)前點(diǎn)與目標(biāo)點(diǎn)連線的距離di，如果di小于等于rs，說明以當(dāng)前方向前進(jìn)會(huì)發(fā)生碰撞，此時(shí)需要考慮該障礙物的斥力；如果di大于rs，說明以當(dāng)前方向前進(jìn)不會(huì)發(fā)生碰撞，此時(shí)不考慮此障礙物的斥力，繼續(xù)沿當(dāng)前直線前進(jìn)，規(guī)劃出來的路徑平滑且較短，如圖3所示。

圖3 障礙物無碰撞風(fēng)險(xiǎn)Fig.3 No collision risk of obstacles

針對圖1（a）的問題，引入安全距離后的算法可以很好地解決，如圖4 所示，由于兩個(gè)障礙物均在安全距離以外，所以不會(huì)對機(jī)器人產(chǎn)生斥力作用，機(jī)器人沿當(dāng)前方向前進(jìn)。

圖4 解決中間平衡點(diǎn)問題Fig.4 Solving problem of intermediate equilibrium point

2.3 當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的距離ri

當(dāng)目標(biāo)點(diǎn)附近存在障礙物時(shí)，機(jī)器人所受的斥力很可能會(huì)大于等于引力，這樣就會(huì)導(dǎo)致機(jī)器人在目標(biāo)點(diǎn)附近震蕩或停止，無法到達(dá)目標(biāo)點(diǎn)，如圖1（b）所示。

針對圖1（b）的問題，在斥力公式中添加因子，ri為當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的距離，機(jī)器人在目標(biāo)點(diǎn)附近時(shí)，ri很小，傳統(tǒng)人工勢場法斥力公式乘以，得到一個(gè)較小的斥力值，使機(jī)器人所受合力指向目標(biāo)點(diǎn)，機(jī)器人能夠順利到達(dá)目標(biāo)點(diǎn)，如圖5所示。

圖5 解決目標(biāo)點(diǎn)附近存在障礙物問題Fig.5 Solving problem of having obstacle near target point

2.4 改進(jìn)后的算法

通過上述分析可知：

1）當(dāng)移動(dòng)機(jī)器人在障礙物影響范圍之外或者移動(dòng)機(jī)器人前進(jìn)方向安全時(shí)，移動(dòng)機(jī)器人受到的斥力為零，在引力的作用下，朝著目標(biāo)點(diǎn)前進(jìn)。

2）當(dāng)移動(dòng)機(jī)器人在障礙物影響范圍內(nèi)，且前進(jìn)方向存在碰撞風(fēng)險(xiǎn)時(shí)，障礙物會(huì)對移動(dòng)機(jī)器人產(chǎn)生斥力作用，阻礙移動(dòng)機(jī)器人碰撞障礙物。為了確保移動(dòng)機(jī)器人不會(huì)碰撞障礙物，引入當(dāng)前位置到障礙物距離的倒數(shù)與障礙物影響范圍距離的倒數(shù)之差，即，可知：當(dāng)移動(dòng)機(jī)器人距離障礙物越近時(shí)，ρo-ρi越大，ρi ρo越小，所以越大，產(chǎn)生的斥力就越大，阻礙移動(dòng)機(jī)器人駛向障礙物；當(dāng)移動(dòng)機(jī)器人與障礙物之間的距離小于1 時(shí)，急劇增大，產(chǎn)生較強(qiáng)的斥力作用，進(jìn)一步阻止移動(dòng)機(jī)器人碰撞障礙物；當(dāng)移動(dòng)機(jī)器人快要到達(dá)目標(biāo)點(diǎn)，且目標(biāo)點(diǎn)附近存在障礙物時(shí)，由于引力作用太弱，斥力作用太強(qiáng)，移動(dòng)機(jī)器人無法到達(dá)目標(biāo)點(diǎn)，此時(shí)可以通過減小斥力作用使移動(dòng)機(jī)器人順利到達(dá)目標(biāo)點(diǎn)。引入因子，ri為當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的距離，移動(dòng)機(jī)器人在目標(biāo)點(diǎn)附近時(shí)，很小，斥力公式乘以，得到一個(gè)很小的斥力值。

改進(jìn)后的斥力公式為：

其中：m表示大于零的斥力場常量；ρi表示當(dāng)前位置與障礙物的距離；ρo表示單個(gè)障礙物影響的最大范圍距離；ri表示當(dāng)前位置與目標(biāo)點(diǎn)的距離；rs表示安全距離；di表示障礙物到當(dāng)前點(diǎn)與目標(biāo)點(diǎn)連線的距離。

2.5 預(yù)測距離Rp

當(dāng)?shù)貓D中存在陷阱區(qū)域時(shí)，如U型障礙物，傳統(tǒng)人工勢場法會(huì)陷入其中，產(chǎn)生震蕩或停止，無法規(guī)劃出完整的路徑，如圖1（c）所示。

針對圖1（c）的問題，引入預(yù)測距離Rp，計(jì)算目標(biāo)一側(cè)的障礙物到當(dāng)前方向所在直線的距離di，當(dāng)di小于等于安全距離rs時(shí)，取滿足以上條件且到當(dāng)前點(diǎn)的距離最近的障礙物為中心障礙物，計(jì)算此障礙物到當(dāng)前點(diǎn)的距離為Rx。判斷Rx是否小于等于預(yù)測距離Rp，若不是，則沿當(dāng)前方向繼續(xù)前進(jìn)；若是，則以Rx為半徑、當(dāng)前點(diǎn)為圓心向障礙物少的一側(cè)旋轉(zhuǎn)。為了使規(guī)劃出的路徑較短，每次旋轉(zhuǎn)3°，每旋轉(zhuǎn)一次，就判斷各障礙物到當(dāng)前點(diǎn)的距離di，當(dāng)所有di都大于安全距離rs時(shí)，停止旋轉(zhuǎn)，以此時(shí)的方向?yàn)樾碌那斑M(jìn)方向，以半徑的另一端點(diǎn)作為新的虛擬目標(biāo)點(diǎn)。通過合理地選取預(yù)測距離Rp的大小，可以在機(jī)器人陷入陷阱區(qū)域之前就改變前進(jìn)方向，避開陷阱區(qū)域，規(guī)劃出來的路徑較短，且由于每次旋轉(zhuǎn)的角度很小，規(guī)劃出來的路徑比較平滑，如圖6所示。

圖6 解決U型障礙物問題Fig.6 Solving problem of U-shaped obstacle

2.6 算法流程

提前預(yù)測人工勢場法的算法流程如下：

步驟1 算法初始化，設(shè)置機(jī)器人初始位置Xo和終點(diǎn)位置Xg，給定相關(guān)參數(shù)：障礙物影響范圍ρo、引力常數(shù)k、斥力常數(shù)m、步長l、安全距離rs、預(yù)測距離Rp。

步驟2 連接當(dāng)前點(diǎn)與終點(diǎn)，得到一條直線，判斷各障礙物到此直線的距離d。

步驟3 如果障礙物在目標(biāo)一側(cè)，同時(shí)滿足障礙物i到直線的距離小于安全距離rs且障礙物i到當(dāng)前點(diǎn)的距離最短，執(zhí)行步驟4；否則，沿當(dāng)前方向前進(jìn)，直到達(dá)到終點(diǎn)。

步驟4 判斷步驟3 中障礙物i到當(dāng)前點(diǎn)的距離，如果小于預(yù)測距離Rp，執(zhí)行步驟5；否則，沿當(dāng)前方向前進(jìn)，直到小于預(yù)測距離Rp。

步驟5 以障礙物i與當(dāng)前點(diǎn)的距離為半徑向障礙物少的一邊旋轉(zhuǎn)，每次旋轉(zhuǎn)3°，直到目標(biāo)一側(cè)所有障礙物到新的方向所在直線的距離大于安全距離rs，則以此時(shí)的方向?yàn)樾碌那斑M(jìn)方向，半徑的另一端點(diǎn)作為新的目標(biāo)點(diǎn)，并沿新的方向前進(jìn)。

步驟6 判斷是否到達(dá)新的目標(biāo)點(diǎn)，如果到達(dá)新的目標(biāo)點(diǎn)，則跳到步驟2，直到到達(dá)終點(diǎn)；否則，沿當(dāng)前方向繼續(xù)前進(jìn)，直到到達(dá)新的目標(biāo)點(diǎn)。

3 算法仿真及結(jié)果分析

為驗(yàn)證本文算法的可行性以及路徑規(guī)劃的效果，在神舟筆記本電腦Z7M-CT5NA 上通過Matlab 進(jìn)行仿真研究，Z7MCT5NA 的硬件信息如下：Core i5-9300H、2.4 GHz 四核處理器，8 GB 內(nèi)存，512 GB 固態(tài)硬盤。將機(jī)器人視為一個(gè)質(zhì)點(diǎn)，初始參數(shù)設(shè)置如下：機(jī)器人初始位置Xo=(0，0)，終點(diǎn)位置Xg=(10，10)，障礙物影響范圍ρo=2.5，引力常數(shù)k=15，斥力常數(shù)m=10，步長l=0.1，安全距離rs=0.6，預(yù)測距離Rp=4。

由于障礙物的特點(diǎn)對機(jī)器人的路徑規(guī)劃有較大的影響，所以在相同的障礙物環(huán)境下，對本文算法與傳統(tǒng)人工勢場法、文獻(xiàn)［19］中的算法（記作B-APF）以及文獻(xiàn)［20］中的算法（記作C-APF）進(jìn)行多次比較，以驗(yàn)證本文算法的優(yōu)越性。

隨機(jī)生成的存在陷阱區(qū)域以及局部極小點(diǎn)的仿真地圖如圖7 所示，幾種算法在同一仿真地圖環(huán)境下運(yùn)行。仿真結(jié)果表明：當(dāng)在復(fù)雜環(huán)境中時(shí)，傳統(tǒng)人工勢場法會(huì)在第一個(gè)陷阱區(qū)域或局部極小點(diǎn)的位置停住，無法到達(dá)終點(diǎn)；B-APF 算法和C-CPF 算法在陷入陷阱區(qū)域后分別通過設(shè)立臨時(shí)障礙物和虛擬目標(biāo)點(diǎn)方法幫助機(jī)器人逃離陷阱區(qū)域和局部極小點(diǎn)，順利到達(dá)終點(diǎn)，但是會(huì)先陷入陷阱區(qū)域，然后再逃離，這樣就導(dǎo)致路徑較長，路徑不夠平滑，多處拐角過大；本文算法通過引入預(yù)測距離Rp，在機(jī)器人陷入陷阱區(qū)域或局部極小點(diǎn)之前做出反應(yīng)，通過設(shè)置合理的虛擬目標(biāo)點(diǎn)，引導(dǎo)機(jī)器人避開陷阱區(qū)域，規(guī)劃出的路徑較短，而且不存在大的拐角，路徑由幾段直線段組成，路徑平滑。

圖7 存在陷阱區(qū)域和局部極小點(diǎn)的地圖Fig.7 Map with trap area and local minimum

存在陷阱區(qū)域以及局部極小點(diǎn)的仿真數(shù)據(jù)如表1 所示。由表1 可以看出，傳統(tǒng)人工勢場法無法規(guī)劃出完整的路徑；本文算法相較于B-APF 算法，規(guī)劃出的路徑縮短了23.08%，算法速度提高了110.96%；本文算法相較于C-APF算法，規(guī)劃出的路徑縮短了21.88%，算法速度提高了67.12%。

表1 地圖中存在陷阱區(qū)域和局部極小點(diǎn)時(shí)的算法數(shù)據(jù)Tab.1 Algorithm data with trap area and local minimum in map

隨機(jī)生成的不存在陷阱區(qū)域以及局部極小點(diǎn)的仿真地圖如圖8 所示，幾種算法在同一仿真地圖環(huán)境下運(yùn)行。仿真結(jié)果表明：相較傳統(tǒng)人工勢場法、B-APF算法和本文算法，C-APF算法規(guī)劃出的路徑存在拐角過大問題，路徑不夠平滑；相較傳統(tǒng)人工勢場法、B-APF 算法和C-APF 算法，本文算法規(guī)劃出的路徑略短，且在距離障礙物一定距離的時(shí)候提前做出反應(yīng)，不會(huì)存在碰撞危險(xiǎn)。

圖8 不存在陷阱區(qū)域和局部極小點(diǎn)的地圖Fig.8 Map without trap area and local minimum

不存在陷阱區(qū)域以及局部極小點(diǎn)的仿真數(shù)據(jù)如表2 所示。由表2可以看出，傳統(tǒng)人工勢場法、B-APF算法、C-APF算法規(guī)劃出的路徑長短相近，本文算法規(guī)劃出的路徑最短；相較于傳統(tǒng)人工勢場法，本文算法規(guī)劃出的路徑長度縮短了5.2%，算法速度提高了405.56%。

通過以上仿真結(jié)果得到如下結(jié)論：當(dāng)環(huán)境地圖復(fù)雜，存在陷阱區(qū)域以及局部極小點(diǎn)時(shí)，本文算法不僅能夠避開陷阱區(qū)域和局部極小點(diǎn)，而且路徑平滑簡短；當(dāng)環(huán)境地圖中不存在陷阱區(qū)域以及局部極小點(diǎn)時(shí)，本文算法規(guī)劃出的路徑依舊比傳統(tǒng)人工勢場法簡短；本文算法的運(yùn)算速度較快。

表2 地圖中不存在陷阱區(qū)域和局部極小點(diǎn)時(shí)算法數(shù)據(jù)Tab.2 Algorithm data without trap area and local minimum in map

4 結(jié)語

傳統(tǒng)人工勢場法在路徑規(guī)劃過程中容易陷入陷阱區(qū)域，容易在局部極小點(diǎn)附近停止或震蕩，在障礙物環(huán)境復(fù)雜的情況下無法規(guī)劃出完整的路徑。本文通過引入安全距離，對斥力的生成條件和大小進(jìn)行了改進(jìn)，消除了無關(guān)障礙物對機(jī)器人的斥力作用；引入預(yù)測距離和虛擬目標(biāo)點(diǎn)，在機(jī)器人陷入局部極小點(diǎn)或者陷阱區(qū)域之前，提前改變方向，設(shè)置虛擬目標(biāo)點(diǎn)，引導(dǎo)機(jī)器人避開陷阱區(qū)域和局部極小點(diǎn)。仿真結(jié)果表明，存在陷阱區(qū)域和局部極小點(diǎn)的環(huán)境下，本文算法可以有效解決傳統(tǒng)人工勢場法路徑規(guī)劃中斷問題，順利達(dá)到終點(diǎn)；且相較近年來提出的B-APF算法和C-APF算法也有進(jìn)一步的改進(jìn)效果。相較于B-APF 算法，本文算法規(guī)劃出的路徑長度縮短了23.08%，算法速度提高了110.96%；相較于C-APF 算法，本文算法規(guī)劃出的路徑長度縮短了21.88%，算法速度提高了67.12%。不存在陷阱區(qū)域和局部極小點(diǎn)的環(huán)境下，相較于傳統(tǒng)人工勢場法，本文算法規(guī)劃出的路徑長度縮短了5.2%，算法速度提高了405.56%。下一步將考慮在現(xiàn)有環(huán)境中隨機(jī)加入動(dòng)態(tài)障礙物，進(jìn)一步提高路徑規(guī)劃的難度和真實(shí)性。