凸近似避障及采樣區優化的智能車輛軌跡規劃

張宜旭，田國富，王海濤

沈陽工業大學 機械工程學院，沈陽 110870

自動駕駛是當前汽車開發技術的重點，而軌跡規劃作為車輛系統的關鍵組成部分[1]，其理論方法最早出現在機器人領域[2]，智能汽車將其優化改進，以符合車輛模型并應對復雜交通環境下的行駛問題。自動駕駛中的路徑規劃分為兩類：即基于高精地圖下的全局路徑規劃和基于車輛周邊環境變化的局部路徑規劃（軌跡規劃）[3]，二者有許多相通的地方。

目前，常用的全局路徑規劃方法主要有基于采樣的方法（隨機采樣、確定性采樣等）、基于搜索的方法等。其中，Dijkstra 算法[4]與A*算法[5]為常用的經典搜索方法。如Bohren 等[6]采用Dijkstra 算法實現路徑規劃，但生成曲線不夠平滑，需二次處理；王紅衛等[7]根據A*方法進行改進，用以解決曲線轉折頻繁問題，但曲率不夠連續。局部路徑規劃算法主要有人工勢場法[8]、神經網絡法等。如劉梓林[9]等基于人工勢場法和點云數據進行道路規劃，但效果受車輛尺寸影響較大。

快速拓展隨機樹（rapidly-exploring random trees，RRT）等采樣方法為當前研究的熱點，但缺點為生成的軌跡不連續[10]。為解決上述問題，馮來春等[11]通過限制RRT 采樣區域來對結果進行優化；Jeon 等[12]基于RRT*算法，通過設計邊界約束生成車輛軌跡。另外，利用多項式方法生成采樣軌跡能夠很好地解決該缺點，Werling等[13]基于Frenet坐標系通過采樣和多項式擬合規劃車輛行駛軌跡；張榮輝等[14]通過多項式方法生成多車換道軌跡簇。

為解決智能車輛避障軌跡規劃時面臨的采樣區域劃分問題，提出基于凸近似避障原理及采樣區域優化的智能車輛軌跡規劃方法：首先，引入凸近似避障原理[15-16]，通過本車與障礙物的位置及體積關系，得到軌跡可行域范圍；對采樣區域進行劃分：將采樣區域分為靜態采樣區、動態采樣區、動態、靜態障礙物采樣區。采用“動態規劃（DP）+二次規劃（QP）”思想求解軌跡：首先利用多項式軌跡對采樣點依次連接，建立動態規劃代價函數并篩選得到粗略軌跡；構造二次規劃的等式與不等式約束，對粗略軌跡進行平滑從而得到最優軌跡。

1 Frenet坐標系及轉換關系

Frenet 坐標系以參考線（reference line）為基準，將車輛的平面運動解耦為縱向和橫向的獨立運動，并用(s,l)描述。其坐標系及參數如圖1和文獻[17]所示。

以(x0,y0)作為參考線起點，并將參考線均勻離散為一定數量的點，當點的數量足夠密時，則縱向位移s近似為：

至此，完成了由直角坐標(xh,yh)向(s,l)坐標的轉換。同理，可將道路環境中其他障礙物用同樣方法進行轉換[17]。

2 車輛軌跡規劃算法模型

2.1 凸近似的局部避障原理

凸近似避障原理實際是由多個凸多邊形構成的車輛安全行駛區域。對自車和障礙車做如下定義：

（1）令自車質心位置為p0=[x0,y0]T，另外設置向量dk∈R2，k=1,2,…,r。由于車輛為矩形，因此可將自車包含在多邊形p0+conv{d1,d2,…,dr} 中。顯然，當dr=0 時，則自車將簡化為一個點。

（2）令障礙車的參考點表示為qi∈R2，i=1,2,…,M。其中，M為障礙物數量。設p0,q1,q2,…,qM∈R2，p0≠qi，i=1,2,…,M，故可以得到凸多邊形區域P：

式中：

