基于方向和步長約束的安全A*算法

陳 嬌，向建平，劉 卿

（西南交通大學(xué) 交通運輸與物流學(xué)院，四川 成都 611756）

1 引言

目前，移動機器人已經(jīng)廣泛應(yīng)用在制造業(yè)、醫(yī)療、物流、居家等多個領(lǐng)域，移動機器人的研究成為熱點問題，而路徑規(guī)劃是移動機器人技術(shù)中的重點和難點問題。路徑規(guī)劃是指為移動機器人規(guī)劃一條從起點到終點無障礙，并付出最小代價的最優(yōu)或者次優(yōu)路徑。路徑規(guī)劃的核心問題是路徑規(guī)劃算法，目前路徑規(guī)劃算法主要分為傳統(tǒng)路徑規(guī)劃算法和智能路徑規(guī)劃算法。傳統(tǒng)路徑規(guī)劃算法主要有人工勢場法[1]、A*[2]、快速擴展隨機樹[3]等，智能路徑規(guī)劃算法主要有遺傳算法[4]、蟻群算法[5]、神經(jīng)網(wǎng)絡(luò)算法[6]等。在路徑規(guī)劃的算法中，A*算法因其簡單、適用于大部分環(huán)境等優(yōu)點，在移動機器人全局路徑規(guī)劃中得到廣泛應(yīng)用。但使用A*算法規(guī)劃的路徑也存在非最短路、平滑度低等缺點，針對A*算法的缺點，國內(nèi)外學(xué)者從多方面進行了改進。

程傳奇等提出了一種融合改進A*算法和動態(tài)窗口法的全局動態(tài)路徑規(guī)劃方法，針對傳統(tǒng)A*算法，設(shè)計了一種關(guān)鍵點選取策略，再結(jié)合動態(tài)窗口法進行實時路徑規(guī)劃，使路徑更加平滑[7]，但該算法未對路徑長度實現(xiàn)優(yōu)化，仍然存在冗余路段。Pei Cao等提出了Any-Angle A*算法，該算法基于可視圖，在起點到目標點的連線附近進行搜索，減少了搜索節(jié)點，縮短了計算時間[8]，但它只適用于可視圖，目前的適用性小，并且尚未考慮到機器人的體積進行防撞處理。文獻[9]結(jié)合跳點搜索算法對A*算法進行改進，對尋路過程中的關(guān)鍵節(jié)點進行預(yù)處理，使用跳點代替搜索過程中大量的不必要節(jié)點，提高了路徑規(guī)劃的效率，但未解決傳統(tǒng)A*算法規(guī)劃的路徑折點多、平滑度低等問題。張紅梅等根據(jù)節(jié)點與障礙物的最小距離安全威脅代價，在估價函數(shù)中加入安全威脅代價，并對規(guī)劃路徑進行平滑優(yōu)化處理，改進后的A*算法在簡單環(huán)境中規(guī)劃的路徑更短，安全性和平滑程度提高[10]，但當路徑規(guī)劃的環(huán)境模型增大、復(fù)雜度加大時，改進算法出現(xiàn)規(guī)劃的路徑更長、轉(zhuǎn)彎次數(shù)較多的問題。

上述改進算法對傳統(tǒng)A*算法從不同的側(cè)重進行了改進，但未能完全解決傳統(tǒng)A*算法所規(guī)劃路徑折點多、路徑冗余、路徑平滑度低的問題，并且沒有考慮到機器人和障礙物的實際大小，不適用于移動機器人在復(fù)雜場景中進行實際應(yīng)用。針對上述問題，本文從搜索方向和安全性兩個方面對傳統(tǒng)的A*算法進行改進，利用改進A*算法規(guī)劃的路徑實現(xiàn)了長度、安全性、平滑度等多方面的綜合優(yōu)化，最后通過實驗仿真證明了改進A*算法的可行性和有效性。

2 問題描述

2.1 環(huán)境建模

A*算法屬于遍歷性啟發(fā)式算法，在進行路徑規(guī)劃之前，需要根據(jù)所得環(huán)境對路徑搜索區(qū)域進行環(huán)境建模?？紤]到移動機器人在行進過程中的安全性，本文將環(huán)境中簡單障礙物膨脹成為邊長為1的單位矩形，大型的復(fù)雜障礙物區(qū)域由多個規(guī)則的單位矩形構(gòu)成。結(jié)合障礙物膨脹方法和柵格法，隨機建立移動機器人路徑規(guī)劃區(qū)域的柵格地圖，如圖1所示。

