融合改進A*算法和貝塞爾曲線優化的路徑規劃算法

謝春麗，高勝寒，孫學志

(東北林業大學交通學院，哈爾濱 150006)

0 引言

路徑規劃是機器人自動行駛的一個重要組成部分，它的主要目的是在特定的具有障礙地圖環境下，在給定起始點以及終點的條件下，在盡可能短的時間內生成一條最短無碰撞路徑的過程，由于汽車自動駕駛是在機器人以及自動引導汽車AGV(automated guided vehicle)的自動行駛基礎上進行的應用，所以路徑規劃也是汽車自動駕駛的必不缺少的組成部分。在路徑規劃領域，許多學者做了大量的算法研究，探索出了很多算法及改進，這些算法各有優劣，常用的路徑規劃方法主要有以下幾種：Dijksta算法、A*算法、蟻群算法、RRT算法、LPA*算法和D*lite算法等。Dijksta算法是貪心算法模式應用于路徑規劃，優點是產生最優解的路徑，但缺點占用內存大，運行時間長[1]；A*算法是現在最成熟、應用最廣的算法，通過引用啟發性函數，平衡運行速度以及最優解之間的關系，缺點是容易陷入局部最優解[2]；蟻群算法是模仿自然界蟻群尋找食物的過程，添加信息素信息正反饋進而尋找路徑，優點是不易產生局部最優，易于找到最優解，缺點是收斂速度慢[3]；RRT算法探索空間能力較好，但容易受到狹窄通道影響而找不到解[4]；LPA*算法與D*lite算法同為增量啟發式算法，但前者為正向搜索，而后者為反向搜索，兩者一般適用于動態地圖環境[5]。

本文主要以A*算法為基礎，對AGV的路徑規劃進行研究，使用A*算法的改進算法跳點搜索算法(jumppointssearch，JPS)進行啟發性函數改良，從而減少Open_list以及Close_list中不必要的內存占用，同時大幅度減少了算法的運算時間，然后通過貝塞爾曲線(Bézier curve)進行全局路徑平滑性優化，再通過python仿真，驗證在給定一些地圖參數的情況下，能得出有關最優路徑的相關數據，使該算法在應用于自動引導汽車時，可以盡量排除模型理想化的影響。

1 傳統A*算法

對于路徑規劃算法方面，學者們在多年來進行不斷研究，提出了許多穩定成熟的算法，這些算法原理簡單，而且可以進行大量實際運用，如在Dijksta算法基礎上提出的A*算法，其公式如下：

f(n)=g(n)+h(n)

(1)

式中：f(n)為起始點到目標點的估計代價；g(n)為起始點到中間點n的實際代價；h(n)為啟發函數，同時表示中間點n到目標點的估計代價。A*算法主要是通過確定起始點和目標點，建立Open_list以及Close_list 2個列表，由起點出發，把當前點放入Open_list作為父節點，然后在當前節點的8個附近節點進行遍歷尋找子節點，將父節點加入Close_list，新的子節點作為父節點并計算f、g、h3個值，反復進行運算直至Open_list列表中存在目標點，然后進行從目標點到起始點的回溯，并從f值選取最合適的路徑，得出最優路徑。A*算法的流程如圖1所示。

圖1 傳統A*算法的流程框圖

在傳統A*算法當中，一般將g值計算值表征為路徑所經過柵格地圖的節點數，而啟發性函數選取節點到目標點距離，一般為歐里幾德距離度量移動代價：

(2)

式中：(x1，y1)，(x2，y2)分別為節點n1，n2的坐標。對于估價函數f(n)，當g(n)=0時，f(n)=h(n)，估價函數完全取決于啟發函數，A*算法變成使用貪心策略的廣度優先搜索，此時計算速度快，但不一定能獲得最優解；當h(n)=0時，f(n)=g(n)，此時算法變成Dijksta算法，往往能獲得最優解，但此時的計算占用內存大，計算速度慢。

