一種改進A*算法在智能車中的應(yīng)用研究

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

（1.成都信息工程大學(xué) 控制工程學(xué)院，成都 610225；2.中國科學(xué)技術(shù)大學(xué) 軟件學(xué)院，合肥 230000）

隨著硬件設(shè)備的升級，將智能駕駛技術(shù)運用到普通汽車上的可能性逐漸增加。而在未來汽車的發(fā)展中，路徑規(guī)劃技術(shù)不僅將作為智能駕駛技術(shù)中的重要組成部分，也將是智能車發(fā)展中不可缺少的部分［1］。到目前為止，針對路徑規(guī)劃技術(shù)的研究，國內(nèi)外眾多學(xué)者做了大量貢獻，其中主要包括：采用模板樹的RRT算法［2］；將柵格鄰域擴展到無限個的A*算法［3］；通過限制障礙物范圍的改進APF［4］和使用信息素的蟻群算法［5］等。其中，A*算法由于能在無向圖中尋找最優(yōu)路徑，因此被廣泛用于解決其他規(guī)劃問題［6］。但是，傳統(tǒng)A*算法將移動載體看作質(zhì)點處理，導(dǎo)致了所生成的路徑不平滑［7］和路徑點與障礙物距離過近的問題［8］。此外，因為本研究的智能車存在阿克曼轉(zhuǎn)向約束，所以不能僅簡單地當作質(zhì)點考慮［9］。為了解決傳統(tǒng)A*算法因為載體不同所帶來的問題，通過建立智能車運動學(xué)模型，獲得智能車的約束條件，將該約束與啟發(fā)函數(shù)相融合，使得擴展節(jié)點更準確。具體過程為：首先通過加入方向代價解決車輛在起始時刻路徑轉(zhuǎn)折角度大于車輛最大轉(zhuǎn)向角的問題，然后使用自適應(yīng)障礙物懲罰代價，自動調(diào)節(jié)路徑點與障礙物之間的距離。最后，將車輛約束條件與改進A*算法的轉(zhuǎn)折點相結(jié)合，并使用自由邊界三次樣條插值算法擬合轉(zhuǎn)折點獲得更優(yōu)路徑。通過對傳統(tǒng)A*算法進行上述改進后，可使所生成的路徑更平滑。

1 基于阿克曼轉(zhuǎn)向的運動學(xué)模型

由于車輛不具有完整性約束，因此本文中只建立了簡易的運動學(xué)模型。以野馬新能源電動車為實驗的載體（如圖1所示），在標準慣性系下，用智能車后軸中心表示車體坐標（x，y），軸距用L＝1.56 m表示，車寬為D，車輛航向角為θ。同時，本實驗室平臺最大轉(zhuǎn)向角為40°，轉(zhuǎn)向角用φ。此外，實驗平臺最大轉(zhuǎn)折曲率和最小轉(zhuǎn)彎半徑分別為0.16 m-1和8 m，曲率和轉(zhuǎn)彎半徑用K和R表示。只考慮智能車在二維平面運動［10］，并認為在行駛的過程中無俯仰和側(cè)傾。因此，智能車運動的瞬間應(yīng)滿足式（1）和（2）：

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

圖2 車輛簡化運動學(xué)模型示意圖

2 A*算法

在進行柵格地圖的路徑搜索時，最常用的規(guī)劃算法是使用啟發(fā)式搜索的A*算法，該算法也是處理其他規(guī)劃問題的常用算法之一。A*算法的評價函數(shù)為：

式中：f（n）表示節(jié)點n的總代價；g（n）表示起點到當前n的實際路徑長度代價；h（n）表示啟發(fā)估計路徑長度代價。

A*算法的基本步驟如下：

步驟1初始化：獲得起點坐標Start和終點坐標Goal，并定義開啟列表（OpenList）和關(guān)閉列表（CloseList）。

步驟2將Start插入OpenList中，并判斷OpenList是否為空。如果為空，則規(guī)劃失敗。否則，執(zhí)行步驟3。