當i=1,2,…,M時，p0存在于凸多邊形p中，qi不在凸多邊形p中，最終形成如圖2 所示的車輛行駛安全區域。

在實際行車過程中需要考慮障礙車外形尺寸，因此，車輛安全行駛空間應較凸空間相對減小。如圖3所示。

圖3 去除障礙物的凸空間示意圖Fig.3 Schematic diagram of convex space for removing obstacles

凸近似避障原理通過本車與障礙車的質心距離及方向，構建出車輛安全行駛區域。以車輛避障為例，其安全行駛區域如圖4所示。

圖4 避障可行域示意圖Fig.4 Schematic diagram of obstacle avoidance feasible domain

本車通過雷達與傳感器，測得兩者質心之間的方向與距離。考慮到障礙車的外形尺寸，故其所占據的區域皆為危險區域（圖4中紅色框所示）。另外，車輛通過高精度地圖獲取道路邊界，進一步限制該可行域范圍，最終其可行凸空間如圖4灰色區域所示。

另外，可對障礙物外形所占據的區域設置一個膨脹系數q，進一步保證其行駛安全性。同理，對于多障礙物，應用此方法同樣可得到可行域。

2.2 問題建模

一般而言，車輛的行駛軌跡可以近似看成一系列離散點，因此，軌跡規劃可以看作離散點(x,y)的規劃，速度規劃可以看作到達每個離散點的時間。因此，軌跡規劃可以看作在三維空間(x,y,t)內找到在約束下的最優離散點。如下式：

式中，z為優化目標，即軌跡點序列，有p個不等式約束和q個等式約束。

為保證車輛行駛安全性和應對突發狀況，軌跡規劃應滿足實時性要求，因此可設置算法以合適的頻率執行。為了減小問題運算規模，優化算法運行時間，可使用交替優化[18]思路將原本(s,l,t)三個維度的優化問題轉換為兩個二維的S-L 與S-T 優化問題，分別對應軌跡規劃與速度規劃。

另外，軌跡規劃與跟蹤算法應考慮合適的車輛運動學或動力學模型，使軌跡完全符合車輛實際運行情況，因此可采用文獻[19]所展示的簡化后的車輛模型。

3 車輛采樣區設計

3.1 采樣區定義

在Frenet 坐標系下對空間進行離散采樣的區域稱為采樣區。按照文獻[20]，當本車探測到前方障礙物并相距一定距離后，本車開始采樣，其采樣區始終為距車輛前方一定長度S的整個路面區域。如圖5所示。

圖5 傳統采樣區Fig.5 Conventional sampling area

由于車輛在行駛過程中，尤其是在高速情況下，其橫向變化量應保持在較小范圍內以維持車輛行駛穩定性。并且，當相距前方障礙物較遠距離時，本車不需要過大的橫向位移去進行避障。因此，在圖5所示的傳統采樣區的基礎上，可將縱向上距本車較近且橫向上距本車較遠的采樣點舍去，為保證避障的安全性，同時稍微增加其采樣距離S。而當本車逐漸逼近障礙物時，采樣點數應逐漸增密，從而保證最優避障軌跡。

根據采樣點數的變化情況，可將采樣區劃分為動態采樣區與靜態采樣區兩部分。并且在障礙物附近設置障礙物采樣區，且始終包含在靜態采樣區內。

以車輛左換道為例，其采樣區劃分如圖6所示。

圖6 采樣區定義Fig.6 Sampling area definition

式中，nsd、njd表示動態采樣區內的縱向、橫向采樣點個數，nsq、njq表示靜態采樣區內的縱向、橫向采樣點個數。s0、s1、s2表示采樣區邊界距本車車頭之間的距離。以道路中心線為零位置，dmax、dmin分別表示采樣寬度的兩側邊界。

為確保車輛不會超出道路邊界，則：

式中，wr為車道線寬度，w為自車車寬。

等間隔的離散點表示為：

式中，s(k+1)、l(k+1)分別表示在第k+1 點的s、l坐標，s(k)、l(k)分別表示在第k點的s、l坐標，Δs與Δl分別為相鄰采樣點的縱向間隔與橫向間隔。