A*算法作為經典路徑規劃算法，雖然可以較穩定地解決路徑尋優問題，但也容易陷入“死循環”，不考慮實際車輛運行情況而產生復雜且繁多轉折點以及在存在動態障礙物的地圖環境中容易碰撞動態障礙物等問題。趙曉等[6]在 A*算法的基礎上，結合跳點搜索算法提出一種改進的A*算法，該算法通過篩選跳點進行擴展，直到生成最終路徑，擴展過程中使用跳點代替A*算法中大量可能被添加到Open_list和Close_list的不必要節點，從而減少計算量。陳豪等[7]提出了基于二維柵格地圖的改進A*算法，其引入了方向矢量和并行搜索，使智能汽車路徑搜索同時具備方向性和并行性。宣仁虎[8]針對A*算法在進行全局路徑規劃時路徑較長，轉折角度過大，路徑不夠平滑的缺點，提出了利用智能蟻群算法對A*算法中的路徑進行迭代優化的改進算法。Zhong等[9]提出了一種新的混合路徑規劃方法，該方法將A*算法與自適應窗口方法相結合，可以在大規模動態環境中為移動智能汽車進行全局路徑規劃，實時跟蹤和避障。張陽偉等[10]提出了一種建立正反向遍歷的Open_list和Close_list一共4個列表，然后采取通過變步長雙向運算，當找到正方向的Open_list和反方向的Close_list存在公共節點時，算法結束得出最優路徑。

綜上所述，A*算法主要存在以下不足：

1)冗余節點會進行過多代價運算，會導致算法運行時間過長；

2)由于Open_list訪問節點過多，所以在運行過程中，會占用大量內存使用空間。

本文的改進算法與A*算法相比，運行時間會大幅縮短以及評估節點數量會在地圖環境較大或高障礙率的條件下大幅減少。

2 融合改進A*算法和貝塞爾曲線優化

2.1 柵格地圖模型建立

考慮自動引導汽車(AGV)主要是在平面空間進行運動，可以進行以下表述：設智能汽車為A，所在的平面空間為Q，整個平面空間可以分為b行b列進行均勻分割b2個均勻方塊區域，對于各個均勻方塊區域，可以進行建立二維坐標系，得到諸多二維坐標，再對每個均勻方塊賦予一個特征值，柵格地圖由柵格M={Mij|Mij=0，1，2，3}組成，0代表該區域為自動引導汽車所在起始柵格qgoal；1代表柵格地圖中自動引導汽車的目的柵格qint，2代表自由區域B，可以進行路徑規劃；3代表存在障礙物的柵格區域C，該區域無法進行路徑規劃，在進行仿真模擬時躲避。則在該平面空間Q中的避障空間就可以表示成為Qa={q∈W|A(q)∩C=?}，其中q為該空間的一點，A(q)表示的是智能汽車在空間所在位置q點。而路徑規劃的目標即尋找由初始位置qint到目標位置qgoal的最短避障路徑。

首先對障礙及其周邊區域的柵格進行賦值，從而確定模擬環境邊界、障礙以及起始點，這樣智能汽車在進行路徑規劃時，可以通過更準確的信息進行規劃，降低AGV撞到障礙的風險。下面為模型的處理。

空間Q的劃分：將平面二維空間均勻劃分成獨立的柵格。

區域邊緣的處理：將區域邊緣視作障礙區域，在規劃路徑過程中將區域邊緣不予以考慮。

2.2 改進A*算法

2.2.1堆結構優化

當AGV進行JPS算法過程中，要進行大量節點代價運算，同時還需要通過代價值進行Open_list以及Close_list中的節點坐標元組的增加和刪除，如果每次都進行坐標的代價比較，將會產生大量的計算，同時會增加較長運行時間。

堆是一種特殊的數據結構，定義為：n個元素的序列{k1，k2，ki，…，kn}當且僅當滿足下關系時，即ki≤k2i且ki≤k2i+1或者ki≥k2i且ki≥k2i+1，則序列{k1，k2，ki，…，kn}為堆。圖2為最小堆的數據結構的運算過程。二叉樹的子節點的數值若是小于父節點的數值，則會進行位置進行置換，以此類推，直至符合所有父節點都小于子節點的數值。

