移動機器人路徑規(guī)劃安全A*算法

葛文雅， 李平

(華僑大學 信息科學與工程學院， 福建 廈門 361021)

路徑規(guī)劃是移動機器人最重要的應用之一[1-5]，是移動機器人研究領域的熱點問題[6-9].移動機器人規(guī)劃的路徑應該是最優(yōu)、無沖突的，且有助于在復雜環(huán)境中完成某項任務，因此，路徑的安全性是首要考慮的問題.現(xiàn)有的路徑規(guī)劃方法主要分為傳統(tǒng)算法和智能算法兩類[10].傳統(tǒng)算法主要包括人工勢場法和可視圖法.人工勢場法是局部路徑規(guī)劃常用的方法，其將機器人工作的環(huán)境定義為勢場，目標點定義為引力源，障礙物定義為斥力源，引力源與斥力源產(chǎn)生的合力共同作用于機器人，使機器人在實時避障的同時向目標點前進.人工勢場法實時性好、數(shù)學原理易懂、規(guī)劃路徑平滑，但存在局部最優(yōu)、目標不可達及易抖動等問題，路徑規(guī)劃的全局性不強[11-12].可視圖法將機器人視為質點，將障礙物視為近似多邊形，通過線段連接質點、目標點和障礙物頂點(禁止連線穿過障礙物多邊形)，搜索起始點到目標點的最短線段集合.可視圖法直觀，容易求得最短路徑，適用于全局和連續(xù)區(qū)域內的路徑規(guī)劃，但可視圖法靈活性缺乏、搜索能力不足，不適用于大型動態(tài)環(huán)境，一般需要結合其他智能算法進行搜索[13-14].智能算法具有很好的全局性，在處理復雜動態(tài)環(huán)境信息情況下的路徑規(guī)劃問題時，來自于自然界的啟示往往能起到很好的作用，如蟻群算法、神經(jīng)網(wǎng)絡算法和遺傳算法[15-20].

傳統(tǒng)算法和智能算法都是把全局最短路徑作為搜索目的，規(guī)劃的路徑缺少路徑安全性的考慮.A*算法是一種啟發(fā)式搜索算法，在多節(jié)點的二維或三維地圖環(huán)境中，能夠搜索出最短路徑(最小通過代價).A*算法常被用于已知環(huán)境下的全局路徑規(guī)劃，其估值函數(shù)計算簡單，可以規(guī)劃出有效的最短路徑，因此，A*算法被廣泛地應用于復雜環(huán)境下的路徑規(guī)劃問題[21-23].然而，A*算法規(guī)劃的路徑和障礙物相鄰，路徑轉角角度過大，機器人在跟蹤規(guī)劃的路徑時，存在與障礙物碰撞和側翻的危險，安全性無法得到保障.Park等[24]提出改進的 A*算法，用配置空間計算模型處理潛在風險，并將風險成本引入A*啟發(fā)式函數(shù)中，保證路徑和障礙物之間留有安全距離.Bayili等[25]提出有限損傷A*算法，該算法以損傷為可行性準則，考慮碰撞風險，以獲得更安全的路徑.Sun等[26]提出一種模糊路徑規(guī)劃算法，通過約束機器人的角速度，保證規(guī)劃路徑的安全性.然而，對于移動機器人路徑規(guī)劃時存在路徑距離障礙物過近和路徑轉角角度過大等問題，上述方法仍考慮得不夠周全.基于此，本文對A*算法進行改進，提出一種移動機器人路徑規(guī)劃安全A*算法.

1 柵格法環(huán)境建模

采用柵格法[27]構建地圖模型，柵格法是移動機器人環(huán)境建模的常用方法.柵格法將移動機器人的工作空間劃分為網(wǎng)格單元.分辨率是度量柵格地圖準確度的一項關鍵參數(shù)，它將一張圖片分別按橫軸、縱軸劃分x×y的像素點.柵格地圖的準確度與分辨率成正比，文中構建的柵格地圖分辨率為100×100.柵格地圖模型，如圖1所示.圖1中：黑色區(qū)域為障礙物信息；白色區(qū)域為機器人可移動區(qū)域.將地圖信息存放于矩陣M(i，j)中，i，j分別為矩陣中的行、列，則有

圖1 柵格地圖模型Fig.1 Grid map model

(1)

在構建柵格地圖時，每一個柵格都與實際環(huán)境地圖中的位置一一對應.假設在一個分辨率為N×N的柵格地圖中，移動機器人的坐標為(xg，yg)，有

(2)

