改進A*算法的智能車路徑規劃研究

楊 瑤，付克昌，蔣 濤 ， 向澤波，劉甲甲

(1.成都信息工程大學 控制工程學院，成都 610225; 2.中國科學技術大學 軟件學院，合肥 230000)

0 引言

隨著智能駕駛技術的快速發展，將智能車運用在日常生活中的可能性也越來越大。而智能駕駛技術是實現智能汽車與智能交通的關鍵技術，也是未來汽車發展的趨勢[1]。目前，學者們對路徑規劃方法進行了大量研究，主要包括：基于采樣的快速搜索隨機樹[2]等方法；基于節點A*算法[3]等；基于模型的人工勢場法[4]等；基于生物啟發式的蟻群算法等[5]。其中，A*算法是能夠有效求解出最優路徑的完備規劃算法[6]，也是在機器人上使用最廣泛的規劃算法之一。但傳統A*算法仍存在一些問題。如：所規劃的路徑不平滑[7]，路徑緊貼障礙物[8]和不滿足車輛運動學等問題。針對上述問題，本文通過對車輛進行建模分析，得到車輛最大轉向角約束和最大曲率約束，并與A*算法的啟發函數相結合使得擴展節點的代價值更為合理。同時，提出車身輪廓代價消除傳統A*算法在擴展過程中遍歷不合理的節點。并提出障礙物距離代價，解決了所規劃的路徑貼合障礙物的問題。然后，提取出改進A*算法的轉折點，并將車輛最大曲率約束與轉折點相結合得到適合智能車行駛的轉折點，再使用貝塞爾曲線擬合轉折點得到最優路徑。通過上述改進使得A*算法所規劃出的路徑更加符合車輛的運動學特性并便于實現智能車的路徑跟蹤。

1 車輛的運動學模型

車輛是一個非完整性約束的系統[9]，分析較為困難，因此本文只考慮車輛的簡化運動模型(如圖2所示)。本文的實驗平臺為野馬E70電動汽車(如圖1所示)。在圖2中的慣性坐標系XOY下，(x,y) 為車體在此坐標系下的坐標位置，φ為車體的橫擺角(航向角) (φ<φmax)，l=1.56米為智能車的軸距，R為智能車的轉彎半徑，θ為車輛縱軸與x軸之間的夾角，D=1.835 m為智能車的車寬。本文中只考慮車輛在二維平面上運動，不考慮俯仰和側傾帶來的影響[9]。在車輛運動瞬間，車輛的速度與車體保持平衡，即滿足:

dx*cos(θ)-dy*sin(θ)=0

(1)

(2)

圖1 基于野馬E70電動汽車改造的智能車駕駛平臺

圖2 本文車輛簡化運動學模型

Kmax=16 m-1車輛最大轉彎曲率，Rmin=8 m 車輛最小轉彎半徑，φmax=40°車輛最大轉向角。

2 A*算法

A*算法是一種在靜態路網中求解最優路徑的最有效的搜索算法，也是解決其他搜索問題的有效算法。A*算法的評價函數的一般形式為：

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

(3)

其中：f(n)是從初始點到節點n的代價和節點n到目標點的估計代價的總和，g(n)是在狀態空間中從初始點到節點n的實際代價，h(n)是從節點n到目標點的估計代價。

A*算法的基本步驟:

1)在柵格地圖上獲得起始點和目標點并初始化開啟列表和關閉列表。

2)將起始點添加入開啟列表中，并將起始點作為父節點添加到關閉列表中。

3)將父節點四周與障礙物無碰撞的擴展節點添加到開啟列表中并計算其擴展節點的代價值，再判斷擴展節點是否為目標點。如果是，則從關閉列表中得到路徑，結束規劃。否則執行步驟4)。

4)從開啟列表中，取得最小代價值的節點作為父節點并將此節點添加到關閉列表中。 再執行步驟3)。