圖2 最小堆的數據結構的運算過程

于是堆總是滿足以下2個性質：

1)堆中某個結點的值總是不大于或不小于其父結點的值；

2)堆總是一顆完整的二叉樹。

于是可以通過最小堆(父節點小于子節點的堆)進行最小代價值的篩選，并快速完成Open_list以及Close_list 2個列表的中節點的篩選。

2.2.2JPS算法

減少A*算法的主要途徑就是減少Open_list以及Close_list中冗雜節點的遍歷過程，而通過跳點可以減少遍歷列表中無關節點的占用內存，馬小陸等[11]提出了在當前節點與目標點之間有無障礙物2種情況進行討論跳點的篩選，本文將對JPS算法對跳點的選擇進行討論。

Case1節點周圍不存在障礙物

如圖3(a)所示，x，g點為柵格地圖上的2個點，xpred為x的父節點，若要由xpred到達g最直接最短路徑就是由xpred→x→g路線進行行進，但是還有其他的路徑規劃方案，例如xpred→a2→a3→g，這種路線明顯可知不是最優路徑，而且進行了復雜節點的運算從而導致A*算法最常見的周圍臨節點的重復訪問。由圖3(a)可知，和第二種的路徑規劃方案類似的非最優方案都會經過圖中的灰色柵格，而要進行跳點的篩選，必須對這些灰色柵格所在的不必要節點進行刪除。所以，當節點的周圍不存在障礙物節點的時候，對于與當前節點在同一水平線或者豎直線上的節點，存在以下的篩選條件：

L(xprep，…，n|x)≤L(xprep，x，n)

(3)

式中：函數L表示路徑的長度；則表示的鄰節點；L(xprep，…，n|x)表示的是以xprep為起點；n為目標點但不經過x的路徑集合；L(xprep，x，n)則表示的是路徑xprep→x→n。

當目標點在非直線柵格方向上如圖3(b)所示，同時在當前節點與目標節點之間并未存在障礙物，同理依據式(4)可得此時目標點在當前節點的非直線柵格方向上的跳點篩選條件：

L(xprep，…，n|x)

(4)

由圖3可知僅由a2，b2，b3為篩選符合條件的跳點，而剩余的灰色節點將被刪除，以免導致計算內存過大和運算繁瑣。

圖3 跳點篩選示意圖

對于這2種情況給出以下條件進行跳點篩選：

1)n不是x的自然鄰節點；

2)L(xprep，…，n|x)>L(xprep，x，n)

(5)

此時節點x周圍存在障礙物柵格，即圖4(a)以及圖4(b)中的黑色柵格，障礙物柵格創建了強制鄰節點(所有滿足篩選條件的x的鄰節點n)，即圖4(a)以及圖4(b)中的紅色柵格。這些鄰節點被創建，是因為一些柵格由于柵格的替代路徑被阻塞而不能被裁剪，圖4(a)中的a3和圖4(b)中的a1都是強制鄰節點，在篩選跳點的時候都放進列表。

圖4 強制鄰節點篩選的一種情況

起始點中間存在障礙物的柵格地圖模型如圖5所示，淺藍色部分為篩選出來的強制性鄰節點，即跳點。對于路徑最優規劃，分別將建立的跳點集合進Open_list，從起點qint以及終點qgoal2個點作為A*算法的起始點，然后通過式(1)，進行A*算法的運算找尋最優路徑。

圖5 JPS算法跳點的篩選

2.2.3改進啟發函數

本文的JPS算法是先進行地圖建模然后進行地圖中跳點的篩選，然后在跳點的基礎上進行A*算法的改進啟發性函數代價運算，從而實現最優路徑尋找。A*算法一般的h(n)為歐里幾德距離(euclidean distance)，或者切比雪夫距離(chebyshev distance)：

d(x，y)=Max{|x1-x2|，|y1-y2|}

(6)

或者Octile距離：

d(x，y)=Max{|x1-x2|，|y1-y2|}+

(7)

或者曼哈頓距離(manhattan distance)：

d(x，y)=|x1-x2|+|y1-y2|

(8)

