基于定向搜索A*算法的平滑路徑規(guī)劃

李圣達(dá)，鄭宇鋒，呂娜，李詩(shī)瑤，祁宇峰

(1.大連交通大學(xué) 電氣信息工程學(xué)院，遼寧 大連 116028；2.大連交通大學(xué) 機(jī)械工程學(xué)院，遼寧 大連 116028；3.大連交通大學(xué) 軟件學(xué)院，遼寧 大連 116028；4.大連理工大學(xué) 化工學(xué)院，遼寧 大連 116023)

移動(dòng)機(jī)器人在當(dāng)今物流行業(yè)中具有舉足輕重的地位.為了適應(yīng)物流行業(yè)的高速發(fā)展，移動(dòng)機(jī)器人路徑規(guī)劃問(wèn)題對(duì)于時(shí)效性的需求日益增加，已逐步成為國(guó)內(nèi)外機(jī)器人學(xué)者研究的熱點(diǎn)問(wèn)題.路徑規(guī)劃指在當(dāng)前環(huán)境下，移動(dòng)機(jī)器人能夠依據(jù)先驗(yàn)地圖信息及實(shí)時(shí)環(huán)境數(shù)據(jù)，自主規(guī)劃出一條從起始點(diǎn)(A點(diǎn))到目標(biāo)點(diǎn)(B點(diǎn))的最優(yōu)或次優(yōu)無(wú)碰撞路徑[1].當(dāng)前主流路徑規(guī)劃算法包括：蟻群算法、Dijkstra算法、A*算法、遺傳算法及可視圖法等.其中A*算法因原理通俗易懂、搜索方式直接及準(zhǔn)確度高等特點(diǎn)，在靜態(tài)全局路徑規(guī)劃算法中占據(jù)了重要地位.然而，傳統(tǒng)A*算法在路徑規(guī)劃中，存在冗余路徑節(jié)點(diǎn)過(guò)多、路徑較長(zhǎng)、算法效率低、移動(dòng)機(jī)器人與障礙物之間無(wú)安全距離且所得路徑非平滑曲線等問(wèn)題.為此,任玉潔等[2]提出了一種拓展搜索領(lǐng)域的平滑A*算法，消除了路徑中的部分冗余節(jié)點(diǎn)，但是其算法拓展了搜索領(lǐng)域，使得搜尋路徑時(shí)間變長(zhǎng)；喬云俠等[3]提出了一種背向障礙物的搜索方法，避免了移動(dòng)機(jī)器人貼合障礙物的邊緣前進(jìn)的可能，但增加了路徑長(zhǎng)度、搜尋時(shí)間；陳靖輝等[4]提出了一種改進(jìn)A*算法，消除了對(duì)稱路徑，縮短了路徑長(zhǎng)度、搜尋時(shí)間，但所得路徑平滑效果不明顯.

通過(guò)分析上述各方法的優(yōu)缺點(diǎn)，本文對(duì)傳統(tǒng)A*算法進(jìn)行改進(jìn).改進(jìn)后的A*算法增加了定向搜索策略、關(guān)鍵點(diǎn)提取策略以及“拉直”處理策略，不僅使路徑規(guī)劃效率得到提升，而且減少了路徑轉(zhuǎn)折次數(shù)并去除了路徑冗余節(jié)點(diǎn)，此外引入貝塞爾曲線對(duì)所得路徑節(jié)點(diǎn)進(jìn)行路徑平滑優(yōu)化處理，使移動(dòng)機(jī)器人更加順暢的移動(dòng)至目標(biāo)點(diǎn).

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

傳統(tǒng)A*算法是靜態(tài)全局路徑規(guī)劃算法中的一種經(jīng)典的啟發(fā)式搜索算法[5-6].其中的代價(jià)函數(shù)可表示為：

f(x)=g(x)+h(x)

(1)

式中:g(x)表示從起始節(jié)點(diǎn)(STRAT)至指定節(jié)點(diǎn)的(x)實(shí)際代價(jià)；h(x)表示點(diǎn)(x)至目標(biāo)點(diǎn)(GOAL)的估計(jì)代價(jià)；f(x)表示點(diǎn)(x)的優(yōu)先搜索程度，其值越小代表該點(diǎn)(x)搜索優(yōu)先級(jí)越高.

