考慮交互軌跡預(yù)測(cè)的自動(dòng)駕駛運(yùn)動(dòng)規(guī)劃算法

劉啟冉， 連 靜，2， 陳 實(shí)， 范 蓉

(1.大連理工大學(xué) 汽車(chē)工程學(xué)院， 遼寧 大連 116024； 2.大連理工大學(xué) 工業(yè)裝備結(jié)構(gòu)分析國(guó)家重點(diǎn)實(shí)驗(yàn)室， 遼寧 大連 116024)

隨著人工智能的發(fā)展，自動(dòng)駕駛技術(shù)在提升交通安全和效率等方面發(fā)揮重要作用.在結(jié)構(gòu)化道路場(chǎng)景中，考慮周?chē)?chē)輛之間的交互軌跡預(yù)測(cè)是自動(dòng)駕駛汽車(chē)運(yùn)動(dòng)規(guī)劃的一大挑戰(zhàn)[1-2].Song等[3]提出使用人工勢(shì)場(chǎng)法預(yù)測(cè)周?chē)?chē)輛運(yùn)動(dòng)的軌跡監(jiān)管器，并進(jìn)行碰撞風(fēng)險(xiǎn)檢查，根據(jù)速度分布進(jìn)行軌跡規(guī)劃，該方法考慮道路的幾何形狀提高舒適性，但實(shí)時(shí)性差.Yang等[4]利用無(wú)跡卡爾曼濾波實(shí)時(shí)預(yù)測(cè)周?chē)?chē)輛軌跡的不確定性，并傳入到模型預(yù)測(cè)控制器中實(shí)現(xiàn)避障規(guī)劃，但該方法沒(méi)有考慮周?chē)?chē)輛之間的交互.Lin等[5]提出一種具有駕駛員行駛風(fēng)格、信息交互、動(dòng)態(tài)交通參與者軌跡預(yù)測(cè)功能的局部軌跡規(guī)劃器，仿真結(jié)果表明，當(dāng)存在碰撞風(fēng)險(xiǎn)時(shí)該規(guī)劃器可以重新規(guī)劃運(yùn)動(dòng)軌跡.Lee等[6]提出計(jì)算周?chē)?chē)輛潛在風(fēng)險(xiǎn)的預(yù)測(cè)占用圖(predictive occupancy map， POM)，然后通過(guò)POM規(guī)劃出風(fēng)險(xiǎn)最低的安全軌跡.但上述方法只能應(yīng)用在車(chē)輛間交互較少的簡(jiǎn)單場(chǎng)景中.

為提升自動(dòng)駕駛汽車(chē)運(yùn)動(dòng)規(guī)劃在復(fù)雜場(chǎng)景中的應(yīng)用性能，基于學(xué)習(xí)的軌跡預(yù)測(cè)方法快速發(fā)展.Jeong等[7]利用長(zhǎng)短時(shí)記憶-遞歸神經(jīng)網(wǎng)絡(luò)對(duì)周?chē)?chē)輛的運(yùn)動(dòng)進(jìn)行預(yù)測(cè)，但該方法適用范圍有限.Li等[8]設(shè)計(jì)一種分層逆強(qiáng)化學(xué)習(xí)的軌跡預(yù)測(cè)框架，同時(shí)考慮周?chē)煌▍⑴c者之間的交互動(dòng)作，但計(jì)算復(fù)雜.Messaoud等[9]提出一個(gè)基于注意機(jī)制的車(chē)輛多模態(tài)軌跡預(yù)測(cè)模型.但當(dāng)前基于數(shù)據(jù)驅(qū)動(dòng)的軌跡預(yù)測(cè)模型存在可解釋性差、復(fù)雜度高等問(wèn)題.