式(2)中：O為網(wǎng)格序列編碼；mod為取余函數(shù)，使用該函數(shù)，可得到網(wǎng)格序列編碼O所在的列數(shù)，即移動機器人的橫坐標xg；int為取整函數(shù)，使用該函數(shù)，可得到網(wǎng)格序列編碼O所在的行數(shù)，即移動機器人的縱坐標yg；每個網(wǎng)格單位為1，加上0.5表示機器人的坐標位于網(wǎng)格的中心.

2 A*算法及其存在的問題

A*算法是靜態(tài)圖搜索最短路徑最有效、直接的方法，啟發(fā)式搜索綜合了迪杰斯特拉(Dijkstra)算法的廣度優(yōu)先及貪婪搜索的做法.A*算法通過待擴展節(jié)點open表和已擴展節(jié)點close表對節(jié)點進行搜索，節(jié)點總是朝著啟發(fā)式函數(shù)最小的方向進行擴展.

在A*算法的距離選取中，對于柵格地圖上的任意兩點(x1，y1)，(x2，y2)的距離，常見的度量方式為歐幾里德距離DEuc、曼哈頓距離DMan和切比雪夫距離DChe，這3種度量方式的計算公式分別為

(3)

DMan=|x2-x1|+|y2-y1|，

(4)

DChe=max (|x2-x1|，|y2-y1|).

(5)

經(jīng)對比分析后可知，歐幾里德距離更符合路徑的長度，因此，A*算法選用歐幾里德距離進行度量.后續(xù)節(jié)點的擴展涉及搜索鄰域的問題，在柵格地圖中，A*算法采用4鄰域或8鄰域進行搜索.4鄰域及8鄰域的搜索方向[28]，如圖2，3所示.4鄰域規(guī)劃的路徑經(jīng)過每個柵格的中心點；8鄰域規(guī)劃的路徑不僅經(jīng)過每個柵格的中心點，也經(jīng)過每個柵格的頂點.每個柵格的中心點被假設為節(jié)點，節(jié)點臨近的4個或8個區(qū)域都是該節(jié)點的擴展個數(shù)，運動方向的角度也被限定為π/2或π/4的整數(shù)倍.

圖2 4鄰域的搜索方向 圖3 8鄰域的搜索方向 Fig.2 Four neighborhood search direction Fig.3 Eight neighborhood search direction

以4鄰域為例，其A*算法搜索路徑規(guī)劃，如圖4所示.圖4中：所選地圖為無障礙地圖，給定起始點和目標點；藍色路徑(圖4(b))為由A*算法得到的自動最佳路徑，路徑有轉折點，較為復雜，并非起始點與目標點之間的最佳期望路徑.因此，在柵格地圖上，A*算法使用4鄰域進行路徑規(guī)劃時，存在以下2個問題:1) 由于鄰域的限制，規(guī)劃路徑與實際最短路徑的差距較大；2) 規(guī)劃路徑的轉角角度相對較大，對路徑安全性影響較大，安全性不高.

(a) 搜索區(qū)域 (b) 規(guī)劃路徑 圖4 A*算法4鄰域搜索路徑規(guī)劃Fig.4 Four neighborhood search path planning of A* algorithm

啟發(fā)式函數(shù)是A*算法的重要參數(shù)，A*算法的搜索效率與其密切相關，A*算法在分析時采用的估價函數(shù)[29]為

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

(6)

式(6)中：f(n)表示從起始點經(jīng)由節(jié)點n到目標點的估價函數(shù)；g(n)為基本項，表示狀態(tài)空間中從起始點到節(jié)點n的實際代價；h(n)為啟發(fā)項(啟發(fā)式函數(shù))，表示節(jié)點n到目標點的估算代價．

然而，A*算法的啟發(fā)式函數(shù)中的樣本信息較為單一，缺少待擴展節(jié)點周圍障礙物的信息，從而導致A*算法的規(guī)劃路徑與障礙物距離過近，路徑安全性較低.

3 安全A*算法

A*算法的搜索區(qū)域和規(guī)劃路徑，如圖5，6所示.由圖5，6可知：A*算法規(guī)劃的路徑轉角角度較大，基本為90°的折角，部分路段和障礙物距離過近，移動機器人在實際環(huán)境行走時的安全性較低.因此，提出安全A*算法，主要改進以下3個方面的內容:1) 擴展搜索鄰域；2) 改進啟發(fā)式函數(shù)，加入安全項，增加樣本信息；3) 對路徑安全性進行量化，提出安全性指數(shù)S.

圖5 A*算法的搜索區(qū)域 圖6 A*算法的規(guī)劃路徑 Fig.5 Search area of A* algorithm Fig.6 Planning path of A* algorithm

