周蘇 王若伊 張友發(fā) 張崗

（1.同濟大學汽車學院，上海 201804；2.同濟大學中德學院，上海 201804）

主題詞：混合A*算法 軌跡優(yōu)化算法 貝塞爾曲線 自動駕駛

1 引言

自動駕駛汽車配備先進的傳感器、控制器和執(zhí)行器，可以感知復雜環(huán)境、進行智能決策和運動控制，最終使車輛達到人類駕駛員駕駛水平[1-2]。

規(guī)劃作為駕駛自動化系統(tǒng)中不可缺少的環(huán)節(jié)[3]，不僅需要從感知層獲取當前的周圍環(huán)境信息和自身定位信息，還需要將規(guī)劃好的動作信息傳到控制層，再交由控制層對汽車進行控制。一個規(guī)劃迅速、反應良好的規(guī)劃層直接影響無人駕駛汽車的性能。

20世紀至今，規(guī)劃方法發(fā)展迅速，根據(jù)具體的計算方法，規(guī)劃算法分為5類：

（1）基于圖搜索的規(guī)劃算法；

（2）基于采樣的規(guī)劃算法；

（3）基于插值的規(guī)劃算法；

（4）基于數(shù)值優(yōu)化的規(guī)劃算法；

（5）基于機器學習的規(guī)劃算法。

各類規(guī)劃算法研究進展如下：

（1）基于圖搜索的規(guī)劃算法

圖搜索算法主要指遍歷從起點到終點的狀態(tài)空間以搜索出當前最優(yōu)的結果，一般使用柵格、格子來表示一個點的狀態(tài)。

迪杰斯特拉算法是1種單源最短路徑的最優(yōu)路徑規(guī)劃算法，在迪杰斯特拉算法中配置狀態(tài)空間，一般被離散為單元網(wǎng)格后再進行搜索[4]。Dijkstra最早給出了這種算法的嚴格證明[5]。在自動駕駛領域，迪杰斯特拉算法已經(jīng)被應用于多車仿真中[6]，本-富蘭克林車隊（The Ben Franklin Racing Team）和維克多-探戈（Vector Tango）將其應用于DARPA城市挑戰(zhàn)賽中[7]。

A*算法是帶有啟發(fā)式函數(shù)的迪杰斯特拉算法，非常適合已知地圖下的路徑規(guī)劃問題[8]，但是當要搜索的區(qū)域過大時，其所耗費的內(nèi)存空間也會隨之增大，搜索速度隨之下降。

A*類算法家族具有很多成員，這些A*類算法相比A*有了很多的提升，也被廣泛應用到自動駕駛、機器人等領域。比較常見的有Hybrid A*算法、D*算法（Dynamic A*,D*）、Field A*算法[9]、Theta*算法[10]、ARA*算法（Anytime Reparing A*,ARA*）和AD*算法（Anytime D*,AD*）[11]。在自動駕駛領域中，Ziegler等[12]在文獻中實現(xiàn)了A*算法用于非結構化空間下（停車場）的規(guī)劃。混合狀態(tài)A*算法被斯坦福大學用于DARPA挑戰(zhàn)賽[13]。2007年的DARPA挑戰(zhàn)賽獲勝車輛Boss也使用了AD*算法[14]。Saeid等[15]使用了Hybrid A*算法實現(xiàn)了停車場中實驗車輛的自動泊車。Francesco等[16]則通過減少車輛轉彎和向后行駛，提高了乘客在自動泊車時的乘坐舒適度。Silvana等[17]使用正態(tài)分布變換算法建圖后，使用Hybrid A*算法進行路徑規(guī)劃，并使用模型預測控制算法進行控制器設計，實現(xiàn)了在園區(qū)內(nèi)穩(wěn)定運行的高爾夫球車。

Howard等[18]將狀態(tài)柵格法應用于崎嶇地形下的輪式移動機器人的全局規(guī)劃和局部規(guī)劃。Likhachev等[8]根據(jù)控制復雜度將狀態(tài)空間離散化，而McNaughton等[19-22]考慮了時間和速度，將狀態(tài)根據(jù)時間空間離散化。

（2）基于采樣的規(guī)劃算法

基于采樣的算法中也有很多實用且高效的算法。如概率路圖法（Probabilistic Roadmap,PRM）[23]，快速搜索隨機樹算法（Rapidly-Exploring Random Trees,RRT）[24]。其中，RRT算法也已經(jīng)被應用到自動駕駛領域。

