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

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

（安徽工程大學 機械工程學院，安徽 蕪湖 241000）

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

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

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

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

圖1 障礙物的矩形包絡模型

2 A*算法思想

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

第5 步，更新路徑，重復上述步驟直至目標點8。

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

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

4 仿真結(jié)果

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

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

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

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

表1 實驗路徑參數(shù)表

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

5 結(jié)論

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