沈顯慶， 馬志鵬， 孫啟智， 王 賀

(黑龍江科技大學(xué) 電氣與控制工程學(xué)院， 哈爾濱 150022)

0 引 言

路徑規(guī)劃是機(jī)器人自主導(dǎo)航領(lǐng)域的關(guān)鍵問(wèn)題之一[1]。相較于經(jīng)典路徑規(guī)劃算法如Dijkstra算法[2]、人工勢(shì)場(chǎng)法[3]和蟻群算法[4]等，A*算法具有搜索時(shí)間短、運(yùn)行速度快，不易陷入“早熟”等優(yōu)點(diǎn)。但傳統(tǒng)A*算法的計(jì)算量會(huì)隨著地圖增大而急劇增加，從而導(dǎo)致算法搜索效率降低、規(guī)劃路徑不平滑等問(wèn)題。對(duì)此，楊璐等[5]提出了A*算法的二次規(guī)劃策略，提高計(jì)算效率，但并未考慮AGV的體積和轉(zhuǎn)角，實(shí)用性不強(qiáng)。A.Botea等[6]提出HPA*算法，提高了算法的搜索速度，但規(guī)劃出的不是最佳路徑。王維等[7]通過(guò)對(duì)A*算法的評(píng)價(jià)函數(shù)進(jìn)行改進(jìn)，減少算法運(yùn)行時(shí)間，但普適性不好。F.Duchon等[8]提出了基于A*算法的跳點(diǎn)搜索策略，減小了算法的計(jì)算量，但并未解決路徑中的拐點(diǎn)問(wèn)題?；谏鲜龇治觯P者提出了一種改進(jìn)A*算法，對(duì)傳統(tǒng)A*算法的子節(jié)點(diǎn)選擇進(jìn)行優(yōu)化，提高算法的尋優(yōu)速度，在此基礎(chǔ)上，對(duì)Floyd-Warshall算法進(jìn)行雙向平滑處理，改善路徑質(zhì)量，通過(guò)Matlab平臺(tái)進(jìn)行仿真研究，驗(yàn)證所提算法的正確性及可行性。

1 柵格法環(huán)境建模

柵格法環(huán)境建模是將三維運(yùn)動(dòng)空間抽象為非離散的二維平面，并將其分割成大小相等的若干網(wǎng)格[9]，其中每個(gè)網(wǎng)格都具有二進(jìn)制參數(shù)，利用0-1矩陣來(lái)判斷網(wǎng)格是否被障礙物覆蓋。當(dāng)網(wǎng)格內(nèi)有障礙物時(shí)賦值為1，用黑色表示；當(dāng)網(wǎng)格內(nèi)無(wú)障礙時(shí)賦值為0，用白色表示。其對(duì)應(yīng)關(guān)系如圖1所示。

圖1 柵格地圖對(duì)應(yīng)關(guān)系Fig. 1 Raster map correspondence

假設(shè)機(jī)器人R在有限個(gè)障礙物的二維平面區(qū)域A內(nèi)移動(dòng)，以A的左下角為坐標(biāo)原點(diǎn)，水平方向?yàn)閤軸，垂直方向?yàn)閥軸，構(gòu)建直角坐標(biāo)系xOy。xmax、ymax分別為x、y軸方向上的最大值。以機(jī)器人的步長(zhǎng)ρ對(duì)坐標(biāo)區(qū)域進(jìn)行劃分，每行、每列的柵格數(shù)為

nx=xmax/ρ，

ny=ymax/ρ。

對(duì)于坐標(biāo)區(qū)域的任一柵格，都有確定的坐標(biāo)及相應(yīng)的序號(hào)與之對(duì)應(yīng)，如圖2所示。定義S={1,2,…,25}為柵格序號(hào)集，以坐標(biāo)區(qū)域的左上角為原點(diǎn)，由左向右、自上至下，對(duì)二維平面區(qū)域A進(jìn)行編號(hào)，坐標(biāo)與序號(hào)之間的關(guān)系為

圖2 柵格坐標(biāo)與序號(hào)關(guān)系Fig. 2 Grid coordinate and serial number relationship

xi=((i-1)modnx)+1，

yi=ny-ceil(i/nx)+1，

