基于混合A*算法的自動(dòng)駕駛汽車(chē)軌跡生成

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

（1.同濟(jì)大學(xué)汽車(chē)學(xué)院，上海 201804；2.同濟(jì)大學(xué)中德學(xué)院，上海 201804）

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

1 引言

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

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

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

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

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

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

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

（5）基于機(jī)器學(xué)習(xí)的規(guī)劃算法。

各類(lèi)規(guī)劃算法研究進(jìn)展如下：

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

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

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

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

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

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

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

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

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

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

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

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

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

（5）基于機(jī)器學(xué)習(xí)的規(guī)劃算法

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

通過(guò)以上分析，目前自動(dòng)駕駛車(chē)輛的運(yùn)動(dòng)規(guī)劃算法存在以下缺陷：

（1）考慮了汽車(chē)動(dòng)力學(xué)的規(guī)劃算法計(jì)算較慢，且不能保證規(guī)劃處的路徑總是無(wú)碰撞的。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

對(duì)于多項(xiàng)表達(dá)式，可以用如下表達(dá)式來(lái)表示：

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

同時(shí)還需滿足如下約束：

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

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

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

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

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

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

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

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

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

圖3 優(yōu)化后汽車(chē)的運(yùn)動(dòng)速度和加速度

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

表1 算法運(yùn)行時(shí)間比較 s

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

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

本文采用的地圖是柵格地圖，以上試驗(yàn)全部是在靜態(tài)層進(jìn)行的試驗(yàn)。實(shí)際應(yīng)用中，從起點(diǎn)到終點(diǎn)的運(yùn)動(dòng)過(guò)程中，汽車(chē)需要不斷地感知周?chē)沫h(huán)境。在靜態(tài)地圖中，當(dāng)汽車(chē)接近突然出現(xiàn)的障礙物或汽車(chē)運(yùn)動(dòng)到某位置才檢測(cè)到周?chē)哂姓系K物時(shí)，可能導(dǎo)致之前規(guī)劃的軌跡失效，需要規(guī)劃器重新做出規(guī)劃。所以此處給汽車(chē)柵格地圖障礙物層加入障礙物信息，并給汽車(chē)加上傳感器再次進(jìn)行試驗(yàn)。

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

圖5 兩種算法的最終運(yùn)動(dòng)軌跡

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

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

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

圖6 算法改進(jìn)策略

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

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

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

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

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

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

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

4.1 貝塞爾曲線

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

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

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

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

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

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

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

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

對(duì)于如下形式的一般多項(xiàng)式：

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

要將其轉(zhuǎn)化為式（10）所示的形式，需要建立一個(gè)映射矩陣如：

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

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

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

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

最終，問(wèn)題轉(zhuǎn)換成了一個(gè)典型的凸優(yōu)化問(wèn)題，可以采用求解凸優(yōu)化問(wèn)題的方法來(lái)求解相應(yīng)的系數(shù)。

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

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

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

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

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

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

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

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

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

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

4.4 對(duì)優(yōu)化結(jié)果的評(píng)價(jià)

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

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

圖10 速度曲線

圖11 加速度和沖擊度

213.66 m2/s5。

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

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

最后，分析優(yōu)化前后路徑點(diǎn)距離周?chē)系K物的最小距離，從圖13可知，優(yōu)化前路徑點(diǎn)到障礙物的最小距離為0.20 m，而優(yōu)化后路徑點(diǎn)到障礙物的最小距離為0.30 m。優(yōu)化前距離的代價(jià)值為6.19，而優(yōu)化后為3.76，在距離這個(gè)評(píng)價(jià)指標(biāo)上，優(yōu)化后的路徑點(diǎn)也是優(yōu)于優(yōu)化前的。

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

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

5 結(jié)語(yǔ)

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