基于改進A*勢場法的機器人動態路徑規劃研究

王曉燕，呂金豆

（西安建筑科技大學，西安 710000）

0 引言

路徑規劃問題一直是機器人研究領域的一個重點問題，其目的是在含有障礙物的環境中，尋找一條從起始位置到達目標位置的無碰撞路徑[1]。在一個含有動態障礙物的環境中對機器人進行路徑規劃是當下的一類研究熱點[2]。

而障礙已知的環境中，障礙又分為靜態障礙和動態障礙，動態障礙一般使用的算法如：滾動路徑規劃法[3]、人工勢場法、蟻群算法[4]、遺傳算法[5]、D*算法[6]等。人工勢場法（Artificial potential field method，APF）由Khatib于 1986 年提出的[7]，它的基本原理是：將移動機器人在一定環境中的運動虛擬為一種在抽象的人造勢力場中的運動，目標點和機器人間產生引力，力的大小與兩者距離成正比；障礙物和機器人間產生斥力，力的大小與兩者距離成反比。通過求合力來對移動機器人的運動進行控制。

在尋路問題上，作為一種較為完善的方法，人工勢場法以其局部實時性、平滑安全性等特點，常被用于含動態障礙物的避障算法中，但其明顯缺少全局搜索能力，較易出現局部最優解、目標點不可達等停滯問題[8]。而A*算法[9]以其全局最優性、完備性和高效性時常用于全局最優路徑規劃中，是一種典型的靜態路徑全局規劃算法，但其無法做到實時避障，不適于含有動態障礙物的環境。

本文的主要思路是在含動態障礙物的模擬環境中，對傳統人工勢場法改進并與A*算法相結合，規劃出一條移動機器人由起始點到目標點的無碰撞路徑。當未進入動態障礙物影響范圍ρ0時，移動機器人按照A*算法所規劃的路徑行使；進入影響范圍ρ0時，采用改進人工勢場法進行實時動態避障。以此來彌補傳統人工勢場法的靜態陷阱和震蕩問題，并有效減小路徑長度。

1 基本算法原理描述

1.1 人工勢場法

傳統人工勢場法是在建模后的環境中假設有虛擬的引力場和斥力場存在，二者的力的作用平衡使得移動機器人能夠躲避障礙。對人工勢場法進行如下定義：

機器人R的當前方位為X=（x，y），目標位置G為Xg=（xg，yg），式(1)為機器人與目標點之間的引力場：

由該引力場所生成的引力：

其中Katt是引力增益系數，|X-Xg|是機器人與目標點的直線距離。機器人與障礙物之間的斥力場如下式：

由斥力場所生成的斥力：

其中Krep是斥力增益系數，Xobs是障礙物的位置，|X-Xobs|是機器人與障礙物的直線距離，ρ0是障礙物的影響距離。

綜合上式，分別可得移動機器人在運動空間中的合勢場及合力：

1.2 A＊算法

A*算法是一種經典的啟發式算法，能夠在靜態環境中高效地求出從起始點到目標點的最優路徑[10]。將其應用于移動機器人全局路徑規劃，其原理可簡述為：將移動機器人的運動環境分割為若干相同的節點，創建兩張表OPEN LIST與CLOSED LIST，CLOSED表用來記錄已訪問過的節點，OPEN表用來保存所有已生成而未檢查的節點。

首先進行初始化，將起始點START放入OPEN表中，CLOSE表清空；

算法開始，從OPEN表內取第一個節點n，若n是目標解則繼續尋找或終止算法，若不是，則按一定規則展開與n點相關的子節點，并將非障礙物、地圖邊緣的子節點以及n放入CLOSE表，其他放入OPEN表；

計算每一個子節點的估計值f（n），將OPEN表按照f（n）由小到大排序；

重復上述過程直至目標點。保存這些f(n)最小的節點，將它們沿目標點到起始點連接，便得到A*算法計算出的全局最優路徑。

