結構化道路下基于層次分析法的智能車避障軌跡規(guī)劃

曾德全 余卓平 熊璐 付志強 張培志

(同濟大學 汽車學院∥新能源汽車工程中心，上海 201804)

作為有望能高效緩解交通壓力、提升道路安全與降低能耗的前沿技術，自動駕駛車輛的研究在近幾年得到了長足發(fā)展[1- 2]；而作為其核心技術之一的車輛軌跡規(guī)劃研究也逐步深入[3- 4]。

作為典型的軌跡規(guī)劃實現(xiàn)方法，Dijkstra算法[5]基于廣度優(yōu)先搜索策略，按路徑長度遞增次序產生最短路徑。為實現(xiàn)算法加速，A*算法在Dijkstra的代價估算中引入節(jié)點到目標點的啟發(fā)值[6]。由于自動駕駛車輛所處的環(huán)境包含部分未知的變化信息，D*算法會利用未受影響的節(jié)點信息進行再規(guī)劃，從而適應于動態(tài)環(huán)境的軌跡規(guī)劃[7]。由于規(guī)劃空間是離散的柵格形式表達的，規(guī)劃的結果為曲率不連續(xù)的折線片段，考慮到存在車輛轉向機構的約束，上述算法的結果不適于車輛直接執(zhí)行，為此，通過改進節(jié)點的擴展方式衍生的Hybrid A*[8]和改進節(jié)點連接方式衍生的State Lattice[9]是基于搜索算法的最新發(fā)展方向。規(guī)劃的結果是圓弧加直線片段組，規(guī)劃路徑的曲率仍然不連續(xù)，基于搜索方法的結果還需要進一步的平滑處理，車輛才能執(zhí)行[10]。與搜索類方法不同，RRT(Rapidly-Exploring Random Tree，快速擴展隨機樹)[11]代表的基于采樣的方法[12]是在連續(xù)的特征空間里擴展得到軌跡節(jié)點。為提高算法的實時性，衍生出從起點和終點雙向擴展的Bi-RRT(雙向RRT)[13]和引入啟發(fā)函數(shù)以提高代價低節(jié)點被擴展概率的hRRT(啟發(fā)RRT)[14]。但是，基于采樣的算法始終包含一個隨機種子生成器，所以不可避免的包含曲折的節(jié)點[10]，使得規(guī)劃結果同樣需要進一步的平滑處理，典型的代表如中國科學技術大學“智能先鋒II”[15]，他們采用B樣條曲線對RRT規(guī)劃的軌跡進行平滑，從而使軌跡滿足曲率連續(xù)。

為使車輛能夠直接執(zhí)行規(guī)劃的軌跡，B-spline、Bezier-spline的樣條曲線[16- 18]、螺旋線[19]、多項式曲線[20]和基于車輛模型的模型預測控制[21]這類可以直接實現(xiàn)曲率連續(xù)的軌跡規(guī)劃方法日趨成為研究熱點。但是，上述方法并不具備算法完備性[1]，所以，多以采樣得到目標點集，生成的軌跡會以簇的形式“鋪滿”特征空間，以盡可能涵蓋可行軌跡[22]，同時，結合多目標(安全性[23]、舒適性[24]、經濟性[25]等)需求，對可行軌跡簇進行評估，以篩選出最優(yōu)的軌跡。文獻[26]以路徑與道路邊界的距離、路徑的曲率和路經長度為指標，構造線性的擇優(yōu)目標函數(shù)。文獻[27]以總時間、加速度平方、加速度增量平方、不合法速度、碰撞安全為指標，構造線性的擇優(yōu)函數(shù)。進一步的，文獻[28]則是從靜態(tài)和動態(tài)兩方面分別構造線性目標函數(shù)。然而，現(xiàn)有單一賦權法構造的線性目標函數(shù)間偏差較大，而且，指標計算涵蓋主客觀兩部分，容易產生權重偏好，魯棒性較差。為此，引入層次分析法(Analytic Hierarchy Process，AHP)實現(xiàn)主客觀指標的量化表達。AHP主要優(yōu)點是，每個準則的重要性是通過成對比較確定的，方法簡單易行[29]。文獻[30]針對無人機航路規(guī)劃的多準則約束問題，應用AHP確立航路的穩(wěn)定性、安全性、任務約束和任務完成4個一級指標權重，模糊綜合評價航路的最優(yōu)性。文獻[31]用模糊AHP改進無人機A*航路規(guī)劃，結果表明算法具有良好的穩(wěn)定性、實時性、合理性。