傳統的A*算法的h(n)多數采用Manhattan距離和Euclidean 距離進行估計，并未考慮車輛的運動學特性和車輛的體積。因此傳統A*所規劃出來的路徑往往不具有連續的曲率，且因轉折次數過多等導致不滿足車輛運動學特性。

3 改進A*算法

為了解決傳統A*算法所規劃的路徑不適用于智能車的問題，本文對傳統A*算法進行改進。首先，將傳統A*算法的啟發函數h(n)，用連續曲線替代。其次，提出車身輪廓代價和障礙物距離代價解決所生成的路徑貼合路沿的問題。同時，對車輛進行運動學建模得到車輛約束條件，將約束條件帶入A*算法所規劃路徑的轉折點中。最后，使用貝塞爾曲線對轉折點進行擬合。

改進 A*算法中的評價函數定義為：

f(n)=g(n)+h(n)×(1+o(n))+s(n)

(4)

(5)

其中：o(n)是障礙物距離代價，s(n)是智能車輪廓代價。公式(5)中，當g(n)

3.1 對函數h(n) 進行改進

由于傳統A*算法的h(n)一般使用Euclidean距離和Manhattan 距離對擴展節點的代價值進行估計，所以往往會造成擴展節點被過小估計或者過大估計。比如：Euclidean距離(如圖3(a)所示)通過計算擴展節點到目標點的直線距離會造成對h(n)估計過小的問題。而Manhattan 距離(如圖3(b)所示)是由擴展節點和目標點的x和y絕對值之差相加所得到，會造成對h(n)估計過高的問題。因此，本文使用圓弧曲線對h(n)進行估計較為合理(如圖3(c)所示)。此外，由于智能車行駛路線為連續可導的曲線，而Euclidean距離和Manhattan 距離都是不連續且估計的代價值相較圖3(c)所估計的代價值誤差較大。

圖3 啟發函數h(n)代價值示意圖

(6)

(7)

圖4 曲線估計示意圖

3.2 車身輪廓代價s(n)

因為智能車具有一定的體積且存在著最小轉彎半徑的問題，所以不能當成一個質點處理。同時，傳統A*算法在擴展過程中會擴展到貼合障礙物的節點(如圖9中的擴展節點)，這對于智能車來說是沒有必要的。此外，傳統A*算法規劃中未能考慮路徑與障礙物的距離。這些問題導致所規劃的路徑不能很好被智能車所執行。對于這個問題，馬飛[8]等人提出碰撞威脅代價來解決A*算法所生成的路徑貼合障礙物的問題，但是通過在每個擴展節點上加入鏟運機9個輪廓特征點來計算碰撞代價，會導致程序計算量增加。因此，本文通過判斷擴展節點p(n)=(x1,y1)四周距離的障礙物檢測點(如圖8所示)是否有為障礙物來對c(n)進行賦值。如果有，則將該節點賦為無窮大，否則賦為零。

圖5 障礙物監測點示意圖

為了測試本文的車身輪廓代價，本文參照文獻[8]的方法在智能車的簡化模型上加入碰撞檢測點，如圖9所示。同時，采用文獻[8]的方法對擴展節點進行打分判斷，并在圖7的測試點上進行測試分析(L=5.555米表示車長，D表示車寬)。

圖6 智能車簡化模型的碰撞檢測點

圖7 測試點坐標位置顯示

圖7中的坐標點為所測試的起始點和目標點，其中S表示起始點，分別選取G1,G2,G3,G4,G5作為目標點。

圖8 兩種不同的方法在測試點的測試對比

在圖8(a)中，x軸表示在圖7所示的測試點，y軸表示規劃出路徑的距離；在圖8(b)中，x軸表示在圖7所示的測試點，y軸表示規劃出路徑的時間。同時，通過圖8的(a)和(b)所示，采用車身輪廓代價和典型的懲罰代價所規劃的路徑和時間對比可知：兩種方法所規劃的距離相差不大，但是所用的時間存在的一定差距。相比使用典型的懲罰代價，通過使用本文的車身輪廓代價能夠減少算法運行的時間。此外，雖然加入車身輪廓代價的A*算法所規劃的距離有一定的增加，但是所用時間比傳統A*算法所用的時間少，這說明本文的車身輪廓代價是具有一定的優勢。