針對(duì)上述方法存在的問(wèn)題，許多研究者對(duì)Helbing等[10]提出的社會(huì)力模型(social force model，SFM)進(jìn)行改進(jìn)，將其應(yīng)用到軌跡預(yù)測(cè)上.楊文彥等[11]利用社會(huì)力模型對(duì)人車(chē)混合路口中的行人軌跡進(jìn)行預(yù)測(cè)，但沒(méi)有預(yù)測(cè)周?chē)?chē)輛之間的交互軌跡.Yoon等[12]基于駕駛員行為的社會(huì)特性，構(gòu)建改進(jìn)社會(huì)力模型進(jìn)行車(chē)輛軌跡預(yù)測(cè)，但該方法對(duì)模型參數(shù)過(guò)于敏感.

基于以上分析，本文提出一種基于交互軌跡預(yù)測(cè)的自動(dòng)駕駛運(yùn)動(dòng)規(guī)劃算法.首先構(gòu)建改進(jìn)社會(huì)力模型，實(shí)現(xiàn)多車(chē)交互場(chǎng)景下的車(chē)輛軌跡預(yù)測(cè)，并基于Frenet坐標(biāo)系采樣得到橫向軌跡序列和縱向軌跡序列，通過(guò)時(shí)間離散化和坐標(biāo)系轉(zhuǎn)換合成運(yùn)動(dòng)規(guī)劃的軌跡集合.然后進(jìn)行軌跡檢查，利用時(shí)空占用圖生成軌跡集合與預(yù)測(cè)軌跡投影點(diǎn)之間的最短距離進(jìn)行碰撞檢查，為提高算法的實(shí)時(shí)性，檢查軌跡的加速度和曲率.最后設(shè)計(jì)代價(jià)函數(shù)，評(píng)估候選軌跡集合中的最優(yōu)運(yùn)動(dòng)軌跡.仿真結(jié)果表明，基于本文方法進(jìn)行的運(yùn)動(dòng)規(guī)劃，在提高規(guī)劃軌跡安全性的同時(shí)提高規(guī)劃的舒適性和行駛效率.

1 交互軌跡預(yù)測(cè)

1.1 改進(jìn)社會(huì)力模型

圖1 社會(huì)力分析示意圖

(1)

1.2 目標(biāo)驅(qū)動(dòng)力

目標(biāo)驅(qū)動(dòng)力Fd為驅(qū)動(dòng)車(chē)輛以最優(yōu)的速度盡快向目的地運(yùn)動(dòng)的主要?jiǎng)恿?，?dāng)受到周?chē)煌▍⑴c者和環(huán)境的干擾時(shí)，目標(biāo)驅(qū)動(dòng)力作用于車(chē)輛向期望速度運(yùn)動(dòng).目標(biāo)驅(qū)動(dòng)力的表達(dá)式為

(2)

式中：ve為期望速度，方向?yàn)橛^測(cè)時(shí)間步長(zhǎng)內(nèi)車(chē)輛起點(diǎn)指向終點(diǎn)的方向；vc為實(shí)際速度，由式(1)計(jì)算得到；τd為反應(yīng)時(shí)間，表示車(chē)輛速度變化的快慢.

1.3 車(chē)輛間相互作用力

為避免發(fā)生碰撞，車(chē)輛行駛時(shí)會(huì)與當(dāng)前所在車(chē)道和相鄰車(chē)道上一定范圍內(nèi)車(chē)輛保持一定的距離.距離越近，作用在主車(chē)上的排斥力越大.車(chē)輛ci受到周?chē)?chē)輛的作用力為

(3)

式中：Av為車(chē)輛間相互作用力的強(qiáng)度；rv為考慮車(chē)輛幾何形狀特性的車(chē)輛間距離；Bv為車(chē)輛間相互作用力的范圍；nij為車(chē)輛cj指向車(chē)輛ci中心的單位向量.

1.4 邊界約束力

在交通法規(guī)和道路環(huán)境的約束下，行駛在道路上的車(chē)輛會(huì)受到車(chē)道線及道路邊界的作用，使主車(chē)保持在當(dāng)前所在車(chē)道的中心線位置.邊界約束力的表達(dá)式為

(4)

式中：Ab為邊界約束力的作用強(qiáng)度；db為車(chē)輛與邊界的距離；Bb為邊界約束力的作用范圍；nb為垂直邊界指向車(chē)輛中心的單位向量.

