基于A*算法和人工勢場法的移動機器人路徑規劃

王洪斌 郝 策 張 平 張明泉 尹鵬衡 張永順

1.燕山大學工業計算機控制工程河北省重點實驗室，秦皇島，0660042.燕山大學國家冷軋帶鋼裝備及工藝工程技術研究中心，秦皇島，066004

0 引言

近年來，移動機器人的路徑規劃問題成為研究熱點，其主要思路是[1]根據能耗、路程、時間等性能指標，保證在存在障礙物的環境下，規劃出一條從初始位置到目標位置的最優路徑。在以往的研究中，根據環境信息的完全已知、部分未知或者完全未知等不同特點，通常把移動機器人的路徑規劃分為兩部分：全局路徑規劃[2-3]和局部路徑規劃[4-5]。

環境建模和路徑搜索是全局路徑規劃研究中的兩個重要問題。在研究環境建模時主要應用可視圖法[6]、拓撲法和柵格法。柵格法[7]是由柵格構成一個連通圖，依據障礙物的分布，用搜索算法(A*算法[8]、遺傳算法[9]、粒子群算法[10]、蟻群算法[11]等)生成從初始節點到目標節點的最優路徑。粒子群算法[12]利用較小種群模式，通過單向信息流機制搜索最優路徑。文獻[13]融合了最大最小蟻群算法與蟻群系統算法的信息素更新方式，設計了與環境模型相符合的適應度函數，實現了路徑規劃。但上述算法數據復雜，計算量大，很難快速規劃出一條平滑的最優路徑。作為一種典型的啟發式搜索算法，A*算法能夠遍歷所有可達節點，再根據估價函數來搜索較優路徑。文獻[14]提出跳點搜索策略，減少了遍歷過程中所需訪問的節點數，運行速度很快，但路徑中仍存在轉折點。由于移動機器人自身存在非完整約束，在轉折點處不能連續運動，為了解決路徑平滑問題，文獻[15]引入多項式曲線實現平滑路徑的生成，但存在曲率峰值過大問題。文獻[16-17]分別提出改進的三階Bezier曲線和改進Pseudo-Dubins曲線方法，這兩種方法均能實現路徑平滑，但算法的過程繁瑣，效率低。

規劃機器人的運動路徑時，若存在結果不可行的情況，一般會在全局規劃后進一步采用局部路徑規劃的算法，這種方法能夠有效提高系統的實時性。目前，主要應用模糊邏輯算法、遺傳算法和人工勢場法等方法來進行局部路徑規劃。其中，人工勢場法結構較為簡單，易于實現實時控制等優點，但該算法存在局部極小值問題。針對上述缺點，文獻[18-20]分別提出了建立多行為體系結構、添加虛擬水流法和增設虛擬中間目標點方法來解決局部極小值問題。但是針對上述方法的研究中，并沒有綜合考慮路徑長度、路徑安全性、轉彎角度等因素來對路徑進行處理，因此，由這種算法得出的路徑綜合適應性不高。

在動態復雜的未知環境中，傳統的路徑規劃方法很難達到最優路徑指標，因此，本文提出了一種將全局路徑規劃與局部路徑規劃相結合的混合路徑規劃算法。在該算法中，需要首先完成全局路徑規劃，然后在全局路徑的基礎上利用二次A*搜索和動態切點調整法優化已規劃路徑，最后根據環境信息的變化狀況，在全局路徑規劃的結果上，采用基于自適應步長調節方法的人工勢場法進行局部優化。該混合方法不但能根據已知的環境信息規劃出全局最優路徑，同時能在局部變化的環境中實現實時避障和動態目標追蹤，較好地克服了傳統算法復雜、計算量大、效率不高等問題。

本文以3自由度非完整輪式移動機器人作為研究對象，相比2自由度機器人，移動機器人不能實現全向移動，轉折點處不能連續直線運動，需要轉動一定角度調整姿態，對路徑規劃有了更多的限制，例如路徑中存在的轉折點個數以及轉動角度等，因此，研究非完整移動機器人的路徑規劃意義重大。