3.1 搜索鄰域的擴展

針對A*算法路徑規(guī)劃存在轉角角度過大的問題，安全A*算法將節(jié)點n的搜索鄰域擴展至24個.搜索鄰域擴展過程如下：對節(jié)點n(x，y)遍歷臨近的24個節(jié)點(圖7，中間黑色區(qū)域為節(jié)點n，周圍白色區(qū)域為其遍歷的臨近節(jié)點，將臨近節(jié)點作為后續(xù)節(jié)點m(x′，y′)，m={(x′，y′)|x′=[x-2，x+2]&y′=[y-2，y+2]}，箭頭為路徑方向).如果后續(xù)節(jié)點m為障礙點，或后續(xù)節(jié)點m在close表中，則跳過，選取下一個臨近點作為后續(xù)節(jié)點；如果后續(xù)節(jié)點m既不是障礙點，又不在close表中，則計算f(m)，判斷m是否在open表中，若判斷失敗，則把后續(xù)節(jié)點m放入open表，若判斷成功，則選取較小的f(m)，更新g(m)，f(m)及后繼節(jié)點m的前向指針.

圖7 24鄰域的搜索方向 圖8 搜索鄰域擴展A*算法的規(guī)劃路徑 Fig.7 Twenty-four neighborhood search direction Fig.8 Planning path of A* algorithm after search neighborhood expansion

經(jīng)搜索鄰域擴展后的A*算法(即搜索鄰域擴展A*算法)的規(guī)劃路徑，如圖8所示.由圖8可知：搜索鄰域擴展A*算法的路徑轉角角度減小較多，路徑安全性可得到一定的改善.

3.2 啟發(fā)式函數(shù)的改進

擴展A*算法節(jié)點n的搜索鄰域可以減小路徑的轉角角度，在移動機器人的實際運動中提高了路徑安全性.然而，路徑和障礙物的距離仍然很近，路徑的安全性還有很大的提升空間.A*算法最核心的部分在于啟發(fā)式函數(shù)h(n)的設計，改進A*算法時利用了A*算法對當前軌跡節(jié)點進行評估的思想，增加了節(jié)點及一定范圍內障礙物的信息，增大了路徑和障礙物的距離，從而使安全A*算法具有更高的路徑安全性.

安全A*算法的估價函數(shù)可以表示為

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

(7)

h′(n)=h(n)+βL(n)，

(8)

(9)

式(9)中：h′(n)為安全A*算法的啟發(fā)式函數(shù)；L(n)為當前臨時節(jié)點對應的危險評估值(安全項)；t為評價范圍內障礙物的個數(shù)；s為機器人與障礙物的最小距離；β為安全項L(n)的權重，文中設定為100.

安全A*算法的核心思想是在A*算法的基礎上，引入一項安全項L(n)，作為評價該節(jié)點處于最優(yōu)軌跡路線的可能性度量.在移動機器人的運動過程中，評價范圍內障礙物的個數(shù)t越小越好，而移動機器人和周圍障礙物的最小距離s越大越好，從而更好地保證機器人移動時的路徑安全性.因此，采用t和s的比值表示安全項，以反映路徑安全程度.當評價范圍內障礙物的個數(shù)t較大，與障礙物的距離較小時(t較大，s較小)，L(n)的數(shù)值較大，對h′(n)的影響增大，使該節(jié)點的估價函數(shù)值較大，導致選擇該節(jié)點的可能性變??；反之，L(n)的數(shù)值較小，對h′(n)的影響減小，使該節(jié)點的估價函數(shù)值較小，導致選擇該節(jié)點的可能性增大.

改進后的估價函數(shù)f(n)使A*算法減少了遍歷節(jié)點的個數(shù)，提高了A*算法的搜索效率，搜索方向可以更快地趨近于目標點，遠離障礙物.

安全A*算法的規(guī)劃路徑，如圖9所示.由圖9可知：安全A*算法具有更高的路徑安全性.

圖9 安全A*算法的規(guī)劃路徑Fig.9 Planning path of safe A* algorithm

3.3 安全性的量化

為了更好地評估算法的路徑安全性，將以后續(xù)節(jié)點m為中心的評價范圍內的障礙物與其的最短距離、評價范圍內障礙物的個數(shù)作為該階段影響路徑安全性的因素，提出安全性指數(shù)S，相應的安全性量化規(guī)則表，如表1所示.

表1 安全性量化規(guī)則表Tab.1 Security quantification rule table

選取評價范圍為7×7的評價矩陣P，即

上式中：1表示評價范圍內的節(jié)點；2表示后續(xù)節(jié)點m.