估價函數為：

式中，f（n）表示起始節點經由節點n到目標節點的估計代價值；g（n）表示在狀態空間中從起始節點到節點n的實際代價值；h（n）表示節點n到目標節點的最小路徑的估計代價值。

2 改進A＊勢場算法

2.1 改進依據

本文采用A*算法進行移動機器人的全局路徑規劃，改進勢場法實現局部在線規劃。利用MATLAB R2014a軟件對兩種基本算法進行仿真，通過大量的仿真與分析，驗證了在靜態環境中，A*算法較人工勢場法全局路徑規劃在軌跡震蕩和路徑長度問題上的表現更加優越。

選取如圖1所示位置隨機的10個障礙物，用以兩種算法仿真普遍結果的比對。從圖中可以看出，當運動環境相同時，采用A*算法進行全局路徑規劃能夠高效減少軌跡轉折次數，消除震蕩，并解決了人工勢場法目標點不可達的問題。

圖1 A*算法與人工勢場法全局靜態路徑

表1 A＊算法與人工勢場法仿真結果對比

由表1可知在相同環境進行全局路徑規劃時，人工勢場法路線較長，累計轉折角遠大于A*算法，并且出現了震蕩、轉折次數多、規劃路徑不平滑等現象。A*算法路線轉折次數少，累計轉折角度遠小于人工勢場法且路徑較平滑。因此本文選用A*算法對已知環境進行全局路徑規劃，人工勢場法作為局部規劃算法。

2.2 引入速度勢場模型

2.2.1 速度斥力場

針對動態環境，本文對傳統人工勢場法的斥力勢場進行了改進，即引入相對速度斥力勢場。對局部動態避碰問題做了如下簡化：已知機器人和障礙物的運動狀態、兩者的相對位置矢量以及相對速度矢量；視機器人與障礙物為質點；移動機器人與障礙物都具有全方位的運動能力。

記機器人剛好進入障礙物影響范圍的時刻為t，圖2為t時刻移動機器人與障礙物的運動狀態，此時生成虛擬目標點G，以速度沿A*算法路徑移動，并與移動機器人速度重合，障礙物以速度作直線運動駛向機器人。

圖2 t時刻移動機器人與障礙物運動狀態

設定速度勢場為：

2.2.2 速度引力場

機器人駛離動態障礙物后需回到原A*路徑上，因此位于A*路徑上的虛擬目標點G是動態的，由t時刻產生，直到移動機器人速度與其再次重合時消失。圖3為期間任意時刻t'移動機器人與障礙物的運動圖示。建立虛擬目標點G的引力勢場模型如式(9)所示，該模型由相對位置和相對速度兩部分勢場函數構成：

圖3 t'時刻移動機器人與障礙物運動狀態

結合上述公式，可求得引入速度勢場后機器人所受的合力：

當移動機器人進入障礙物影響范圍ρ0，且兩者運動方向相靠近時，機器人所受總合力由綜合引力Fatt、斥力Frep及速度斥力Frv構成；當移動機器人進入障礙物影響范圍，但兩者的運動方向相背離時，機器人所受總合力由綜合引力Fatt及斥力Frep構成；當移動機器人在影響范圍之外，總合力為0。

2.3 改進算法具體步驟

圖4所示為算法流程圖，改進算法具體步驟為：

步驟1：初始化參數。選擇起始位置xStart、yStart，目標位置xTarget、yTarget，創建列表OPEN、CLOSED，將起始節點設為第一個節點。

步驟2：初始化參數。設置人工勢場法步長l，循環迭代次數J，增益系數Katt、Krep，速度斥力常量λrv，速度勢場增益系數Kattp和Kattv等其他參數的初始化。

步驟3：A*算法路徑規劃。將起始節點放入關閉列表CLOSED，更新后續節點放入打開列表OPEN，對后續節點做最小f（n）檢查并選取。遍歷列表，通過最后一個節點（如果它是目標節點）開始，然后識別它的交節點，直到它到達起始節點，生成靜態最優路徑。