RRT算法是一種快速的在線搜索算法[25]，可以在半結構化空間中進行快速規(guī)劃，考慮了非完整約束[24]。MIT將該算法應用到了DARPA挑戰(zhàn)賽中[26]，但其結果并不穩(wěn)定，而且曲率也不連續(xù)。與A*類算法一致，RRT類算法也有很多成員，如RRT*算法[27]、Informed RRT*算法[28]、Anytime RRT*算法[29]，以及Kino-Dynamic RRT*算法[29]。

（3）基于插值的規(guī)劃算法

Reeds等[31-32]使用了直線和圓弧插值算法來近似汽車規(guī)劃。螺旋曲線插值中對應的曲線是根據(jù)菲涅爾積分定義而來的[33]，使用回旋曲線可以定義曲率線性變化的軌跡，平滑過渡直線段和彎曲線段[34]。多項式插值算法非常適合多段軌跡的插值問題。Piazzi等在文獻[35]、[36]和[37]中分別闡述了相關的計算方法。貝塞爾曲線插值廣泛應用于航空航天、汽車設計、游戲眾多領域，在自動駕駛領域的研究中，三階和四階的貝塞爾曲線被用來規(guī)劃當前可以轉彎、避障的軌跡[38-39]。

（4）基于數(shù)值優(yōu)化的規(guī)劃算法

基于數(shù)值優(yōu)化[40-41]的方法時間上是使受不同約束的函數(shù)最大化或者最小化，大多數(shù)是基于凸優(yōu)化的算法，還有一些是基于非線性優(yōu)化的算法。這種算法實際上已經(jīng)被廣泛應用于移動機器人在狹窄通道中避障的勢場方法（Potential Field Method,PFM），其軌跡可以包含一階連續(xù)性[42]甚至更高階（速度、加速度、沖擊度等）連續(xù)性的軌跡。

（5）基于機器學習的規(guī)劃算法

基于機器學習的方法具有較快的計算速度和較強的泛化能力，但是使用機器學習的方法之前需要進行一定的離線學習。以下機器學習相關的方法被應用在規(guī)劃領域：強化學習[43]、流形學習[44]、卷積神經(jīng)網(wǎng)絡[45]、隱馬爾可夫模型[46]、高斯混合模型[47]和徑向基函數(shù)神經(jīng)網(wǎng)絡[48]。

通過以上分析，目前自動駕駛車輛的運動規(guī)劃算法存在以下缺陷：

（1）考慮了汽車動力學的規(guī)劃算法計算較慢，且不能保證規(guī)劃處的路徑總是無碰撞的。

（2）汽車的路徑規(guī)劃與某些機器人和無人機的路徑規(guī)劃不同，汽車的運動規(guī)劃屬于非完整約束問題，且大多數(shù)規(guī)劃算法規(guī)劃出的結果是路徑，并沒有規(guī)劃出其軌跡。而且即便是有軌跡規(guī)劃的算法，其路徑和速度是不匹配的，需要對速度重新進行規(guī)劃。

（3）大多數(shù)規(guī)劃算法并沒有考慮多車協(xié)同問題?，F(xiàn)在絕大多數(shù)的規(guī)劃算法都是考慮已知地圖下的單車規(guī)劃問題。

因此，本文以提高汽車舒適性為目的，基于汽車的最小沖擊度進行建模，在MATLAB和柵格地圖中進行了算法預研，解決了自動駕駛汽車運動過程中舒適度不足的問題。然后，針對優(yōu)化算法可能導致軌跡碰撞的問題，對優(yōu)化算法進行了改進，保證了汽車的安全性，并給出了“走廊”自動調(diào)整的策略。

2 基于混合狀態(tài)A*算法的路徑規(guī)劃模型

混合狀態(tài)A*算法最早出現(xiàn)于2007年美國DRAPA挑戰(zhàn)賽[49]，它并不像迪杰斯特拉算法、A*算法那樣經(jīng)過數(shù)學推導而得出，但這并不影響該算法的實用性。2007年，斯坦福大學的車隊就是使用該算法跑完了全程?；旌螦*算法是A*算法的一種變體，它不僅繼承了A*算法的特性，還可以在更高維度的狀態(tài)空間進行搜索，并提出新的狀態(tài)更新規(guī)則。