3.4 安全A*算法的路徑規(guī)劃

安全A*算法在執(zhí)行路徑規(guī)劃時主要通過open表和close表實現(xiàn)節(jié)點的擴展和最優(yōu)節(jié)點的選取.open表保存搜索過程中遇到的擴展節(jié)點，并將這些節(jié)點按估價函數(shù)值進行排序；close表保存open表中估價函數(shù)值最小的后續(xù)節(jié)點；safe表保存后續(xù)節(jié)點的安全性指數(shù).

安全A*算法路徑規(guī)劃過程有以下7個步驟.

1) 將起點納入open表，將障礙點納入close表.

2) 選取open表中估價函數(shù)值最小的節(jié)點n，將其納入close表中，將節(jié)點n的安全性指數(shù)S納入safe表.

3) 判斷n是否為目標點，若n為目標點，則根據(jù)它的前向指針生成最優(yōu)路徑；若n不是目標點，則擴展節(jié)點n，生成后續(xù)節(jié)點m.

4) 在open表中建立從后續(xù)節(jié)點m返回n的指針，計算

f(m)=g(m)+h′(m)，

(10)

h′(m)=h(m)+βL(m)，

(11)

L(m)=t/s.

(12)

5) 增加判斷語句，判斷open表中是否已有后續(xù)節(jié)點m，如果判斷失敗，則將節(jié)點m納入open表，并將節(jié)點m的安全性指數(shù)S納入safe表；若判斷成功，則比較不同前向指針的f(m)的大小，并選取較小的f(m).

6) 更新g(m)，f(m)及后續(xù)節(jié)點m的前向指針.

7) 按照數(shù)值大小進行正序排列，將估價函數(shù)值在open表中重新排序，并返回步驟2).

4 仿真結果與分析

選用3種障礙物復雜程度的地圖環(huán)境(簡單地圖、低復雜地圖和高復雜地圖)在MATLAB中進行仿真實驗.

從規(guī)劃路徑、搜索區(qū)域及安全性曲線3個方面，對A*算法、搜索鄰域擴展A*算法和安全A*算法(文中算法)進行對比分析，結果如圖10～12所示.圖11中：灰色區(qū)域為搜索區(qū)域.

由圖10～12可知：與A*算法相比，文中算法在路徑規(guī)劃、搜索區(qū)域及安全性曲線3個方面都具有明顯優(yōu)勢.

(a) 簡單地圖(A*算法) (b) 簡單地圖(搜索鄰域擴展A*算法) (c) 簡單地圖(文中算法)

(d) 低復雜地圖(A*算法) (e) 低復雜地圖(搜索鄰域擴展A*算法) (f) 低復雜地圖(文中算法)

(g) 高復雜地圖(A*算法) (h) 高復雜地圖(搜索鄰域擴展A*算法) (i) 高復雜地圖(文中算法)圖10 3種算法路徑規(guī)劃的對比Fig.10 Comparison of path planning of three algorithms

(a) 簡單地圖(A*算法) (b) 簡單地圖(搜索鄰域擴展A*算法) (c) 簡單地圖(文中算法)

(d) 低復雜地圖(A*算法) (e) 低復雜地圖(搜索鄰域擴展A*算法) (f) 低復雜地圖(文中算法)

(g) 高復雜地圖(A*算法) (h) 高復雜地圖(搜索鄰域擴展A*算法) (i) 高復雜地圖(文中算法)圖11 3種算法搜索區(qū)域的對比Fig.11 Comparison of search area of three algorithms

(a) 簡單地圖(A*算法) (b) 簡單地圖(搜索鄰域擴展A*算法) (c) 簡單地圖(文中算法)

(d) 低復雜地圖(A*算法) (e) 低復雜地圖(搜索鄰域擴展A*算法) (f) 低復雜地圖(文中算法)

(g) 高復雜地圖(A*算法) (h) 高復雜地圖(搜索鄰域擴展A*算法) (i) 高復雜地圖(文中算法)圖12 3種算法安全性曲線的對比Fig.12 Comparison of security curves of three algorithms