為此，考慮在智能車輛軌跡擇優(yōu)中引入AHP策略，解決單一賦權法存在的權重偏好、魯棒性較差等問題。本研究在三次B樣條曲線生成曲率連續(xù)軌跡簇的基礎上，以平滑性和經濟性為準則，以路徑長度、曲率和、曲率變化率和以及偏離目標點的距離為子準則，構造AHP路徑擇優(yōu)體系，篩選出最優(yōu)的路徑；其次，以三次多項式來規(guī)劃速度曲線；然后，得到曲率和速度連續(xù)的最優(yōu)軌跡；最后，基于國家標準，結合設計的仿真平臺與實車平臺對該方法的穩(wěn)定性與算法的實時性進行驗證。

1 路徑規(guī)劃

1.1 路徑模型

受轉向執(zhí)行器的物理限制，智能車前輪轉角既不能突變，亦不可超過極限。根據(jù)阿克曼轉角模型κ·b=tanδ≈δ(其中，κ為轉彎曲率，b為軸距，δ為前輪轉角)的描述可知，路徑可執(zhí)行需滿足兩大基本條件：曲率連續(xù)和曲率不可超過極限。

考慮路徑可執(zhí)行的第一個基本條件(曲率連續(xù))，選擇三次B樣條曲線作為路徑生成曲線。對于兩條控制線段五個控制點的三次B樣條曲線，文獻[16，18，32]給出了曲率最值的關系式：

(1)

其中，κmax為B樣條曲線的曲率最值，Lmin為兩條控制線段中最短線段的長度，αmin為兩條控制線段的夾角。

由式(1)知，通過控制構成的B樣條曲線的控制線段長度及其夾角，進而實現(xiàn)控制B樣條曲線的曲率最值以滿足路徑可執(zhí)行的第二個條件(曲率不可超過極限)。對于一條完整的避障路徑，采用三條控制線段構成，如圖1所示。

如圖1所示，以AB、BC和CD為路徑的三條控制線段，線段長度依次為l1、l2和l3，控制線段間夾角為α1和α2。應滿足的物理關系為

(2)

其中，|κ1|為控制線段AB和BC控制的B樣條曲線的最大曲率，|κ2|為控制線段BC和CD控制的B樣條曲線的最大曲率。

1.2 路徑簇生成

對于行駛在結構化道路上的智能車，其運動空間被限定在可行駛車道內(參考GB 5768—2009[33]，單個車道的寬度為3.5 m，兩個車道為7.0 m)，行駛方向也受車道的性質決定。

為此，如圖2所示，以目標車道中心線(圖中自上而下第二條曲線(虛線))為基準，沿車道中心線法向(圖中點gn和gm連接的直線段)以等間距d進行目標點采樣，可得到目標點集Gtarget={gi}i∈[n，m](n≤m；若n、m為負，表明位于當前車道中心線的左側；若n、m為正，表明位于當前車道中心線的右側)。對于點集中任意一點gi有：

(3)

圖中，G(xG，yG，θG)是上層決策給定的參考目標點，(xG，yG)和航向θG分別為目標車道中心線上基準點的位置。對應的，(xgi，ygi)為目標點集Gtarget中第i個節(jié)點的位置，θgi為節(jié)點的航向。

1.3 候選路徑提取

由于行車環(huán)境中不可避免的存在障礙物，為確保行車安全，必須將存在碰撞的路徑予以刪除。

智能車中，占據(jù)柵格地圖是最為典型的環(huán)境表達方式，如圖3所示。無障礙物占據(jù)的柵格多以0表示，該區(qū)域也是智能車軌跡可通過的地方；非0的占據(jù)柵格表明該區(qū)域存在其它物體構成的障礙物，智能車不可能從該區(qū)域通過。碰撞檢測最直接的形式便是遍歷智能車沿規(guī)劃軌跡行駛時，車體所投影的每一個柵格是否為0。若不為0，則該條路徑存在碰撞；反之，則不存在碰撞。文中構造的占據(jù)柵格地圖粒度為0.1 m×0.1 m，考慮到運行效率，縱向探測距離設計為40 m，橫向探測距離設計為15 m。