對于柵格地圖，若為四方向移動，曼哈頓距離(manhattan distance)是最合適的啟發函數。若八方向(相較于四方向增加對角線方向)移動，可以用切比雪夫距離(chebyshev distance)作為啟發性函數。歐里幾德距離(euclidean distance)適合方向自由度更高的地圖作為其啟發函數，但是歐式距離中具有一個開方的過程，計算量會較其他算法更大。而Octile距離主要是用于45°轉彎，比較適合本文中的八方向地圖環境，而且不存在開方等復雜計算。

在地圖環境中的最小估計函數會被地圖中的障礙物影響，h(n)不僅會因為跳躍點之間的代價而改變，而且還會因為障礙物的分布而改變[12]，但該文獻對障礙率對數化后未絕對值化，導致h(n)一直是負優化。本文為了估計最小路徑距離值，設置了障礙率代表地圖環境。障礙率即當前地圖環境下障礙節點數與整體地圖環境節點總數的比，為了減少邊界無關區域障礙的影響，此時障礙率P如式(6)所示，也可以稱之為有效障礙率，是由于將起始點所形成矩形區域以外的障礙物除去，其中式(9)中起始節點為(xs，ys)，終點為(xg，yg)。同時為了使數據更平滑，對障礙率進行對數化處理，然后再與Octile距離進行結合處理作為h(n)，即式(10)。

(9)

(10)

由上式可知，該啟發性函數在障礙率較低時，h(n)將會在f(n)中占比較小，此時搜索速度較快，但尋優性較差，但也符合低障礙率的路徑規劃情景；該啟發性函數在障礙率較高時，h(n)將會在f(n)中占比較大，此時搜索速度較慢，但尋優性較為良好。

由于本文主要是以八方向的移動為主，所以不考慮曼哈頓距離，通過在相同地圖模型環境，相同起始點進行JPS算法，h(n)分別采用以上距離進行仿真，具體數據如表1所示。以下數據搜索時間僅為主算法時間，不包括地圖環境構建以及可視化運行時間。地圖環境為圖6，此時有效障礙率38.4%。表1為不同啟發性函數在相同地圖環境下的運行數據，3種啟發性函數中，在較高的障礙率的地圖中，所生成路徑長度一致，但是切比雪夫距離的評估節點數量較另外2種算法多20.31%，占用內存較大。而改進的啟發性函數，在高障礙率地圖環境中，與歐里幾德距離相比較，評估節點數量和路徑長度一致，但是搜索時間上減少38.01%，通過表1仿真數據決定本文采取改進啟發性函數作為啟發式函數。

圖6 測試啟發式函數地圖環境

表1 同啟發性函數在相同地圖環境下的運行數據

3 貝塞爾曲線的車輛路徑軌跡優化

3.1 貝塞爾曲線相關特性

貝塞爾曲線是由伯恩斯坦多項式(bernstein polynomial)推導而來[13]，貝塞爾曲線一般用來進行折線的平滑化處理，本文運用貝塞爾曲線對改進的算法產生出來的折線路徑進行平滑處理。

伯恩斯坦多項式的第n階項有如下形式：

(11)

(12)

其中，βi叫做伯恩斯坦系數。

由文獻[14]中的結論，高階貝塞爾曲線的數值穩定性較差，為了解決數值穩定性，同時為了滿足AGV的曲率約束，采取二階貝塞爾曲線和三階貝塞爾曲線相結合進行路徑平滑性優化，針對二階和三階貝塞爾曲線如下屬性進行使用。

1)二階貝塞爾曲線表達式如式(13)所示，三階貝塞爾曲線表達式如式(14)所示：

P(t)=P0(1-t)2+2P1(1-t)t+P2t2，

t∈[0，1]

(13)

P(t)=P0(1-t)3+3P1(1-t2)t+

3P2(1-t)t2+P3t3，t∈[0，1]

(14)

2)二階貝塞爾曲線會經過第1、3個控制點；三階貝塞爾曲線經過第1、4個控制點。

3)曲線在端點切向量表達式如下：

P′(t)=-2P0(1-t)+2P1(1-2t)+2P2t2，

