凸近似避障及采樣區(qū)優(yōu)化的智能車輛軌跡規(guī)劃

張宜旭，田國(guó)富，王海濤

沈陽(yáng)工業(yè)大學(xué) 機(jī)械工程學(xué)院，沈陽(yáng) 110870

自動(dòng)駕駛是當(dāng)前汽車開(kāi)發(fā)技術(shù)的重點(diǎn)，而軌跡規(guī)劃作為車輛系統(tǒng)的關(guān)鍵組成部分[1]，其理論方法最早出現(xiàn)在機(jī)器人領(lǐng)域[2]，智能汽車將其優(yōu)化改進(jìn)，以符合車輛模型并應(yīng)對(duì)復(fù)雜交通環(huán)境下的行駛問(wèn)題。自動(dòng)駕駛中的路徑規(guī)劃分為兩類：即基于高精地圖下的全局路徑規(guī)劃和基于車輛周邊環(huán)境變化的局部路徑規(guī)劃（軌跡規(guī)劃）[3]，二者有許多相通的地方。

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

快速拓展隨機(jī)樹(shù)（rapidly-exploring random trees，RRT）等采樣方法為當(dāng)前研究的熱點(diǎn)，但缺點(diǎn)為生成的軌跡不連續(xù)[10]。為解決上述問(wèn)題，馮來(lái)春等[11]通過(guò)限制RRT 采樣區(qū)域來(lái)對(duì)結(jié)果進(jìn)行優(yōu)化；Jeon 等[12]基于RRT*算法，通過(guò)設(shè)計(jì)邊界約束生成車輛軌跡。另外，利用多項(xiàng)式方法生成采樣軌跡能夠很好地解決該缺點(diǎn)，Werling等[13]基于Frenet坐標(biāo)系通過(guò)采樣和多項(xiàng)式擬合規(guī)劃車輛行駛軌跡；張榮輝等[14]通過(guò)多項(xiàng)式方法生成多車換道軌跡簇。

為解決智能車輛避障軌跡規(guī)劃時(shí)面臨的采樣區(qū)域劃分問(wèn)題，提出基于凸近似避障原理及采樣區(qū)域優(yōu)化的智能車輛軌跡規(guī)劃方法：首先，引入凸近似避障原理[15-16]，通過(guò)本車與障礙物的位置及體積關(guān)系，得到軌跡可行域范圍；對(duì)采樣區(qū)域進(jìn)行劃分：將采樣區(qū)域分為靜態(tài)采樣區(qū)、動(dòng)態(tài)采樣區(qū)、動(dòng)態(tài)、靜態(tài)障礙物采樣區(qū)。采用“動(dòng)態(tài)規(guī)劃（DP）+二次規(guī)劃（QP）”思想求解軌跡：首先利用多項(xiàng)式軌跡對(duì)采樣點(diǎn)依次連接，建立動(dòng)態(tài)規(guī)劃代價(jià)函數(shù)并篩選得到粗略軌跡；構(gòu)造二次規(guī)劃的等式與不等式約束，對(duì)粗略軌跡進(jìn)行平滑從而得到最優(yōu)軌跡。

1 Frenet坐標(biāo)系及轉(zhuǎn)換關(guān)系

Frenet 坐標(biāo)系以參考線（reference line）為基準(zhǔn)，將車輛的平面運(yùn)動(dòng)解耦為縱向和橫向的獨(dú)立運(yùn)動(dòng)，并用(s,l)描述。其坐標(biāo)系及參數(shù)如圖1和文獻(xiàn)[17]所示。

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

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

2 車輛軌跡規(guī)劃算法模型

2.1 凸近似的局部避障原理

凸近似避障原理實(shí)際是由多個(gè)凸多邊形構(gòu)成的車輛安全行駛區(qū)域。對(duì)自車和障礙車做如下定義：

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

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

式中：

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

在實(shí)際行車過(guò)程中需要考慮障礙車外形尺寸，因此，車輛安全行駛空間應(yīng)較凸空間相對(duì)減小。如圖3所示。

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

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

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

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

另外，可對(duì)障礙物外形所占據(jù)的區(qū)域設(shè)置一個(gè)膨脹系數(shù)q，進(jìn)一步保證其行駛安全性。同理，對(duì)于多障礙物，應(yīng)用此方法同樣可得到可行域。