步驟3根據(jù)OpenList中所有節(jié)點代價值的大小，更新OpenList中節(jié)點的代價值，并選取最小代價值的當前節(jié)點（CurrentNode）。判斷Current-Node是否為Goal。如果是，規(guī)劃成功，反向形成路徑。否則，執(zhí)行步驟4。

步驟4擴展CurrentNode的周圍節(jié)點（NeighborNode），并將CurrentNode從OpenList中刪除，加入到CloseList中。

步驟5判斷NeighborNode是否為障礙物。如果是，則加入CloseList，并返回步驟3。否則，判斷NeighborNode是否在OpenList。如果是，則更新NeighborNode代價值大小。否則該節(jié)點還未被擴展過，重新計算NeighborNode代價值，并插入到OpenList，并執(zhí)行步驟3。

在傳統(tǒng)A*算法的啟發(fā)代價中大多使用曼哈頓距離和歐式距離來估計當前節(jié)點到目標點的近似距離，往往使得估計值不合理。同時，傳統(tǒng)A*算法在執(zhí)行的過程中未能考慮智能車運動特性，使得生成路徑不能很好地被底層控制模塊實現(xiàn)。因此，本文中針對傳統(tǒng)A*算法的上述問題進行改進。

3 改進A*算法

由于傳統(tǒng)A*算法所針對的移動載體不是智能車，因此所生成的路徑不能滿足智能車的運動學(xué)特性。為了優(yōu)化傳統(tǒng)A*算法，本文采用連續(xù)曲線取代傳統(tǒng)A*算法用曼哈頓距離或歐氏距離估計到目標點的方式，提出方向代價和自適應(yīng)障礙物懲罰代價方法解決所生成的路徑不滿足車輛運動學(xué)的問題。然后，使用智能車的最大前輪轉(zhuǎn)角約束優(yōu)化轉(zhuǎn)折點。最后，用自由邊界三次樣條插值算法對優(yōu)化后的轉(zhuǎn)折點進行插值，使路徑更為平滑。

改進A*算法中的估價函數(shù)定義為

式中：o（n）是車輛的方向代價；a（n）是起始點到擴展節(jié)點n的角度代價；d（n）是起始點到擴展節(jié)點n的直線代價；c（n）是擴展節(jié)點n到障礙物的懲罰代價；A是角度代價的系數(shù)；nangle是起始點的朝向與起始點到擴展節(jié)點n所形成的夾角之差；B是直線代價的系數(shù)；dnode是起始點到擴展節(jié)點的距離。由于加入方向代價主要是解決車輛初始時刻路徑轉(zhuǎn)折角度較大的問題，所以o（n）使用單調(diào)遞增的函數(shù)；dth為擴展節(jié)點到起始點距離的閾值；d為擴展節(jié)點到障礙物的距離。

3.1 對函數(shù)h（n）進行改進

在A*算法中，用于估計節(jié)點n的啟發(fā)代價值h（n）決定著A*算法對其他節(jié)點的擴展速度及所擴展節(jié)點是否合理。假定當前節(jié)點到達終點的實際最小代價為H（n）。當h（n）大于H（n）時，A*算法遍歷的節(jié)點最少，算法運行速度快，但是很難獲得最優(yōu)路徑；當h（n）等于H（n）時，為A*算法最理想的時候，所規(guī)劃出的路徑最短，算法運行速度最快；當h（n）小于H（n）時，A*算法所計算的節(jié)點數(shù)目最多，算法運行速度較慢，但是此時能夠生成較優(yōu)的路徑［19］。同時，曼哈頓距離和歐式距離是常被用于A*算法啟發(fā)代價h（n）的計算，但往往造成所計算的距離過大或過小。因此，本文中提出使用曲率連續(xù)函數(shù)對當前節(jié)點的代價值進行較為合理的估值。

首先，通過連接起始點和目標點來判斷路徑上是否有障礙物，生成預(yù)判函數(shù)yh＝k*x+b（yh中x取值從起始點到終點）。如果沒有障礙物，則說明直線運動的代價最小，則使用y＝k*x+b作為啟發(fā)函數(shù)h（n），用于計算路徑點的代價值。否則，使用二次函數(shù)y＝a*x2+b*x+c計算擴展節(jié)點啟發(fā)函數(shù)h（n）的代價值。如令擴展節(jié)點為p（n）＝（x1，y1），終點q＝（x2，y2），則：