1 A*全局路徑規劃算法

1.1 環境模型描述

機器人的環境模型是對其進行控制的基礎，為了有效地描述機器人所處環境，將被控目標看作在二維平臺上移動的點狀物體，于是目標的運動軌跡可以為柵格地圖上的黑色塊，而可行區域則由白色塊表示。環境M由柵格Mij構成：

M={Mij,Mij=0,1,2,3}

其中，Mij=0表示移動機器人的起始位置單元柵格；Mij=1表示無障礙區域單元柵格；Mij=2表示障礙區域單元柵格；Mij=3表示目標所處位置。

柵格大小的選取對于整個路徑規劃是很重要的，柵格選取較大，計算量就會減少，但得到的路徑長度可能會增大；相反，柵格選取太小，路徑規劃的準確度就會提高，但規劃過程緩慢。柵格大小主要由實驗環境確定。柵格長度

其中，r為障礙物半徑，R為機器人半徑，δ為設定的安全距離。綜上所述，可以將移動機器人的路徑規劃問題概述為通過在已得到的環境模型中的無障礙區域搜索，得到一條由柵格組成的連續的最短路徑。

1.2 A*全局路徑規劃算法

A*算法[21]是一類搜索算法，它根據所定義的估價函數大小來確定最優路徑。代價函數表示為

f(n)=g(n)+h(n)

(1)

其中，n為當前節點；f(n)為節點n的估價函數；g(n)為從起始點到當前節點n的實際代價值；h(n)為當前節點n到目標點的估計代價值。一般來說h(n)為歐氏距離，其定義如下：

其中，(xn,yn)表示當前位置處柵格的中心坐標；(xgoal,ygoal)則表示目標柵格的中心。通過優化可以得到當前位置到目標位置的最短路徑。

傳統A*算法能夠有效地對目標進行全局路徑規劃，但其優化后得到的路徑冗余點較多，且該方法得到的運動路線折線多、轉折次數多、轉折角度大，這些缺陷嚴重影響了路徑規劃的效果。本文根據傳統A*算法的原理，進一步提出了一種新型A*算法。該算法利用二次A*搜索優化了目標路徑的長度，這種方法能夠僅保留路徑中的起點、轉折點和終點。對于存在非完整約束的移動機器人，其特性造成移動機器人不能在路徑中的轉折處進行直線運動，需要不斷轉動來調整自身的運動姿態，從而引起目標運動的角速度以及向心加速度發生變化。對此，本文提出了動態切點調整算法對路徑進行平滑處理。

1.2.1路徑點序列優化

在實現路徑優化過程中，為了縮短路徑長度，引入二次A*搜索算法。首先，將式(1)得到的估價函數作為初始值，并選取擴展點，擴展點的選擇方法是選取與路徑節點不相鄰的后續路徑點；進一步，以代價函數大小和是否穿過障礙作為判斷依據，來確定上述兩個節點之間是否存在新的路徑。判斷標準是，如果滿足代價函數小于初始值、無障礙物這兩個條件，則兩點之間構成新的路徑，并剔除中間節點，否則，路徑為原有路徑。最后，所生成的路徑為最短路徑，僅包含起點、轉折點和目標點。

對兩種A*算法在柵格地圖為10 m×10 m的情況下進行仿真對比，結果如圖1、圖2和表1所示。

圖1 一次A*算法尋優Fig.1 Linear A* algorithm optimization

圖2 二次A*算法尋優Fig.2 Twice A* algorithm optimization

機制累計轉彎角度(°)路徑長度(m)路徑長度降低率(%)轉彎角度降低率(%)一次A?算法18010.07二次A?算法112.629.842.2137.43

比較圖1、圖2和表1的仿真數據可知，二次A*算法有效剔除了冗余點和折線，減小了規劃路徑長度，有效減少了轉彎角度，實現了路徑優化。