2 路徑擇優(yōu)

層次分析法(Analytic Hierarchy Process，簡稱AHP)[34- 35]是一種簡便、靈活且實用的多準則決策方法。對于一些關系復雜且難于定量分析的系統(tǒng)，層次分析法通過建立結構清晰的層次模型對系統(tǒng)中涉及的影響因素進行科學地分析管理。層次分析法是定性分析與定量分析相結合的數(shù)學方法，是把人的思維過程層次化、數(shù)量化，并用數(shù)學方法為決策提供定量依據(jù)[34- 36]。其基本思想是將復雜的問題分解為若干層次，在比原來系統(tǒng)簡單很多的層次上逐步分析，通過比較若干因素對同一目標的影響，把決策者的主觀判斷用數(shù)量的形式表達和處理，從而確定他在目標中的比重，最終選擇出比重最大的系統(tǒng)方案[34- 36]。層次分析法的主要流程是[34- 36]：①明確問題；②建立層次結構模型；③成對比較法構造判斷矩陣；④層次排序，獲得權重向量；⑤一致性檢驗，確認合理性；⑥層次總排序及一致性檢驗；⑦獲取最優(yōu)系統(tǒng)方案。

圖3 占據(jù)柵格地圖

2.1 擇優(yōu)體系

路徑可提取的評價指標有：路徑長度Sg、曲率和κg、曲率導數(shù)和dκg、偏離目標點的距離loff。

(1)路徑長度Sg

路徑長度Sg與車輛的行駛里程數(shù)直接相關，直接作用于經濟性。計算方法為

(4)

其中，M為路徑總的節(jié)點數(shù)量，sj為當前節(jié)點相對上一節(jié)點的長度增量。

(2)曲率和κg

路徑的曲率與車輪轉向角存在幾何上的對應關系，路徑的曲率和直接反映了車輛行駛路徑的彎曲程度，影響了路徑的平滑性。計算方法為

(5)

其中，κj為路徑節(jié)點j處的曲率。

(3)曲率導數(shù)和dκg

路徑曲率導數(shù)為相鄰兩個路徑點的曲率差對路徑長度的導數(shù)，反映了在此路段轉向輪轉角的變化率，考量了車輛在某條路徑上行駛時曲率變化的綜合快慢。計算方法為

(6)

(4)偏離目標點的距離loff

當智能車行駛時，希望在不發(fā)生碰撞并且不軋車道線的基礎上盡可能地使目標位置靠近車道中心線。這樣，在避障情況下有利于車輛在行駛完此段路徑之后回到原目標車道上，在換道以及轉向情況下有利于車輛行駛進目標車道。

結合以上指標，基于AHP設計的路徑擇優(yōu)體系如圖4所示。

圖4 擇優(yōu)體系

將不同因素兩兩比較的相對重要程度用數(shù)值表示，寫成矩陣的形式即判斷矩陣。構造判斷矩陣是層次分析法將研究問題從定性分析轉為定量分析的關鍵步驟。假設對某上層元素Ai，存在有N個下層元素B1，…，BN，且下層元素構造的判斷矩陣為B，形式如表1所示。其中，數(shù)值bij表示下層元素Bi相比下層元素Bj對上層元素Ai的重要性，該重要性以數(shù)值為表現(xiàn)形式，通常采用數(shù)字1-9及其倒數(shù)作為標度。兩個因素相比較，當數(shù)值越大，表示某因素相比另一因素更為重要。另外，兩個因素之間相互的重要性之比互為倒數(shù)關系，即bij=1/bji。

表1 判斷矩陣B

層次元素的重要性次序的權值，可以通過計算判斷矩陣特征值與特征向量的方式確定，即對于判斷矩陣B，有

BW=λmaxW

(7)

其中，λmax為矩陣B的最大特征值，W為λmax所對應的特征向量。

