基于節(jié)點(diǎn)優(yōu)化的A＊算法路徑規(guī)劃

蔡 誠，梁利東，賈文友，江 磊

（安徽工程大學(xué) 機(jī)械工程學(xué)院，安徽 蕪湖 241000）

路徑規(guī)劃技術(shù)是機(jī)器人研究領(lǐng)域中的一個重要分支。路徑規(guī)劃問題即是按照某種優(yōu)化指標(biāo)（如時間、距離、能量等）規(guī)劃出一條從給定起始點(diǎn)到給定目標(biāo)點(diǎn)且與所在環(huán)境中的障礙物無碰撞的最優(yōu)或次最優(yōu)路徑[1]。根據(jù)對環(huán)境信息的掌握程度，路徑規(guī)劃技術(shù)包括環(huán)境已知的全局路徑規(guī)劃和環(huán)境未知的局部路徑規(guī)劃[2]。

A*算法在路徑規(guī)劃中的應(yīng)用較為成熟和廣泛，但隨著地圖面積的增大其搜索空間將會產(chǎn)生多余的搜索節(jié)點(diǎn)，導(dǎo)致算法效率降低、規(guī)劃路徑出現(xiàn)不必要的轉(zhuǎn)折等問題[3]。針對這些問題，眾多學(xué)者從不同的角度提高和改進(jìn)了A*算法的搜索效率。本文基于節(jié)點(diǎn)優(yōu)化策略提出了一種改進(jìn)的A*算法，通過對搜索過程進(jìn)行優(yōu)化，減少計(jì)算量和耗時，并通過關(guān)鍵點(diǎn)選取方法，消除路徑中的冗余點(diǎn)，使路徑更加平滑。

1 路徑規(guī)劃環(huán)境建模

環(huán)境建模的目的是為路徑規(guī)劃提供可用的數(shù)據(jù)分析平臺，常用的建模方法主要有柵格地圖法[4]、幾何特征地圖法[5]和Voronoi 圖法[6]。柵格法應(yīng)用最為廣泛，它將搜索環(huán)境分成網(wǎng)格單元，且這些網(wǎng)格單元具有二值信息，柵格的大小決定著環(huán)境信息存儲量的大小以及路徑規(guī)劃時間的長短，所有節(jié)點(diǎn)可分為障礙物節(jié)點(diǎn)和自由節(jié)點(diǎn)兩大類，每個自由節(jié)點(diǎn)連接相鄰的自由節(jié)點(diǎn)組成一個圖，用搜索算法生成從初始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的路徑。由于柵格法地圖的創(chuàng)建和維護(hù)比較容易，每個柵格的信息直接與環(huán)境中某區(qū)域?qū)?yīng)，通過地圖很容易進(jìn)行路徑規(guī)劃，且易于擴(kuò)展到三維環(huán)境[7]。因此本文選擇柵格法進(jìn)行環(huán)境建模，將移動機(jī)器人的起點(diǎn)和目標(biāo)點(diǎn)均看成質(zhì)點(diǎn)并將環(huán)境中任意形狀的障礙物用矩形代替，障礙物的矩形包絡(luò)如圖1所示。

圖1 障礙物的矩形包絡(luò)模型

2 A*算法思想

A*算法是一種靜態(tài)環(huán)境下最優(yōu)的搜索算法，由于其高效性，一經(jīng)提出便被大量地應(yīng)用在路徑規(guī)劃研究中，其核心表達(dá)式為

其中，n表示當(dāng)前被搜索節(jié)點(diǎn)；f(n)表示從起點(diǎn)經(jīng)過當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計(jì)成本；g(n)表示起點(diǎn)到當(dāng)前節(jié)點(diǎn)實(shí)際成本；h(n)為啟發(fā)函數(shù)，表示當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計(jì)成本。

A*算法通過函數(shù)f(n)搜尋每一步代價最小的節(jié)點(diǎn)從而找到代價最小的路徑，一般需要構(gòu)造兩個表：open list 和close list。open list 作用是保存搜索過程中遇到的擴(kuò)展節(jié)點(diǎn)，同時將這些節(jié)點(diǎn)按代價的大小進(jìn)行排序。close list 作用是保存open list 表中代價最小的可擴(kuò)展節(jié)點(diǎn)。主要算法流程為：在二維空間中，以起點(diǎn)為初始，把起點(diǎn)s 放入到open list 中，close list 為空，遍歷open list，查找f(n)值最小的節(jié)點(diǎn)，把它作為當(dāng)前要處理的節(jié)點(diǎn)并把這個節(jié)點(diǎn)移到close list，對其相鄰的8 方向單位柵格中的每一個節(jié)點(diǎn)分別計(jì)算f(n)值。選取子節(jié)點(diǎn)中f(n)值中最小的點(diǎn)作為下一個位置，在close list中加入此節(jié)點(diǎn)，并將該節(jié)點(diǎn)作為新的當(dāng)前節(jié)點(diǎn)（父節(jié)點(diǎn)）。重復(fù)以上步驟，直至找到終點(diǎn)。其中close list 中按照先后順序添加的節(jié)點(diǎn)所組成的路徑就是A*算法所規(guī)劃的路徑。