1.2.2路徑平滑處理

二次A*算法可以規劃出一條由線段組成的完整路徑，而平滑的路徑更便于移動機器人的控制。本文利用動態切點調整算法去除凹凸點，該方法能夠得到既有曲率連續性，同時滿足幾何特性的路徑。路徑平滑示意圖見圖3。

圖3 路徑平滑示意圖Fig.3 Schematic diagram of the path smooth

機器人的初始位置為A1(x1,y1)，終點位置為An(xn,yn)。從初始位置開始，依次對Ai(xi,yi)(i=2,3,…,n-1)轉折點進行平滑處理。圖3中，單調地使用固定切點會使機器人陷入包含障礙物的死區，因此，本文將固定切點改為動態切點，提出了動態切點調整算法，具體步驟如下：

(1)比較Ai-1Ai和AiAi+1兩線段長度，選擇較短邊的端點P(xp,yp)為初始切點，過點P作垂線，與∠Ai-1AiAi+1(i=2,3,…,n-1)的角平分線AiQi-1相交于點Oi-1(x0,y0)：

x0=(xp+k01yp+k01(k0x2-y2))/(1+k0k01)

(2)

y0=k0(x0-x2)+y2

(3)

相切圓的半徑R可表示為

(4)

相切圓方程為

(x-x0)2+(y-y0)2=R2

(5)

式中，k01為較短邊斜率；k0為角平分線斜率。

(2)判斷相切圓是否與長邊之間有交點S，如果有，則轉至步驟(3)；否則，轉至步驟(4)。

(3)判斷圓弧PS上是否存在障礙物，如果存在，則轉至步驟(4)；否則，用圓弧PS替代Ai-1AiAi+1，并轉至步驟(5)；

(4)切點P(xp,yp)沿著所在線段移動到P2(xp2,yp2)，xp2可表示為

xp2=xp+λ|x2-xp|λ∈(0,1)

(6)

其中，λ依據實際情況設置。同時將P2設置為初始切點，并返回步驟(1)。

(5)判斷此步驟確定的路徑是否已經在環境模型中遍歷所有的路徑節點，若是，則返回步驟(1)，否則結束。

2 改進人工勢場局部路徑規劃算法

在機器人的移動運行過程中，能夠通過機器人機身攜帶的傳感器完成對所處局部環境變化的感知，根據實時檢測的障礙物或目標的變化情況，采用人工勢場法做出行為決策，及時避開障礙物，實現目標跟蹤。

人工勢場[22]由斥力勢場和引力勢場組成。在復雜的工作環境中，環境信息不斷變化，為了進一步滿足當前路徑規劃的需求，不能僅僅考慮機器人、目標位置和障礙三者之間的相對位置關系。為此，本文將機器人、目標與障礙三者間的相對速度關系作為優化路徑的指標。引力勢場函數Uatt可定義為

Uatt(q)=[mρ2(q,qg)+kv1ρ2(V,Vg)]/2

(7)

相應的引力Fatt可表示為

Fatt(q)=mρ(q,qg)+kv1ρ(V,Vg)

(8)

式中，m、kv1為引力場正常量；ρ(q,qg)為機器人與目標點之間的歐氏距離；ρ(V,Vg)為機器人和目標點之間的相對速度。

斥力勢場函數Urep可定義為

(9)

Urepv=kv2ρ2(V,Vobs)/2

(10)

(11)

相應斥力Frep可表示為

(12)

Frepv=kv2ρ(V,Vobs)

(13)

(14)

其中，k、kv2為斥力場正常量；ρ(q,qobs)表示機器人與障礙物兩點之間的歐氏距離；ρ(V,Vobs)表示機器人和動態障礙物之間的相對速度；ρ0為常數，表示障礙物對機器人產生作用的最大范圍；α為障礙物的相對移動方向和移動機器人與障礙物連線的夾角，當α∈(-π/2,π/2)時，障礙物的移動方向確定為靠近移動機器人。