計算得特征向量后，還需要計算一致性指標CI來檢驗矩陣的一致性。檢驗的目的主要是檢查判斷矩陣中是否所有的優(yōu)先級關系滿足或近似滿足“所有元素都有一個假想權值”的情況，越靠近這種情況，檢驗結果越好。一致性指標CI定義為

(8)

對于有N個下層元素構成的判斷矩陣，存在對應的平均隨機一致性指標RI。下層元素數(shù)量N和RI之間的關系如表2所示。

表2 平均隨機一致性指標RI

隨后，通過計算一致性比例CR可判斷所構造的判斷矩陣是否需要作適當修改。

(9)

當CR<0.10時，認為判斷矩陣的一致性是可以接受的。

2.2 層次單排序和一致性校驗

由于路徑的平滑性直接影響了車輛行駛的平順性，路徑越平滑，平順性越好，實際跟蹤的路徑就越符合期望路徑，因此本研究認為路徑平滑性的重要程度大于經濟性，設定兩者的比例為

B1b：B2b=3：1=0.75：0.25

(10)

其中：B1b代表平滑性相比經濟性對最優(yōu)路徑的重要程度；B2b代表經濟性相比平滑性對最優(yōu)路徑的重要程度。

對于子準則層，首先建立評價路徑平滑性的判斷矩陣。在靜態(tài)結構模型的4個影響因素中，路徑上的曲率和影響車輛方向盤角度的大小，曲率導數(shù)和影響方向盤角度變化快慢，路徑目標點與參考目標點的距離則影響了目標路徑的橫向偏移量，因此這3種因素對平滑性影響較明顯，而路徑長度對平滑性的影響不大，因此4種因素的重要性關系為：曲率和C2c>曲率導數(shù)和C3c>目標點與參考目標點的距離C4c>路徑長度C1c。判斷矩陣以及相應計算結果如表3所示。

表3 平滑性判斷矩陣

根據(jù)式(7)，可得λmax=4.117 0。進一步，結合式(8)，可得一致性指標為CI=0.039 0。最終，一致性比例CR=0.043 3<0.10，由此可知平滑性判斷矩陣構造合理。

其次，構建經濟性判斷矩陣。在上述4個因素中，路徑長度與車輛行駛的里程數(shù)直接相關，因此影響最大；其次，如果曲率和越大，則路徑越彎曲，可認為間接地降低了經濟性。本研究認為曲率導數(shù)和以及路徑目標點與參考目標點的距離對經濟性影響非常小，因此4種因素的重要性關系為：路徑長度C1c>曲率和C2c>目標點與參考目標點的距離C4c>曲率導數(shù)和C3c。經濟性判斷矩陣以及計算結果如表4所示。

表4 經濟性判斷矩陣

同樣得，根據(jù)式(7)，可得λmax=4.051 2。進一步，結合式(8)，可得一致性指標為CI=0.017 0。最終，一致性比例CR=0.018 9<0.10，由此可知經濟性判斷矩陣構造合理。

2.3 層次總排序和一致性校驗

(11)

將式(10)、表3和表4的數(shù)據(jù)代入式(11)，可得總排序結果，如表5所示。

表5 總排序結果

對于層次總排序同樣需要一致性檢驗：

(12)

其中，CIi和RIi分別為與準則層元素系數(shù)bi所對應的子準則層的一致性指標與隨機一致性指標。

由式(10)知，b1=B1b=0.75，b2=B2b=0.25。一致性指標分別為CI1=0.039 0，CI2=0.017 0。平均隨機一致性指標RI1=RI2=0.90。最終可得一致性比例為CR=0.037 2<0.10，由此可知擇優(yōu)體系設計合理。

3 速度規(guī)劃

實際車輛在加速時，油門或制動踏板總有一個從初始位置到目標位置的角度變化過程，通常不存在突變的現(xiàn)象。為此，采用式(13)所示的三次多項式來表示速度對時間的變化，以保證速度與加速度連續(xù)，以及加速度的導數(shù)連續(xù)。

v(t)=at3+bt2+ct+d

(13)