3.2 動態采樣區與靜態采樣區

動、靜態采樣區逐漸過渡過程如圖7所示。

圖7 采樣區過渡Fig.7 Sampling area transition

圖7（a）為本車采樣初始階段，由于障礙物并未進入采樣區內，即障礙物距離s3大于采樣區長度s2，本車并不需要進行避障求解，此時動態采樣區與動態采樣區未產生過渡，故此時動態采樣區面積最大。采樣區長度s2由采樣點縱向間隔Δs及采樣點總列數nL決定，根據3.1節，有：

式中，s0為本車車頭距第一列采樣點的縱向距離，采樣點總列數nL=nsd+nsq。

根據圖7，采樣區內采樣點的最大行數nH為：

另外，由于動態采樣區內采樣點數量隨著本車距障礙物的距離的降低而逐漸增加，因此，可設置動態采樣區內相鄰兩列的采樣點的個數相差1，如圖7所示，故動態采樣區采樣點隨障礙物的接近逐漸增加一行可形成對靜態采樣區的過渡。

根據圖7（a），此時動態采樣區其右邊界s1為：

應將障礙物始終包含在靜態采樣區內，該區域內采樣點數足夠多，以保證可求得最優解。有：

式中，δ1、δ2為縱向間隔系數，用以保證障礙物與靜態采樣區左邊界的距離大小。二者相差越大，則距離越大。

根據圖7 與式（11），隨著障礙物的逼近，兩采樣區間進行過渡，采樣點數量逐漸增多，直至s1=s0時，采樣點數量達到最大。

3.3 障礙物采樣區

為簡化運算，可在障礙物附近設置障礙物采樣區，根據距離代價函數，將區域內滿足代價大小的采樣點選擇性忽略。

一般而言，道路障礙物分為靜態、動態障礙物兩種，應根據不同情況分別設置障礙物采樣區。

3.3.1 靜態障礙物采樣區

在自然坐標系下，若障礙物速度vobs滿足：

可將其視為靜態障礙物。式中vmax為動、靜態障礙物的速度邊界。另外，設置障礙物距離代價函數：

式中，k為距離代價常數，d1、d2為距離常數，xiq,jq為靜態采樣區內采樣點Γq(siq,ljq)對障礙物質心(s0,l0)的距離，對采樣點做遍歷：

3.3.2 動態障礙物采樣區

相對于靜態障礙物，動態障礙物與本車的兩次相遇（本車頭部與障礙物尾部、本車尾部與障礙物頭部）之間的行駛范圍皆為危險區域，因此可將該區域作為靜態障礙物進行處理：

首先對動態障礙物進行篩選：

式中，vs為本車車速。當滿足上述情況時，本車與障礙物距離逐漸降低。在該過程中，動態采樣區與靜態采樣區進行正常過渡，其動態障礙物采樣區按照3.3.1 節的方法進行計算，使得障礙物質心始終包圍在應舍去的采樣點區域內，正如圖7 所示。由于障礙物向前運動，因此障礙物采樣點隨障礙物質心進行更新。

當本車與障礙物的距離代價滿足一定條件時，本車執行換道操作。此刻二者關系如圖8（a）所示。

圖8 超車過程Fig.8 Overtaking process

圖8（a）中黑色采樣點為按照3.3.1小節的方法計算出的障礙物采樣點。另外，在未來采樣時刻，由于障礙物向前運動，障礙物采樣區內采樣點個數應在此基礎上隨著障礙物運動方向逐漸增加，直至本車二次換道繞過障礙物。

圖8（a）中障礙物采樣點位置及相對于障礙物質心(s0,l0)代價已知，本車對其記錄。設置算法計算頻率為fM，則每隔秒，本車對采樣區進行重新遍歷計算。