2.2 問(wèn)題建模

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

式中，z為優(yōu)化目標(biāo)，即軌跡點(diǎn)序列，有p個(gè)不等式約束和q個(gè)等式約束。

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

另外，軌跡規(guī)劃與跟蹤算法應(yīng)考慮合適的車輛運(yùn)動(dòng)學(xué)或動(dòng)力學(xué)模型，使軌跡完全符合車輛實(shí)際運(yùn)行情況，因此可采用文獻(xiàn)[19]所展示的簡(jiǎn)化后的車輛模型。

3 車輛采樣區(qū)設(shè)計(jì)

3.1 采樣區(qū)定義

在Frenet 坐標(biāo)系下對(duì)空間進(jìn)行離散采樣的區(qū)域稱為采樣區(qū)。按照文獻(xiàn)[20]，當(dāng)本車探測(cè)到前方障礙物并相距一定距離后，本車開(kāi)始采樣，其采樣區(qū)始終為距車輛前方一定長(zhǎng)度S的整個(gè)路面區(qū)域。如圖5所示。

圖5 傳統(tǒng)采樣區(qū)Fig.5 Conventional sampling area

由于車輛在行駛過(guò)程中，尤其是在高速情況下，其橫向變化量應(yīng)保持在較小范圍內(nèi)以維持車輛行駛穩(wěn)定性。并且，當(dāng)相距前方障礙物較遠(yuǎn)距離時(shí)，本車不需要過(guò)大的橫向位移去進(jìn)行避障。因此，在圖5所示的傳統(tǒng)采樣區(qū)的基礎(chǔ)上，可將縱向上距本車較近且橫向上距本車較遠(yuǎn)的采樣點(diǎn)舍去，為保證避障的安全性，同時(shí)稍微增加其采樣距離S。而當(dāng)本車逐漸逼近障礙物時(shí)，采樣點(diǎn)數(shù)應(yīng)逐漸增密，從而保證最優(yōu)避障軌跡。

根據(jù)采樣點(diǎn)數(shù)的變化情況，可將采樣區(qū)劃分為動(dòng)態(tài)采樣區(qū)與靜態(tài)采樣區(qū)兩部分。并且在障礙物附近設(shè)置障礙物采樣區(qū)，且始終包含在靜態(tài)采樣區(qū)內(nèi)。

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

圖6 采樣區(qū)定義Fig.6 Sampling area definition

式中，nsd、njd表示動(dòng)態(tài)采樣區(qū)內(nèi)的縱向、橫向采樣點(diǎn)個(gè)數(shù)，nsq、njq表示靜態(tài)采樣區(qū)內(nèi)的縱向、橫向采樣點(diǎn)個(gè)數(shù)。s0、s1、s2表示采樣區(qū)邊界距本車車頭之間的距離。以道路中心線為零位置，dmax、dmin分別表示采樣寬度的兩側(cè)邊界。

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

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

等間隔的離散點(diǎn)表示為：

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

3.2 動(dòng)態(tài)采樣區(qū)與靜態(tài)采樣區(qū)

動(dòng)、靜態(tài)采樣區(qū)逐漸過(guò)渡過(guò)程如圖7所示。

圖7 采樣區(qū)過(guò)渡Fig.7 Sampling area transition

圖7（a）為本車采樣初始階段，由于障礙物并未進(jìn)入采樣區(qū)內(nèi)，即障礙物距離s3大于采樣區(qū)長(zhǎng)度s2，本車并不需要進(jìn)行避障求解，此時(shí)動(dòng)態(tài)采樣區(qū)與動(dòng)態(tài)采樣區(qū)未產(chǎn)生過(guò)渡，故此時(shí)動(dòng)態(tài)采樣區(qū)面積最大。采樣區(qū)長(zhǎng)度s2由采樣點(diǎn)縱向間隔Δs及采樣點(diǎn)總列數(shù)nL決定，根據(jù)3.1節(jié)，有：

式中，s0為本車車頭距第一列采樣點(diǎn)的縱向距離，采樣點(diǎn)總列數(shù)nL=nsd+nsq。

根據(jù)圖7，采樣區(qū)內(nèi)采樣點(diǎn)的最大行數(shù)nH為：

