基于方向和步長(zhǎng)約束的安全A*算法

陳 嬌，向建平，劉 卿

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

1 引言

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

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

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

2 問題描述

2.1 環(huán)境建模

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

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

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

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

傳統(tǒng)A*算法主要有四個(gè)搜索方向和八個(gè)搜索方向，如圖2所示，圖中圓點(diǎn)表示移動(dòng)機(jī)器人當(dāng)前位置，實(shí)線箭頭表示機(jī)器人在移動(dòng)過程中的搜索方向。四方向A*算法的搜索方向?yàn)樯稀⑾隆⒆蟆⒂宜膫€(gè)，移動(dòng)機(jī)器人的最小轉(zhuǎn)角為，單次移動(dòng)步長(zhǎng)為1；八方向A*算法的搜索方向在上、下、左、右四個(gè)方向的基礎(chǔ)上加入了左上、左下、右上、右下四個(gè)，移動(dòng)機(jī)器人的最小轉(zhuǎn)角為，單次移動(dòng)步長(zhǎng)為1或。

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

在無障礙物的環(huán)境中，算法的搜索方向即為移動(dòng)機(jī)器人的可移動(dòng)方向；在有障礙物的環(huán)境中，移動(dòng)機(jī)器人的搜索方向不變，移動(dòng)方向?yàn)闊o障礙物的方向，如圖3所示。

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

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

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

3 改進(jìn)的A＊算法

3.1 搜索方向和步長(zhǎng)限制

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

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

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

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

3.2 路徑安全性改進(jìn)

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

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

圖6 移動(dòng)機(jī)器人俯視圖和外徑到重心距離

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

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

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

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

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

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

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

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

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

式中：f(xi)為xi的估價(jià)函數(shù)；g(xi)為xi到起點(diǎn)S的實(shí)際代價(jià)；h(xi)為xi到目標(biāo)點(diǎn)d的估計(jì)代價(jià)；ds為移動(dòng)機(jī)器人的安全半徑。

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

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

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

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

圖8 改進(jìn)A＊算法路徑圖

4 仿真對(duì)比實(shí)驗(yàn)

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

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

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

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

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

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

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

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

5 結(jié)論

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