根據(jù)邊界條件和車輛約束，最終可得到速度關于時間的分段多項式函數(shù)。其中由上層決策給出的邊界條件有初始速度v0、初始加速度a0、終點車速vg以及終點加速度ag。另外，在路徑簇生成模塊已經得到路徑曲線，因此路徑長度Sg已經確定，假設走完路徑所需的時間為tg，在速度規(guī)劃完成之前，tg未知。將邊界條件代入式(13)得方程組如下：

(14)

將上述方程組做變換后可得式(15)所示的一元二次方程：

(15)

因此確定速度曲線的關鍵在于確定各段的邊界條件。但在確定邊界條件的過程中，速度生成還需要考慮車輛的動力學約束。車輛受到的動力學約束，不僅體現(xiàn)在執(zhí)行器客觀條件的限制(如最大車速、最大加速度、最大減速度等)，還體現(xiàn)在車輛操穩(wěn)性的限制(如車輛最大側向加速度限制)。為此，結合上文確定的車輛模型，還需滿足的約束有：

(16)

由于路徑點的曲率大都是互不相同的，若要考慮每個路徑點上曲率對速度的約束，計算過程將會非常復雜，因此將整條路徑上的曲率對允許車速的約束轉化為整條路徑上的最大曲率κmax對允許車速vallow的約束：

(17)

4 仿真與實車測試

仿真和實車試驗所用的車輛參數(shù)都是榮威E50的參數(shù)，如表6所示，其中最大允許車速是參考城市工況設定的。仿真試驗環(huán)境中車道線等交通標識參考GB 5768—2009[33]。實車測試場地選擇在與同濟大學智能網聯(lián)汽車測評基地西南相連的雨霧區(qū)直道上，如圖5所示。

表6 關鍵參數(shù)

如圖6所示，聯(lián)仿平臺基于Visual Studio(C++)、MATLAB/Simulink、Carsim、PreScan 4個軟件。Visual Studio(C++)用以開發(fā)設計的規(guī)劃算法；在MATLAB/Simulink中運行軌跡跟蹤算法；輸出車輛控制指令給Carsim中的E50車輛模型，Carsim求解出車輛狀態(tài)參數(shù)之后反饋給Simulink；PreScan用于參數(shù)化建立仿真場景，并且提供仿真實時的可視化界面。仿真驗證在處理器是1.80 GHz Intel ? CoreTMi5- 8250U，帶有7.86 GB內存的筆記本電腦上完成。

圖5 測試區(qū)

圖6 仿真平臺

實車平臺具有線控驅動、線控轉向和線控制動系統(tǒng)，如圖7所示。裝備RT3003高精定位系統(tǒng)以及研華ARK- 3500工控機。軌跡跟蹤策略采用的是層次運動控制[37- 38]。

圖7 實車平臺

4.1 仿真結果與分析

作為典型的避障場景，圖8所示為單障礙物避障過程及仿真結果。

(a)避障過程

(b)碰撞檢測

(c)路徑擇優(yōu)

(d)5 000次循環(huán)的耗時情況

(e)5 000次循環(huán)的耗時分布圖

如圖8(a)所示，在同向兩車道上，智能車E同向車道的前方16 m處有一靜止車輛O1，為規(guī)避障礙物，智能車換道至左側車道繼續(xù)往前行駛。規(guī)劃的路徑碰撞檢測如圖8(b)所示，圖中的原點是規(guī)劃路徑簇中的安全節(jié)點(無碰撞節(jié)點)。經碰撞檢測后，剩余完整路徑構成的候選路徑簇如圖8(c)所示，其中，自上而下的第2條線(實線)便是篩選出的最優(yōu)路徑。圖8(d)是場景A(單障礙物避障)的5 000次循環(huán)測試的耗時數(shù)據(jù)，其分布結果如圖8(e)所示，由圖8(e)可見，0.10 s內能規(guī)劃出可行軌跡的概率在94%，而0.16 s內則100%能規(guī)劃出可行軌跡。