另外，由于動(dòng)態(tài)采樣區(qū)內(nèi)采樣點(diǎn)數(shù)量隨著本車距障礙物的距離的降低而逐漸增加，因此，可設(shè)置動(dòng)態(tài)采樣區(qū)內(nèi)相鄰兩列的采樣點(diǎn)的個(gè)數(shù)相差1，如圖7所示，故動(dòng)態(tài)采樣區(qū)采樣點(diǎn)隨障礙物的接近逐漸增加一行可形成對(duì)靜態(tài)采樣區(qū)的過(guò)渡。

根據(jù)圖7（a），此時(shí)動(dòng)態(tài)采樣區(qū)其右邊界s1為：

應(yīng)將障礙物始終包含在靜態(tài)采樣區(qū)內(nèi)，該區(qū)域內(nèi)采樣點(diǎn)數(shù)足夠多，以保證可求得最優(yōu)解。有：

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

根據(jù)圖7 與式（11），隨著障礙物的逼近，兩采樣區(qū)間進(jìn)行過(guò)渡，采樣點(diǎn)數(shù)量逐漸增多，直至s1=s0時(shí)，采樣點(diǎn)數(shù)量達(dá)到最大。

3.3 障礙物采樣區(qū)

為簡(jiǎn)化運(yùn)算，可在障礙物附近設(shè)置障礙物采樣區(qū)，根據(jù)距離代價(jià)函數(shù)，將區(qū)域內(nèi)滿足代價(jià)大小的采樣點(diǎn)選擇性忽略。

一般而言，道路障礙物分為靜態(tài)、動(dòng)態(tài)障礙物兩種，應(yīng)根據(jù)不同情況分別設(shè)置障礙物采樣區(qū)。

3.3.1 靜態(tài)障礙物采樣區(qū)

在自然坐標(biāo)系下，若障礙物速度vobs滿足：

可將其視為靜態(tài)障礙物。式中vmax為動(dòng)、靜態(tài)障礙物的速度邊界。另外，設(shè)置障礙物距離代價(jià)函數(shù)：

式中，k為距離代價(jià)常數(shù)，d1、d2為距離常數(shù)，xiq,jq為靜態(tài)采樣區(qū)內(nèi)采樣點(diǎn)Γq(siq,ljq)對(duì)障礙物質(zhì)心(s0,l0)的距離，對(duì)采樣點(diǎn)做遍歷：

3.3.2 動(dòng)態(tài)障礙物采樣區(qū)

相對(duì)于靜態(tài)障礙物，動(dòng)態(tài)障礙物與本車的兩次相遇（本車頭部與障礙物尾部、本車尾部與障礙物頭部）之間的行駛范圍皆為危險(xiǎn)區(qū)域，因此可將該區(qū)域作為靜態(tài)障礙物進(jìn)行處理：

首先對(duì)動(dòng)態(tài)障礙物進(jìn)行篩選：

式中，vs為本車車速。當(dāng)滿足上述情況時(shí)，本車與障礙物距離逐漸降低。在該過(guò)程中，動(dòng)態(tài)采樣區(qū)與靜態(tài)采樣區(qū)進(jìn)行正常過(guò)渡，其動(dòng)態(tài)障礙物采樣區(qū)按照3.3.1 節(jié)的方法進(jìn)行計(jì)算，使得障礙物質(zhì)心始終包圍在應(yīng)舍去的采樣點(diǎn)區(qū)域內(nèi)，正如圖7 所示。由于障礙物向前運(yùn)動(dòng)，因此障礙物采樣點(diǎn)隨障礙物質(zhì)心進(jìn)行更新。

當(dāng)本車與障礙物的距離代價(jià)滿足一定條件時(shí)，本車執(zhí)行換道操作。此刻二者關(guān)系如圖8（a）所示。

圖8 超車過(guò)程Fig.8 Overtaking process

圖8（a）中黑色采樣點(diǎn)為按照3.3.1小節(jié)的方法計(jì)算出的障礙物采樣點(diǎn)。另外，在未來(lái)采樣時(shí)刻，由于障礙物向前運(yùn)動(dòng)，障礙物采樣區(qū)內(nèi)采樣點(diǎn)個(gè)數(shù)應(yīng)在此基礎(chǔ)上隨著障礙物運(yùn)動(dòng)方向逐漸增加，直至本車二次換道繞過(guò)障礙物。

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