圖1中，柵格地圖分為空閑區(qū)域和占據(jù)區(qū)域，白色柵格區(qū)域為空閑區(qū)域，移動機器人在行進過程中在保證不與障礙物發(fā)生碰撞的前提下，可以在空閑區(qū)域的任意移動位置和方向通行或停留；黑色區(qū)域為障礙物區(qū)域，移動機器人在行進過程中不能與障礙物發(fā)生碰撞或穿越障礙物，且最好與障礙物保持一定的安全距離。

圖1 路徑規(guī)劃柵格地圖

2.2 傳統(tǒng)A＊搜索方向

傳統(tǒng)A*算法主要有四個搜索方向和八個搜索方向，如圖2所示，圖中圓點表示移動機器人當前位置，實線箭頭表示機器人在移動過程中的搜索方向。四方向A*算法的搜索方向為上、下、左、右四個，移動機器人的最小轉(zhuǎn)角為，單次移動步長為1；八方向A*算法的搜索方向在上、下、左、右四個方向的基礎(chǔ)上加入了左上、左下、右上、右下四個，移動機器人的最小轉(zhuǎn)角為，單次移動步長為1或。

圖2 傳統(tǒng)A＊算法的搜索方向

在無障礙物的環(huán)境中，算法的搜索方向即為移動機器人的可移動方向；在有障礙物的環(huán)境中，移動機器人的搜索方向不變，移動方向為無障礙物的方向，如圖3所示。

圖3 傳統(tǒng)A＊算法在障礙物環(huán)境中的可移動方向

觀察圖3可知，傳統(tǒng)A*算法在障礙物環(huán)境中的可移動方向十分有限，在路徑規(guī)劃過程中有可能錯過最佳路徑的方向。利用傳統(tǒng)八搜索方向A*算法在圖1所示的柵格環(huán)境中進行路徑規(guī)劃的結(jié)果如圖4所示。由圖4可以看出，傳統(tǒng)A*算法所規(guī)劃路徑存在多個與障礙物距離過近的危險節(jié)點，移動機器人運行過程中可能與環(huán)境中的物品發(fā)生碰撞，安全性低。由圖4還可以看出，該路線曲率不連續(xù)，平滑度低，不符合移動機器人的運動規(guī)則。

圖4 傳統(tǒng)八方向A＊算法路徑圖

3 改進的A＊算法

3.1 搜索方向和步長限制

利用傳統(tǒng)八搜索方向A*算法進行移動機器人的路徑規(guī)劃時存在許多不足，一方面移動機器人在路徑規(guī)劃的過程中搜索方向有限，使移動機器人可能錯過最佳的行進方向，從而偏離最優(yōu)路徑；另一方面?zhèn)鹘y(tǒng)A*算法規(guī)劃路徑的最小轉(zhuǎn)角為轉(zhuǎn)彎半徑較大，路徑平滑度低，不利于移動機器人的實際運行。本文基于傳統(tǒng)A*算法存在的不足，對傳統(tǒng)A*算法的搜索方向和步長進行改進。

針對算法的搜索方向，在當前節(jié)點尋找下一拓展節(jié)點時，在傳統(tǒng)A*算法原有的八個搜索方向的基礎(chǔ)上，在兩個相鄰方向中間增加一個新的搜索方向，算法的最小搜索角度從縮小到，算法的搜索方向增加到16個。隨著算法搜索方向的增加，算法的搜索節(jié)點、機器人可移動方向及機器人的單次移動步長也進行相應(yīng)的改進。改進算法的搜索節(jié)點與算法的搜索方向同步增加為16個，各節(jié)點之間的間隔從1縮小到0.5；改進算法的機器人單次移動步長在傳統(tǒng)A*算法1、 2的基礎(chǔ)上，增加，即機器人單次移動步長可選范圍變?yōu)橐詧D1所示的柵格地圖為例，將(-1,-1)與(1,1)之間的柵格區(qū)域局部放大，假設(shè)點(0,0)為當前節(jié)點，改進A*算法的搜索方向和搜索節(jié)點如圖5所示。

圖5 改進的A＊算法搜索方向和搜索節(jié)點