式中：i——第i個(gè)序號(hào)；

mod——取余運(yùn)算；

ceil——進(jìn)一取整運(yùn)算；

nx、ny——每行、每列的柵格數(shù)；

xi、yi——序號(hào)i所處柵格的中心橫坐標(biāo)和縱坐標(biāo)。

2 A*算法的改進(jìn)

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

A*算法是一種啟發(fā)式搜索算法[10]，通過(guò)啟發(fā)函數(shù)計(jì)算周圍8個(gè)柵格節(jié)點(diǎn)的估價(jià)值，選擇代價(jià)值最小的節(jié)點(diǎn)作為下一步搜索的父節(jié)點(diǎn)，并繼續(xù)搜索這一父節(jié)點(diǎn)周圍相鄰的柵格節(jié)點(diǎn)，不斷循環(huán)直到搜索到目標(biāo)節(jié)點(diǎn)，可在保證得到最優(yōu)路徑的前提下，提高算法的搜索效率。A*算法的代價(jià)函數(shù)為

f(m)=g(m)+h(m)，

式中：f(m)——節(jié)點(diǎn)m的代價(jià)函數(shù)；

g(m)——起始節(jié)點(diǎn)S與節(jié)點(diǎn)m的代價(jià)；

h(m)——節(jié)點(diǎn)m與目標(biāo)節(jié)點(diǎn)T的啟發(fā)式估值。

g(m)一般是固定數(shù)值，相當(dāng)于Dijkstra算法中到起始節(jié)點(diǎn)的最優(yōu)路徑。h(m)是A*算法的關(guān)鍵，直接影響著算法的求解速度和搜索效率。h(m)應(yīng)滿足的條件為

h(m)≤h*(m)，

式中,h*(m)——節(jié)點(diǎn)m到目標(biāo)點(diǎn)T的真實(shí)最小代價(jià)，h(m)與h*(m)差距不能過(guò)大。

同時(shí)為了減小A*算法的計(jì)算量，選用Euclidean距離來(lái)近似替代啟發(fā)函數(shù)為

式中：(xm,ym)——節(jié)點(diǎn)m所處柵格的中心坐標(biāo)；

(xT,yT)——目標(biāo)節(jié)點(diǎn)T所處柵格的中心坐標(biāo)。

2.2 子節(jié)點(diǎn)的優(yōu)化選擇

傳統(tǒng)A*算法在規(guī)劃路徑時(shí)，將機(jī)器人當(dāng)作一個(gè)質(zhì)點(diǎn)，忽略了機(jī)器人的體積和運(yùn)動(dòng)特性，因此，規(guī)劃出的路徑是理想路徑[11]，可能會(huì)產(chǎn)生規(guī)劃路徑斜穿障礙物頂點(diǎn)的情況，存在路徑安全性問(wèn)題，規(guī)劃路徑風(fēng)險(xiǎn)如圖3所示。

圖3 規(guī)劃路徑風(fēng)險(xiǎn)Fig. 3 Planning path risk

當(dāng)機(jī)器人從柵格A移動(dòng)到柵格B時(shí)，考慮到機(jī)器人具有一定的體積，因此，在紅點(diǎn)處可能會(huì)發(fā)生碰撞，使機(jī)器人損壞。

對(duì)此，文中通過(guò)對(duì)A*算法子節(jié)點(diǎn)的擴(kuò)展順序進(jìn)行改進(jìn)，降低在實(shí)際運(yùn)動(dòng)過(guò)程中與障礙物發(fā)生碰撞的風(fēng)險(xiǎn)，相鄰節(jié)點(diǎn)優(yōu)先分組，如圖4所示。設(shè)當(dāng)前節(jié)點(diǎn)為O，周圍相鄰節(jié)點(diǎn)分別為A、B、C、D、E、S、W、N。

圖4 相鄰節(jié)點(diǎn)優(yōu)先級(jí)分組Fig. 4 Priority grouping of adjacent nodes

將這些鄰域節(jié)點(diǎn)進(jìn)行等級(jí)劃分，其中，E、S、W、N四個(gè)節(jié)點(diǎn)為高級(jí)組，將A、B、C、D四個(gè)節(jié)點(diǎn)為普通組。在子節(jié)點(diǎn)的生成過(guò)程中，首先搜索高級(jí)組中的子節(jié)點(diǎn)，再根據(jù)表1所示規(guī)則生成普通組的子節(jié)點(diǎn)。