傳統(tǒng)A*算法中，啟發(fā)函數(shù)h(x)的計(jì)算方法直接決定了A*算法的結(jié)果.一般來(lái)說(shuō)，傳統(tǒng)A*算法中有三種啟發(fā)函數(shù)計(jì)算方法：曼哈頓距離、對(duì)角線距離及歐氏距離.其中歐式距離是在N維空間中的兩點(diǎn)之間的真實(shí)最短距離，精確度最高且不易產(chǎn)生誤差.本文也是應(yīng)用歐氏距離方法計(jì)算啟發(fā)函數(shù)h(x)：

(2)

2 定向搜索的A*算法

傳統(tǒng)的A*算法采取8鄰域搜索方式[7]進(jìn)行全局路徑規(guī)劃.該方法不但存在搜索鄰域過(guò)多、耗時(shí)長(zhǎng)及計(jì)算量大等不足，而且無(wú)法避免移動(dòng)機(jī)器人貼合障礙物邊緣前進(jìn)的不良情況.為解決上述問(wèn)題,本文提出定向搜索方式改進(jìn)的A*算法.首先在傳統(tǒng)的A*算法的基礎(chǔ)上增加定向引導(dǎo)啟發(fā)函數(shù)γs作為搜索最高優(yōu)先級(jí)，隨后加入避障約束條件，明確搜索方向；其次對(duì)所得初始路徑節(jié)點(diǎn)進(jìn)行關(guān)鍵節(jié)點(diǎn)提取操作并進(jìn)行路徑“拉直”處理；最后通過(guò)貝塞爾曲線對(duì)經(jīng)過(guò)上述步驟處理后的路徑節(jié)點(diǎn)進(jìn)行平滑路徑優(yōu)化，得到一條最優(yōu)平滑路徑.

(1)定向搜索策略

通過(guò)當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)坐標(biāo)所組成的直線求出γs值，并選擇合適的方位判定閾值(ε1，ε2)與其進(jìn)行差值比較，確定搜索方向.其中定向?qū)б龁l(fā)函數(shù)公式為：

(3)

具體為以下四種情況：

1)γs大于ε1且小于ε2時(shí)，優(yōu)先搜索方向?yàn)闁|北、西南方向；

2)γs大于等于-ε2且小于等于-ε1時(shí)，優(yōu)先搜索東南、西北方向；

3)γs大于-ε1且小于-ε2時(shí)，優(yōu)先搜索東、西方向；

4)γs大于ε2或者小于-ε2或不存在時(shí)，優(yōu)先搜索南、北方向.

再依據(jù)起始點(diǎn)與目標(biāo)點(diǎn)之間的橫坐標(biāo)或縱坐標(biāo)的關(guān)系和障礙物約束條件進(jìn)一步明確搜索方向，并確定下一待擴(kuò)展節(jié)點(diǎn)，解決了傳統(tǒng)A*算法盲目搜索和耗時(shí)長(zhǎng)的問(wèn)題.定向搜索策略示意圖見圖1.

(a) 障礙物在直線上

利用定向?qū)б瘮?shù)值確定搜索方向后，若出現(xiàn)圖1(a)所示情況：當(dāng)障礙物在目標(biāo)節(jié)點(diǎn)與當(dāng)前節(jié)點(diǎn)之間的連線上時(shí)，因需要避開障礙物，則終止該方向搜索；若出現(xiàn)圖1(b)所示情況：當(dāng)障礙物在連線的某一側(cè)時(shí)，則可以繼續(xù)搜索；若出現(xiàn)圖1(c)所示情況：當(dāng)障礙物在連線的兩側(cè)且位于正方向時(shí)，可以繼續(xù)搜索.待在初步確定的方向搜索完成后，轉(zhuǎn)而搜索東、南、西、北四個(gè)方向，以其中代價(jià)函數(shù)值最小的節(jié)點(diǎn)作為下一待擴(kuò)展節(jié)點(diǎn).

本文中定向搜索的A*算法不再對(duì)當(dāng)前節(jié)點(diǎn)的周圍8鄰域進(jìn)行搜索，而是依據(jù)定向?qū)б龁l(fā)函數(shù)γs進(jìn)行定向搜索，有效縮短搜索時(shí)間，提高路徑規(guī)劃效率.

(2)關(guān)鍵節(jié)點(diǎn)提取策略

柵格法規(guī)劃出的路徑包含許多路徑節(jié)點(diǎn)，其中有大量路徑節(jié)點(diǎn)為路徑冗余節(jié)點(diǎn)，只有少數(shù)節(jié)點(diǎn)為生成路徑所需的必要節(jié)點(diǎn)，如：起始點(diǎn)、目標(biāo)點(diǎn)及路徑轉(zhuǎn)折點(diǎn).針對(duì)此問(wèn)題，本文采用了關(guān)鍵節(jié)點(diǎn)提取策略.具體策略如下：