由于障礙物的運(yùn)動(dòng)，障礙物質(zhì)心坐標(biāo)由上一個(gè)采樣時(shí)刻的(s0,l0)變?yōu)?s1,l1)（正如圖8（a）（b）所示），此時(shí)根據(jù)采樣點(diǎn)與障礙物質(zhì)心坐標(biāo)間的距離代價(jià)，其障礙物采樣區(qū)域在障礙物前進(jìn)方向遞增一列（正如圖8（b）中綠色采樣點(diǎn)），此時(shí)上一時(shí)刻所記錄的障礙物采樣點(diǎn)（圖8（b）中黑色采樣點(diǎn)）代價(jià)以及當(dāng)前時(shí)刻所遞增的采樣點(diǎn)代價(jià)在當(dāng)前時(shí)刻得到更新，對(duì)于某個(gè)重復(fù)的采樣點(diǎn)，其代價(jià)有：

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

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

其每列的采樣點(diǎn)個(gè)數(shù)以障礙物中心為參考，對(duì)稱分布。有：

式中，w為障礙物寬度，n1為每列障礙物采樣點(diǎn)個(gè)數(shù)，應(yīng)取滿足上式的最小整數(shù)。如圖8（c）所示，其值為3。

至此，動(dòng)態(tài)障礙物與本車的兩次相遇過(guò)程之間障礙物行駛區(qū)域，以及障礙物前方長(zhǎng)度為L(zhǎng)的區(qū)域已作靜態(tài)障礙物處理，利用上節(jié)對(duì)靜態(tài)障礙物的處理辦法，將此類采樣點(diǎn)從整體中剔除，為下文實(shí)現(xiàn)對(duì)動(dòng)態(tài)障礙物的避障提供條件。

4 車輛軌跡規(guī)劃

采用“動(dòng)態(tài)規(guī)劃+二次規(guī)劃”的思想。首先根據(jù)Δs與Δl確定采樣點(diǎn)的分布并利用多項(xiàng)式軌跡進(jìn)行連接，通過(guò)構(gòu)建動(dòng)態(tài)規(guī)劃目標(biāo)函數(shù)對(duì)軌跡進(jìn)行篩選，從而得到粗糙換道軌跡。之后利用二次規(guī)劃理論對(duì)粗糙軌跡進(jìn)行平滑，從而得到符合車輛運(yùn)動(dòng)特性的換道軌跡。

4.1 基于五次多項(xiàng)式的車輛運(yùn)動(dòng)軌跡

首先，應(yīng)需要選擇合適的多項(xiàng)式軌跡對(duì)采樣點(diǎn)進(jìn)行連接。在Frenet坐標(biāo)系下，車輛軌跡信息為：

其中，自變量為縱向位移s，因變量為橫向位移l。設(shè)車輛準(zhǔn)備換道時(shí)刻其質(zhì)心為(s0,l0)，根據(jù)坐標(biāo)系轉(zhuǎn)換原理及車輛傳感器，有：

式中，θ為車輛航向角。假設(shè)在換道時(shí)刻，本車與采樣點(diǎn)的關(guān)系如圖9所示。

圖9 本車及其采樣點(diǎn)Fig.9 This vehicle and sampling points

為保證車輛行駛穩(wěn)定性，則本車在相距最近的采樣點(diǎn)處的側(cè)向速度與加速度應(yīng)為零，則位姿信息為：

同理，根據(jù)圖9，在另外采樣點(diǎn)的位姿信息有：

以此類推，可得到每個(gè)采樣點(diǎn)處的位姿信息。因此，若對(duì)相鄰兩列中的兩個(gè)采樣點(diǎn)做平滑軌跡連接時(shí)，其軌跡信息應(yīng)滿足如上6個(gè)條件。因此應(yīng)選擇5次多項(xiàng)式軌跡，其未知項(xiàng)系數(shù)的個(gè)數(shù)為6，其一般形式為：

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

4.2 車輛軌跡的動(dòng)態(tài)規(guī)劃

