兩階段搜索的A*全局路徑規(guī)劃算法

趙衛(wèi)東 蔣 超

(安徽工業(yè)大學(xué)電氣與信息工程學(xué)院 安徽 馬鞍山 243032)

0 引 言

隨著自動化和智能化技術(shù)的發(fā)展，移動機器人在生產(chǎn)和生活中的應(yīng)用越來越廣泛。移動機器人的路徑規(guī)劃問題是移動機器人研究的核心問題之一。所謂移動機器人路徑規(guī)劃[1]，就是當(dāng)移動機器人處于已知或未知的環(huán)境中，其根據(jù)自身傳感器反饋的信息來感知環(huán)境，從而避開障礙物自行規(guī)劃出一條最優(yōu)的安全運行路線。移動機器人路徑規(guī)劃研究最早始于20世紀(jì)60年代[2]。時至今日，移動機器人路徑規(guī)劃可分為傳統(tǒng)路徑規(guī)劃方法和現(xiàn)代路徑規(guī)劃方法。傳統(tǒng)的路徑規(guī)劃方法包括Dijkstra算法[3]、A*算法[4]、D*算法[5]等，現(xiàn)代路徑規(guī)劃算法包括蟻群算法[6]、遺傳算法[7]、神經(jīng)網(wǎng)絡(luò)算法[8]、模糊路徑算法[9]等。相較于蟻群算法等智能算法，傳統(tǒng)算法環(huán)境建模簡單，易于實現(xiàn)。

在眾多傳統(tǒng)算法中，A*算法是一種基于柵格地圖的路徑規(guī)劃算法，相比于主要應(yīng)用在環(huán)境未知的動態(tài)場景的D*算法，對于環(huán)境已知的靜態(tài)場景A*算法能更加有效地求得最優(yōu)路徑[10]。同時，相比于Dijkstra算法，A*算法采用了啟發(fā)式的搜索方式，大幅度降低了搜索節(jié)點的數(shù)量，從而極大地提高了搜索效率。但是，A*算法存在以下問題：其規(guī)劃出的全局路徑中易出現(xiàn)轉(zhuǎn)折點，而考慮到機器人的運動學(xué)限制，路徑中存在過多的轉(zhuǎn)折點對于機器人的運動控制是不利的。

本文重點研究了A*算法生成的全局路徑平滑處理以及提高該算法實時性的有效手段，采用兩階段搜索算法，通過選擇合適的估價函數(shù)，指導(dǎo)搜索朝著最有希望的方向前進(jìn)，以期獲得最優(yōu)路徑。

1 傳統(tǒng)的A*算法

A*算法是比較常用的移動機器人路徑規(guī)劃算法，它是一種啟發(fā)式搜索方法。A*算法中的評價函數(shù)定義為：

f(n)=s(n)+h(n)

(1)

式中：n表示機器人在路徑中的當(dāng)前位置節(jié)點；s(n)代表機器人從起始節(jié)點到當(dāng)前節(jié)點的實際花費代價和；h(n)是啟發(fā)函數(shù)，表示機器人從當(dāng)前節(jié)點到目標(biāo)節(jié)點的距離估計代價。通過估價函數(shù)來引導(dǎo)其搜索方向，傳統(tǒng)A*算法使用曼哈頓距離或者歐氏距離來定義啟發(fā)函數(shù)。曼哈頓距離表示兩個點在標(biāo)準(zhǔn)坐標(biāo)系上的絕對軸距總和，距離計算方向僅分為橫向和縱向，計算簡單但對實際距離存在明顯的高估。歐氏距離表示兩個點在坐標(biāo)系中的直線最短距離，是對實際距離很好的描述，所以采用歐氏距離來構(gòu)建啟發(fā)函數(shù)性能較好。即：

(2)

式中：nx、ny分別為當(dāng)前節(jié)點n在直角坐標(biāo)系中的橫坐標(biāo)和縱坐標(biāo)；g是目標(biāo)節(jié)點；gx、gy分別為目標(biāo)節(jié)點g在直角坐標(biāo)系中的橫坐標(biāo)和縱坐標(biāo)。

A*算法采用八鄰域搜索，如圖1所示。通過中心節(jié)點向附近8個鄰域擴散，然后通過每個鄰域的評價函數(shù)值的大小來選擇路徑方向，運動方向的角度也被限定為這8個方向。因此，規(guī)劃出來的路徑會有很多的轉(zhuǎn)折點，而對于移動機器人自身機械結(jié)構(gòu)所決定的運動限制來說，這些轉(zhuǎn)折點明顯不符合運動學(xué)原理，所以A*算法在實際工程應(yīng)用中效果并不理想。