t∈[0，1]

(15)

P′(t)=-3P0(1-t)2+3P1(3t2-4t+1)-

6P2(1-t)t+3P3t2，t∈[0，1]

(16)

4)貝塞爾曲線上任意一點曲率為：

(17)

5)貝塞爾曲線軌跡起始點曲率為：

(18)

6)仿射變換不變特性

通過式(17)二階貝塞爾曲線上曲率和三階貝塞爾曲線上曲率可知，路徑軌跡曲線連續條件為x′(t)、y′(t)不同時為0，若x′(t)、y′(t)同時為0，可知κ(t)=0，而這使得路徑軌跡無法到達后續控制點，但這與貝塞爾曲線定義相矛盾，所以由二階貝塞爾曲線確定的軌跡曲線和由三階貝塞爾曲線確定的軌跡曲線曲率處處連續。

對于AGV，控制其貝塞爾曲線軌跡的每一個控制點的曲率是不太容易的，但對于AGV最大前輪轉向角是有一定范圍的，通過式(19)，可以知道控制點曲率與最大前輪轉向角的關系。

(19)

式中：κ為曲率;γ為曲率半徑即轉彎半徑;α為前輪轉向角;L為前后輪軸距，所以存在式(20)，即為曲率有界條件。

Kmin≤κ(τ)≤Kmax

(20)

式中：Kmin和Kmax分別為最小、最大曲率。

貝塞爾曲線具有以下性質：擬合曲線的起始點和終止點與被平滑化處理的折線的起始點和終止點重合；折線點一旦確定，擬合曲線唯一。貝塞爾曲線平滑處理后，使生成路徑與折線相比更貼近實際，更適合AGV的實際運行。

3.2 貝塞爾曲線優化問題以及改進

由于高階貝塞爾曲線數值穩定性較差，所以一般的路徑優化都采取二階和三階貝塞爾曲線優化[14]。在不同控制點的情況，會存在二階貝塞爾曲線以及三階貝塞爾曲線之間的優化選擇，如圖7所示，對前3個點進行二階貝塞爾曲線優化，則第4個點的優化會被遺漏，從而導致更適合的三階貝塞爾曲線優化被二階貝塞爾曲線優先替代，不能達到最優。

圖7 二階貝塞爾曲線與三階貝塞爾曲線判定以及優化

圖8為常見的二階貝塞爾曲線以及三階貝塞爾曲線的優化。至于更多控制點的優化則采取n組二階貝塞爾曲線以及m組三階貝塞爾曲線進行組合優化而不采用數值不穩定高階貝塞爾曲線。

圖8 二階貝塞爾曲線與三階貝塞爾曲線的判定特定節點及其優化

將JPS算法所生成路徑，代入到貝塞爾曲線改進程序中，由于二階貝塞爾曲線和三階貝塞爾曲線所需控制點數量不同，應先判斷能否進行三階貝塞爾曲線優化，若能則進行優化，否則進行二階貝塞爾曲線優化，而對于能否進行三階貝塞爾曲線優化的判斷，通過記錄控制點坐標，然后記錄方向也即4個控制點的3個方向向量，當存在如下9種情況時，即可進行三階貝塞爾曲線優化。

(21)

(22)

(23)

(24)

(25)

(26)

(27)

(28)

(29)

以上的這些公式中的dxn為第n個橫坐標方向變化量，dyn為第n個縱坐標方向變化量。

這樣，全局路徑規劃產生的路徑將會被二階貝塞爾曲線和三階貝塞爾曲線共同優化，而不會產生三階貝塞爾曲線優化中控制點不足以及二階貝塞爾曲線優化中容易遺漏一個控制點而未進行三階貝塞爾曲線優化這2種情況導致的曲線優化不理想的情況。

4 仿真與分析

4.1 改進算法仿真