由于障礙物的運動，障礙物質心坐標由上一個采樣時刻的(s0,l0)變為(s1,l1)（正如圖8（a）（b）所示），此時根據采樣點與障礙物質心坐標間的距離代價，其障礙物采樣區域在障礙物前進方向遞增一列（正如圖8（b）中綠色采樣點），此時上一時刻所記錄的障礙物采樣點（圖8（b）中黑色采樣點）代價以及當前時刻所遞增的采樣點代價在當前時刻得到更新，對于某個重復的采樣點，其代價有：

為保證本車在相鄰車道的行駛安全以及在第二次換道過程中避免被障礙物追及，可在障礙物前方設置長度為L的障礙物區域，將該區域內所包括的采樣點作為障礙物采樣點，如圖8（c）中橘黃色采樣點所示，有：

其中，n=1,2,…,n。其n值受障礙物速度影響，若障礙物速度越高，為保證足夠安全，n取值越大，障礙物前方采樣點越多。由圖8（c）所示，n=1。

其每列的采樣點個數以障礙物中心為參考，對稱分布。有：

式中，w為障礙物寬度，n1為每列障礙物采樣點個數，應取滿足上式的最小整數。如圖8（c）所示，其值為3。

至此，動態障礙物與本車的兩次相遇過程之間障礙物行駛區域，以及障礙物前方長度為L的區域已作靜態障礙物處理，利用上節對靜態障礙物的處理辦法，將此類采樣點從整體中剔除，為下文實現對動態障礙物的避障提供條件。

4 車輛軌跡規劃

采用“動態規劃+二次規劃”的思想。首先根據Δs與Δl確定采樣點的分布并利用多項式軌跡進行連接，通過構建動態規劃目標函數對軌跡進行篩選，從而得到粗糙換道軌跡。之后利用二次規劃理論對粗糙軌跡進行平滑，從而得到符合車輛運動特性的換道軌跡。

4.1 基于五次多項式的車輛運動軌跡

首先，應需要選擇合適的多項式軌跡對采樣點進行連接。在Frenet坐標系下，車輛軌跡信息為：

其中，自變量為縱向位移s，因變量為橫向位移l。設車輛準備換道時刻其質心為(s0,l0)，根據坐標系轉換原理及車輛傳感器，有：

式中，θ為車輛航向角。假設在換道時刻，本車與采樣點的關系如圖9所示。

圖9 本車及其采樣點Fig.9 This vehicle and sampling points

為保證車輛行駛穩定性，則本車在相距最近的采樣點處的側向速度與加速度應為零，則位姿信息為：

同理，根據圖9，在另外采樣點的位姿信息有：

以此類推，可得到每個采樣點處的位姿信息。因此，若對相鄰兩列中的兩個采樣點做平滑軌跡連接時，其軌跡信息應滿足如上6個條件。因此應選擇5次多項式軌跡，其未知項系數的個數為6，其一般形式為：

按照此方法，將相鄰兩列的采樣點進行遍歷，形成如圖6所示的軌跡束。

4.2 車輛軌跡的動態規劃

當規劃出軌跡束之后，應從中篩選出一條最優軌跡，因此，可通過設立合適的動態規劃目標函數實現。

（1）軌跡應保持平滑，防止車輛跟蹤時發生抖動，因此其軌跡平滑代價如下：

（2）本車應盡量在車道中心行駛，防止相鄰車道的其他車輛對本車的影響。因此，設置參考線距離代價：

式中，w4為偏移權重系數，li為軌跡的橫向位移，lref為道路中心線的橫向位移，一般作為參考線，其值為0。

因此，兩者代價之和可作為動態規劃目標函數：

由于軌跡連接相鄰兩列的采樣點，且采樣點為多列，故軌跡為分段短軌跡，如圖10所示。應采用合適方法對其進行連接，組成一條能夠避障且代價函數最小的長軌跡。

圖10 分段短軌跡示意圖Fig.10 Schematic diagram of piecewise short trajectory

首先，對于其某個終點采樣點（如圖10中的(s2,l3)），從(s0,l0)出發，其軌跡連接方式有：

最終，對軌跡束進行遍歷篩選，選取其小值進行短軌跡拼接，從而得到最優長軌跡。如圖11所示。

圖11 最優長軌跡Fig.11 Optimal length trajectory