圖1 八鄰域搜索

2 改進(jìn)的A*算法

A*算法折點過多，機器人運動軌跡不平滑的原因有兩種：1)采用八鄰域的搜索方式，其子節(jié)點與父節(jié)點之間的實際移動代價也是按照八鄰域方向計算的，這就造成了采用歐氏距離描述的兩點之間實際距離所估計的代價低于實際移動代價而導(dǎo)致算法擴展多余節(jié)點。2)評價函數(shù)只考慮到起點到當(dāng)前節(jié)點和當(dāng)前節(jié)點到終點的代價，并沒有考慮到機器人自身的運動學(xué)約束的代價。因此，本文首先改進(jìn)A*算法的評價函數(shù)，實現(xiàn)一級搜索，然后對一級搜索的結(jié)果進(jìn)行二級平滑處理，最終通過兩級搜索的方式實現(xiàn)對路徑的平滑糾正的目的。

2.1 優(yōu)化的啟發(fā)函數(shù)

根據(jù)A*算法的八鄰域搜索方向：橫向、縱向、對角線方向，把A*算法啟發(fā)函數(shù)hE(n)采用的歐氏距離分解成這三個方向的組合，這樣能夠更準(zhǔn)確地表示柵格地圖上任意兩點的實際移動代價，并使算法盡量處于僅尋得最佳路徑而不擴展其他擴展點的理想情況。優(yōu)化A*的啟發(fā)函數(shù)為：

(3)

優(yōu)化的啟發(fā)函數(shù)相對于使用曼哈頓距離的啟發(fā)函數(shù)不會高估到目標(biāo)點的移動代價，也不會像使用歐氏距離的啟發(fā)函數(shù)那樣低估移動代價。

2.2 角度約束函數(shù)

A*算法作為一種經(jīng)典的路徑規(guī)劃方法，通過結(jié)合機器人的幾何特征對地圖上的障礙物進(jìn)行輪廓膨脹，把機器人簡化為一個質(zhì)點，從而達(dá)到簡化計算的目的，所以A*忽略了機器人實體自身的運動特性，導(dǎo)致算法規(guī)劃出的路徑機器人執(zhí)行難度較大，甚至無法執(zhí)行。另外，在實際應(yīng)用中，機器人的路徑實施過程依賴于其傳感器定位，路徑存在過多的轉(zhuǎn)向需求(折點)也會令定位傳感器的觀測數(shù)據(jù)產(chǎn)生累計誤差，使機器人在地圖中自定位逐漸不準(zhǔn)確，甚至位置丟失，而引入角度約束可以有效地抑制這些問題。

綜合機器人自身的運動約束，機器人行進(jìn)方式一般為區(qū)域性向前，不會產(chǎn)生大幅度的轉(zhuǎn)向、頻繁且長時間的后退行為[11]，在路徑規(guī)劃的第一階段搜索過程中引入角度約束，使轉(zhuǎn)向行為具有一定的代價來抑制路徑轉(zhuǎn)折點的產(chǎn)生，即評價函數(shù)變?yōu)椋?/p>

f(n)=s(n)+h(n)+a(n)

(4)

新的評價函數(shù)在原有的基礎(chǔ)上增加了一個新的約束，該約束為機器人相對上一運動方向的轉(zhuǎn)角約束a(n)，其約束函數(shù)為：

(5)

式中：k為轉(zhuǎn)角約束系數(shù)，通過新的評價函數(shù)對當(dāng)前節(jié)點不同轉(zhuǎn)角的子節(jié)點的評價可以確定k的取值范圍，在此范圍內(nèi)，k越大，角度約束的效果就越明顯；θn為當(dāng)前時刻機器人的朝向角；θl為上一時刻機器人朝向角；l為單格步長。當(dāng)機器人上一時刻轉(zhuǎn)角與當(dāng)前時刻相同時，新的約束函數(shù)即為0，當(dāng)機器人轉(zhuǎn)角越大，則約束越大。圖2和圖3分別給出了A*算法和本文改進(jìn)算法的評價函數(shù)搜索方式。

圖2 傳統(tǒng)算法評價函數(shù)

圖3 改進(jìn)算法的評價函數(shù)