2.1 虛擬子目標的增設

移動機器人在行進過程中，存在一種嚴重影響運行的局部最小值陷阱，即障礙物和目標點對移動機器人產生的排斥力和吸引力相等時，使得移動機器人不能順利抵達目標點。因此，本文提出增設虛擬子目標法。借助虛擬外力，使機器人擺脫陷阱，成功抵達目標點。具體方法如下。

(1)首先依據移動機器人的狀態信息判斷是否進入局部最小值陷阱，以連續五個步長作為判斷依據，當總的移動距離小于βL(L為步長，β∈[2,4])時，改變β，移動機器人可以提前檢測到自身陷入局部極小值陷阱。

(2)程序跳轉到虛擬子目標增設模塊，存儲使移動機器人陷入局部最小值陷阱的障礙物位置信息，形成障礙物群，充分考慮目標點位置與障礙物群位置信息，判斷障礙物在移動機器人與目標點連接線左右兩端數目多少，選擇障礙物群左側或右側為目標障礙物，然后利用下式得到虛擬子目標位置：

(15)

式中，(xob,yob)為目標障礙物位置；(xg,yg)為原目標所處位置；(x,y)為機器人的當前位置；β1,β2為可調參數，β1,β2>0。

移動機器人擺脫上述陷阱后，將撤出虛擬子目標，移動機器人不再受外力作用。移動機器人將在原目標和障礙物的合力作用下向原目標點靠近。

2.2 自適應步長調節

通過控制步長的變化，能夠使搜索更加靈活。在障礙物較少的情況下，增大步長可以降低機器人調整位姿的頻率，縮短運動時間。但是步長不能隨意改變，盲目增大步長會增加碰撞的概率。為此，本文使用自適應步長調節的方法來減少碰撞的概率。

這種算法需要選擇補償，主要考慮環境的復雜度以及機器人與目標的相對距離。機器人機身攜帶的傳感器，能夠探測到周圍障礙物數目以及障礙物與機器人自身的位置關系，由此判斷環境的復雜程度。本文設立的判定標準如下：當機器人探測到的障礙物數目大于設定值或機器人探測到障礙物與自身相對位置小于設定值時，環境即為復雜環境。在這種情況下，機器人步長取Lmin；而當機器人離目標很近時，為避免出現步長太大而超過目標，或者步長太小而無法跟蹤目標的情況，步長取Lmid；當機器人處于簡單環境時，步長取Lmax。這樣既節省運行時間，還能優化路徑。

采用改進的人工勢場法有效避免了傳統人工勢場法中目標不可達、易陷入局部極小值等問題，在動靜態障礙物干擾下，實現對動態目標的快速跟蹤。

3 仿真與分析

利用MATLAB仿真軟件對本文算法進行仿真，并與傳統算法進行仿真對比。

3.1 靜態障礙物+靜態目標

采用新型A*全局路徑規劃算法和基于三次B樣條函數的A*算法在柵格地圖為10 m×10 m的情況下進行仿真，結果如圖4、圖5所示，其中系數λ=0.1。

由圖4、圖5可以發現，利用兩種算法均可實現規劃一條起點至終點的平滑路徑。由圖5中的局部路徑放大圖可以看出，A*算法與三次B樣條函數法的路徑規劃算法在處理路徑平滑過程中，單獨使用三次B樣條函數會導致移動機器人陷入障礙物死區，并與障礙物發生碰撞，而本文提出的改進的A*全局路徑規劃算法在處理轉折點時，采用動態切點調整方法，因此可以規劃出平滑、安全的最優路徑。

圖4 改進的A*算法的路徑規劃Fig.4 Path planning for improved A* algorithm

圖5 三次B樣條函數和A*算法的路徑規劃Fig.5 Path planning of cubic B-splines function and A* algorithm

3.2 動靜態障礙物+動態目標