式中yh為預(yù)判函數(shù)。

當預(yù)判函數(shù)yh在起始點和終點間沒有障礙物且車輛的朝向和直線的夾角φ2＞φmax時，最優(yōu)的路經(jīng)應(yīng)該是1條曲線，就需要對啟發(fā)函數(shù)y＝k*x+b進行修改，即使用二次函數(shù)對擴展節(jié)點的代價值進行估計。因為在規(guī)劃前只知道起始點和終點的坐標不能擬合二次函數(shù)，所以通過設(shè)置2個點來進行擬合。設(shè)起始點終點是q＝（x2，y2）（將起始點s設(shè)為坐標原點，便于后續(xù)處理）。

式中n＜N，n從0開始。

n是point1和point2遇到障礙物的次數(shù)的迭代次數(shù)。如果point1和point2中有1個點為障礙物，則n+1并重新計算新的point1和point2。N是最多迭代的次數(shù)。當?shù)淖鴺伺c起始點終點q＝（x2，y2）的坐標的x或y相同或n＞N時，則使用yn（n）＝k*x1+b來估計擴展節(jié)點的代價值。如果所預(yù)置的點并不在障礙物中，則使用起始點和終點q＝（x2，y2），擬合一條曲線y1（x）＝a1*x2+b1*x+c1，替換原本的yh（n）＝k*x1+b。

圖3、4分別是采用傳統(tǒng)A*算法的曼哈頓距離和本文改進啟發(fā)函數(shù)所規(guī)劃的路徑。從圖中可看出：與傳統(tǒng)的A*算法相比，改進的啟發(fā)函數(shù)所規(guī)劃的路徑更加平滑，轉(zhuǎn)折次數(shù)更少，路徑更短。

圖4 改進A*算法所規(guī)劃的路徑示意圖

當預(yù)判函數(shù)yh與車身初始航向的夾角大于φmax且在yh中x取值在起始點到終點之間無障礙時，根據(jù)車輛的運動學(xué)特性可知：最優(yōu)路徑應(yīng)為一條曲率連續(xù)的弧線。因此，用二次曲線y1（x）代替yh（n）對當前節(jié)點進行估值。所規(guī)劃的路徑如圖5所示，能夠很好滿足車輛的運動學(xué)特性。

圖5 改進h（n）所規(guī)劃的路徑

3.2 方向代價o（n）

通過對圖2中車輛運動學(xué)的建模分析，可知車輛在運動過程中存在著最大轉(zhuǎn)角。而在傳統(tǒng)的A*算法中未考慮車輛的最大轉(zhuǎn)角和車輛的朝向，因此規(guī)劃出的路徑在起始的時刻存在著不合理的情況，導(dǎo)致智能車不能很好地跟蹤所規(guī)劃的路徑。為解決上述問題，本文在傳統(tǒng)A*算法基礎(chǔ)上提出方向代價，將車輛朝向和車輛最大轉(zhuǎn)角考慮到估價函數(shù)中，從而解決傳統(tǒng)A*算法不能處理車輛運動學(xué)的問題。

優(yōu)化步驟如下：

步驟1獲得車輛的實際朝向（本文設(shè)正東方向為零度，逆時針為正）。

步驟2如果擴展節(jié)點到起始點的距離dnode大于dth，則將方向代價賦值為零。否則，將計算擴展節(jié)點與起始點的夾角和距離并代入式（5）（6）中。

通過對比圖6、7可看出：加入方向代價后，能夠較好地解決初始時刻規(guī)劃不合理的問題。同時，也能夠減少50%以上的擴展節(jié)點數(shù)。

圖6 未加方向代價的路徑點示意圖

圖7 加入方向代價的路徑點示意圖

3.3 自適應(yīng)障礙物懲罰代價c（n）

