改進(jìn)的A*算法移動機(jī)器人路徑規(guī)劃*

柴紅杰李建軍姚 明

(江蘇大學(xué)汽車與交通工程學(xué)院，江蘇鎮(zhèn)江 212001)

當(dāng)今社會移動機(jī)器人在各行各業(yè)都扮演著重要角色，但是路徑規(guī)劃仍然是移動機(jī)器人應(yīng)用中面臨的一個重要難題。它的目的是在有障礙物的環(huán)境中為移動機(jī)器人從起始位置到目標(biāo)位置規(guī)劃出一條最優(yōu)或次優(yōu)安全無碰撞可行路徑并且得到的路徑要滿足一定的約束條件[1-2]。機(jī)器人路徑規(guī)劃的常用方法主要有蟻群算法[3]，粒子群優(yōu)化算法[4]，柵格法[5]等。其中，由于柵格法具有結(jié)構(gòu)簡單，易于實(shí)現(xiàn)，對傳感器容錯性強(qiáng)等優(yōu)點(diǎn)，被廣泛應(yīng)用于機(jī)器人路徑規(guī)劃中?；跂鸥竦貓D的A*算法適用于環(huán)境信息已知的一類路徑規(guī)劃方法。已有多種改進(jìn)的A*算法被提出，顧辰[6]在對機(jī)器人進(jìn)行路徑規(guī)劃過程中，把擴(kuò)展結(jié)點(diǎn)進(jìn)行優(yōu)先分級，避免穿過障礙物頂點(diǎn)，與障礙物有一定的安全距離，但路徑依然存在較多轉(zhuǎn)折點(diǎn)和安全問題；辛煜等[7]通過A*算法搜索離散的8 個鄰域擴(kuò)展到無限個，增加了搜索方向，提高了路徑平滑性的性能指標(biāo)，但計算量增大，致使搜索效率明顯降低；陳諾男[8]等提出更改障礙搜索矩陣的尺寸來獲得不同的安全間距，以保證不同機(jī)器人在不同地圖環(huán)境下的安全性，但未考慮機(jī)器人本體尺寸不利于實(shí)際環(huán)境規(guī)劃；李沖等[9]提出了一種單邊矩形擴(kuò)展A*算法，采用受迫擴(kuò)展規(guī)則，單條公共邊取代2 條相鄰冗余邊，簡化了終止條件，但在復(fù)雜環(huán)境搜索時效率明顯降低。卜新蘋等[10]提出改進(jìn)的三階Bezier 曲線方法，雖然能實(shí)現(xiàn)路徑平滑，但是算法計算較繁瑣，實(shí)現(xiàn)效率低。

綜上所述，如果考慮到機(jī)器人的本體尺寸，基本的A*算法在搜索路徑時很有可能撞到障礙物，對機(jī)器人和障礙物來說是不安全的，而且不能得到次優(yōu)或最優(yōu)路徑。因此提出了一種在障礙物邊緣放置虛擬障礙進(jìn)行緩沖和改進(jìn)啟發(fā)搜索函數(shù)的算法，并在靜態(tài)和動態(tài)障礙物環(huán)境下進(jìn)行仿真，最后利用動態(tài)切點(diǎn)法對路徑進(jìn)行二次平滑處理，提高了算法的實(shí)時性和機(jī)器人的穩(wěn)定性。

1 A*算法的機(jī)器人路徑規(guī)劃

1.1 環(huán)境場景的建立

環(huán)境模型的建立是對機(jī)器人進(jìn)行控制的基礎(chǔ)，為實(shí)現(xiàn)路徑規(guī)劃算法，移動機(jī)器人看作二維平面移動的質(zhì)點(diǎn)[11]，將障礙物信息以及機(jī)器人運(yùn)動軌跡記錄在柵格上，黑色塊表示障礙物單元柵格，白色塊表示可通行區(qū)域單元柵格。機(jī)器人運(yùn)動空間為，將柵格化的環(huán)境信息映射到直角坐標(biāo)系XOY中，且Xmax和Ymax分別為X、Y軸上的最大值。假設(shè)機(jī)器人的移動步長為σ，對X、Y軸分別以σ進(jìn)行劃分如圖1 所示。

圖1 柵格化的移動機(jī)器人環(huán)境