本文優化步驟如下:

1)判斷擴展節點四周的障礙物檢測點是否為障礙物。

2)如果是，則在擴展節點上增加障礙物標志位。否則，進行執行3)。

3)判斷擴展節點是否為目標點。如果否，則執行步驟1);如果是則形成路徑。

3.3 障礙物距離代價o(n)

圖9 道路示意圖

為了進一步使規劃的路徑更為合理和可靠，本文采用Voronoi地圖對規劃所需要的柵格地圖進行處理[10]。其中，Voronoi地圖是距離地圖和廣義泰森多邊形相結合組成。首先，通過Voronoi地圖獲得擴展節點到達障礙物的最近距離dmin。然后，將該距離作為約束條件帶入到代價函數中，如式8所示：

(8)

在式(8)中，δ表示障礙物安全距離代價的常數。dmin取倒數是因為A*算法是選擇代價值最小的節點作為擴展節點，距離障礙物越遠的節點對于智能車來說應該是越安全，因此應該優先考慮該節點。

圖10為加入障礙物距離的A*算法和傳統A*算法在圖7上的測試結果。其中，在圖10(a)中，x軸表示在圖7所示的測試點，y軸表示規劃出路徑的距離；在圖10(b)中，x軸表示圖7中的測試點，y軸表示規劃出路徑的時間。通過圖10(a)可以看出加入障礙物距離后的A*算法和傳統A*算法在測試點所規劃的路徑距離大致相等。在G1,G2所測試的時間差距不大，但是在后面的測試結果中傳統A*算法所用的時間與改進后的A*算法所用時間差距越來越大，如圖10(b)所示。

圖10 兩種不同的方法在測試點的測試對比

改進A*算法流程如圖11所示。

圖11 改進A*算法流程圖

3.4 路徑優化

由于車輛在轉向中存在最小轉彎半徑，不能像麥克納姆輪一樣全向移動。而傳統的A*算法所規劃出的路徑轉折數目太多且不夠平滑，使得車輛在貼合路徑時，搖擺不定。所以必須對規劃出來的路徑進行一步優化，使其路徑更加適用于車輛的運動特性。優化步驟如下:

1)首先獲得改進A*算法的路徑path1，并刪出冗余點[11]，得到轉折點。

2)依次從path1中取pose[i-2],pose[i-1],

pose[i]進行擬合y(x)1=a*x2+b*x+c。同時，判斷提取出來的轉折點中，是否包含終點。如果含有終點，保存轉折點到path2，結束對轉折點優化的過程。否則，執行下一步。

4)重復步驟2)。

(9)

K

(10)

indexnew=

(11)

indexnew是移動后節點的索引,index是此刻節點的索引，nx表示地圖的寬度。(如圖12所示)。

圖12 改進A*算法所規劃路徑后優化流程圖

圖13 改進A*算法轉折點移動示意圖之一

3.5 路徑平滑

為了使所規劃路徑更為平滑，有將轉折路徑用圓弧所替代已形成光滑路徑[12]，也有用貝塞爾曲線或者樣條曲線等方法對路徑點優化的。本文采用貝塞爾曲線[13]對改進A*算法的路徑進行平滑處理。其中，貝塞爾曲線是法國工程師皮埃爾.貝塞爾在1962年提出，其形狀可由曲線的控制點確定。因此，將改進改進A*算法所規劃的路徑點作為貝塞爾曲線的輸入，消除路徑不平滑的情況。