相比A*算法，混合狀態(tài)A*算法具有更高的搜索維度，主要體現(xiàn)在以下方面：

（1）一般混合狀態(tài)A*算法會在x坐標、y坐標、朝向角和轉彎半徑4個維度進行路徑規(guī)劃。而A*算法會選取當前累計代價和啟發(fā)式函數(shù)代價最小的點作為下一點來膨脹；

（2）與此類似，混合狀態(tài)A*算法則是基于A*算法和Reeds_Shepp曲線[31]來決定下一節(jié)點，其節(jié)點可以不是柵格的頂點。

圖1是混合狀態(tài)A*算法的流程圖。

圖1 混合狀態(tài)A*算法流程

3 基于最小沖擊度的優(yōu)化模型

由于很多路徑規(guī)劃算法會將汽車視為點來進行路徑規(guī)劃，這導致某些規(guī)劃算法規(guī)劃出的路徑超出了汽車的正常行駛范圍，且不能保證車內(nèi)乘客的舒適性。為了提高汽車在運動過程中的舒適性，本文將對混合A*算法的結果基于最小沖擊度進行優(yōu)化。

3.1 基于最小沖擊度的一維優(yōu)化模型的建立

本文建立的基于最小沖擊度的優(yōu)化模型，取最小沖擊度的L2范數(shù)的平方作為優(yōu)化的目標函數(shù)，優(yōu)化目標是最小化最小沖擊度的平方，目標函數(shù)如式（1）所示。

式中，Jk為目標函數(shù)；T為優(yōu)化模型時間域；jk為最小沖擊度。

選取當前的狀態(tài)量為

式中，sk為當前的狀態(tài)量；pk當前狀態(tài)的位置；vk是當前的速度；ak是當前的加速度。同時，將外界輸入定義為：

整個系統(tǒng)的狀態(tài)表達為：

式中，fs(sk,uk)為當前狀態(tài)量與外界輸入的狀態(tài)表達式。結合龐特里亞金極大值（極小值）原理，可得最終的狀態(tài)表達式[50]：

式中，s*(t)為系統(tǒng)的最優(yōu)狀態(tài)表達式；t為時間；α、β、γ為任意常數(shù)；α0、a0、v0、p0為t=0時的系數(shù)、加速度、速度和位置。

3.2 多段優(yōu)化模型邊值問題求解

對于軌跡規(guī)劃問題，工程中很難只用一段多項表達式就擬合出一個符合預期的從起點到終點的軌跡，更一般性的做法是用多段連續(xù)的多項表達式擬合。本文中，后續(xù)出現(xiàn)的多項式連續(xù)條件均指位置連續(xù)、速度連續(xù)和加速度連續(xù)。同時，路徑的多項表達式還需要滿足運動的初始條件和終止條件。

對于多項表達式，可以用如下表達式來表示：

式中，f(t)為多段連續(xù)多項式的總表達式；fM(t)為各段多項式表達式；TM-1≤t≤TM為時間分段。

同時還需滿足如下約束：

式（7）是指多項表達式中每相鄰2段表達式之間連接處需要滿足連續(xù)性約束；而式（8）是指第j段表達式的頭和尾部需要滿足該處的狀態(tài)約束（位置、速度以及加速度）。本文中，每相鄰2段的狀態(tài)約束只包含位置約束。

有了以上目標函數(shù)和約束還不夠，為了求解多段表達式的最小沖擊度，還需要知道每段上的時間。本文中的時間按照常數(shù)或者梯形速度曲線進行分配。

綜上可以得出最終優(yōu)化表達式：

式中，min式為凸目標函數(shù)；s.t.式為約束函數(shù)；Ti為仿射函數(shù)。

3.3 基于最小沖擊度的優(yōu)化結果

在以下場景中，設置汽車起始位置為[9,16,0]，再繞過障礙物后，需要停在停車位上，最終的狀態(tài)為[12,8,pi]（pi表示停車時車頭朝向角為π，后文均采用此種表述）?；旌蠣顟B(tài)A*算法的結果圖2所示。方點代表汽車的起點，圓點代表終點，點線代表算法的歷史前向搜索記錄，點劃線代表算法的歷史后向搜索記錄，實線是最終的運動軌跡。

圖2 混合狀態(tài)A*算法的搜索結果

根據(jù)優(yōu)化算法得出的結果，可以計算出此時的速度圖像、加速度圖像，來近一步分析優(yōu)化后的結果。

