李圣達，鄭宇鋒，呂娜，李詩瑤，祁宇峰

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

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

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

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

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

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

(1)

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

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

(2)

2 定向搜索的A*算法

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

(1)定向搜索策略

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

(3)

具體為以下四種情況：

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

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

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

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

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

(a) 障礙物在直線上

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

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

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

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

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

(3)“拉直”處理策略

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

(a) 起始點與目標點之間無障礙情況示意圖

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

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

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

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

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

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

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

圖4 改進后的A*算法流程圖

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

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

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

7)輸出路徑.

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

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

貝塞爾曲線的定義取決于控制點的個數(shù)，當有n+1個控制點時，就可以確定n次多項式的貝塞爾曲線參數(shù)方程：

(4)

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

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

(5)

貝塞爾曲線上任意一點的曲率公式為：

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

(6)

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

4 基于ROS平臺的仿真實驗

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

4.1 定向搜索的A*算法仿真實驗

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

圖6 A*算法改進前后仿真實驗結(jié)果對比

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

表1 不同算法實驗數(shù)據(jù)結(jié)果對比

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

4.2 貝塞爾曲線平滑路徑仿真實驗

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

圖7 貝塞爾曲線平滑路徑

5 結(jié)論

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