表1 普通組子節(jié)點(diǎn)生成規(guī)則

2.3 路徑平滑處理

雖然對(duì)A*算法子節(jié)點(diǎn)的選擇進(jìn)行優(yōu)化，可以提高規(guī)劃路徑的安全性，但并未克服規(guī)劃路徑轉(zhuǎn)折次數(shù)多，路徑不平滑等問(wèn)題[12]。針對(duì)這些問(wèn)題，在優(yōu)化選擇子節(jié)點(diǎn)的基礎(chǔ)上，將雙向平滑理念引入到Floyd-Warshall算法中對(duì)A*算法進(jìn)行改進(jìn)，同時(shí),設(shè)置障礙物的安全距離，避免碰撞風(fēng)險(xiǎn)。

Floyd-Warshall算法[13]是一種簡(jiǎn)單且廣泛使用的動(dòng)態(tài)規(guī)劃算法，其不需要設(shè)定相應(yīng)的參數(shù)和估計(jì)函數(shù)，通過(guò)建立兩點(diǎn)之間路徑長(zhǎng)度的二維數(shù)組來(lái)計(jì)算最短路徑。將雙向平滑理念引入到Floyd-Warshall算法中，即在優(yōu)化正向路徑的基礎(chǔ)上，加入目標(biāo)點(diǎn)T到起始點(diǎn)S的反向優(yōu)化。具體改進(jìn)步驟如圖5所示。

圖5 改進(jìn)Floyd-Warshall算法優(yōu)化路徑 Fig. 5 Improved Floyd-Warshall algorithm to optimize path

Step1對(duì)路徑中同一直線上的中間冗余節(jié)點(diǎn)進(jìn)行刪除，僅保留起始點(diǎn)S、拐點(diǎn)和目標(biāo)點(diǎn)T。刪除冗余節(jié)點(diǎn)后的路徑為S→p1→p2→p3→T。

Step2從起始點(diǎn)S開始，在保留節(jié)點(diǎn)pi、pj之間每q步取一節(jié)點(diǎn)，判斷取的節(jié)點(diǎn)與上一路徑節(jié)點(diǎn)之間有無(wú)障礙物。若障礙物存在，則路徑節(jié)點(diǎn)不變；若障礙物不存在，則計(jì)算障礙物與節(jié)點(diǎn)pi、pj之間連線的距離。

障礙物距離如圖6所示，假設(shè)節(jié)點(diǎn)O所在的灰色柵格為障礙物柵格，其坐標(biāo)為(xO,yO)；SP為規(guī)劃的路線，S點(diǎn)坐標(biāo)為(xS,yS)，P點(diǎn)坐標(biāo)為(xP,yP)；障礙點(diǎn)O到SP的距離為dOB，與規(guī)劃路線SP相交于點(diǎn)A，距離為dOA；路線SP與x軸之間的夾角為β。

圖6 障礙物距離Fig. 6 Obstacle distance

由S、P及O三點(diǎn)坐標(biāo)可推導(dǎo)出dOB為

dOB=dOAcosβ，

根據(jù)柵格的大小將路徑安全距離設(shè)置為d，當(dāng)d>dOB時(shí)，該路徑可選擇，否則該路徑不可選擇，加入安全距離后的路徑為S→p*3→T。

Step3自目標(biāo)點(diǎn)T反方向取點(diǎn)判斷，其判別方法與Step2相同。反向優(yōu)化后的路徑為S→p*2→T。

Step4輸出路徑，算法結(jié)束。

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