利用MATLAB軟件對比改進的人工勢場法與傳統方法，其中參數選取根據經驗法[23-25]、仿真環境和實驗環境得到。為了有效對比算法的先進性，仿真過程兩種算法的參數選取如下：m=2000，kv1=1000，k=100，kv2=1000，ρ0=0.5m，其他參數見表2，仿真結果如圖6、圖7和表3所示。

表2 實驗參數

圖6 改進的人工勢場法的路徑規劃Fig.6 Path planning for improved artificial potential field algorithm

圖7 傳統人工勢場法的路徑規劃Fig.7 Path planning for traditional artificial potential field algorithm

圖7中，移動機器人在障礙物附近存在不良震蕩現象。最終，運行步數為154。由圖6的仿真結果可以發現，采用自適應步長調節算法既可以安全躲避障礙物，提高實時性，又能節省運行時間?？傔\行步數為54。由表3可以看出，相比傳統人工勢場法，所提算法運行時間和路徑長度分別減少了18.6%和51.3%。

表3 算法仿真數據比較

3.3 動靜態障礙物+動態目標(特殊環境)

機器人、障礙物和目標位置處于同一條直線，且障礙物位于中間位置時，機器人很容易陷入局部極小值陷阱。采用改進的人工勢場法進行仿真實驗，結果如圖8、圖9和表3所示。

圖8 改進的人工勢場法的路徑規劃Fig.8 Path planning for improved artificial potential field algorithm

圖9 傳統人工勢場法的路徑規劃Fig.9 Path planning for traditional artificial potential field algorithm

由圖9可以看出，當引力和斥力一直處于同一條直線時，機器人無法有效地避開障礙物，一直在障礙物附近徘徊，無法向目標點移動?？傄苿硬綌禐?40。而在圖8中，當移動機器人檢測到陷入局部極小值陷阱時，增設的虛擬子目標及時的對移動機器人產生外力，幫助移動機器人擺脫了局部極小值陷阱，同時避免在障礙物群附近產生震蕩?？偟囊苿硬綌禐?1。由表3不難看出，相比傳統人工勢場法，改進的人工勢場法在運行時間上縮短了59.13%，在最優路徑長度上也減少了52.3%。

4 實驗

本實驗采用綠色圓形紙盒作為靜態障礙物，兩個3自由度QUANSER QBot2作為追蹤機器人和動態目標，整個實驗場地大小為2.5 m×3.0 m，柵格大小設定為0.6 m×0.6 m。機器人半徑為0.17 m，障礙物半徑為0.11 m，δ=0.15 m，實驗參數設定m=1 200，kv1=1 000，k=200，kv2=1 000，ρ0=0.3 m，其余參數設定同表2。其中，紅外攝像頭實時傳輸目標機器人的位置信息。實驗環境和實驗情況如圖10和圖11所示。

圖10 實驗設備Fig.10 Experimental Equipment

圖11 路徑規劃實驗情況Fig.11 Path planning experiments

圖11中，機器人從起點(圖11a)開始，在目標引力與障礙物斥力的合力作用下，實時完成避障(圖11b)，而且迅速跟蹤移動的目標機器人。值得注意的是，機器人自身存在非完整約束，且試驗場地地面較為光滑，故本實驗中，機器人運行的轉角較大。

5 結論

本文主要研究動態復雜非結構化環境下移動機器人的路徑規劃問題，提出了將全局規劃算法與局部規劃算法相融合的路徑規劃方法。基于新型A*算法，實現了移動機器人路徑規劃，利用二次A*算法與動態切點調整法對路徑進行平滑處理，同時依據機器人環境感知信息和全局路徑規劃結果，采用基于自適應步長調節算法的人工勢場法，實現了移動機器人局部路徑的動態規劃。最終移動機器人實現以較快速度沿著較短平滑路徑追蹤目標。通過對比分析，證明了本文所提算法的有效性。在后續的工作中，將結合圖像對更多復雜環境進行進一步改進分析。