盡管A*算法已經(jīng)成為移動機(jī)器人導(dǎo)航中廣泛使用的路徑規(guī)劃算法，如何解決算法規(guī)劃路徑中拐點(diǎn)多、轉(zhuǎn)折次數(shù)多的問題是本文的研究重點(diǎn)。

3 基于節(jié)點(diǎn)優(yōu)化的A*算法

3.1 優(yōu)化搜索過程

改進(jìn)A*算法搜索程序結(jié)構(gòu)如圖2 所示。

圖2 改進(jìn)A*算法搜索過程優(yōu)化程序框圖

close list 中保存獲取的從起始節(jié)點(diǎn)開始的路徑節(jié)點(diǎn)，open list 僅保存當(dāng)前擴(kuò)展節(jié)點(diǎn)的后續(xù)節(jié)點(diǎn)，不保存其他已擴(kuò)展節(jié)點(diǎn)的后續(xù)節(jié)點(diǎn)。改進(jìn)算法的估價函數(shù)為：f(pi)=g(pi)+h(pi)，g(pi)表示從起始節(jié)點(diǎn)到節(jié)點(diǎn)pi的路徑長度；h(pi)為啟發(fā)函數(shù)，使用歐幾里得距離，表示從節(jié)點(diǎn)pi到目標(biāo)點(diǎn)之間的直線距離，節(jié)點(diǎn)pi距離終點(diǎn)越近，f(pi)越小，以此引導(dǎo)搜索的方向，使得逐步靠近目標(biāo)點(diǎn)。

算法程序思路用如下文字方式描述：首先設(shè)置各項(xiàng)參數(shù)，創(chuàng)建open list 和close list，把起點(diǎn)加入到close list，若目標(biāo)點(diǎn)不在close list 中，則清空open list，從close list 中選取節(jié)點(diǎn)進(jìn)行擴(kuò)展，將擴(kuò)展節(jié)點(diǎn)的后續(xù)節(jié)點(diǎn)加入open list。計(jì)算open list 中所有節(jié)點(diǎn)的f(pi)，選取f(pi)最小的節(jié)點(diǎn)，將其加入到close list，并修改插入節(jié)點(diǎn)的路徑長度；若目標(biāo)點(diǎn)在open list 中，則直接將其添加到close list，省去計(jì)算估價值和比較估價值的步驟，節(jié)省路徑規(guī)劃時間。擴(kuò)展open list 時，遍歷V中幾何點(diǎn)構(gòu)造的所有節(jié)點(diǎn)，若節(jié)點(diǎn)pi不在close list 中且與close list 的擴(kuò)展節(jié)點(diǎn)可視（之間沒有障礙物），則將其作為一個后續(xù)節(jié)點(diǎn)。

改進(jìn)算法與A*算法的主要區(qū)別是open list 僅保存當(dāng)前擴(kuò)展節(jié)點(diǎn)的后續(xù)節(jié)點(diǎn)，不保存已擴(kuò)展節(jié)點(diǎn)的其他后續(xù)節(jié)點(diǎn)，這大大減少了open list 保存的節(jié)點(diǎn)數(shù)量，減少了計(jì)算量和時耗。

3.2 優(yōu)化搜索路徑

close list 中保存的是一條從起點(diǎn)到終點(diǎn)的無碰路徑，受啟發(fā)函數(shù)的限制，路徑中的某些節(jié)點(diǎn)和折點(diǎn)在存儲路徑的時候是多余的，冗余節(jié)點(diǎn)造成存儲的浪費(fèi)，而冗余轉(zhuǎn)折點(diǎn)會導(dǎo)致機(jī)器人在行走過程中不必要轉(zhuǎn)彎，不利于機(jī)器人的控制。針對這個問題，提出一種搜索路徑關(guān)鍵節(jié)點(diǎn)選取策略，原理如下：