若生成的路徑節(jié)點(diǎn)集合為L(zhǎng)={Qi|i=0,1,2,…，n}，其中Qi表示路徑上的第i個(gè)路徑節(jié)點(diǎn).依據(jù)三點(diǎn)共線原理判斷Q0、Q1、Q2三個(gè)節(jié)點(diǎn)是否共線，若三個(gè)節(jié)點(diǎn)處于同一條直線上，則說(shuō)明Q1節(jié)點(diǎn)為可舍節(jié)點(diǎn)，執(zhí)行去除Q1節(jié)點(diǎn)并更新路徑L的操作.隨后繼續(xù)判斷Q0、Q1、Q2三個(gè)節(jié)點(diǎn)是否存在可舍節(jié)點(diǎn)，若不存在，則繼續(xù)遍歷Q2、Q3、Q4三個(gè)節(jié)點(diǎn)，直至遍歷所有的路徑節(jié)點(diǎn)為止.遍歷完成后，得到僅含有起始節(jié)點(diǎn)、路徑轉(zhuǎn)折點(diǎn)以及目標(biāo)節(jié)點(diǎn)的集合L.

(3)“拉直”處理策略

利用定向搜索策略所得路徑，雖然減少了搜索鄰域、提高了路徑規(guī)劃效率，但可能存在鋸齒效應(yīng)，產(chǎn)生“Z”形路徑，且該現(xiàn)象不利于移動(dòng)機(jī)器人的運(yùn)動(dòng).因此將最初得到的路徑進(jìn)行“拉直”處理，“拉直”處理策略示意圖見圖2.具體流程如下:

(a) 起始點(diǎn)與目標(biāo)點(diǎn)之間無(wú)障礙情況示意圖

經(jīng)過(guò)關(guān)鍵節(jié)點(diǎn)提取策略優(yōu)化后的路徑節(jié)點(diǎn)集合為S={Qi|i=0,1,2,…,n}，確定起始點(diǎn)Q0與目標(biāo)節(jié)點(diǎn)Qn所在直線方程，遍歷該線段所經(jīng)過(guò)的節(jié)點(diǎn)坐標(biāo)，判斷兩者之間是否存在障礙物.若無(wú)障礙物，則刪除兩者之間的其他路徑點(diǎn)，并將Q0和Qn兩點(diǎn)保留至新的路徑，同時(shí)退出程序，見圖2 (a)；否則連接Q0，Qn-1，若兩者之間的連線滿足無(wú)障礙物條件，則保留Q0，Qn-1，Qn三個(gè)路徑節(jié)點(diǎn)作為最終路徑節(jié)點(diǎn)集合；否則連接Q0，Qn-2，以此類推，直至找到與起始點(diǎn)之間的連線滿足條件的Qi節(jié)點(diǎn).隨后將Qi節(jié)點(diǎn)作為新的起始點(diǎn)Q0，重復(fù)上述步驟，直至路徑中不存在多余路徑節(jié)點(diǎn)，見圖2 (b).采取關(guān)鍵點(diǎn)提取策略及“拉直”處理策略后與無(wú)處理策略的對(duì)比效果,見圖3.

(a)無(wú)處理策略 (b)本文處理策略

本文通過(guò)定向搜索策略、關(guān)鍵節(jié)點(diǎn)提取策略及“拉直”處理策略，形成優(yōu)化后的A*算法.改進(jìn)后的A*算法流程圖見圖4，具體流程如下：

1)初始化節(jié)點(diǎn)信息.

2)若指定節(jié)點(diǎn)是目標(biāo)節(jié)點(diǎn)，跳轉(zhuǎn)至7)，否則繼續(xù)執(zhí)行.

3) 計(jì)算出起始節(jié)點(diǎn)或指定節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)之間的定向?qū)б瘮?shù)值，確定優(yōu)先搜索方向.

4)依據(jù)障礙物的分布情況，選擇待拓展節(jié)點(diǎn).將待指定節(jié)點(diǎn)和東、南、西、北四個(gè)方向添加到Open_list，同時(shí)將障礙物節(jié)點(diǎn)添加到Close_list.

圖4 改進(jìn)后的A*算法流程圖

5)找出最小代價(jià)函數(shù)f(x)對(duì)應(yīng)的節(jié)點(diǎn)作為指定節(jié)點(diǎn)，并將該指定節(jié)點(diǎn)標(biāo)記為Close_list中的

元素，記錄其父節(jié)點(diǎn)并繼續(xù)執(zhí)行2).