傳統(tǒng)的A*算法主要是應(yīng)用于全向移動機器人和差速移動機器人等，因此在規(guī)劃模塊使用A*算法生成路徑時，常常將運動載體當成質(zhì)點處理。但是，由于智能車體積較大，因此在規(guī)劃過程中，不能簡單將智能車看作質(zhì)點處理。此外，因為定位模塊和地圖模塊存在誤差，所以需要路徑規(guī)劃模塊所規(guī)劃產(chǎn)生的路徑要與障礙物形成一定的安全距離。為了獲得安全距離，馬飛等［8］通過在A*算法上加入鏟運機的形狀作為約束條件，并使用9個輪廓特征點表示鏟運機外形形狀，同時，將該項約束條件命名為碰撞威脅代價，通過在擴展節(jié)點上加入該打分代價進而獲得安全距離。但是，在每個擴展節(jié)點加入9個輪廓特征點降低了算法的效率。因此，本文通過判斷擴展節(jié)點p（n）＝（x1，y1）四周距離的障礙物檢測點（圖8）是否有障礙物來對c（n）進行賦值。如果有，則將障礙物威脅代價賦為無窮大，否則賦為零。同時，由于道路寬度不一致，導(dǎo)致閾值D不能很好地被設(shè)置，所以提出自適應(yīng)障礙物懲罰代價。通過判斷智能車與障礙物的距離是否合理，自動調(diào)整閾值D進行重規(guī)劃。通過這樣的方式能夠較大地提高A*算法的運行效率，避免手動調(diào)參。

圖8 障礙物監(jiān)測點示意圖

為了測試本文中的懲罰代價，參照文獻［8］的方法，在智能車的簡化模型上加入碰撞檢測點（如圖9所示）。同時，采用文獻［8］的方法對擴展節(jié)點進行打分判斷，并在圖10的測試點上進行測試分析（L1表示車長，D表示車寬）。

圖9 智能車簡化模型的碰撞檢測點示意圖

圖10 懲罰代價和典型的懲罰代價的測試點示意圖

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

圖11為使用本文懲罰代價與典型懲罰代價在圖10的測試結(jié)果對比。通過圖11（a）可知：在保證規(guī)劃路徑與障礙物形成安全距離的前提下，使用本文障礙物懲罰代價與采用典型懲罰代價，所生成的路徑長度并無明顯的差距。但是，由圖11（b）可知：典型懲罰代價使用的時間比自適應(yīng)障礙物懲罰代價所用的時間更多。此外，通過圖11（a）和（b）可知：隨著A*算法所規(guī)劃距離增加，采用典型懲罰代價的時間增幅比本文的懲罰代價更大，因此本文的方法在長距離規(guī)劃中優(yōu)勢更為明顯。

圖11 2種不同的方法在測試點的測試曲線

本文中優(yōu)化步驟如下：

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

步驟2如果是，則在擴展節(jié)點上增加障礙物標志位。否則，執(zhí)行步驟3。

步驟3判斷擴展節(jié)點是否為目標點。如果否，則執(zhí)行步驟一。如果是則擴展到目標點，判斷CloseList中的節(jié)點四周是否有障礙物標志位。如果有，則減小閾值D，進行重新規(guī)劃，直到閾值D小于零。否則，從CloseList中輸出路徑。

通過對比圖12（a）和（c）可知，加入自適應(yīng)障礙物懲罰代價的A*算法所規(guī)劃的路徑與障礙物保持一定距離，使得車輛在行駛過程中更加安全。同時，由于圖9中道路最窄處為2.8 m，將閾值設(shè)置為D＝1.6 m導(dǎo)致所規(guī)劃的路徑貼合路沿（如圖12（b）所示），不能被智能車所執(zhí)行。在CloseList中形成路徑時，判斷節(jié)點存在著障礙物標志位，所以降低閾值D重新規(guī)劃（如圖12（c）所示），得到使用于智能車的路徑。

圖12 3種路徑結(jié)果示意圖

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

圖13 改進A*算法流程框圖

3.4 路徑優(yōu)化

因為智能車存在阿克曼轉(zhuǎn)向約束，所以導(dǎo)致智能車有最大轉(zhuǎn)向角的限制。為了解決該限制，本文將車輛最大轉(zhuǎn)角約束與改進A*算法規(guī)劃的路徑相結(jié)合，具體實現(xiàn)步驟如下：