2 運(yùn)動(dòng)規(guī)劃算法

2.1 Frenet坐標(biāo)系

由于車(chē)輛動(dòng)力學(xué)和輸入到控制模塊的軌跡是基于笛卡爾坐標(biāo)系定義的，因此規(guī)劃后得到的軌跡需進(jìn)行坐標(biāo)系轉(zhuǎn)換，如圖2所示.在全局笛卡爾坐標(biāo)系下，車(chē)輛的運(yùn)動(dòng)狀態(tài)可以描述為[x，θt，kt，vt，at]，其中：x為車(chē)輛位置，x=(xt，yt)；θt為方位角；kt為曲率；vt為速度；at為加速度.τr，nr分別為參考線上某一點(diǎn)的單位切向量和單位法向量；τt，nt分別為實(shí)際行駛軌跡上某一點(diǎn)的單位切向量和單位法向量；r(s)為笛卡爾坐標(biāo)系下車(chē)輛當(dāng)前位置在參考線上的投影點(diǎn)的位置，r(s)=(xr，yr).

圖2 Frenet坐標(biāo)系與全局笛卡爾坐標(biāo)系的轉(zhuǎn)換示意圖

(5)

基于文獻(xiàn)[16]的公式推導(dǎo)，F(xiàn)renet坐標(biāo)系與全局笛卡爾坐標(biāo)系的轉(zhuǎn)換可由式(6)確定.

(6)

2.2 軌跡集合求解

關(guān)于時(shí)間t的三維運(yùn)動(dòng)規(guī)劃問(wèn)題在Frenet坐標(biāo)系下可以分解為橫向運(yùn)動(dòng)規(guī)劃d(t)和縱向運(yùn)動(dòng)規(guī)劃s(t).本文選用式(7)所示的五次多項(xiàng)式來(lái)規(guī)劃運(yùn)動(dòng)軌跡p(t).

p(t)=α0+α1t+α2t2+α3t3+α4t4+α5t5

.

(7)

(8)

(9)

令t0=0簡(jiǎn)化計(jì)算，可得

(10)

設(shè)t1=T，則系數(shù)α3，α4，α5可由式(11)求解：

(11)

2.3 軌跡檢查

為進(jìn)行碰撞檢查，基于文獻(xiàn)[17]提出的時(shí)空占用圖如圖3所示，記碰撞點(diǎn)為P(x，y，t).本文將上述得到的周?chē)?chē)輛預(yù)測(cè)軌跡和主車(chē)的軌跡集合投影到時(shí)空占用圖中，并分別計(jì)算軌跡集合中每條軌跡的投影點(diǎn)與預(yù)測(cè)軌跡投影點(diǎn)之間的距離，當(dāng)計(jì)算得到軌跡的最短距離dmin小于安全閾值時(shí)，即0

圖3 時(shí)空占用圖

由于車(chē)輛本身運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)及行駛工況的限制，同時(shí)為提高算法的實(shí)時(shí)性，去除沒(méi)有通過(guò)最大加速度和最大曲率約束條件的軌跡，得到候選軌跡集合.通過(guò)加速度檢查和曲率檢查[18]的條件為

(12)

式中：api為軌跡i離散化后相應(yīng)軌跡點(diǎn)的加速度；kpi為軌跡i離散化后相應(yīng)軌跡點(diǎn)的曲率；N為軌跡離散化后軌跡點(diǎn)的最大數(shù)量；amax和kmax分別為最大加速度和最大曲率.

2.4 代價(jià)函數(shù)設(shè)計(jì)

設(shè)計(jì)評(píng)估候選軌跡集合的代價(jià)函數(shù)[19]，求解候選軌跡中代價(jià)函數(shù)值最小的即為最優(yōu)運(yùn)動(dòng)軌跡.

Ctotal=wtCt+wpCp+wsCs+wjCj.

(13)

(14)