當(dāng)規(guī)劃出軌跡束之后，應(yīng)從中篩選出一條最優(yōu)軌跡，因此，可通過(guò)設(shè)立合適的動(dòng)態(tài)規(guī)劃目標(biāo)函數(shù)實(shí)現(xiàn)。

（1）軌跡應(yīng)保持平滑，防止車輛跟蹤時(shí)發(fā)生抖動(dòng)，因此其軌跡平滑代價(jià)如下：

（2）本車應(yīng)盡量在車道中心行駛，防止相鄰車道的其他車輛對(duì)本車的影響。因此，設(shè)置參考線距離代價(jià)：

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

因此，兩者代價(jià)之和可作為動(dòng)態(tài)規(guī)劃目標(biāo)函數(shù)：

由于軌跡連接相鄰兩列的采樣點(diǎn)，且采樣點(diǎn)為多列，故軌跡為分段短軌跡，如圖10所示。應(yīng)采用合適方法對(duì)其進(jìn)行連接，組成一條能夠避障且代價(jià)函數(shù)最小的長(zhǎng)軌跡。

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

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

最終，對(duì)軌跡束進(jìn)行遍歷篩選，選取其小值進(jìn)行短軌跡拼接，從而得到最優(yōu)長(zhǎng)軌跡。如圖11所示。

圖11 最優(yōu)長(zhǎng)軌跡Fig.11 Optimal length trajectory

4.3 車輛軌跡的二次規(guī)劃

4.3.1 二次規(guī)劃目標(biāo)函數(shù)構(gòu)造

當(dāng)動(dòng)態(tài)規(guī)劃篩選出粗略軌跡之后，可利用二次規(guī)劃進(jìn)行平滑。

二次規(guī)劃（QP）是應(yīng)用非常廣泛的非線性規(guī)劃問(wèn)題。其標(biāo)準(zhǔn)形式定義為：

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

首先設(shè)置合適的目標(biāo)函數(shù)以求得最優(yōu)軌跡，目標(biāo)函數(shù)由如下三部分組成：

（1）平滑性

（2）靠近道路中心線

與式（25）類似，本車應(yīng)盡量在車道中心行駛，故軌跡應(yīng)靠近道路中心。式中l(wèi)max、lmin分別為道路上下邊界，li為軌跡橫向位移，wmid為權(quán)重。

（3）軌跡終點(diǎn)狀態(tài)約束

至此，二次規(guī)劃目標(biāo)函數(shù)為：

對(duì)該式進(jìn)行標(biāo)準(zhǔn)化轉(zhuǎn)換。設(shè)二次規(guī)劃優(yōu)化變量為：

梯度向量f為：

至此，將目標(biāo)函數(shù)轉(zhuǎn)換為如式（29）所示的標(biāo)準(zhǔn)形式。

4.3.2 二次規(guī)劃約束條件的構(gòu)造

轉(zhuǎn)換成矩陣形式，有：

優(yōu)化變量為式（34）所示，根據(jù)式（38），有：

至此，完成目標(biāo)函數(shù)的等式約束條件。

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

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

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

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

另外，如圖12 所示，當(dāng)車輛接近左前方障礙物時(shí)，p1邊角點(diǎn)會(huì)首先碰撞，此時(shí)p1點(diǎn)與距該點(diǎn)橫向的障礙物邊界距離lmax為零。為保證車輛行駛安全，應(yīng)：

同理，當(dāng)車輛靠近右前方障礙物時(shí)，有：

綜合可得：

優(yōu)化變量為式（34）所示，根據(jù)式（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。至此，完成目標(biāo)函數(shù)的不等式約束條件。

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

圖13 車輛二次規(guī)劃最優(yōu)軌跡Fig.13 Optimal trajectory of vehicle quadratic programming

對(duì)比圖13與圖11，通過(guò)二次規(guī)劃之后，其軌跡較為平滑，符合車輛跟蹤條件。另外，平滑程度與目標(biāo)函數(shù)內(nèi)的權(quán)重系數(shù)有關(guān)，若權(quán)重系數(shù)越高，則軌跡越平滑，但求解速度會(huì)變慢。因此，應(yīng)合理選取其權(quán)重系數(shù)。最后，求解出的最優(yōu)軌跡通過(guò)坐標(biāo)系轉(zhuǎn)換原理轉(zhuǎn)換至全局坐標(biāo)系下供車輛跟蹤，實(shí)現(xiàn)車輛的避障。