圖9所示為雙障礙物避障場景及仿真結果。如圖9(a)所示，在同向兩車道上，智能車E同向車道的前方25 m處有一靜止障礙物O1，左側車道與車E縱向距離10 m的位置還有一障礙車輛O2。規(guī)劃的路徑碰撞檢測如圖9(b)所示，圖中的原點是規(guī)劃路徑簇中的安全節(jié)點。經碰撞檢測后，剩余完整路徑構成的候選路徑簇如圖9(c)所示，其中，自上而下的第1條線(實線)便是篩選出的最優(yōu)路徑。圖9(d)是場景B(雙障礙物避障仿真)的5 000次循環(huán)測試的耗時數(shù)據(jù)，其分布結果如圖9(e)所示，由圖9(e)可見，0.08 s內能規(guī)劃出可行軌跡的概率在97%，而0.10 s內則100%能規(guī)劃出可行軌跡。場景B相比場景A軌跡規(guī)劃時間較短的原因在于，場景B中與障礙物O2碰撞的路徑不再參與下一步的計算導致的。

(a)避障過程

(b)碰撞檢測

(c)路徑擇優(yōu)

(d)5 000次循環(huán)的耗時情況

(e)5 000次循環(huán)的耗時分布圖

仿真結果表明，規(guī)劃的軌跡能夠規(guī)避障礙物，并且具有較高的實時性。并且0.10 s內能規(guī)劃出可行軌跡的概率不低于94%，說明算法具有較高實時性與穩(wěn)定性。

4.2 實車結果與分析

針對4.1節(jié)中的兩種典型仿真場景，搭建實車測試的環(huán)境，如圖10(a)和圖11(a)所示，其中，圖10(a)對應于圖8(a)的場景，圖11(a)對應于圖9(a)的場景。實車測試結果分別如 圖10(b)-10(e)、圖11(b)-11(e)所示。

圖10(b)和圖11(b)的數(shù)據(jù)表明，車輛在跟蹤規(guī)劃的軌跡時，能夠規(guī)避障礙物。圖10(c)和圖11(c)的數(shù)據(jù)則表明規(guī)劃的車速執(zhí)行器有能力跟蹤。具體的，圖10(d)和圖11(d)的峰值橫向誤差小于0.21 m，這表明規(guī)劃的路徑便于車輛跟蹤；圖10(e)和圖11(e)的峰值車速誤差小于0.42 m/s，平均車速誤差小于0.11 m/s，總體呈收斂的趨勢，這表明規(guī)劃的軌跡便于車輛跟蹤，方法具有穩(wěn)定性。

(a)避障過程

(b)規(guī)劃與實際路徑曲線

(c)規(guī)劃與實際車速曲線

(d)橫向跟蹤誤差

(e)速度跟蹤誤差

(a)避障過程

(b)規(guī)劃與實際路徑曲線

(c)規(guī)劃與實際車速曲線

(d)橫向跟蹤誤差

(e)速度跟蹤誤差

5 結語

基于層次分析法設計了軌跡擇優(yōu)體系以實現(xiàn)結構化道路下智能車最優(yōu)避障軌跡的規(guī)劃。路徑規(guī)劃器以三次B樣條曲線為核心，實現(xiàn)路徑曲率連續(xù)且曲率最值受控，并以此為基礎構造路徑簇。構造AHP路徑擇優(yōu)體系以量化主客觀指標，其中，以平滑性和經濟性為準則，以路徑長度、曲率和、曲率變化率和以及偏離目標點的距離為子準則，篩選出最優(yōu)的路徑。速度規(guī)劃采用三次多項式曲線，以滿足速度、加速度、加速度導數(shù)的連續(xù)。參考國家標準設計測試場景，經5 000次循環(huán)測試表明，0.10 s內能規(guī)劃出可行軌跡的概率在94%，0.16 s內則能100%規(guī)劃出可行軌跡，這表明算法具有較高的實時性。此外；實車峰值橫向誤差小于0.21 m，這表明規(guī)劃的路徑便于車輛跟蹤，峰值車速誤差小于0.42 m/s，平均車速誤差小于0.11 m/s，總體呈收斂的趨勢，這表明規(guī)劃的軌跡便于車輛跟蹤，方法具有較高的穩(wěn)定性。

由于結構化道路下，智能車工作空間不可避免的有交通參與車輛，甚至是行人，為此，接下來研究將考慮動態(tài)場景下的最優(yōu)避障軌跡規(guī)劃。此外，由于感知系統(tǒng)存在的噪聲以及他車駕駛意圖的難以觀測，感知不確定性下的軌跡規(guī)劃也是未來研究的重點。