圖1 中，行列的柵格數(shù)分別為m＝采用“柵格-存儲”的映射法[12]將格子信息存儲在第m行、第n列，記為C(m，n)。定義從原點(diǎn)開始第一個柵格C(1，1)的序號i為1，C(2，1)的序號為2，…，在m×n的柵格中，坐標(biāo)(x，y) 與柵格序號的映射關(guān)系為:

式中:mod，int 分別為求余、取整運(yùn)算。

1.2 機(jī)器人路徑規(guī)劃的A*算法

A*算法是典型的啟發(fā)式搜索算法，其搜索區(qū)域?yàn)樗姆较蚧虬朔较颉＞唧w搜索如圖2 所示。

圖2 格柵場地及機(jī)器人運(yùn)動方向

代價函數(shù)的核心表達(dá)式為:

式中:f(n)表示從起始節(jié)點(diǎn)Nstart到目標(biāo)節(jié)點(diǎn)Ggoal總的代價消耗。g(n)表示從起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)Current 的實(shí)際消耗，h(n)為當(dāng)前節(jié)點(diǎn)Current 到目標(biāo)節(jié)點(diǎn)Ggoal的估計代價消耗值，f(n)的大小取決于h(n)的大小，h(n)越接近實(shí)際距離，消耗的總代價越小。因此分析A*的關(guān)鍵所在就是h(n)。假設(shè)真實(shí)路徑代價值用Ch(n，goal)表示，擴(kuò)展搜索范圍用EX 表示，h(n)與Ch(n，goal)的不同對A*搜索算法的影響如表1 所示。

表1 h(n)對A*算法的影響

全局路徑規(guī)劃可分為3 個部分，首先是機(jī)器人環(huán)境模型的建立，其次調(diào)用A*算法進(jìn)行路徑搜索，最后路徑輸出，如圖3 所示。

圖3 路徑規(guī)劃流程圖

2 改進(jìn)的A*路徑規(guī)劃算法

2.1 虛擬障礙物的設(shè)置

A*算法把機(jī)器人運(yùn)動的整個空間分成2 部分:自由空間和障礙物空間。沒有考慮到機(jī)器人的實(shí)體尺寸、轉(zhuǎn)彎半徑、定位與距離誤差等因素的影響。針對上述問題，提出了把整個運(yùn)動區(qū)域進(jìn)行安全等級劃分，在障礙物周圍放置虛擬障礙物形成緩沖區(qū)。

為保障機(jī)器人能安全快速的通過障礙物區(qū)域到達(dá)目標(biāo)點(diǎn)，可把整個運(yùn)動區(qū)域進(jìn)行安全等級劃分。機(jī)器人在路徑規(guī)劃時選擇安全等級較高的優(yōu)先進(jìn)行規(guī)劃，遠(yuǎn)離障礙物，減少干擾，提高安全性。機(jī)器人當(dāng)前位置的代價值與距離最近障礙物距離D成反比。規(guī)定代價的范圍為[0 255]，代價值255 用MCV(Max Cost Value)表示。最近障礙物距離D 規(guī)定0 至BR(Buffer radius)，BR 一般取機(jī)器人半徑的2 倍。緩沖區(qū)代價等級R計算公式用如式(3)所示，R值越小安全等級越高。

式中:Rd(Robot radius)表示機(jī)器人半徑。

經(jīng)過放置虛擬障礙物處理的地圖如圖4 所示。

圖4 虛擬障礙物地圖處理

圖4 中粗黑色部分代表障礙物所占據(jù)的空間，淺黑色(灰色)部分為虛擬放置障礙物膨脹部分，白色為自由無障礙區(qū)。其中R值越小代表障礙等級越低，在機(jī)器人路徑規(guī)劃中經(jīng)優(yōu)先規(guī)劃R較小的區(qū)域。

2.2 啟發(fā)函數(shù)的改進(jìn)

分析表1 可知，A*算法的搜索性能受h(n)的影響，當(dāng)?shù)拇笮≡浇咏鎸?shí)代價消耗的總代價越小，h(n)搜索效率越高。實(shí)際中很難估計h(n)大小，一般有曼哈頓距離(Manhattan)、切比雪夫距離和歐氏距離3 種方法。歐氏距離相對實(shí)際距離偏短，Manhattan 距離相對于實(shí)際距離偏大。所以可以取Manhattan 距離和歐氏距離的中間值作為h(n)的估計值。中間值h(n)的估計值如圖5(a)、5(b)、5(c)所示。