6)路徑關(guān)鍵節(jié)點(diǎn)提取及“拉直”處理.

7)輸出路徑.

3 貝塞爾曲線路徑優(yōu)化

本文提出的定向搜索的A*算法雖然減少了搜索路徑所耗時(shí)長(zhǎng)，避免了移動(dòng)機(jī)器人無(wú)限接近障礙物的問(wèn)題，但其生成的部分路徑仍為折線并且存在尖峰拐點(diǎn)，并不利于移動(dòng)機(jī)器人的轉(zhuǎn)彎運(yùn)動(dòng).而貝塞爾曲線[8-10]具有端點(diǎn)性質(zhì)以及凸包性，非常適用于路徑平滑工作，也使其成為在全局路徑優(yōu)化領(lǐng)域[11-12]中最重要的方法.因此本文中引用貝塞爾曲線對(duì)改進(jìn)后的A*算法所生成的路徑進(jìn)行平滑處理.

貝塞爾曲線的定義取決于控制點(diǎn)的個(gè)數(shù)，當(dāng)有n+1個(gè)控制點(diǎn)時(shí)，就可以確定n次多項(xiàng)式的貝塞爾曲線參數(shù)方程：

(4)

式中:Bi,n(t)為貝塞爾曲線基函數(shù)；t為歸一化時(shí)間變量；n為n次貝塞爾曲線優(yōu)化.

由于貝塞爾曲線的導(dǎo)數(shù)由控制點(diǎn)所決定，因此貝塞爾曲線的一階導(dǎo)數(shù)可以由式(5)、式(6)表示：

(5)

貝塞爾曲線上任意一點(diǎn)的曲率公式為：

圖5 貝塞爾曲線優(yōu)化流程圖

(6)

式中，x′(t)、y′(t)、x″(t)和y″(t)分別為貝塞爾曲線的坐標(biāo)x和y方向上的分量對(duì)x和y的一階導(dǎo)數(shù)和二階導(dǎo)數(shù).貝塞爾曲線優(yōu)化的具體流程見圖5.

4 基于ROS平臺(tái)的仿真實(shí)驗(yàn)

ROS機(jī)器人操作系統(tǒng)是針對(duì)機(jī)器人編程及應(yīng)用的一種開源操作系統(tǒng).ROS能夠同時(shí)支持多種編程語(yǔ)言來(lái)編寫所用的功能包，比如最常見的C++、PYTHON、JAVA等.與此同時(shí)，還能支持GAZEBO仿真平臺(tái)和計(jì)算圖可視化工具、數(shù)據(jù)繪圖工具、圖像渲染工具及RVIZ三維可視化工具等，為廣大ROS使用者提供方便.

4.1 定向搜索的A*算法仿真實(shí)驗(yàn)

在GAZEBO仿真平臺(tái)下，采用RBPF粒子濾波算法生成20×20的柵格地圖，單位為m，其中生成的先驗(yàn)地圖的分辨率為0.05 m/柵格；同時(shí)為保證粒子濾波算法中的粒子多樣性及地圖的精確度，將其粒子數(shù)設(shè)置為30.在已經(jīng)生成的先驗(yàn)地圖的基礎(chǔ)上，分別選取其中無(wú)障礙物區(qū)間和有障礙物區(qū)間進(jìn)行了路徑規(guī)劃的仿真實(shí)驗(yàn).其中仿真實(shí)驗(yàn)結(jié)果見圖6，點(diǎn)劃線為傳統(tǒng)A*算法所生成路徑；直線為改進(jìn)后A*算法所生成的路徑.

圖6 A*算法改進(jìn)前后仿真實(shí)驗(yàn)結(jié)果對(duì)比

為進(jìn)一步驗(yàn)證實(shí)驗(yàn)的準(zhǔn)確性，在無(wú)障礙物區(qū)間選取 (31,38)，(99,32) 作為X向無(wú)障礙物區(qū)間實(shí)驗(yàn)的起始點(diǎn)與目標(biāo)點(diǎn)；在無(wú)障礙物區(qū)間選取(64,86)，(92,154)作為Y向無(wú)障礙物區(qū)間實(shí)驗(yàn)的起始點(diǎn)與目標(biāo)點(diǎn)；在有障礙物區(qū)間選取(98,70)，(123,159)作為一般復(fù)雜障礙物區(qū)間實(shí)驗(yàn)的起始點(diǎn)與目標(biāo)點(diǎn)，選取(98,70)，(123,183)作為較復(fù)雜障礙物區(qū)間實(shí)驗(yàn)的起始點(diǎn)與目標(biāo)點(diǎn).每組進(jìn)行10次重復(fù)仿真實(shí)驗(yàn)，并記錄10次實(shí)驗(yàn)中的傳統(tǒng)A*算法、定向搜索的A*算法路徑規(guī)劃過(guò)程中的拓展節(jié)點(diǎn)集合N、路徑節(jié)點(diǎn)集合L、路徑轉(zhuǎn)折點(diǎn)P、搜尋路徑時(shí)間T1及計(jì)算各路徑所走長(zhǎng)度S，并將各項(xiàng)數(shù)據(jù)的平均值記錄到表1中.