步驟1獲得轉(zhuǎn)折點：去掉改進A*算法所生成路徑中的冗余點［11］，得到轉(zhuǎn)折路徑點（path1）。

步驟2在路徑點（path1）中依次獲得3個路徑點pose［i-2］，pose［i-1］，pose［i］將其擬合為二次曲線y（x）1＝a*x2+b*x+c。如果轉(zhuǎn)折點中有目標點，則將轉(zhuǎn)折點添加到路徑點path2中，同時完成優(yōu)化轉(zhuǎn)折點的過程。否則，在曲線y（x）1上的路徑點到障礙物距離都大于D／2的時，執(zhí)行步驟3。如果路徑點與障礙物之間的距離都小于D／2，則重新執(zhí)行步驟2。

步驟3判斷轉(zhuǎn)折點形成的角度φmax是否滿足式（10），如果滿足，則執(zhí)行步驟2，否則執(zhí)行步驟4。

步驟4將中間的轉(zhuǎn)折點向曲線y（x）1開口方向移動一個柵格，如圖14的B·點所示。再將代入式（10）中，判斷是否成立。如果成立，重新執(zhí)行步驟2并將轉(zhuǎn)折點添加到path2，否則執(zhí)行步驟4（圖15）。

indexnew是移動后節(jié)點的索引，index是此刻節(jié)點的索引。

圖14 轉(zhuǎn)折點移動示意圖

A，B，C為轉(zhuǎn)折點；B·為移動后的轉(zhuǎn)折點；黑色曲線為轉(zhuǎn)折移動之前所擬合的曲線；紅色曲線為移動之后所擬合的曲線；φposes為A，B，C所形成的角度，為A，B·，C所形成的角度。

3.5 自由邊界三次樣條插值算法

為了使所規(guī)劃路徑更為平滑，將改進A*算法的路徑進行平滑處理。其中，平滑算法所得的曲線如圖16所示，粉紅色的數(shù)據(jù)點代表改進A*算法的轉(zhuǎn)折點，使用藍色、綠色和紅色分別表示三次樣條擬合、三次多項式擬合和自由邊界三次樣條插值算法。因為結(jié)合車輛運動學(xué)規(guī)律，車輛應(yīng)盡可能走直線，只是在轉(zhuǎn)變方向的地方形成符合車輛運動學(xué)的曲線，而三次樣條曲線在第1個點和第2點之間產(chǎn)生一個曲線弧度，所以不是最優(yōu)的。此外，從第6個點到第7個點，三次多項式擬合的變化斜率比自由邊界三次樣條插值的變化斜率大。因此，相比三次樣條擬合和三次多項式擬合，自由邊界三次樣條插值算法較優(yōu)。

圖15 路徑后優(yōu)化流程框圖

圖16 平滑算法所得曲線

由于智能車在實際的行駛過程中車輛的速度和加速度都是連續(xù)的，而高次多項式計算較為復(fù)雜。所以本文使用1階導(dǎo)數(shù)連續(xù)和二節(jié)導(dǎo)數(shù)連續(xù)的優(yōu)化方法就能滿足需求。通過圖16的分析研究發(fā)現(xiàn)，自由邊界三次樣條插值較優(yōu)。因此本文使用自由邊界三次樣條插值算法使曲線在銜接點處連續(xù)和光滑，且避免了高次擬合算法所帶來的龍格現(xiàn)象［12-13］。假設(shè)S1＝（x1，y1），S2＝（x2，y2）和S3＝（x3，y3）是path2的3個路徑點。式（13）使用（S1，S2）2個擬合點，式（14）使用（S2，S3）2個擬合點。

由于2點曲線都經(jīng)過S2＝（x2，y2），則可得式（15）

由于樣條曲線銜接處的1階導(dǎo)數(shù)和2階導(dǎo)數(shù)連續(xù)，則可得式（16）～（18）

通過綜合式（13）～（18）確定2條3次樣條曲線的系數(shù)，用于對轉(zhuǎn)折點進行擬合得到路徑點。

4 算法驗證