圖5 中，線段d1和d2分別代表當(dāng)前節(jié)點(diǎn)N點(diǎn)到目標(biāo)節(jié)點(diǎn)G點(diǎn)的橫向距離與縱向距離，d1和d2之和為Manhattan 距離。斜線段為歐氏距離，虛線段為Manhattan 距離和歐氏距離的中間值，即改進(jìn)的啟發(fā)函數(shù)h*(n)。改進(jìn)后的h*(n)的推導(dǎo)如下所示:

圖5 改進(jìn)啟發(fā)函數(shù)示意圖

(1)當(dāng)d1＝d2時；

(2)當(dāng)d1＞d2時；

(3)同理當(dāng)d1＜d2時；

綜上h*(n)表達(dá)式如(7)所示。

式中:d1與d2的計算公式如(8)所示:其中d1與d2是在二維平面內(nèi)的距離。N(xn，yn)為當(dāng)前節(jié)點(diǎn)坐標(biāo)，G(xg，yg)為目標(biāo)點(diǎn)坐標(biāo)。

改進(jìn)后的算法流程圖如圖6 所示。

圖6 改進(jìn)后A*算法

3 仿真結(jié)果和分析

3.1 靜態(tài)障礙物環(huán)境下

靜態(tài)障礙物環(huán)境也就是機(jī)器人所處的空間障礙物是固定不變的，不會突然出現(xiàn)移動的障礙物等情況。實(shí)驗(yàn)環(huán)境:Intel(r)Core(TM)I5-6500 CPU。編譯工具M(jìn)ATLAB 2017a。分別對A*算法和改進(jìn)A*算法進(jìn)行仿真，地圖尺寸為30 m ×30 m，設(shè)起點(diǎn)為Start，目標(biāo)點(diǎn)為Goal 進(jìn)行仿真計算，結(jié)果如圖7 所示。

圖7(a)為A*算法沒有進(jìn)行虛擬障礙物放置處理和未進(jìn)行h*(n)改進(jìn)的路徑規(guī)劃，可以很明顯地看出機(jī)器人貼近障礙物附近運(yùn)行且轉(zhuǎn)彎幅度過大，生成的路徑不平滑且可靠性低[13]，這會導(dǎo)致機(jī)器人的碰撞幾率增大，燃料成本增加。

改進(jìn)A*啟發(fā)算法轉(zhuǎn)折角度明顯減小，機(jī)器人遠(yuǎn)離障礙物有效地降低了機(jī)器人與障礙物的碰撞幾率，提高了路徑的可靠性，如圖7(b)所示。

圖7 基本A*算和改進(jìn)A*算仿真結(jié)果

圖8、圖9 為基本算法與改進(jìn)算法運(yùn)行時間和路徑規(guī)劃長度對比圖。

圖8 規(guī)劃時間對比圖

圖9 路徑長度對比圖

由圖9 可以看出在20 次仿真實(shí)驗(yàn)中，基本A*算法路徑相對較短，但是最小轉(zhuǎn)折角度太過于小，很容易使機(jī)器人不能轉(zhuǎn)彎，也會導(dǎo)致電機(jī)磨損加重降低壽命。改進(jìn)A*算法路徑長度比基本算法路徑長9.8%(路徑長度變長是由于規(guī)劃路徑遠(yuǎn)離障礙物和機(jī)器人轉(zhuǎn)彎半徑增大的結(jié)果)，運(yùn)行時間縮短16.3%，最小轉(zhuǎn)折角度也得到了很大的改善。累計轉(zhuǎn)折角度減少10%～20%。仿真結(jié)果證明改進(jìn)A*啟發(fā)算法符合機(jī)器人最優(yōu)路徑規(guī)劃需求，能有效地解決實(shí)體機(jī)器人通過狹窄區(qū)域或通道的路徑規(guī)劃問題，提高了機(jī)器人的穩(wěn)定性。

表2 實(shí)驗(yàn)結(jié)果平均值

3.2 動態(tài)障礙物環(huán)境下

在動態(tài)環(huán)境下，除了存在靜止的障礙物外，當(dāng)突然有動態(tài)障礙物出現(xiàn)在移動機(jī)器人的工作環(huán)境中時，機(jī)器人就可能與其發(fā)生碰撞，這就需要機(jī)器人避開障礙物進(jìn)行規(guī)劃。圖10 是在復(fù)雜動態(tài)環(huán)境中進(jìn)行規(guī)劃的結(jié)果。起始點(diǎn)Start，目標(biāo)點(diǎn)Goal，黑色、淺色方框分別為靜態(tài)和動態(tài)障礙物。由仿真結(jié)果可知改進(jìn)算法可以有效地避開靜態(tài)和動態(tài)障礙物，提高機(jī)器人控制的可靠性。