為驗(yàn)證上述理論分析及改進(jìn)A*算法的有效性，在Matlab 2018a[14]實(shí)驗(yàn)平臺(tái)下分別對(duì)傳統(tǒng)的A*算法、優(yōu)化選擇子節(jié)點(diǎn)改進(jìn)的A*算法和雙向Floyd-Warshall改進(jìn)的A*算法進(jìn)行2組仿真對(duì)比，以避免實(shí)驗(yàn)結(jié)果的偶然性。其中d=0.8 m，q=0.1 m，柵格大小為1 m。黑色柵格為障礙物區(qū)域，占地圖總面積分別為19.5%和13.5%，灰色柵格為遍歷的節(jié)點(diǎn)區(qū)域，圓形形狀代表起始位置，三角形狀代表目標(biāo)位置。兩組柵格地圖路徑規(guī)劃結(jié)果如圖7、8所示。同時(shí)對(duì)比3種算法在兩種不同地圖下的路徑長(zhǎng)度和搜索時(shí)間，結(jié)果如表2所示。

表2 3種算法的路徑長(zhǎng)度和搜索時(shí)間數(shù)據(jù)對(duì)比

由表2的對(duì)比數(shù)據(jù)可得，優(yōu)化選擇子節(jié)點(diǎn)改進(jìn)的A*算法與傳統(tǒng)的A*算法路徑長(zhǎng)度相差不大，但前者搜索效率更高，搜索時(shí)間平均減少了35.44%；雙向Floyd-Warshall改進(jìn)的A*算法在優(yōu)化選擇子節(jié)點(diǎn)的基礎(chǔ)上刪除了路徑中的冗余節(jié)點(diǎn)，轉(zhuǎn)折次數(shù)較傳統(tǒng)的A*算法平均減少了37.5%，路徑長(zhǎng)度和搜索時(shí)間平均減少了1.8%及41.06%，提高了路徑規(guī)劃性能。

由圖7～8的仿真波形可得，傳統(tǒng)A*算法所規(guī)劃的路徑存在斜穿障礙物頂點(diǎn)的情況，普適性較差。改進(jìn)后的A*算法在設(shè)置安全距離后，規(guī)劃路徑與障礙物的距離始終大于d，避免了移動(dòng)機(jī)器人距障礙物過(guò)近而發(fā)生碰撞的風(fēng)險(xiǎn)，提高了路徑的安全性。雖然，優(yōu)化選擇子節(jié)點(diǎn)改進(jìn)的A*算法較傳統(tǒng)A*算法轉(zhuǎn)折次數(shù)有所增加，但尋優(yōu)時(shí)間顯著降低，僅為傳統(tǒng)A*算法的64.56%，控制效果較好。雙向Floyd-Warshall改進(jìn)的A*算法拐點(diǎn)和折點(diǎn)數(shù)量較傳統(tǒng)A*算法有所減少，規(guī)劃的路徑更加平滑，在實(shí)際應(yīng)用中，可以減少機(jī)器人運(yùn)動(dòng)過(guò)程中的轉(zhuǎn)動(dòng)角度，有效提高移動(dòng)機(jī)器人運(yùn)動(dòng)的平穩(wěn)性和工作效率，具有一定的實(shí)用價(jià)值。

圖7 第1組柵格地圖路徑規(guī)劃結(jié)果Fig. 7 First group of grid map path planning results

圖8 第2組柵格地圖路徑規(guī)劃結(jié)果Fig. 8 Second group of grid map path planning results

4 結(jié) 論

(1)為提高A*算法的搜索效率及規(guī)劃路徑的安全性，提出在A*算法的基礎(chǔ)上對(duì)子節(jié)點(diǎn)的擴(kuò)展順序進(jìn)行優(yōu)化，將8個(gè)領(lǐng)域節(jié)點(diǎn)劃分為高級(jí)組和普通組，縮短尋優(yōu)時(shí)間，避免規(guī)劃路徑斜穿障礙物頂點(diǎn)。

(2)針對(duì)A*算法搜索路徑拐點(diǎn)個(gè)數(shù)多、路徑不平滑等問(wèn)題，對(duì)Floyd-Warshall算法進(jìn)行雙向平滑處理，在優(yōu)化正向路徑的基礎(chǔ)上，添加反向優(yōu)化，從而減少轉(zhuǎn)角次數(shù)，改善路徑質(zhì)量。

(3)改進(jìn)后的A*算法在搜索時(shí)間、路徑長(zhǎng)度和轉(zhuǎn)折次數(shù)方面均優(yōu)于A*算法，路徑的曲率變化連續(xù)，更加符合機(jī)器人的運(yùn)動(dòng)控制，具有一定的現(xiàn)實(shí)意義。