5 多場(chǎng)景仿真驗(yàn)證

為驗(yàn)證本方法的有效性，可通過(guò)搭建Prescan-Carsim-Matlab/Simulink聯(lián)合仿真平臺(tái)進(jìn)行驗(yàn)證。其中：Prescan用來(lái)構(gòu)建道路行駛環(huán)境，設(shè)置汽車、行人等障礙物變化；Carsim 用來(lái)設(shè)置車輛本身配置等系統(tǒng)參數(shù)；Matlab/Simulink用來(lái)搭建系統(tǒng)算法。

為更直觀地觀察方法的優(yōu)劣，搭建與文獻(xiàn)[17]參數(shù)相同的方法做對(duì)比，在相同場(chǎng)景下觀察二者避障軌跡規(guī)劃效果。本方法仿真參數(shù)如表1所示。

表1 仿真參數(shù)Table 1 Simulation parameters

本文采用同向筆直雙車道仿真場(chǎng)景，該道路通過(guò)道路邊界及中心線三組離散點(diǎn)擬合而成。設(shè)計(jì)三種場(chǎng)景對(duì)本車進(jìn)行驗(yàn)證：即本車分別對(duì)靜態(tài)障礙物、低速障礙物、動(dòng)態(tài)障礙物這三種場(chǎng)景的避障操作。

5.1 靜態(tài)障礙物仿真驗(yàn)證

智能車通過(guò)該場(chǎng)景時(shí)的軌跡如圖14 所示，其中藍(lán)色線表示自車按本文方法在當(dāng)前時(shí)刻所規(guī)劃的最優(yōu)軌跡，綠色線為文獻(xiàn)[17]的方法所規(guī)劃的最優(yōu)軌跡。

圖14 靜態(tài)障礙物避障過(guò)程Fig.14 Static obstacle avoidance process

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

5.2 低速障礙物仿真驗(yàn)證

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

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

黑色、紅色虛線矩形框表示分別表示本車與障礙車開(kāi)始避障時(shí)刻的位置，當(dāng)換道結(jié)束時(shí)刻，障礙物位置由紅色矩形框所示，本車位置由最右方黑色矩形框所示。

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

5.3 動(dòng)態(tài)障礙物仿真驗(yàn)證

（1）障礙物做勻速運(yùn)動(dòng)

設(shè)定障礙車勻速運(yùn)動(dòng)的車速為6 m/s，由于其速度高，故本方法應(yīng)按照動(dòng)態(tài)障礙物處理，另外，參考方法應(yīng)選擇合適的碰撞損失權(quán)重對(duì)其進(jìn)行避障。該過(guò)程如圖16所示。

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

（2）障礙物變速運(yùn)動(dòng)運(yùn)動(dòng)

設(shè)定障礙物前10 s 做速度為6 m/s 的勻速運(yùn)動(dòng)，之后保持靜止。其避障過(guò)程如圖17（a）。

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

設(shè)定障礙物前10 s 做速度為6 m/s 的勻速運(yùn)動(dòng)，之后以1 m/s2的加速度做勻減速運(yùn)動(dòng)。其避障過(guò)程如圖17（b）。

設(shè)定障礙物前10 s 做速度為5 m/s 的勻速運(yùn)動(dòng)，之后以1 m/s2的加速度做勻加速運(yùn)動(dòng)。其避障過(guò)程如圖17（c）。

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

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

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

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

綜上，對(duì)于靜態(tài)障礙物而言，二者效果相差不大。對(duì)于動(dòng)態(tài)障礙物的避障，本文方法優(yōu)于參考方法。

6 結(jié)論

針對(duì)結(jié)構(gòu)化道路下做勻速運(yùn)動(dòng)的智能車輛避障軌跡規(guī)劃問(wèn)題，以及基于采樣的軌跡規(guī)劃方法未將采樣環(huán)境做區(qū)域劃分問(wèn)題，提出基于凸近似避障原理及采樣區(qū)域優(yōu)化的智能車輛軌跡規(guī)劃方法，并與文獻(xiàn)[17]方法進(jìn)行對(duì)比。仿真結(jié)果表明，該方法能夠有效地得到平滑軌跡并避開(kāi)障礙物。