A*的評價函數(shù)在八鄰域搜索時并沒有考慮到機器人偏航角問題，步長與角度沒有關(guān)聯(lián)，容易出現(xiàn)偏航角折點。改進(jìn)后的A*算法添加了角度約束，轉(zhuǎn)偏航角度越大，則總步長越長，抑制了折點的產(chǎn)生，相比于傳統(tǒng)算法對路徑的平滑性方面的考慮上有較為明顯的提升。

2.3 二次搜索

通過上述改進(jìn)后，規(guī)劃路徑中仍然存在較多冗余點，為了獲得更優(yōu)路徑，對上述路徑進(jìn)行二次搜索，剔除不必要的節(jié)點，在保證減少折點個數(shù)的同時又縮短了總路徑的長度。具體操作步驟如下：

步驟一提取上一步驟規(guī)劃的全局路徑節(jié)點集P(N)，P(N)包含整個路徑的節(jié)點。

步驟二從起點節(jié)點P(1)向后遍歷P(N)中的所有節(jié)點，篩選出所有角度約束函數(shù)a(n)≠0的節(jié)點定義為折點，提取這些折點組成的節(jié)點集T(M)。

步驟三遍歷折點節(jié)點集T(i)，i=1,2,…，m，找到當(dāng)前節(jié)點T(i)相鄰的前后節(jié)點T(i-1)和節(jié)點T(i+1)，連接這兩個節(jié)點，判斷兩點連線是否穿過障礙物；若沒有，則剔除節(jié)點T(i)。

步驟四遍歷所有節(jié)點集T(m)，剔除所有冗余折點，得到最終節(jié)點集P(L)，P(L)作為最終的全局路徑節(jié)點集。

采用15×15的柵格地圖對上述算法進(jìn)行測試與仿真，仿真效果如圖4-圖6所示，其中：圖4是優(yōu)化啟發(fā)函數(shù)的A*算法的仿真結(jié)果；圖5是添加了角度約束的第一階段搜索結(jié)果；圖6是經(jīng)過二次搜索后的最終結(jié)果。圖中三角形標(biāo)志代表路徑起點，叉號標(biāo)志代表路徑終點。由圖中可知，傳優(yōu)化啟發(fā)函數(shù)的A*算法出現(xiàn)5個折點，15個擴展點，總轉(zhuǎn)折角度為675°。由圖5可知，對優(yōu)化啟發(fā)函數(shù)的A*算法加了角度約束的A*算法只出現(xiàn)了2個折點，也是15個擴展點，總轉(zhuǎn)折角度為270°，相對于傳統(tǒng)A*，在總擴展點不變的前提下轉(zhuǎn)折角度明顯降低。由圖6可知，采用Two-Stage(二級搜索)A*算法只出現(xiàn)了1個折點，擴展節(jié)點減少到6個，路徑累計轉(zhuǎn)折角度為121.45°。

圖4 優(yōu)化啟發(fā)函數(shù)的A*算法規(guī)劃仿真

圖5 添加角度約束的A*算法規(guī)劃仿真

圖6 兩階段A*算法規(guī)劃仿真

算法不同優(yōu)化階段的仿真結(jié)果如表1所示。改進(jìn)的兩階段A*算法通過引入新的約束函數(shù)以及剔除冗余折點的方式，相比于優(yōu)化啟發(fā)函數(shù)的A*算法，在路徑平滑性上有明顯提升并顯著減少了路徑點個數(shù)，累計轉(zhuǎn)折角度相比于傳統(tǒng)算法減小了82%，路徑點個數(shù)相比于傳統(tǒng)算法降低了60%，路徑的總長度相比于傳統(tǒng)算法縮短了2.89%。

表1 算法不同優(yōu)化階段的仿真結(jié)果

3 實 驗

為了驗證改進(jìn)A*算法在移動機器人路徑規(guī)劃中的實用性，將改進(jìn)A*算法應(yīng)用到如圖7所示的自主搭建的機器人平臺。該機器人平臺配備三個全向輪，可以進(jìn)行全方位的運動控制，其處理器采用Intel i3-4010U，采用德國SICK TIM561激光雷達(dá)安裝于機器人底盤前部用以獲取外界環(huán)境信息，其最遠(yuǎn)探測距離可達(dá)10 m。該實驗平臺基于ROS[12]操作系統(tǒng)，系統(tǒng)框架如圖8所示。利用自適應(yīng)蒙特卡洛方法進(jìn)行機器人定位，最后通過move_base程序模塊實現(xiàn)全局和局部路徑規(guī)劃，其輸入為機器人激光雷達(dá)點云、編碼器里程數(shù)據(jù)，輸出為機器人各個驅(qū)動輪的實時控制速度信息。