圖3是根據(jù)優(yōu)化后的結果得出的汽車狀態(tài)圖像。從中可知優(yōu)化后的質(zhì)心速度大多數(shù)時間穩(wěn)定在3 m/s左右，而質(zhì)心加速度則變化幅度比較大。實際上，汽車在行駛過程中，最大加速度一般在10 m/s2左右，此處計算出的加速度已經(jīng)超過了實際的最大加速度。在這種搜索步長下，優(yōu)化算法對混合狀態(tài)A*算法搜索出的結果優(yōu)化效果不明顯。同時，又由于缺少不等式約束，導致計算出的加速度容易超出汽車實際的最大加速度。因此算法值得進一步的改進。

圖3 優(yōu)化后汽車的運動速度和加速度

優(yōu)化過程需要花費的時間也是優(yōu)化算法十分重要的評價指標?；谧钚_擊度的優(yōu)化算法的運行速度實際上是比較快的。如表1所示，Hybrid A*代表混合狀態(tài)A*算法的運行時間，QP和CF則分別代表二次規(guī)劃求解器和閉式解求解方法的運行時間。在混合狀態(tài)A*算法搜索出50個路徑點的條件下，基于二次規(guī)劃求解器的求解算法和基于閉式解的求解算法生成了576個系數(shù)，共7 224個數(shù)值。在這種計算規(guī)模下，MATLAB中的3次試驗結果顯示，不論是基于二次規(guī)劃求解器的求解方式，還是基于閉式解的求解方式，優(yōu)化算法的計算時間都不會超過0.1 s。即便是加上混合狀態(tài)A*算法的耗時，運算時間也基本保持在0.5 s左右。

表1 算法運行時間比較 s

對于前文中（圖3）加速度超出實際范圍的情況，可以在優(yōu)化之后再單獨加一層速度規(guī)劃層，便可以基于優(yōu)化結果得出更好的速度和加速度結果。此處采用梯形速度分配公式來做速度規(guī)劃，設置最高速度為3 m/s，便可以得出如圖4所示的結果。

圖4 速度、加速度重規(guī)劃

本文采用的地圖是柵格地圖，以上試驗全部是在靜態(tài)層進行的試驗。實際應用中，從起點到終點的運動過程中，汽車需要不斷地感知周圍的環(huán)境。在靜態(tài)地圖中，當汽車接近突然出現(xiàn)的障礙物或汽車運動到某位置才檢測到周圍具有障礙物時，可能導致之前規(guī)劃的軌跡失效，需要規(guī)劃器重新做出規(guī)劃。所以此處給汽車柵格地圖障礙物層加入障礙物信息，并給汽車加上傳感器再次進行試驗。

假設汽車可以感知到周圍環(huán)境120°范圍內(nèi)的障礙物，探測范圍在[0,20]之間。設置汽車起始位置為[9,16,0]，最終停在停車位上的狀態(tài)為[22,8,pi]。用實線代表當前算法搜索出的軌跡，虛線代表歷史搜索出的軌跡，而輻射范圍代表傳感器可探測到的范圍，方塊代表檢測到障礙物后膨脹的方格。需要注意的是，如果規(guī)劃的軌跡存在“局部碰撞”（未來可能發(fā)生碰撞），算法也會重新規(guī)劃路徑。最后圖5顯示了2種算法到達終點附近的運動歷史軌跡，由虛線可知，2種算法均會做出多次重新規(guī)劃。同時，優(yōu)化算法在某些障礙物附近的曲率變化略小于混合狀態(tài)A*算法。

圖5 兩種算法的最終運動軌跡

直觀的來看，基于優(yōu)化算法的最終軌跡相對于混合狀態(tài)A*算法的曲率變化率較小，但是還是不能改變混合狀態(tài)A*算法在某些點距離障礙物較近的缺點。雖然這種缺點是有可能通過調(diào)整混合狀態(tài)A*算法的參數(shù)來解決的，但僅從優(yōu)化的角度來看，基于最小沖擊度的優(yōu)化算法是很難解決這樣的問題的。

4 硬約束下基于最小沖擊度的優(yōu)化模型