由圖5可以看出，在搜索方向上，改進A*算法在傳統(tǒng)A*算法的已有八個搜索方向（實線箭頭表示）的基礎(chǔ)上，新增了8個搜索方向（虛線箭頭表示），搜索節(jié)點也相應(yīng)增加至16個，新增的八個搜索方向上的單次移動步長為

3.2 路徑安全性改進

在使用A*算法進行路徑規(guī)劃時大多把移動機器人看作是質(zhì)點，未考慮機器人和障礙物的自身體積，導(dǎo)致移動機器人在行進中與障礙物的距離過近，容易與障礙物發(fā)生碰撞或擦劃等安全性問題。針對傳統(tǒng)A*算法所規(guī)劃路徑存在的上述問題，本文提出一種路徑安全性檢測算法對傳統(tǒng)A*算法進行安全性改進，使移動機器人在路徑規(guī)劃過程中，能夠?qū)ふ衣窂阶疃痰慌c障礙物發(fā)生碰撞的安全路徑。

首先結(jié)合實際，針對圓形、矩形、其他多邊形等外形較規(guī)則移動機器人，獲取移動機器人前、后、左、右共四個方向最外徑到重心的距離l{li,lb,ll,lr}，以圖6（a）、圖6（b）所示的常見矩形輪式和圓形輪式機器人為例，移動機器人最外徑到重心的距離求解示意圖如圖6（c）、圖6（d）所示。

圖6 移動機器人俯視圖和外徑到重心距離

獲取移動機器人的距離屬性l{li,lb,ll,lr}后，再結(jié)合改進A*算法進行路徑規(guī)劃，對傳統(tǒng)A*算法安全性改進的具體步驟如下：

（1）獲取移動機器人前、后、左、右共四個方向最外圍到重心的距離l{li,lb,ll,lr}；

（2）計算待選子節(jié)點與周圍障礙物之間的最短距離，計算公式如下：

式中：xi(xi,yi)為待選節(jié)點，oj(xoj,yoj)為xi周圍的障礙物；d(xi)為xi與oj之間的距離；X為包含所有xi的集合；O為包含所有oj的集合。

假設(shè)現(xiàn)有路徑穿過圖3所示的障礙物環(huán)境，待選子節(jié)點xi與周圍三個障礙物o1、o2、o3之間的直線距離示意圖如圖7（a）所示。

（3）計算待選子節(jié)點與當前節(jié)點的中點與周圍障礙物之間的最短距離，計算公式如下：

式中：p(xp,yp)為當前節(jié)點，xi(xi,yi)為待選節(jié)點，m是 p和xi的中點，oj(xoj,yoj)為xi周圍的障礙物；l(xi)為m與oj之間的距離；X為包含所有xi的集合；O為包含所有oj的集合。

假設(shè)現(xiàn)有路徑穿過圖3所示的障礙物環(huán)境，待選子節(jié)點xi與當前節(jié)點p的中點m與周圍三個障礙物o1、o2、o3之間的直線距離示意圖如圖7（b）所示。

（4）計算 d(xi)、l(xi)中的最短距離 dmin，比較 dmin與l{li,lb,ll,lr}。 l≤dmin時，執(zhí)行操作（5）；dmin

式中：f(xi)為xi的估價函數(shù)；g(xi)為xi到起點S的實際代價；h(xi)為xi到目標點d的估計代價；ds為移動機器人的安全半徑。

圖7 路徑上節(jié)點與障礙物直線距離示意圖

（5）判斷是否已經(jīng)計算所有待選子節(jié)點的估價函數(shù)。如果已經(jīng)計算完成，執(zhí)行操作（6）；如果還未計算完成，返回執(zhí)行操作（2）。

（6）選擇估價函數(shù)最小的待選子節(jié)點為拓展節(jié)點。

利用改進A*算法在圖1所示的柵格環(huán)境中進行路徑規(guī)劃的結(jié)果如圖8所示，由圖8可以看出，利用改進A*算法規(guī)劃的路徑所有節(jié)點與障礙物之間都保留了一定的安全距離，無危險節(jié)點，說明移動機器人在實際的運行過程中不會與環(huán)境中的物品發(fā)生碰撞。由圖8還可以看出，相比傳統(tǒng)A*算法規(guī)劃的路徑，該路線轉(zhuǎn)彎的角度更小，路徑也更為平滑，更符合移動機器人的運動特性。

圖8 改進A＊算法路徑圖