4.3 車輛軌跡的二次規劃

4.3.1 二次規劃目標函數構造

當動態規劃篩選出粗略軌跡之后，可利用二次規劃進行平滑。

二次規劃（QP）是應用非常廣泛的非線性規劃問題。其標準形式定義為：

式中，X為待求解變量；H為Hessian 矩陣；f為梯度向量。若H矩陣為半正定或正定，則該問題稱為凸二次規劃問題。

首先設置合適的目標函數以求得最優軌跡，目標函數由如下三部分組成：

（1）平滑性

（2）靠近道路中心線

與式（25）類似，本車應盡量在車道中心行駛，故軌跡應靠近道路中心。式中lmax、lmin分別為道路上下邊界，li為軌跡橫向位移，wmid為權重。

（3）軌跡終點狀態約束

至此，二次規劃目標函數為：

對該式進行標準化轉換。設二次規劃優化變量為：

梯度向量f為：

至此，將目標函數轉換為如式（29）所示的標準形式。

4.3.2 二次規劃約束條件的構造

轉換成矩陣形式，有：

優化變量為式（34）所示，根據式（38），有：

至此，完成目標函數的等式約束條件。

另外，在規劃出粗略凸空間之后，由于車輛有形狀與體積，因此每個邊界點碰撞情況都不同，如圖12所示。

圖12 車輛碰撞情況圖Fig.12 Vehicle collision diagram

圖12中，p1、p2、p3、p4為車輛四個邊角點；d1、d2為質心距車頭、車尾之間的距離；L為質心距參考軌跡的橫向距離；w為車輛寬度；θ為車輛行駛方向與參考軌跡的夾角。此時：

式中，lp1、lp2、lp3、lp4為車輛邊角點距參考軌跡的縱向距離，將上式進行簡化，令sinθ≈tanθ≈l′，cosθ≈1。

另外，如圖12 所示，當車輛接近左前方障礙物時，p1邊角點會首先碰撞，此時p1點與距該點橫向的障礙物邊界距離lmax為零。為保證車輛行駛安全，應：

同理，當車輛靠近右前方障礙物時，有：

綜合可得：

優化變量為式（34）所示，根據式（43）可得：

式中，A=O8n×3n;b=O8n×1;其中A(8i-7:8i;3i-2:3i)=As；b(8i-7:8i;1)=bs；i=2,3,…,n。至此，完成目標函數的不等式約束條件。

綜上，通過matlab 中的quadprog 求解器，通過以上約束條件，結合車輛的縱向位移s，可計算出一條可供車輛行駛的最優軌跡。如圖13所示。

圖13 車輛二次規劃最優軌跡Fig.13 Optimal trajectory of vehicle quadratic programming

對比圖13與圖11，通過二次規劃之后，其軌跡較為平滑，符合車輛跟蹤條件。另外，平滑程度與目標函數內的權重系數有關，若權重系數越高，則軌跡越平滑，但求解速度會變慢。因此，應合理選取其權重系數。最后，求解出的最優軌跡通過坐標系轉換原理轉換至全局坐標系下供車輛跟蹤，實現車輛的避障。

5 多場景仿真驗證

為驗證本方法的有效性，可通過搭建Prescan-Carsim-Matlab/Simulink聯合仿真平臺進行驗證。其中：Prescan用來構建道路行駛環境，設置汽車、行人等障礙物變化；Carsim 用來設置車輛本身配置等系統參數；Matlab/Simulink用來搭建系統算法。

為更直觀地觀察方法的優劣，搭建與文獻[17]參數相同的方法做對比，在相同場景下觀察二者避障軌跡規劃效果。本方法仿真參數如表1所示。

表1 仿真參數Table 1 Simulation parameters

本文采用同向筆直雙車道仿真場景，該道路通過道路邊界及中心線三組離散點擬合而成。設計三種場景對本車進行驗證：即本車分別對靜態障礙物、低速障礙物、動態障礙物這三種場景的避障操作。

5.1 靜態障礙物仿真驗證