步驟4：改進勢場法動態避障。進入ρ0后，勢場法開始運行，虛擬目標點沿步驟3）生成路徑以運動。移動機器人依據式(8)進行避碰，依據式(9)回到步驟3）生成路徑，使得保存機器人走過的每個坐標，生成路徑。

步驟5：輸出最優路徑。將步驟4中求出的動態路徑與步驟3）求出的靜態路徑相結合，可得出本文全局動態路徑規劃最優路徑。

圖4 算法流程

3 實驗研究與仿真分析

將本文的改進A*及勢場算法應用于機器人動態路徑規劃問題求解，為驗證本文算法的可行性和有效性，進行大量仿真實驗，并通過與傳統人工勢場法進行比對，驗證本文算法的優越性。算法運行環為：Windows10 64bit，仿真軟件MATLAB R2014a。

為了驗證改進算法動態避障的可行性，選取靜態障礙物位置同圖1。設改進勢場法初始參數引力場常量Katt=2，速度引力常量Kattv=2，斥力場常量Krep=1，速度斥力常量λrv=1，斥力影響范圍ρ0=1.5，步長l=0.1，步長設置不同，每一次循環迭代機器人移動的距離不同。選取四個不同循環迭代次數下，移動機器人的實時路徑，如圖5所示。其中實心方塊為動態障礙物，沿y軸負方向作速度為0.1/s的勻速直線運動，空心方框為靜態障礙物，粗實線為A*算法路徑，細實線為改進勢場法路徑。

圖5(a)表示改進算法開始，機器人剛好進入移動障礙物影響范圍，移動機器人開始局部路徑規劃；圖5(b)、圖5(c)分別表示第10步與第18步，移動機器人基于改進勢場法進行動態避障；圖5(d)表示機器人在第25步完成避碰要求，開始追蹤原路徑。

圖5 改進算法動態避障實時路徑

當機器人行駛至第36步，即循環迭代次數J=36時，移動機器人追蹤到虛擬目標點，并繼續沿A*算法規劃的原路徑繼續行使至目標點，路徑規劃結束。圖5驗證了基于本文改進算法的移動機器人對動態路徑規劃的可行性。

將移動機器人走過的位置相連，得到最終全局規劃路徑，如圖6(a)所示，與人工勢場法路徑圖6(b)相比，可以看出本文算法改善了動態避障中人工勢場法的震蕩問題，使得路徑更加平滑，轉折次數更少。對比改進算法與人工勢場法仿真數據，如表2所示。

圖6 改進算法與人工勢場法全局動態路徑規劃最優路徑

表2 兩種算法仿真結果對比

通過表2可得出，本文算法較人工勢場法規劃路徑長度減少3.1712，累計轉折角度減少12.5574rad，運行時間相當。

4 結語

本文將A*算法與改進的人工勢場法相結合，提出了一種改進的動態避碰策略。利用MATLAB仿真軟件，模擬在含有動態障礙物的環境中，對移動機器人從起始點至目標點進行了全局的動態路徑規劃，同時達到避碰要求。

1）該算法利用A*算法求得的初始路徑作為全局最優路徑，改進勢場法執行動態避障，兩者結合轉換，使得傳統人工勢場法動態路徑規劃中目標點不可達的問題得到了解決，并消除震蕩現象。

2）針對局部動態避障問題，提出了引入速度斥力場對障礙物的運動進行判斷；當移動機器人駛離障礙物影響范圍時，提出并解決了“動態追蹤”問題，引入速度引力場，使機器人駛回原A*規劃的路線，簡化了結合算法的規劃策略。

3）改進算法對動態障礙物進行規劃，與全局勢場法規劃相比，最優路徑長度降低約20.4%，累計轉折角度減少約86.6%。