使用不同擬合方法示意圖如圖14所示，其中A點，B點和C點表示改進A*算法的路徑點。從圖14可以看出，使用二次樣條插值所生成的路徑的曲率較大，二次B樣條曲線擬合的起始點是A點和B點連線的中點，目標點是B點和C點連線的中點，導致所生成的路徑在起始點和目標點處不連續，而使用二次貝塞爾曲線擬合能夠較好解決上述二次樣條插值和二次B樣條曲線擬合所帶來的問題。

(12)

B(t)=(1-t)2*P0+2*t(1-t)*P1+t2*P2t∈[0,1]

(13)

公式(12)為貝塞爾曲線的一般方程，公式(13)是本文所使用的二階貝塞爾曲線，P0，P1，P2為二階貝塞爾曲線的控制點。

4 算法驗證

為了驗證改進后的A*算法的有效性，本文分別將Dijkstra算法、A*算法(使用Manhattan距離)、A*算法(使用Euclidean距離)和改進A*算法應用于成都信息工程大學校園的柵格地圖(4 846 ×2 816)上，如圖15所示，柵格大小為0.298 m/pixel，選擇不同的地點進行實現。同時，本實驗基于ROS(開源機器人操作系統)平臺，并使用Rviz進行可視化。計算機配置為:Ubuntu16.04 LTS，處理器Intel i5-6500，主頻為3.2 Ghz，運行內存為8 G。

本次實驗通過在地圖上選擇起始點和終點，上述4種算法進行路徑規劃。通過比較規劃路徑的長度、與障礙物的距離、路徑最大轉角和遍歷的節點數等方式來進行判斷路徑的優劣。對區域一、區域二和區域三進行路徑規劃，規劃結果見圖16、圖17和圖18。表1、表2和表3分別給出了不同區域規劃的指標對比。

圖15 學校的校園地圖

圖16 在區域一測試的結果

圖17 在區域二測試的結果

圖18 在區域三測試的結果

從圖16、圖17和圖18中可以看出，改進的A*算法所規劃出來的路徑較為平滑，具有連續的曲率并能夠較好地滿足車輛運動學模型。同時，Dijkstra算法、A*算法(使用Manhattan距離)、A*算法(使用Euclidean距離) 在測試區域測試中都存在起始時刻路徑轉折角度過大，路徑貼合路沿，路徑不平滑和轉折次數較多等問題。在加入車身輪廓代價、障礙物距離代價和對路徑優化過后能夠較好地解決這些問題(如圖16、17和18的(d)圖所示)。這都說明改進后的A*算法能夠更好地解決傳統A*算法路徑規劃所存在的問題。

表1 區域一測試對比

表2 區域二測試對比

由表1、表2和表3中能夠看出改進后的A*算法規劃時間小于Dijkstra算法、A*算法(使用Manhattan距離)、A*算法(使用Euclidean距離)的規劃時間。同時，所遍歷的節點數最少，并且與路沿的最近距離都大于車身寬度

表3 區域三測試對比

的一半。此外，改進A*算法所生成路徑的最大轉角都是小于車輛最大轉向角。因此，通過該方法所生成的路徑是能夠被智能車所執行的。

5 結束語

本文針對傳統A*算法所規劃的路徑轉折角度較大，路徑不平滑，所規劃的路徑貼合障礙物和未考慮車輛自身約束條件等問題進行改進，得到如下結論:

1)通過建立車輛運動學模型得到約束條件將其帶入啟發函數中使得節點代價值更為合理。

2)利用車身輪廓代價去掉A*算法所存在不合理的擴展節點，進而減少A*算法所遍歷的節點數目。

3)通過加入障礙物距離代價解決路徑靠近障礙物的問題，使得所規劃的路徑更為合理，更為安全。

4)使用貝塞爾曲線對路徑轉折點進行擬合使所規劃的路徑更平滑，更適用于智能車的運動特性。

5)由于本文算法是主要針對于在實驗階段中的AGV或者智能車，并且所實驗的載體是在中低速的環境中運行。其次，將該算法進行改進并用于高速行駛的移動載體是后續研究的重點。