智能車通過該場景時的軌跡如圖14 所示，其中藍色線表示自車按本文方法在當前時刻所規劃的最優軌跡，綠色線為文獻[17]的方法所規劃的最優軌跡。

圖14 靜態障礙物避障過程Fig.14 Static obstacle avoidance process

設定本車行駛速度始終為10 m/s，從t=0 s 仿真開始階段，自車向正前方運動；在t=1.5 s 時，車輛前方障礙物由于距離較遠，未對本車軌跡產生影響；在t=5.5 s時，車輛已探測到前方障礙物，這兩種方法皆只規劃出對車輛最近的障礙物的避障軌跡；在t=10.5 s 時，在越過第一個障礙物之后，接著對下一個障礙物進行避障軌跡規劃；在t=15 s時，車輛繼續前行，避障完成。由圖14可知，此兩種方法所規劃出的軌跡相差不大。

5.2 低速障礙物仿真驗證

設定障礙車車速為0.9 m/s，對于本文方法而言，由于其速度不高，可按照靜態障礙物處理。參考方法未牽扯到對動態障礙物的避障操作，但可通過提高其碰撞損失權重，從而增加規劃的軌跡對障礙物的距離以保證其安全性。其避障過程如圖15所示。

圖15 低速障礙物避障過程Fig.15 Low speed obstacle avoidance process

黑色、紅色虛線矩形框表示分別表示本車與障礙車開始避障時刻的位置，當換道結束時刻，障礙物位置由紅色矩形框所示，本車位置由最右方黑色矩形框所示。

由圖15 可知，兩種方法在兩次換道階段軌跡相差不大，但本文方法在相鄰車道上距障礙物的縱向距離比參考方法較遠，其安全性更高。而對于參考方法來說，合適的碰撞損失權重能提高其碰撞安全性。

5.3 動態障礙物仿真驗證

（1）障礙物做勻速運動

設定障礙車勻速運動的車速為6 m/s，由于其速度高，故本方法應按照動態障礙物處理，另外，參考方法應選擇合適的碰撞損失權重對其進行避障。該過程如圖16所示。

圖16 勻速障礙物避障過程Fig.16 Constant speed obstacle avoidance process

（2）障礙物變速運動運動

設定障礙物前10 s 做速度為6 m/s 的勻速運動，之后保持靜止。其避障過程如圖17（a）。

圖17 變速障礙物避障過程Fig.17 Variable speed obstacle avoidance process

設定障礙物前10 s 做速度為6 m/s 的勻速運動，之后以1 m/s2的加速度做勻減速運動。其避障過程如圖17（b）。

設定障礙物前10 s 做速度為5 m/s 的勻速運動，之后以1 m/s2的加速度做勻加速運動。其避障過程如圖17（c）。

對于勻速障礙物而言，由圖16所示，兩種方法皆能避過障礙物，但參考方法與障礙物距離較低，不能保證換道安全性。

對于變速運動障礙物，由圖17（a）所示，本文方法參考方法未能及時處理先運動后靜止的障礙物，因此在二次換道時刻，參考方法距障礙物距離過遠，不利于車輛通行效率，另外，在相鄰車道行駛過程中，距離障礙物過近。

對于先勻速后減速的障礙物如圖17（b）所示，與圖17（a）類似，參考方法在二次換道段距障礙物稍遠，在相鄰車道段距障礙物過近。

對于先勻速后加速的障礙物，如圖17（c）所示，參考方法在二次換道段與障礙物相撞，且在相鄰車道行駛段距障礙物過近。

綜上，對于靜態障礙物而言，二者效果相差不大。對于動態障礙物的避障，本文方法優于參考方法。

6 結論

針對結構化道路下做勻速運動的智能車輛避障軌跡規劃問題，以及基于采樣的軌跡規劃方法未將采樣環境做區域劃分問題，提出基于凸近似避障原理及采樣區域優化的智能車輛軌跡規劃方法，并與文獻[17]方法進行對比。仿真結果表明，該方法能夠有效地得到平滑軌跡并避開障礙物。