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

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

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

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

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

圖1 基于野馬E70電動(dòng)汽車改造的智能車駕駛平臺(tái)

圖2 車輛簡化運(yùn)動(dòng)學(xué)模型示意圖

2 A*算法

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

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

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

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

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

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

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

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

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

3 改進(jìn)A*算法

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

改進(jìn)A*算法中的估價(jià)函數(shù)定義為

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

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

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

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

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

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

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

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

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

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

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

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

3.2 方向代價(jià)o（n）

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

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

步驟1獲得車輛的實(shí)際朝向（本文設(shè)正東方向?yàn)榱愣?，逆時(shí)針為正）。

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

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

圖6 未加方向代價(jià)的路徑點(diǎn)示意圖

圖7 加入方向代價(jià)的路徑點(diǎn)示意圖

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

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

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

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

圖9 智能車簡化模型的碰撞檢測點(diǎn)示意圖

圖10 懲罰代價(jià)和典型的懲罰代價(jià)的測試點(diǎn)示意圖

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

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

圖11 2種不同的方法在測試點(diǎn)的測試曲線

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

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

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

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

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

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

改進(jìn)A*算法流程如圖13所示。

圖13 改進(jìn)A*算法流程框圖

3.4 路徑優(yōu)化

因?yàn)橹悄苘嚧嬖诎⒖寺D(zhuǎn)向約束，所以導(dǎo)致智能車有最大轉(zhuǎn)向角的限制。為了解決該限制，本文將車輛最大轉(zhuǎn)角約束與改進(jìn)A*算法規(guī)劃的路徑相結(jié)合，具體實(shí)現(xiàn)步驟如下：

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

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

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

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

indexnew是移動(dòng)后節(jié)點(diǎn)的索引，index是此刻節(jié)點(diǎn)的索引。

圖14 轉(zhuǎn)折點(diǎn)移動(dòng)示意圖

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

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

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

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

圖16 平滑算法所得曲線

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

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

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

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

4 算法驗(yàn)證

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

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

圖17 校園柵格地圖

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

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

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

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

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

5 結(jié)論

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

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

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

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

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