圖10 動態(tài)路徑規(guī)劃結(jié)果

3.3 路徑平滑

改進(jìn)的A*算法規(guī)劃的路徑雖然得到明顯提高但頻繁轉(zhuǎn)向會使機(jī)器人發(fā)生抖動，而平滑的路徑更便于移動機(jī)器人的控制。采用切線交點(diǎn)畫圓法進(jìn)行路徑平滑。定義了Smooth 函數(shù)如圖11 所示:(1)取中間某一節(jié)點(diǎn)為中間節(jié)點(diǎn)，連接中間節(jié)點(diǎn)前后兩接點(diǎn)，若不與障礙物相交則去除中間節(jié)點(diǎn)，否則保持原路徑[14]，原理如圖12(a)所示；(2)若遇到轉(zhuǎn)折點(diǎn)則做切線交點(diǎn)畫圓法，如12(b)所示。

機(jī)器人起始位置N1(x1，y1)，終點(diǎn)位置Nn(xn，yn)。從起始位置依次對轉(zhuǎn)折點(diǎn)Ni(xi，yi)(i＝1，2，…，n-1)進(jìn)行平滑處理，具體步驟如下:

(1)取中間節(jié)點(diǎn)連接前后相鄰節(jié)點(diǎn)。若不與障礙物相交則去除中間節(jié)點(diǎn)，否則保持原路徑。

(2)比較Ni-1Ni和NiNi+1。選擇較短的邊作為切點(diǎn)，切線與∠Ni-1NiNi+1角平分線交于Oi。

圖11 Smooth 流程

圖12 路徑平滑示意圖

(3)判斷圓弧是否與長邊Ni-1Ni有交點(diǎn)A。若有，轉(zhuǎn)至步驟(4)，若無轉(zhuǎn)至(5)。

(4)用圓弧PA代替邊Ni-1NiNi+1，并轉(zhuǎn)至步驟(6)。

(5)無交點(diǎn)就繼續(xù)在較短的邊取下一臨近節(jié)點(diǎn)，同理(2)。

(6)判斷是否遍歷所有節(jié)點(diǎn)。若是，返回(1)，否則結(jié)束。

路徑平滑后的結(jié)果如圖13、圖14 所示。

圖13 靜態(tài)路徑平滑結(jié)果

由圖13 可以看出平滑后路徑去除了冗余的節(jié)點(diǎn)，轉(zhuǎn)折點(diǎn)得到圓弧過渡，減少了機(jī)器人的轉(zhuǎn)彎頻率?？梢钥闯鑫锤倪M(jìn)算法路徑會與障礙物邊緣發(fā)生摩擦，如圖14 中的②、③所示。平滑后的路徑可以實(shí)時避開突然出現(xiàn)的障礙物，更接近真實(shí)環(huán)境，如圖14 中的①所示，因此改進(jìn)A*算法提高了機(jī)器人的穩(wěn)定性、安全性和實(shí)時跟蹤效率，同時減少了電機(jī)磨損程度和燃料成本。

圖14 動態(tài)路徑平滑結(jié)果

4 結(jié)論

(1)針對移動機(jī)器人路徑規(guī)劃問題，從路徑的安全性、機(jī)器人的穩(wěn)定性出發(fā)，提出一種在障礙物邊緣放置虛擬障礙物形成緩沖區(qū)和改進(jìn)搜索啟發(fā)函數(shù)的算法。

(2)仿真結(jié)果表明，改進(jìn)A*算法相比基本A*算法規(guī)劃的路徑更加遠(yuǎn)離障礙物且累計轉(zhuǎn)折角度變小，并能有效避開動態(tài)障礙物，規(guī)劃時間相對縮短，證明了改進(jìn)算法的有效性。

(3)最后對靜態(tài)障礙物環(huán)境和動態(tài)障礙物環(huán)境規(guī)劃后的路徑進(jìn)行二次平滑處理。結(jié)果表明，平滑后的路徑更符合實(shí)體機(jī)器人通過狹窄的區(qū)域或通道，提高了機(jī)器人的穩(wěn)定性、安全性、控制性和實(shí)時跟蹤效率。