表1 不同算法實(shí)驗(yàn)數(shù)據(jù)結(jié)果對(duì)比

從表1、圖6中的數(shù)據(jù)和仿真結(jié)果可以清晰地得出以下結(jié)論：a)在無(wú)障礙物區(qū)間內(nèi)：雖然改進(jìn)后的A*算法路徑搜索時(shí)間較慢，但減少了移動(dòng)機(jī)器人轉(zhuǎn)彎次數(shù)并且得到較短的路徑；b)在障礙物區(qū)間內(nèi)：較傳統(tǒng)A*算法，改進(jìn)后A*算法在搜索過(guò)程中產(chǎn)生的拓展節(jié)點(diǎn)數(shù)平均減少了26.90%，路徑轉(zhuǎn)折點(diǎn)數(shù)平均減少了50%，路徑節(jié)點(diǎn)數(shù)平均減少了96.76%，為改進(jìn)A*算法的高效完成提供了有力的保障.同時(shí)也因路徑轉(zhuǎn)折點(diǎn)數(shù)的減少以及更短的路徑長(zhǎng)度，更有利于移動(dòng)機(jī)器人運(yùn)動(dòng).與傳統(tǒng)A*算法相比，應(yīng)用改進(jìn)后的A*算法所生成的路徑不再有與障礙物無(wú)限接近的位置，能夠保證一定的安全距離，滿足機(jī)器人基本避障需求；改進(jìn)后的A*算法尋路效率與傳統(tǒng)A*算法尋路效率優(yōu)勢(shì)會(huì)隨著待搜索路徑的長(zhǎng)度以及復(fù)雜程度突顯出來(lái)，由較復(fù)雜障礙物區(qū)間實(shí)驗(yàn)數(shù)據(jù)可知，改進(jìn)后A*算法尋路時(shí)間比傳統(tǒng)A*算法減少了50.32%，大大提升了算法搜索效率.

4.2 貝塞爾曲線平滑路徑仿真實(shí)驗(yàn)

將利用定向搜索的A*算法所得到的路徑，進(jìn)行貝塞爾曲線平滑處理，平滑路徑結(jié)果如圖7所示，灰色線代表改進(jìn)后A*算法生成的路徑，沒有進(jìn)行平滑優(yōu)化；黑色線代表貝塞爾曲線平滑后的路徑.結(jié)果顯而易見：定向搜索的A*算法在沒有加入貝塞爾曲線優(yōu)化之前，所得路徑存在尖峰拐點(diǎn)，不利于移動(dòng)機(jī)器人轉(zhuǎn)向運(yùn)動(dòng)；而加入貝塞爾曲線之后，路徑變得平滑有利于移動(dòng)機(jī)器人快速通過(guò)路徑拐角.

圖7 貝塞爾曲線平滑路徑

5 結(jié)論

針對(duì)傳統(tǒng)A*算法在規(guī)劃路徑時(shí)存在盲目搜索、算法效率低、規(guī)劃路徑轉(zhuǎn)折點(diǎn)尖銳及路徑與障礙物之間安全距離不足等問(wèn)題，本文提出了基于定向搜索A*算法的平滑路徑規(guī)劃.改進(jìn)后的A*算法雖然在無(wú)障礙物區(qū)域進(jìn)行路徑規(guī)劃時(shí)耗時(shí)略長(zhǎng)，但在有障礙物區(qū)域的路徑規(guī)劃效率的優(yōu)勢(shì)會(huì)隨著路徑長(zhǎng)度的增加而顯現(xiàn)出來(lái)，同時(shí)通過(guò)增加關(guān)鍵點(diǎn)提取策略以及“拉直”處理策略去除了路徑冗余點(diǎn)并減少了路徑轉(zhuǎn)折次數(shù)；此外引入了貝塞爾曲線，明顯使路徑中的尖峰拐點(diǎn)部分變得平滑，能夠更好地滿足移動(dòng)機(jī)器人路徑規(guī)劃的實(shí)際需求.