本文在Window 10，處理器為i7-6900H的環境下通過python3.9進行仿真，為驗證本文的改進JPS算法的可行性，地圖環境的障礙物位置均為隨機選取，在隨機的地圖環境能更好地驗證算法的有效性以及先進性。仿真分別在20*20、30*30、50*50的有效障礙率為20%的低障礙率的地圖環境以及20*20、30*30、50*50的有效障礙率為40%左右的高障礙率地圖環境，分別進行傳統A*算法、JPS算法以及改進JPS算法仿真。同時為驗證改進算法的路徑生成效果以及貝塞爾曲線的優化效果，在驗證啟發性函數的30*30，有效障礙率為38.4%的地圖環境下進行相應的仿真運行，仿真結果如圖9—12所示。

圖9 傳統A*算法的路徑搜索

4.2 評價指標

為從實驗結果中分析算法的有效性，在仿真實驗中選取路徑規劃算法評估節點數量、搜索時間以及產生路徑長度作為算法的評價指標。傳統A*算法、JPS算法、改進JPS算法在較低障礙率地圖環境下測試結果見表2，在高障礙率地圖環境下測試結果如表3。數據運行時間只是主算法運行時間，不包括地圖建模時間，可視化時間以及曲線優化時間。同時運行時間過短會產生波動較大，所以如下數據為進行5次仿真取的平均值。同時由于不同障礙率的地圖環境的障礙分布不同，也會對時間產生一些影響。

圖10 JPS算法所篩選的跳點

圖11 改進JPS算法的路徑搜索

圖12 改進貝塞爾曲線所生成的改進JPS算法所生成路徑

表2 傳統A*算法、JPS算法以及改進JPS算法的路徑搜索在較低障礙率地圖環境下的數據

表3 傳統A*算法、JPS算法以及改進JPS算法的路徑搜索在高障礙率地圖環境下的數據

由表2可知，在低障礙率地圖環境下，在地圖環境簡單的20*20的條件下，無論是評估節點個數還是搜索時間，改進算法與A*算法以及JPS算法相比會存在劣勢，但生成的路徑長度所差不大。當地圖環境變大時，評估節點數會大于A*算法，但會小于JPS算法，而運行時間相對另2種算法在30*30的地圖環境下分別減少99.5%和14.1%，在50*50，20%有效障礙率的地圖環境下分別減少99.7%和9.3%。對于后2組數據，體現改進算法在有效障礙率變大時，評估節點數會大幅減少，而且運行時間相較于本身也減少11.2%。

通過表3發現，雖然在路徑長度上可能略長于A*算法，但路徑長度基本與其他2種算法無異，而且改進算法不論是在評估節點數量與其他2個算法相比，分別減少了14.9%和25%；在搜索時間上，相較于其他2種算法，分別減少99.8%和59.2%。

為了驗證評估節點數量對貝塞爾曲線的擬合效果影響，分別進行了對低障礙率的改進算法的20%障礙率的20*20、30*30和50*50的地圖環境產生的路徑進行貝塞爾曲線優化。優化結果如圖13—15所示，其中綠點為起點，粉色點為終點，發現評估節點數量并不影響擬合效果。

圖13 在20*20地圖環境下的貝塞爾曲線擬合曲線

圖14 在30*30地圖環境下的貝塞爾曲線擬合曲線

圖15 在50*50地圖環境下的貝塞爾曲線擬合曲線

5 結論

對AGV的路徑規劃來說，可行性是前提，快速性是其目標。本文改良JPS算法實現過程中，對軌跡進行了貝塞爾曲線優化，以保證規劃的路徑滿足AGV運動可行性。同時使用了最小堆數據結構對Open_list取最小值進行優化，該方法降低了內存消耗，加快了算法的運算。對比傳統的算法，改良JPS算法的改進之處在于啟發性函數這一改善。在python編程環境下，進行較低有效障礙率以及高障礙率的20*20、30*30、50*50地圖環境下的仿真得出的試驗結果表明，該算法在有效障礙率較低時，運算時間短，尋優結果較好；在有效障礙率較高時，運算時間也較對照組相比更短，訪問節點數較少，節約了內存空間，搜索時間大幅度縮短。對于仿真生成的路徑軌跡，通過改進的貝塞爾曲線優化，滿足了曲率連續有界約束，驗證了算法的有效性與高效性。