式中：Cs表示從安全性考慮對(duì)碰撞的代價(jià)；l=(xi-xob)2+(yi-yob)2為障礙物到軌跡的距離，(xob，yob)為障礙物的位置；由于車(chē)輛幾何尺寸的影響，設(shè)ln為安全距離閾值，lc為碰撞距離閾值，當(dāng)lc≤l≤ln時(shí)，距離障礙物越近則發(fā)生碰撞的可能性越大；ρ為障礙物危險(xiǎn)程度距離影響因子，其與車(chē)輛的車(chē)速v成正比.

(15)

3 仿真與結(jié)果分析

車(chē)輛行駛行為主要分為縱向行為和橫向行為，所以本文分別設(shè)計(jì)減速行駛場(chǎng)景和換道行駛場(chǎng)景來(lái)驗(yàn)證所提算法的有效性.利用Matlab中的遺傳算法(genetic algorithm,GA)函數(shù)[20]對(duì)改進(jìn)社會(huì)力模型的參數(shù)Av，Bv，Ab，Bb進(jìn)行標(biāo)定，具體標(biāo)定結(jié)果如表1所示.針對(duì)關(guān)鍵采樣參數(shù)，經(jīng)過(guò)大量的仿真實(shí)驗(yàn)總結(jié)，本文具體取值如表2所示.

表1 模型參數(shù)標(biāo)定結(jié)果

表2 仿真參數(shù)

3.1 減速行駛場(chǎng)景

圖4所示的減速行駛場(chǎng)景中，主車(chē)c3初始車(chē)速為5.5 m/s，周?chē)?chē)c1速度為8 m/s，周?chē)?chē)c2速度為2 m/s，從c1與c2的交互預(yù)測(cè)軌跡可以看出，c2從岔口駛出右轉(zhuǎn)時(shí)距離較高車(chē)速的c1較近，所以c1在行駛過(guò)程中為避免與c2發(fā)生碰撞突然向左駛?cè)胫鬈?chē)c3當(dāng)前所在車(chē)道.

圖4 減速場(chǎng)景

若主車(chē)在運(yùn)動(dòng)規(guī)劃中不考慮周?chē)?chē)輛之間的交互預(yù)測(cè)軌跡，根據(jù)圖5所示的減速行駛場(chǎng)景下時(shí)空占用圖，主車(chē)c3在7.5 s時(shí)與c1存在碰撞風(fēng)險(xiǎn).

圖5 減速場(chǎng)景下時(shí)空占用圖

如圖6a所示，沒(méi)有考慮多車(chē)交互的運(yùn)動(dòng)規(guī)劃對(duì)c1換道動(dòng)作反應(yīng)不及時(shí)，主車(chē)在0～10 s的時(shí)間內(nèi)仍處于較高行駛速度，在10 s時(shí)才對(duì)已經(jīng)駛?cè)胫鬈?chē)c3當(dāng)前所在車(chē)道的c1做出相應(yīng)減速，導(dǎo)致速度曲線不平穩(wěn)，降低行駛效率同時(shí)增加碰撞風(fēng)險(xiǎn).在圖6b，6c中，加速度和加速度變化率波動(dòng)較大，降低乘車(chē)舒適性.相對(duì)于沒(méi)有考慮軌跡預(yù)測(cè)的運(yùn)動(dòng)規(guī)劃，本文方法規(guī)劃出的速度跟隨曲線更加平穩(wěn)，加速度、加速度變化率波動(dòng)更小，說(shuō)明駕駛舒適性更高.因此，本文方法可以規(guī)劃出一條最優(yōu)運(yùn)動(dòng)軌跡.

圖6 減速場(chǎng)景下結(jié)果對(duì)比

在圖7中，基于本文運(yùn)動(dòng)規(guī)劃方法主車(chē)c3提前預(yù)測(cè)到周?chē)?chē)輛的運(yùn)動(dòng)軌跡，在縱向運(yùn)動(dòng)軌跡候選集合中，為避免與c1產(chǎn)生碰撞，c3的最優(yōu)縱向運(yùn)動(dòng)在4 s時(shí)提前減速.同時(shí)c3從初始位置向車(chē)道中心線規(guī)劃并沿車(chē)道中心線向前行駛，為保證行駛效率，在減速確保安全后進(jìn)行加速.