在規(guī)劃路徑方面，對比圖10(a)和10(b)，圖10(d)和10(e)，圖10(g)和圖10(h)可知：在3種復雜程度不同的地圖上，相較于A*算法，搜索鄰域擴展A*算法的規(guī)劃路徑具有更小的轉角角度，路徑的平滑性更高，安全性得到了提高，但搜索鄰域擴展A*算法沒有改善規(guī)劃路徑和障礙物之間距離過近的問題，甚至在簡單地圖中，搜索鄰域擴展A*算法的規(guī)劃路徑和障礙物之間的距離更近，這是因為搜索鄰域擴展A*算法的擴展領域通過減少路徑折角使路徑變得更加平滑，以此提高安全性，未考慮規(guī)劃路徑和障礙物之間的距離.對比圖10(b)和圖10(c)，圖10(e)和圖10(f)，圖10(h)和圖10(i)可知：文中算法在啟發(fā)式函數(shù)中引入安全項，增大了路徑和障礙物之間的距離，因此，文中算法不僅具有更小的轉角角度，路徑的平滑性更高，而且能夠適應不同的地圖，和障礙物保持一定的距離，具有更高的安全性.

在搜索區(qū)域方面，由圖11可知：相較于A*算法和搜索鄰域擴展A*算法，文中算法的搜索區(qū)域較小，尤其是在高復雜地圖中，文中算法的搜索區(qū)域比A*算法縮小了很多.

3種地圖環(huán)境下搜索節(jié)點數(shù)的對比，如表2所示.表2中：c為搜索節(jié)點數(shù).由表2可知：在簡單地圖中，文中算法搜索的節(jié)點數(shù)和搜索鄰域擴展A*算法相仿，但比A*算法減少了78.5%；在低復雜地圖中，文中算法搜索的節(jié)點數(shù)分別比搜索鄰域擴展A*算法、A*算法減少了33.7%和35.5%；在高復雜地圖中，文中算法搜索的節(jié)點數(shù)分別比搜索鄰域擴展A*算法、A*算法減少了68.0%和74.3%.因此，文中算法具有更低的時間成本和空間成本.

表2 3種地圖環(huán)境下搜索節(jié)點數(shù)的對比Tab.2 Comparison of search node number in three map environments

在安全性曲線方面，由圖12可知：A*算法經(jīng)過搜索鄰域擴展后的安全性曲線變得更加雜亂，安全指數(shù)S下降；低復雜地圖和高復雜地圖的搜索鄰域擴展A*算法安全性指數(shù)(圖12(e)，12(h))降至2，路徑安全性變得更差.這是因為A*算法經(jīng)搜索鄰域擴展后只改善了路徑轉角角度過大的問題，并未考慮對路徑與障礙物之間的距離，而文中算法在搜索鄰域擴展A*算法的基礎上，對啟發(fā)式函數(shù)進行改進，引入安全項，使規(guī)劃路徑和障礙物之間的距離增加，安全性曲線得到優(yōu)化，安全性指數(shù)明顯提高，即使在高復雜地圖中，文中算法的安全性指數(shù)也能夠保持在9以上，表明文中算法具有更高的安全性.

對路徑規(guī)劃而言，安全性是最重要的考慮指標，在安全性得到保證的前提下，應盡量縮短路徑的長度(l，柵格地圖中無量綱).A*算法以路徑長度為主要指標規(guī)劃路徑，選擇長度較短，但距離障礙物較近的路徑節(jié)點，由于未考慮周圍障礙物的信息，導致路徑安全性過低.文中算法將節(jié)點周圍障礙物的信息引入了啟發(fā)式函數(shù)，把安全性作為主要的衡量指標，在路徑規(guī)劃時，選擇與障礙物有一定距離且路徑較短的節(jié)點，其路徑長度雖比A*算法略有增加，但可以保證路徑安全性.由于安全性指標更為重要，所以通過一定的路徑長度換取安全性是值得的.

3種地圖環(huán)境下規(guī)劃路徑長度的對比，如表3所示.由表3可知：在簡單地圖、低復雜地圖和高復雜地圖中，搜索鄰域擴展A*算法的路徑長度比A*算法分別減少了24.4%，17.3%和8.6%；文中算法雖然犧牲了一定的路徑長度，但路徑安全性得到了較大改善，比搜索鄰域擴展A*算法的路徑長度略有增加，比A*算法的路徑長度略有減少.

表3 3種地圖環(huán)境下規(guī)劃路徑長度的對比Tab.3 Comparison of planned path length in three map environments

5 結束語

針對路徑規(guī)劃安全性不高的問題，對A*算法進行相應改進，以柵格環(huán)境為基礎，對搜索鄰域進行擴展，減小轉角角度，避免不必要的折角；針對啟發(fā)函數(shù)的選取，引入周圍障礙物的信息作為評估要素，使樣本信息更充分，增加了路徑和障礙物之間的距離；對路徑進行安全性量化，更好地對路徑安全性進行評估.在MATLAB軟件上進行仿真實驗，結果表明，相較于A*算法和搜索鄰域擴展A*算法，文中算法的路徑質量和安全性更佳.