由于很多路徑規(guī)劃算法會將汽車視為點來進行路徑規(guī)劃，這導致某些規(guī)劃算法規(guī)劃出的路徑超出了汽車的正常行駛范圍，且不能保證車內(nèi)乘客的舒適性。為提高汽車在運動過程中的舒適性，實際工程中會根據(jù)最小沖擊度的大小對當前運動情況進行調(diào)整。所以，在本文對混合A*算法的結果基于最小沖擊度進行優(yōu)化，但是發(fā)現(xiàn)基于最小沖擊度的優(yōu)化算法在大步長時生成的軌跡會碰撞到障礙物（圖6a）。如果假設在自由空間EFree存在一條從起點到終點的無碰撞“走廊”（圖6b），規(guī)劃出的軌跡位于走廊之內(nèi)，那么最終的軌跡無論如何都不可能發(fā)生碰撞。因此本文嘗試采用此方法求解最終軌跡。

圖6 算法改進策略

硬約束下基于沖擊度的優(yōu)化模型就是要將最終的目標函數(shù)化為帶有不等約束的目標函數(shù)，此處依舊按照解凸優(yōu)化問題的求解器進行求解。與之相反的，軟約束下的優(yōu)化模型就是要將所有的約束寫入目標函數(shù)中，最終使用梯度下降法、牛頓法或擬牛頓法進行求解。

本文基于貝塞爾曲線實現(xiàn)軌跡的無碰撞，其優(yōu)點如下：

（1）傳統(tǒng)的基于貝塞爾曲線的軌跡規(guī)劃方式中，不同階數(shù)的貝塞爾曲線對應著不同數(shù)量的控制點，業(yè)內(nèi)經(jīng)常利用4次和3次貝塞爾曲線來做插值[50-51]。通常的做法是，根據(jù)速度、加速度、貝塞爾曲線的階數(shù)約束，先得到相應階數(shù)下從起點到終點的控制點，然后再根據(jù)控制點計算出最終的軌跡。而本文中，貝塞爾曲線的階數(shù)是固定的，前文已經(jīng)證明基于最小沖擊度的軌跡可以用5次多項式來表示，所以此處的伯恩斯坦多項式也是5次多項式。

（2）本文將首先根據(jù)規(guī)劃算法（混合A*類算法）規(guī)劃出全局的路徑點。然后，在規(guī)劃出的路徑點附近形成一系列連續(xù)的“走廊”，這些“走廊”全部位于自由空間EFree，最后在走廊中生成貝塞爾曲線。

（3）傳統(tǒng)的貝塞爾曲線無所避免的會出現(xiàn)“打結”現(xiàn)象，而本文中只固定起點和終點，而“釋放”其它規(guī)劃的路徑點，最終形成的多段貝塞爾曲線可以避免“打結”。

（4）本文中利用貝塞爾曲線來構建一個基于最小沖擊度的優(yōu)化問題，屬于優(yōu)化問題范疇；傳統(tǒng)貝塞爾插值式的路徑規(guī)劃算法屬于插值式的規(guī)劃算法，屬于規(guī)劃范疇。

本文中“走廊”內(nèi)規(guī)劃的思想也與自動駕駛時代“高精度地圖+管道式行駛”的精神相契合。

4.1 貝塞爾曲線

由規(guī)劃算法得出的路徑點得到“走廊”之后，如何確保最終生成的軌跡不超出走廊是目前需要考慮的問題。而貝塞爾曲線無疑是一個比較好的選擇，只要貝塞爾曲線的控制點都在“走廊”以內(nèi)，那么貝塞爾曲線也在“走廊”以內(nèi)。這個邏輯之所以成立，是因為貝塞爾曲線具有凸包性。貝塞爾曲線對應的多項式具有以下形式：

式中，Bj(t)為各走廊內(nèi)貝塞爾曲線表達式為貝塞爾曲線的控制點;為各控制點的系數(shù)表達式，與控制點個數(shù)與時間t有關。

本文中主要利用了貝塞爾曲線的以下性質(zhì)：

（1）貝塞爾曲線的凸包性，即由一組控制點形成的貝塞爾曲線始終位于這組控制點組成的凸包以內(nèi)。

（2）貝塞爾曲線的導數(shù)仍然是貝塞爾曲線。

（3）貝塞爾曲線中的t始終在[0,1]區(qū)間內(nèi)。

4.2 硬約束下基于最小沖擊度優(yōu)化模型的建立