4 仿真對比實驗

為了驗證改進A*算法的有效性，采用改進算法與傳統(tǒng)八方向A*算法在不同規(guī)模地圖和不同復(fù)雜程度地圖中進行了多組仿真對比研究，實驗環(huán)境為：64位WIN10操作系統(tǒng)，運行內(nèi)存為8GB，編譯環(huán)境為：Python3.7。

采用本文改進的A*算法與傳統(tǒng)的A*算法分別在規(guī)模為30*30、50*50、100*100且存在多種不規(guī)則形狀障礙物、圓形障礙物以及混合障礙物的環(huán)境地圖中進行移動機器人路徑規(guī)劃仿真，仿真結(jié)果如圖9—圖11所示。

圖9 30＊30不規(guī)則障礙物環(huán)境中路徑規(guī)劃仿真對比

圖10 50＊50圓形障礙物環(huán)境中路徑規(guī)劃仿真對比

對比圖9、圖10、圖11中傳統(tǒng)A*算法規(guī)劃的路徑圖與改進A*算法規(guī)劃的路徑圖可以發(fā)現(xiàn)，傳統(tǒng)A*算法規(guī)劃的路徑在進行仿真的三個地圖中都存在與障礙物接觸的危險節(jié)點；改進A*算法所規(guī)劃路徑與環(huán)境中的障礙物都保持著一定的安全距離，不存在與障礙物接觸的危險節(jié)點。傳統(tǒng)A*算法規(guī)劃的路徑轉(zhuǎn)彎角度較大，路徑平滑度較低；改進A*算法規(guī)劃的路徑較傳統(tǒng)A*算法規(guī)劃的路徑轉(zhuǎn)彎角度更小，路徑更為平滑。對傳統(tǒng)A*算法和改進A*算法在規(guī)模為30*30、50*50、100*100的環(huán)境地圖中仿真的路徑長度、危險節(jié)點數(shù)、最大轉(zhuǎn)彎角度、累積轉(zhuǎn)折角度四個方面進行數(shù)據(jù)對比，結(jié)果見表1。

圖11 100＊100混合障礙物環(huán)境中路徑規(guī)劃仿真對比

表1 傳統(tǒng)A*算法與改進A*算法的仿真結(jié)果數(shù)據(jù)對比

對比表1中的實驗數(shù)據(jù)可以發(fā)現(xiàn)，在不同規(guī)模和不同障礙物的仿真環(huán)境中，相比傳統(tǒng)A*算法，改進A*算法實現(xiàn)了多方面的優(yōu)化。在路徑長度方面，改進A*算法規(guī)劃的路徑長度均小于傳統(tǒng)A*算法規(guī)劃的路徑長度，且地圖規(guī)模越大、環(huán)境越復(fù)雜時，改進A*算法的路徑長度優(yōu)勢越明顯。在危險節(jié)點數(shù)方面，傳統(tǒng)A*算法規(guī)劃路徑的危險節(jié)點隨著地圖規(guī)模的增加而增加，但是改進A*算法在任意規(guī)模的地圖中規(guī)劃的路徑都不存在危險節(jié)點。在最大轉(zhuǎn)彎角度與累積轉(zhuǎn)彎角度方面，傳統(tǒng)A*算法規(guī)劃的路徑最大轉(zhuǎn)彎角度是改進A*算法的兩倍，且傳統(tǒng)A*算法所規(guī)劃路徑的累積轉(zhuǎn)彎角度始終大于改進A*算法的累積轉(zhuǎn)彎角度。綜合以上實驗結(jié)果說明，改進A*算法較傳統(tǒng)A*算法在路徑長度、路徑安全性、路徑平滑度三方面都實現(xiàn)了一定程度的優(yōu)化和改進。

5 結(jié)論

本文針對傳統(tǒng)A*算法規(guī)劃的安全系數(shù)低、平滑度低等問題，提出了一種基于搜索方向與安全性改進的A*算法。通過仿真實驗研究，對比了傳統(tǒng)A*算法與其他文獻中的路徑規(guī)劃算法，驗證了本文改進A*算法實現(xiàn)了路徑長度、安全性以及平滑性的優(yōu)化。未來將研究在多種實際應(yīng)用場景中多移動機器人的協(xié)調(diào)與路徑規(guī)劃算法，進一步推動移動機器人的算法研究和實際應(yīng)用。