圖7 實驗平臺

圖8 改進(jìn)后的ROS系統(tǒng)框架

實驗采用ROS系統(tǒng)中GMapping[13-14]分別在兩種常見的現(xiàn)實場景中建立地圖并進(jìn)行全局路徑規(guī)劃。

測試場景1為狹長的走廊環(huán)境，地圖大小為10 m×10 m，測試場景及其建立的地圖如圖9所示。測試場景2為空間復(fù)雜度較高的大廳環(huán)境，地圖大小為12 m×8 m，測試場景及建立的地圖如圖10所示。建立出的地圖柵格分辨率為0.05 m×0.05 m，實驗設(shè)置機器人最大速度為0.5 m/s，最大轉(zhuǎn)角速度為0.6 rad/s。

圖9 實物測試場景1及建立的地圖

圖10 實物測試場景2及建立的地圖

同一機器人在同一地圖中取相同的起始點和目標(biāo)點的情況下場景1測試效果圖如圖11所示，場景2測試效果圖如圖12所示。圖中圓點表示機器人起點，白色區(qū)域中的曲線表示機器人的規(guī)劃路徑。可以看出，改進(jìn)后的算法規(guī)劃的路徑上轉(zhuǎn)折點更少，降低了不必要的路徑彎曲程度，整體上縮短了機器人所需要實現(xiàn)的位移。同時改進(jìn)后的算法減小了機器人在軌跡上偏航方向上的航向震蕩，使機器人運動過程更加平穩(wěn)。

(a)A*算法 (b)改進(jìn)A*算法

(a)A*算法 (b)改進(jìn)A*算法

實物測試結(jié)果如表2所示。相比傳統(tǒng)A*算法，改進(jìn)算法在擴展點個數(shù)上明顯降低。場景1中算法擴展點總個數(shù)降低了266個，規(guī)劃路徑的總長度減少了1.89 m，算法規(guī)劃過程耗時增加了0.028 s；場景2中算法擴展點總個數(shù)降低了181個，規(guī)劃路徑的總長度減少了0.92 m，算法規(guī)劃過程耗時增加了0.48 s。雖然改進(jìn)算法較優(yōu)化啟發(fā)函數(shù)的A*算法耗時增加，但可以生成最優(yōu)路徑，同時在很大程度上縮減了路徑長度，方便了機器人的運動控制，使路徑執(zhí)行過程耗時明顯減少。其在實驗場景1下路徑執(zhí)行過程耗時減少了6.73 s；在實驗場景2下路徑執(zhí)行過程耗時減少了2.46 s。在實驗設(shè)定的兩種常見的測試場景中，改進(jìn)的A*算法明顯優(yōu)于傳統(tǒng)A*算法。

表2 兩種場景的實物測試結(jié)果

4 結(jié) 語

為了解決A*算法在機器人全局路徑規(guī)劃中產(chǎn)生過多折點的問題，本文提出了一種兩階段搜索A*算法，其采用添加角度約束的代價評價策略進(jìn)行路徑搜索，進(jìn)而對搜索結(jié)果進(jìn)行線形優(yōu)化。理論和實驗結(jié)果表明，本文算法的啟發(fā)函數(shù)能夠更準(zhǔn)確地估計路徑代價，讓第一階段搜索的效果接近最理想狀態(tài)，減少了算法運算量，并加以角度約束抑制路徑轉(zhuǎn)折點的產(chǎn)生，不僅使路徑考慮到了機器人運動約束，還盡量壓縮了第二階段冗余折點的計算量。再通過第二階段搜索對路徑線形地進(jìn)一步優(yōu)化，在保證生成滿足機器人運動限制的最優(yōu)路徑的同時顯著縮短規(guī)劃的路徑長度。該算法在較大面積的環(huán)境中優(yōu)化效果更佳顯著。將改進(jìn)的A*算法應(yīng)用于自主搭建的機器人平臺上的實驗表明，兩階段搜索的改進(jìn)A*算法優(yōu)化效果明顯，且抑制了累計轉(zhuǎn)角所引起的傳感器累計誤差，優(yōu)化了機器人路徑執(zhí)行過程中的自定位，保證了路徑執(zhí)行的可靠性，滿足實際應(yīng)用要求。