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

（武漢理工大學(xué)自動化學(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ù)多，在未知環(huán)境下規(guī)劃的實(shí)時性不好等問題。人工勢場法在進(jìn)行路徑規(guī)劃時，不需要提前生成完整路徑，只需要根據(jù)當(dāng)前姿態(tài)以及周邊障礙物來確定下一步的運(yùn)動，算法反應(yīng)速度快、計(jì)算量小，便于實(shí)時性控制，且規(guī)劃出的路徑平滑安全［15］。但是傳統(tǒng)人工勢場法容易陷入局部極小點(diǎn)，且當(dāng)障礙物環(huán)繞時，容易陷入陷阱區(qū)域，當(dāng)路徑中存在局部極小點(diǎn)或者陷阱區(qū)域時，傳統(tǒng)人工勢場法無法規(guī)劃出完整的路徑。

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

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

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

3）當(dāng)存在復(fù)雜障礙物環(huán)境，如U型障礙物時，機(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)行一定的處理，將所有障礙物分割成若干個圓形障礙物，圓的半徑為Ro，要求圓形障礙物將原障礙物全覆蓋且兩個鄰近的圓形障礙物圓心之間的距離小于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ì)算斥力時引入安全距離，使機(jī)器人避免不必要的斥力作用。

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

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

針對圖1（a）的問題，引入安全距離后的算法可以很好地解決，如圖4 所示，由于兩個障礙物均在安全距離以外，所以不會對機(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)附近存在障礙物時，機(jī)器人所受的斥力很可能會大于等于引力，這樣就會導(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)附近時，ri很小，傳統(tǒng)人工勢場法斥力公式乘以，得到一個較小的斥力值，使機(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)移動機(jī)器人在障礙物影響范圍之外或者移動機(jī)器人前進(jìn)方向安全時，移動機(jī)器人受到的斥力為零，在引力的作用下，朝著目標(biāo)點(diǎn)前進(jìn)。

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

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

其中：m表示大于零的斥力場常量；ρi表示當(dāng)前位置與障礙物的距離；ρo表示單個障礙物影響的最大范圍距離；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ū)域時，如U型障礙物，傳統(tǒng)人工勢場法會陷入其中，產(chǎn)生震蕩或停止，無法規(guī)劃出完整的路徑，如圖1（c）所示。

針對圖1（c）的問題，引入預(yù)測距離Rp，計(jì)算目標(biāo)一側(cè)的障礙物到當(dāng)前方向所在直線的距離di，當(dāng)di小于等于安全距離rs時，取滿足以上條件且到當(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時，停止旋轉(zhuǎn)，以此時的方向?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è)，同時滿足障礙物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，則以此時的方向?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ī)器人視為一個質(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)境中時，傳統(tǒng)人工勢場法會在第一個陷阱區(qū)域或局部極小點(diǎn)的位置停住，無法到達(dá)終點(diǎn)；B-APF 算法和C-CPF 算法在陷入陷阱區(qū)域后分別通過設(shè)立臨時障礙物和虛擬目標(biāo)點(diǎn)方法幫助機(jī)器人逃離陷阱區(qū)域和局部極小點(diǎn)，順利到達(dá)終點(diǎn)，但是會先陷入陷阱區(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ù)據(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ī)劃出的路徑略短，且在距離障礙物一定距離的時候提前做出反應(yīng)，不會存在碰撞危險(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)時，本文算法不僅能夠避開陷阱區(qū)域和局部極小點(diǎn)，而且路徑平滑簡短；當(dāng)環(huán)境地圖中不存在陷阱區(qū)域以及局部極小點(diǎn)時，本文算法規(guī)劃出的路徑依舊比傳統(tǒng)人工勢場法簡短；本文算法的運(yùn)算速度較快。

表2 地圖中不存在陷阱區(qū)域和局部極小點(diǎn)時算法數(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ī)加入動態(tài)障礙物，進(jìn)一步提高路徑規(guī)劃的難度和真實(shí)性。