為了測試傳統(tǒng)A*算法和本文中所提出的改進A*算法之間的優(yōu)劣，將上述算法進行對比測試。同時，本實驗測試使用的地圖為課題組制作的學(xué)校校園地圖，該地圖長有4 846個柵格、寬有2 816個柵格，地圖的柵格大小為0.298 m，并且本課題將坐標原點定義在校園地圖的左下角。校園柵格地圖見圖17。此外，本課題研究采用ROS系統(tǒng)作為測試軟件平臺，并通過3D可視化工具RVIZ用于顯示。使用曼哈頓距離的傳統(tǒng)A*算法和改進A*算法在學(xué)校的柵格地圖上進行仿真測試。其測試結(jié)果主要通過算法所規(guī)劃路徑的距離、路徑的平滑性和規(guī)劃所用的時間、與障礙物的距離等指標來進行判斷。

為了更好地驗證改進A*算法的實用性，在測試地圖上選取2處連續(xù)轉(zhuǎn)彎的測試區(qū)域1和2進行測試，分別使用傳統(tǒng)A*算法和改進A*算法規(guī)劃行駛路徑。其中，如圖18、19分別為傳統(tǒng)算法和改進算法在2處區(qū)域中所規(guī)劃的路徑。最后，本文中對比的測試指標如表1所示。

圖17 校園柵格地圖

圖18 區(qū)域1規(guī)化

圖19 區(qū)域2規(guī)化

通過對比在區(qū)域1和區(qū)域1的測試結(jié)果（a）和（b）可以看出：相比傳統(tǒng)A*算法而言，改進A*算法生成的路徑更為平滑，滿足車輛的運動特性，所規(guī)劃路徑更為安全、可靠。同時，通過傳統(tǒng)A*算法在區(qū)域1和區(qū)域2的規(guī)劃結(jié)果能夠看出該算法存在起始時刻路徑不合理，規(guī)劃出的路徑存在著某些路徑點與障礙物過近及所生成路徑較為曲折等諸多問題。為了解決傳統(tǒng)A*算法帶來的問題，在傳統(tǒng)A*算法上加入自適應(yīng)障礙物懲罰代價，方向代價并將車輛約束與轉(zhuǎn)折點相結(jié)合以能夠較好地解決傳統(tǒng)A*算法所帶來的問題（如圖18（a）和19（a）所示）。最后，通過對2種算法進行對比分析，可知改進A*算法能有效改善傳統(tǒng)A*算法未考慮車輛運動學(xué)特性的問題。改進A*算法和傳統(tǒng)A*算法規(guī)化結(jié)果見表1。

表1 改進A*算法和傳統(tǒng)A*算法規(guī)化結(jié)果

由表1能夠看出改進A*算法雖然在運行時間方面比傳統(tǒng)A*算法略長，但是所增加的時間也在可接受的范圍內(nèi)。通過犧牲少量運行時間獲得了更加平滑、安全，路徑長度更短的路徑，這樣的做法是可以被接受的。

5 結(jié)論

由于傳統(tǒng)A*算法存在著路徑點與障礙物距離過近，路徑轉(zhuǎn)折次數(shù)較多，初始時刻路徑不合理和未結(jié)合車輛運動學(xué)特性的問題。因此，本研究針對這些問題本文對其進行改進，主要貢獻如下：

1）對智能車進行運動學(xué)分析，并建立其運動模型，從而獲得車輛約束條件，再將約束條件代入A*算法的代價函數(shù)中使得對節(jié)點的估計值更能滿足實際需求。

2）利用方向代價解決車輛起始時路徑轉(zhuǎn)折角度不合理的問題。

3）為了解決傳統(tǒng)A*算法所規(guī)劃的路徑點與障礙物過近的問題，通過提出自適應(yīng)障礙物懲罰代價判斷估計點代價值是否合理，從而調(diào)整障礙物檢測點的范圍，進行重新規(guī)劃。

4）將車輛約束條件與路徑轉(zhuǎn)折點相結(jié)合并使用自由邊界3次樣條插值擬合使得路徑點更平滑，使之更好地被用于智能車的路徑規(guī)劃。