（1）設(shè)x1、x2、x3為不在同一直線上的三個點(diǎn)，若x1-x2連線、x1-x3連線都不與障礙物發(fā)生碰撞，則刪除點(diǎn)x2，保存x1、x3；若x1-x2連線不與障礙物發(fā)生碰撞，x1-x3連線與障礙物發(fā)生碰撞，則保存x1、x2、x3，以x2為下一起點(diǎn)依次向下選取兩點(diǎn)繼續(xù)構(gòu)造三角形。

（2）若選取后的x3、x4和x2為在同一直線上的三個點(diǎn)且x2-x3連線、x3-x4連線均不與障礙物發(fā)生碰撞，則刪除點(diǎn)x3，保存x2、x4；更新路徑，重復(fù)上述操作直至目標(biāo)點(diǎn)。

路徑節(jié)點(diǎn)篩選如圖3 所示，具體步驟如下：

圖3 路徑節(jié)點(diǎn)篩選示意圖

第1 步，以1 為起點(diǎn)，連接1-2、1-3、2-3，均未與障礙物發(fā)生碰撞，刪除點(diǎn)2，保存點(diǎn)1、點(diǎn)3。

第2 步，同理，連接1-4、1-5、3-4、4-5，均未與障礙物發(fā)生碰撞，刪除點(diǎn)3、點(diǎn)4，保存點(diǎn)1、點(diǎn)5。

第3 步，連接1-5、5-6，未與障礙物發(fā)生碰撞，連接1-6，發(fā)生碰撞，保存點(diǎn)1、點(diǎn)5、點(diǎn)6。

第4 步，以5 為起點(diǎn)向下依次選取兩點(diǎn)，點(diǎn)5點(diǎn)6 點(diǎn)7 在同一直線且5-6、6-7 均不與障礙物發(fā)生碰撞，刪除點(diǎn)6，保存點(diǎn)5、點(diǎn)7。

第5 步，更新路徑，重復(fù)上述步驟直至目標(biāo)點(diǎn)8。

篩選后的路徑如圖4 所示。

圖4 路徑節(jié)點(diǎn)篩選后

4 仿真結(jié)果

為了驗(yàn)證本文算法的優(yōu)越性，設(shè)計(jì)仿真實(shí)驗(yàn)來進(jìn)行驗(yàn)證，構(gòu)建20*20 的柵格地圖，可以自由設(shè)置起點(diǎn)和終點(diǎn)及障礙物分布，仿真功能就是實(shí)現(xiàn)從起點(diǎn)至終點(diǎn)尋找一條最優(yōu)路徑。實(shí)驗(yàn)分別采用A*算法和改進(jìn)A*算法進(jìn)行路徑規(guī)劃，路徑仿真結(jié)果如圖5 和圖6 所示。

圖5 A*算法軌跡規(guī)劃

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

分析對比實(shí)驗(yàn)結(jié)果可知：在機(jī)器人起始位置和障礙物位置相同的情況下，改進(jìn)后的算法路徑總長度縮短，節(jié)點(diǎn)和轉(zhuǎn)折點(diǎn)數(shù)量降低，有效提高了路徑平滑度，更加適合移動機(jī)器人的運(yùn)動特性。表1 是以路徑長度、搜索節(jié)點(diǎn)數(shù)、轉(zhuǎn)向次數(shù)以及所需規(guī)劃時間等作為評價指標(biāo)對兩種算法的詳細(xì)參數(shù)對比情況。

表1 實(shí)驗(yàn)路徑參數(shù)表

從表1 可知，本文算法的路徑搜索性能有了一定提升：在路徑長度上減少了38.79%，搜索節(jié)點(diǎn)數(shù)減少了77.27%，轉(zhuǎn)向次數(shù)降低了50%，規(guī)劃時間減少了32.23%?？梢娫撍惴ǜ咝У厮阉鞯阶顑?yōu)路徑，減少了搜索節(jié)點(diǎn)數(shù)，降低了路徑成本，使得路徑更加平滑。

5 結(jié)論

本文通過改進(jìn)A*算法，對搜索過程優(yōu)化，減少計(jì)算量和耗時，并通過關(guān)鍵點(diǎn)選取方法，剔除冗余節(jié)點(diǎn)和轉(zhuǎn)折點(diǎn)，使搜索到的路徑更加平滑。仿真結(jié)果表明，改進(jìn)后的A*算法比原A*算法所規(guī)劃出的路徑長度減少了，拐點(diǎn)和轉(zhuǎn)彎次數(shù)也都減少了，節(jié)省了機(jī)器人路徑規(guī)劃的時間，縮短了路徑長度并提高了路徑的平滑性，從而提高了機(jī)器人的路徑規(guī)劃的效率，更加適用于機(jī)器人的路徑規(guī)劃。