由于仍然是基于最小沖擊度的一維優(yōu)化模型，所以硬約束下最小沖擊度模型的目標函數(shù)仍然與式（9）相似，但需要將一般多項式轉化為伯恩斯坦多項式。另一方面，為了解決速度、加速度會超出計算范圍的問題，還需要給模型加上不等式約束。等式約束仍舊是連續(xù)性約束（位置連續(xù)、速度連續(xù)和加速度連續(xù)）、邊界約束（初始時刻和終止時刻的位置約束、速度約束和加速度約束）和中間點位置約束。由于此處的目標函數(shù)已經(jīng)轉化為伯恩斯坦多項式，所以此處所有的約束也需要轉換為相應形式。

對于如下形式的一般多項式：

式中，pj(t)為n階伯恩斯坦多項式；為多項式中的算子。

要將其轉化為式（10）所示的形式，需要建立一個映射矩陣如：

式中，p為一般多項式（11）中的系數(shù)向量；c為貝塞爾曲線多項式（10）中的系數(shù)向量；M為系數(shù)向量p、c的映射矩陣。

添加速度約束和加速度約束：

根據(jù)這些目標函數(shù)和約束，可以最后推到出多段硬約束下基于最小沖擊度下的優(yōu)化模型如式所示：

式中，min式為凸目標函數(shù)；s.t.式為約束函數(shù)。

最終，問題轉換成了一個典型的凸優(yōu)化問題，可以采用求解凸優(yōu)化問題的方法來求解相應的系數(shù)。

4.3 對混合狀態(tài)A*算法的優(yōu)化結果

由于基于最小沖擊度的優(yōu)化對混合A*的優(yōu)化效果并不明顯，且加速度會超出實際汽車的最大加速度。本文采用硬約束下基于最小沖擊度的優(yōu)化算法對混合狀態(tài)A*算法進行優(yōu)化，當路徑點距離障礙物較近時，在原來路徑點基礎上得出走廊會存在和障礙物重合的現(xiàn)象。這時得出的最終軌跡也會因此發(fā)生碰撞，優(yōu)化結果應在無碰撞的基礎上，對原來的路徑點做出優(yōu)化。

本文采用矯正走廊位置的方法，即將當前存在碰撞的“走廊”向自由空間挪動。如圖7，對于有一個頂點和障礙物重合的情況，可以沿著對角線附近方向微調(diào)，不一定沿著對角線方向，因為這樣的調(diào)整方法可能會造成“走廊”不連續(xù)的情況。對于2個頂點和障礙物重合的情況，只需要將走廊中心沿著另一端平移即可。而對于4個頂點與障礙物重合但中心不重合的情況，則需要沿2端的連線方向縮小走廊，然后再平移走廊。所以，可以得出圖8的走廊調(diào)整算法。

圖7 “走廊”調(diào)整策略

圖8 “走廊”調(diào)整算法

設置初始狀態(tài)為[9,16,0],最終狀態(tài)為[12,8,pi]，繞過障礙物后汽車需要停在停車位上。星點代表了由混合狀態(tài)A*算法搜索得到的路徑點，方框內(nèi)的實線是混合狀態(tài)A*算法得出的軌跡，菱形是根據(jù)混合狀態(tài)A*算法搜索得到的路徑點調(diào)整的“走廊”的中心，方框構成了最終的“走廊”。在不改變其它條件的情況下，調(diào)整“走廊”在x和y方向上的長度都為3 m。如圖9所示，對比起走廊約束分別為2 m的結果，更大的約束下得出的優(yōu)化結果與混合狀態(tài)A*得出的軌跡差別更大。

圖9 不同走廊約束下的優(yōu)化結果

表2是在混合狀態(tài)A*算法搜索出49個路徑點，優(yōu)化算法得到588個系數(shù)，4 949個路徑點的條件下，在MATLAB中混合狀態(tài)A*算法以及硬約束下的優(yōu)化算法的運行時間。此處進行了3次試驗。

表2 硬約束下基于最小沖擊度優(yōu)化算法運行時間 s

在相同條件下試驗，最終的走廊不再與障礙物重合，所以最終的軌跡也不再發(fā)生碰撞。不過從運行時間來看，優(yōu)化算法的運行時間并不快。與上一節(jié)類似，最后需要在柵格地圖的障礙物層加入障礙物信息，給汽車加上傳感器，以探究動態(tài)過程中算法的表現(xiàn)。從起點到終點的運動過程中，汽車需要不斷地感知周圍的環(huán)境。在地圖中，當汽車接近突然出現(xiàn)的障礙物或汽車運動到某位置才檢測到周圍具有障礙物時，需要規(guī)劃器重新做出規(guī)劃。