圖7 減速場(chǎng)景下運(yùn)動(dòng)軌跡候選集合

3.2 換道行駛場(chǎng)景

圖8所示換道行駛場(chǎng)景中，主車(chē)c3初始車(chē)速為5.5 m/s，周?chē)?chē)c1速度為4 m/s，周?chē)?chē)c2速度為8 m/s.根據(jù)預(yù)測(cè)c1與c2的運(yùn)動(dòng)軌跡，c2突然駛?cè)隿1當(dāng)前所在車(chē)道，c1減速行駛，因而c3提前向左換道.

圖8 換道場(chǎng)景

從圖9所示的時(shí)空占用圖中可以看出，若主車(chē)c3不對(duì)周?chē)?chē)輛之間交互的預(yù)測(cè)軌跡進(jìn)行考慮，則不會(huì)提前采取換道決策，在11 s時(shí)存在與c1發(fā)生碰撞的風(fēng)險(xiǎn).

圖9 換道場(chǎng)景下時(shí)空占用圖

如圖10所示，沒(méi)有考慮預(yù)測(cè)軌跡間交互的運(yùn)動(dòng)規(guī)劃方法所規(guī)劃的速度曲線滯后2 s進(jìn)行換道，降低行車(chē)效率，同時(shí)速度變化較大，降低行駛舒適性.本文方法提前決策駕駛行為，規(guī)劃出更加平穩(wěn)的速度變化曲線，加速度和加速度變化率波動(dòng)程度也較小，提升了行駛舒適性和行駛效率.

圖10 換道場(chǎng)景下結(jié)果對(duì)比

基于本文所提方法提前對(duì)周?chē)?chē)輛的交互運(yùn)動(dòng)軌跡進(jìn)行預(yù)測(cè)，在圖11中，主車(chē)c3考慮c2突然換道和c1減速的預(yù)測(cè)軌跡，為避免與c1發(fā)生碰撞，c3在4 s時(shí)向左加速換道，保證行駛安全性.綜上，本文提出的運(yùn)動(dòng)規(guī)劃算法適用多種交互場(chǎng)景，可以規(guī)劃出安全性、舒適性和行駛效率更高的運(yùn)動(dòng)軌跡，為自動(dòng)駕駛汽車(chē)降低碰撞風(fēng)險(xiǎn)提供依據(jù).

圖11 換道場(chǎng)景下運(yùn)動(dòng)軌跡候選集合

4 結(jié) 語(yǔ)

本文針對(duì)自動(dòng)駕駛汽車(chē)運(yùn)動(dòng)規(guī)劃存在的交互問(wèn)題，構(gòu)建改進(jìn)社會(huì)力模型，提出考慮周?chē)?chē)輛間交互軌跡預(yù)測(cè)的運(yùn)動(dòng)規(guī)劃算法.首先利用改進(jìn)社會(huì)力模型對(duì)周?chē)?chē)輛的行駛軌跡進(jìn)行預(yù)測(cè).然后基于Frenet坐標(biāo)系采樣得到自動(dòng)駕駛汽車(chē)的軌跡集合，將軌跡集合與預(yù)測(cè)軌跡投影到時(shí)空占用圖上，計(jì)算投影點(diǎn)之間的最短距離，并結(jié)合加速度和曲率檢查對(duì)軌跡進(jìn)行篩選，得到候選軌跡集合.最后求解候選軌跡集合中代價(jià)函數(shù)最小的軌跡即為最優(yōu)運(yùn)動(dòng)軌跡.仿真結(jié)果表明，本文提出的算法有效地提高了運(yùn)動(dòng)軌跡的安全性、舒適性和行駛效率.在未來(lái)的研究工作中，考慮感知盲區(qū)的不確定性對(duì)軌跡預(yù)測(cè)的概率影響和提高運(yùn)動(dòng)規(guī)劃算法對(duì)復(fù)雜工況的自適應(yīng)性是今后的重要研究方向.