4.4 對優(yōu)化結果的評價

此處對圖9（b）的結果進行進一步討論。已知初始狀態(tài)為[9,16,0]，最終狀態(tài)為[12,8,pi/2]，繞過障礙物后汽車需要停在停車位上?；旌蠣顟B(tài)A*算法的最小轉彎半徑為2 m，搜索步長為2 m。設置“走廊”在x和y方向上的長度都為3 m。汽車的最大速度為20 m/s，汽車的最大加速度為10 m/s2。

在得出從起點到終點的路徑之后，對多項式求導便可以得出其速度、加速度和沖擊度。由此獲得速度曲線、加速度曲線和沖擊度曲線如圖10和圖11。由這2幅圖可知，優(yōu)化后的曲線是近似梯形的，最高速度是3.87 m/s，對應的速度代價值，即路程為60.85 m。同時，最大加速度也在合理范圍內(nèi)，為3.97 m/s2。除去起始和終止時刻，沖擊度也較小，優(yōu)化后的代價值為

圖10 速度曲線

圖11 加速度和沖擊度

213.66 m2/s5。

至于優(yōu)化前后軌跡的曲率，可以通過優(yōu)化前后的路徑點來計算。除去起點和終點的曲率無法計算外，其它點最終都可以得出一個曲率值。優(yōu)化前有320個路徑點，最終得出318個點的曲率。優(yōu)化后的點有505個點，最終得出503個點的曲率?？捎蓤D12知，優(yōu)化前的曲率振蕩振幅很大。由于優(yōu)化前后計算曲率的方法一致，考慮到可能是由于優(yōu)化前的點過于稠密，所以刪除點的個數(shù)為原來的1/4，便可得出圖12b所示的曲率。此時，優(yōu)化后的曲率是小于優(yōu)化前的曲率。由于數(shù)值計算的不穩(wěn)定，所以初始和終止的曲率會比較大。事實上，當具體分析起點和終點的曲率數(shù)值時，可以看出初始時刻，只有4個點是過大的，終止時刻，則有8個點較大，但是相比起505點，這12個點只占2%左右。也就是說，絕大多數(shù)計算出的曲率是在正常范圍以內(nèi)的。

圖12 優(yōu)化前后的曲率

最后，分析優(yōu)化前后路徑點距離周圍障礙物的最小距離，從圖13可知，優(yōu)化前路徑點到障礙物的最小距離為0.20 m，而優(yōu)化后路徑點到障礙物的最小距離為0.30 m。優(yōu)化前距離的代價值為6.19，而優(yōu)化后為3.76，在距離這個評價指標上，優(yōu)化后的路徑點也是優(yōu)于優(yōu)化前的。

圖13 優(yōu)化前后路徑點到障礙物的距離

從上面的試驗結果可以看出，優(yōu)化后的軌跡不僅是基于最小沖擊度，同時也可以得出近似梯形的速度曲線。優(yōu)化后的曲率和到障礙物的距離也優(yōu)于優(yōu)化前的曲率和距離。這說明硬約束下基于最小沖擊度的優(yōu)化算法在整體上是優(yōu)于優(yōu)化前的算法，可以在保證舒適性的前提下，得出較優(yōu)的軌跡，具有應用于實際場景的潛力。

5 結語

本文主要針對自動駕駛汽車的軌跡優(yōu)化問題展開研究，聚焦如何通過優(yōu)化手段，得出汽車最終可行駛優(yōu)化軌跡的同時提高自動駕駛汽車的舒適性。主要研究了最小沖擊度下汽車的軌跡生成問題，基于硬約束下的最小沖擊度模型對原有的優(yōu)化模型進行了改進。對比分析后可知：雖然硬約束下的優(yōu)化算法計算速度不占優(yōu)勢，但是優(yōu)化后的軌跡無碰撞，且在基于最小沖擊度的前提下，曲率更小、加速度保持在正常范圍內(nèi)、距離代價值也降低了39.3%?？傮w上硬約束下基于最小沖擊度的優(yōu)化算法解決了原有模型優(yōu)化后相關狀態(tài)量超出實際范圍及優(yōu)化后出現(xiàn)碰撞的問題，使得原有模型更加適合于非結構化空間場景。