基于行為識(shí)別和曲率約束的車輛軌跡預(yù)測(cè)方法研究*

謝 楓，婁靜濤，趙 凱，齊 堯

（1.陸軍軍事交通學(xué)院，天津 300161； 2.軍事交通運(yùn)輸研究所，天津 300161）

前言

對(duì)駕駛?cè)藳Q策特性的研究表明，諸如自車與周圍運(yùn)動(dòng)車輛的相對(duì)速度、相對(duì)距離等因素會(huì)極大地影響駕駛員的決策［1］，進(jìn)而影響行車安全。對(duì)于駕駛環(huán)境中的靜態(tài)車輛，智能車可安全地沿規(guī)劃的軌跡行駛，對(duì)于動(dòng)態(tài)車輛，人類駕駛員可以利用過去的經(jīng)驗(yàn)和直覺預(yù)測(cè)其他駕駛員的行為來避免潛在的事故［2］，而智能車需要通過實(shí)時(shí)預(yù)測(cè)其周圍運(yùn)動(dòng)車輛的軌跡，提高其行駛安全性。

目前在車輛軌跡預(yù)測(cè)方面，主要的方法有基于物理運(yùn)動(dòng)模型的軌跡預(yù)測(cè)、基于駕駛行為的軌跡預(yù)測(cè)和基于意圖識(shí)別的軌跡預(yù)測(cè)等方法［3］。彭曲等［4］根據(jù)城市交通網(wǎng)絡(luò)，依靠統(tǒng)計(jì)并利用歷史軌跡，提出了基于馬爾可夫鏈的軌跡預(yù)測(cè)，但預(yù)測(cè)準(zhǔn)確率較低，僅在40%左右。Gambs等［5］提出了高階馬爾可夫模型的軌跡預(yù)測(cè)方法，精度較高，但是計(jì)算開銷大，目前很難滿足智能車的實(shí)時(shí)性要求；Wiest等［6］提出了VGMM用于車輛長(zhǎng)期運(yùn)動(dòng)預(yù)測(cè)，但該方法僅在模擬環(huán)境中進(jìn)行了測(cè)試，而且需要輸入很長(zhǎng)一段時(shí)間的歷史軌跡信息。喬少杰等［7］運(yùn)用卡爾曼濾波設(shè)計(jì)了動(dòng)態(tài)軌跡預(yù)測(cè)算法，但預(yù)測(cè)軌跡的平均誤差為12.5 m，誤差較大。張金旺等［8］分別采用統(tǒng)計(jì)距離和馬爾科夫鏈對(duì)車輛橫向運(yùn)動(dòng)（垂直于車前進(jìn)方向）和縱向運(yùn)動(dòng)（車前進(jìn)方向）進(jìn)行軌跡預(yù)測(cè)，但其算法在3 s內(nèi)平均預(yù)測(cè)誤差為2.5 m，平均耗時(shí)為93 ms，誤差與耗時(shí)都較高。Schubert［9］等對(duì)目標(biāo)跟蹤的不同運(yùn)動(dòng)模型做了比較，CTRA模型在城市環(huán)境和高速公路環(huán)境下都給出了較好的結(jié)果。Berthelot［10］和Tmake［11］等利用CTRA模型來預(yù)測(cè)車輛軌跡。

為對(duì)運(yùn)動(dòng)車輛軌跡做出更準(zhǔn)確地預(yù)測(cè)，本文中提出了一種基于行為識(shí)別和曲率約束的車輛軌跡預(yù)測(cè)方法，該方法將車輛運(yùn)動(dòng)分解為沿車道線方向（縱向）的運(yùn)動(dòng)與垂直于車道線方向（橫向）的運(yùn)動(dòng)。首先接收感知得到的環(huán)境車輛信息，包括車輛的位置、速度、加速度、航向角和橫擺角速度，結(jié)合高精度地圖提供的車道線信息和車輛的速度、橫擺角速度信息，對(duì)車輛進(jìn)行行為識(shí)別；然后依據(jù)行為識(shí)別結(jié)果、車輛的位置、速度和加速度，確定車輛在橫、縱向上的初始狀態(tài)和結(jié)束狀態(tài)，再根據(jù)運(yùn)動(dòng)狀態(tài)分別建立橫、縱向多項(xiàng)式方程預(yù)測(cè)其在橫、縱向的運(yùn)動(dòng)；最后，以高精度地圖中的車道線曲率作為約束，篩選出一條最優(yōu)的預(yù)測(cè)軌跡。算法處理過程平均耗時(shí)為0.103 ms，滿足智能車實(shí)時(shí)性的要求。若智能車周圍不存在運(yùn)動(dòng)車輛則不進(jìn)行預(yù)測(cè)。方法的流程框圖如圖1所示。

圖1 本文中方法流程框圖

1 車輛行為識(shí)別

通常車輛在車道上行駛時(shí)，有3種基礎(chǔ)的行為：車道保持、換道和轉(zhuǎn)彎。其它行為可由這3種行為中的幾種組合得到。對(duì)于保持當(dāng)前車道行駛的車輛，駕駛員一般是沿著車道中線居中行駛或是在當(dāng)前車道“平行”車道線行駛；對(duì)于換道或是轉(zhuǎn)彎的車輛，在一般情況下，駕駛員會(huì)尋求最小的當(dāng)前機(jī)動(dòng)持續(xù)時(shí)間，同時(shí)考慮到車輛乘坐人員的舒適性，車輛的橫向加速度不會(huì)太大，而且實(shí)際軌跡也會(huì)比較平滑。

行為識(shí)別是利用感知得到的障礙物信息和高精度地圖信息實(shí)現(xiàn)的，其主要步驟如下。

（1）接收并存儲(chǔ)感知傳入的當(dāng)前時(shí)刻車輛的狀態(tài)向量 X=［x，y，θ，v，a］T，其中，x，y是由感知得到的目標(biāo)車輛在大地坐標(biāo)系下的位置（此處假設(shè)車輛做平面運(yùn)動(dòng)，不考慮其在z軸方向的運(yùn)動(dòng)），θ為車輛的航向角，v，a則分別是車輛當(dāng)前時(shí)刻的速度和加速度。

（2）結(jié)合高精度地圖，沿車道線方向?yàn)榭v向（即s坐標(biāo)軸），垂直于車道線的方向?yàn)闄M向（即l坐標(biāo)軸）建立如圖2所示的s-l坐標(biāo)系。每條車道線的起點(diǎn)處s=0，在車道中線上l=0。

（3）結(jié)合高精度地圖信息以及車輛當(dāng)前狀態(tài)和歷史狀態(tài)信息，識(shí)別車輛行為。主要有3種方法：

a.根據(jù)車輛當(dāng)前狀態(tài)信息，計(jì)算其在橫向的速度分量vl，根據(jù)vl的大小和方向判定車輛將要做出的行為；

圖2 s-l坐標(biāo)系

b.結(jié)合高精度地圖信息，在接近路口處判斷車輛所處車道的車道類型，以此判定車輛將要做出的行為；

c.利用車輛當(dāng)前狀態(tài)信息和歷史狀態(tài)信息，計(jì)算車輛的橫擺角速度ω，根據(jù)橫擺角速度大小識(shí)別車輛行為。

2 車道線曲率約束

高精度地圖作為智能車發(fā)展的重要支撐，在精確定位、基于車輛模型的駕駛決策輔助、障礙物檢測(cè)和避讓等方面發(fā)揮著重要作用，是自動(dòng)駕駛的核心技術(shù)之一［12］。高精度地圖不僅有準(zhǔn)確的坐標(biāo)，還能準(zhǔn)確地描繪道路形狀、車道線、車道中心線等信息［13］。

高精度地圖中車道（lane）是由車道線組成，車道線是由車道線段（lane segment）組成，車道線段則是由車道點(diǎn)（lane point）組成。首先以車輛當(dāng)前時(shí)刻的位置為起點(diǎn)，根據(jù)車輛的速度選擇車輛當(dāng)前所在車道前方一段距離，然后沿車道線方向（即縱向），在給定距離內(nèi)從起點(diǎn)開始每隔一定距離取一個(gè)車道點(diǎn)，得到車道點(diǎn)集合｛LanePoint（n），n=0，1，2，…｝。根據(jù)曲率計(jì)算公式，求出每?jī)蓚€(gè)車道點(diǎn)之間的曲率。不同的曲率意味著不同的車速，曲率越大速度越低。再根據(jù)曲率對(duì)速度做插值運(yùn)算，得到結(jié)束狀態(tài)時(shí)車輛的速度。曲率由以下公式得到：

式中：Δα為兩個(gè)車道點(diǎn)所屬車道線段的角度差值；Δs為兩個(gè)車道點(diǎn)之間縱向位移之差（由于選取的兩點(diǎn)之間距離較近，近似認(rèn)為是弧長(zhǎng)）。

本文中方法基于高精度地圖建立，所使用的高精度地圖為實(shí)驗(yàn)室自主研發(fā)，精度在厘米級(jí)，其主要制作過程有以下幾個(gè)步驟：

（1）使用三維激光雷達(dá)采集點(diǎn)云數(shù)據(jù)，并對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行預(yù)處理；

（2）點(diǎn)云配準(zhǔn)和后處理；

（3）生成反射率地圖；

（4）使用JOSM軟件繪制高精度地圖。

由于所采用的高精度地圖中的車道線為人工描繪，并不夠平滑，所以本文中所用曲率與車速極值的對(duì)應(yīng)關(guān)系與實(shí)際相比有所不同，其對(duì)應(yīng)關(guān)系如表1所示。

表1 曲率與車速極值對(duì)應(yīng)表

3 車輛軌跡預(yù)測(cè)方法

3.1 初始狀態(tài)

由于感知得到的障礙物信息是基于笛卡爾坐標(biāo)系的，而行為識(shí)別中的多項(xiàng)式方程是以s-l坐標(biāo)系為基礎(chǔ)建立的。所以需要進(jìn)行坐標(biāo)系的轉(zhuǎn)換，轉(zhuǎn)換公式如下：

式中：l0為車輛初始狀態(tài)時(shí)相對(duì)于其所在車道的橫向位移；s0為車輛初始狀態(tài)時(shí)相對(duì)于其所在車道的縱向位移；θsg為車輛位置點(diǎn)所屬車道線段的方向；p為由車輛位置點(diǎn)所屬當(dāng)前車道線段的起點(diǎn)指向車輛位置點(diǎn)的向量；d為車輛位置點(diǎn)所屬車道的有向線段；ssum為車輛位置點(diǎn)所屬車道線段相對(duì)于當(dāng)前車道起點(diǎn)的縱向位移。

3.2 結(jié)束狀態(tài)

假設(shè)車輛在保持或改變行為的過程中，其加速度恒定，根據(jù)行為識(shí)別結(jié)果，可以得到結(jié)束狀態(tài)時(shí)車輛的狀態(tài)：

對(duì)于到達(dá)結(jié)束狀態(tài)時(shí)，預(yù)測(cè)車輛在車道中心線上運(yùn)動(dòng)的行為，可令l′=0；而對(duì)于車輛會(huì)在預(yù)測(cè)車道上“平行”車道線行駛的行為，可以結(jié)合車輛的初始運(yùn)動(dòng)狀態(tài)，改變l′的值即可。

對(duì)于行為識(shí)別結(jié)果為保持車道或換道的行為，認(rèn)為車輛在縱向做勻變速運(yùn)動(dòng)，即t，行為識(shí)別結(jié)果為轉(zhuǎn)彎行為時(shí)，認(rèn)為車輛在縱向做勻速運(yùn)動(dòng)，即

3.3 橫向運(yùn)動(dòng)描述

由于車輛橫向運(yùn)動(dòng)的初始狀態(tài)與結(jié)束狀態(tài)都可根據(jù)行為識(shí)別結(jié)果確定，所以可建立一個(gè)與時(shí)間有關(guān)的5次多項(xiàng)式來描述車輛的橫向運(yùn)動(dòng)：

給定起始時(shí)間t0=0。由于多項(xiàng)式有4階導(dǎo)，保證了車輛在橫向的加速度是連續(xù)的。多項(xiàng)式的系數(shù)可由以下等式求得：

式中：t0為起始時(shí)間；t1為結(jié)束時(shí)間。

3.4 縱向運(yùn)動(dòng)描述

由于車輛縱向運(yùn)動(dòng)中結(jié)束狀態(tài)的s1無法根據(jù)行為識(shí)別結(jié)果確定，所以可建立一個(gè)與時(shí)間有關(guān)的4次多項(xiàng)式來描述車輛的縱向運(yùn)動(dòng)：

由于多項(xiàng)式有4階導(dǎo)，保證了車輛在縱向的加速度是連續(xù)的。多項(xiàng)式的系數(shù)為

所有被調(diào)查者中，41.7%的學(xué)生不清楚本、專科護(hù)理的差別。在臨床中，本?？茖W(xué)歷的護(hù)士工資相差不大，工作分工也無太大差別。不少學(xué)生認(rèn)為，專科生學(xué)年制為三年制，而本科生則為四年或者五年，在學(xué)年制比?？粕嗔艘坏絻赡甑臅r(shí)間，然而轉(zhuǎn)到臨床之后，本、?？粕旧咸幱谕鹊匚?，至少在學(xué)歷和薪酬方面沒有體現(xiàn)出足夠的優(yōu)勢(shì)。專業(yè)基礎(chǔ)知識(shí)也只能說比?？粕钊肴?，卻缺少了更具本科學(xué)歷特色的內(nèi)容。

3.5 軌跡選擇

通常，在結(jié)構(gòu)化道路下，當(dāng)車輛需要改變行為時(shí)，會(huì)考慮到車內(nèi)乘坐人員的舒適性和安全性，橫向速度不會(huì)太大。同時(shí)，根據(jù)駕駛習(xí)慣，駕駛員會(huì)尋求最小的當(dāng)前機(jī)動(dòng)持續(xù)時(shí)間。由于車輛橫、縱向軌跡預(yù)測(cè)方程是關(guān)于時(shí)間t的函數(shù)，即定義車輛變換行為所需時(shí)間不同，所得到的軌跡也不同，可以認(rèn)為最優(yōu)軌跡的選擇等價(jià)于最優(yōu)時(shí)間的選擇?；诖耍⒔Y(jié)合下文實(shí)驗(yàn)部分，正常情況下車輛變換行為在時(shí)間t=4～8 s之間較好。當(dāng)然，對(duì)于不同的實(shí)驗(yàn)環(huán)境或條件，t會(huì)有所不同，可根據(jù)實(shí)際情況選擇t的取值。認(rèn)為車輛變換行為過程中加速度恒定，可得t的求解公式為

式中Δs為曲率最大點(diǎn)位置相對(duì)于起始車道點(diǎn)位置的縱向位移。

由上述描述可知，輸入不同的到達(dá)結(jié)束狀態(tài)所需的時(shí)間t（即t1）可得到不同的軌跡。依據(jù)上文所述車道線曲率約束條件，根據(jù)曲率與車速極值對(duì)應(yīng)表得到在Δs處車輛的速度vt，結(jié)合車輛初始速度v0可求得t值。

（1）Kmax＜Cmin，認(rèn)為車輛將保持當(dāng)前車速行駛，此時(shí)t=4。

（2）Cmin＜Kmax＜Cmax，但 vt≥v0，認(rèn)為車輛將保持當(dāng)前車速行駛，此時(shí)t=4。

（3）Cmin＜Kmax＜Cmax，但 vt＜v0，利用式（8）計(jì)算時(shí)間 t，但 t應(yīng)滿足條件：4≤t≤8。即若計(jì)算所得 t＜4，則令t=4，若計(jì)算所得t＞8，則令t=8，其它情況直接代入t即可。其中，Kmax為預(yù)測(cè)軌跡中的最大曲率值；Cmin為表1中的曲率極小值；Cmax為表1中的曲率極大值。

最后代入t便可從一組預(yù)測(cè)軌跡中篩選出一條最優(yōu)的預(yù)測(cè)軌跡。

4 實(shí)驗(yàn)結(jié)果與分析

為驗(yàn)證本文中所提方法的準(zhǔn)確性、有效性和時(shí)間性能，采用“JJUV8”智能車為實(shí)驗(yàn)平臺(tái)，以Velodyne HDL-64E型64線激光雷達(dá)實(shí)時(shí)采集的城市動(dòng)態(tài)環(huán)境下的障礙物數(shù)據(jù)為研究對(duì)象進(jìn)行實(shí)車實(shí)驗(yàn)。主控計(jì)算機(jī)參數(shù)為IntelRCoreTMi7-6700 CPU@3.40GHz x 8，16GB ARM，Ubuntu 16.04操作系統(tǒng)，Qt Creator 5.7.0開發(fā)環(huán)境，開發(fā)語言為C++，實(shí)驗(yàn)參數(shù)設(shè)置如表2所示。

表2 實(shí)驗(yàn)參數(shù)設(shè)置

分別采用本文算法與文獻(xiàn)［10］和文獻(xiàn)［11］中所用CTRA模型進(jìn)行對(duì)比實(shí)驗(yàn)，對(duì)不同行為車輛進(jìn)行軌跡預(yù)測(cè)。實(shí)驗(yàn)所選環(huán)境中的車輛較多、路況相對(duì)復(fù)雜且車輛有不同的行為，可以代表城市動(dòng)態(tài)環(huán)境。本文算法預(yù)測(cè)結(jié)果如圖3（a）所示，CTRA模型預(yù)測(cè)結(jié)果如圖3（b）所示。圖中長(zhǎng)方體框代表感知到的動(dòng)態(tài)車輛，曲線為各個(gè)車輛的預(yù)測(cè)軌跡，曲線上的點(diǎn)代表預(yù)測(cè)得到的車輛在未來一段時(shí)間內(nèi)的具體位置。

為定量考察本文中所述方法的準(zhǔn)確性，將預(yù)測(cè)得到的軌跡點(diǎn)與車輛真實(shí)軌跡點(diǎn)對(duì)比，進(jìn)行定量評(píng)估，并計(jì)算方法耗時(shí)，驗(yàn)證其時(shí)間性能。

圖3 兩種方法軌跡預(yù)測(cè)結(jié)果

圖4 ～圖6分別為車輛不同行為下4 s內(nèi)的軌跡預(yù)測(cè)結(jié)果。其中黑線為Velodyne HDL-64E型64線激光雷達(dá)得到的車輛位置點(diǎn)組成的軌跡，可認(rèn)為是車輛的真實(shí)軌跡。由圖可以看出，在不同行為下，雖然兩種方法得到的預(yù)測(cè)軌跡都比較平滑，但本文算法得到預(yù)測(cè)軌跡更為合理，準(zhǔn)確度更高，即利用本文算法進(jìn)行的軌跡預(yù)測(cè)準(zhǔn)確度要優(yōu)于CTRA模型，驗(yàn)證了該算法的有效性。表3～表5分別對(duì)比了車輛不同行為下兩種方法在4 s內(nèi)的軌跡預(yù)測(cè)誤差，實(shí)驗(yàn)結(jié)果取200次計(jì)算結(jié)果的平均值。對(duì)比結(jié)果表明，本文算法對(duì)車輛不同行為的軌跡預(yù)測(cè)誤差更小、準(zhǔn)確度更高。驗(yàn)證了該算法的準(zhǔn)確性。

圖4 車道保持行為軌跡預(yù)測(cè)結(jié)果對(duì)比

圖5 換道行為軌跡預(yù)測(cè)結(jié)果對(duì)比

表3 車道保持行為軌跡預(yù)測(cè)誤差

圖6 轉(zhuǎn)彎行為軌跡預(yù)測(cè)結(jié)果對(duì)比

表4 換道行為軌跡預(yù)測(cè)誤差

表5 轉(zhuǎn)彎行為軌跡預(yù)測(cè)誤差

由表3～表5可知，本文算法所得結(jié)果與車輛真實(shí)軌跡誤差較小，預(yù)測(cè)的準(zhǔn)確性較好，且預(yù)測(cè)軌跡合理。

圖7為單個(gè)障礙物連續(xù)200幀兩種軌跡預(yù)測(cè)方法的耗時(shí)?？梢钥闯?，雖然CTRA模型耗時(shí)相對(duì)較低，但本文方法計(jì)算時(shí)間也不高，平均耗時(shí)僅為0.103 ms，驗(yàn)證了本文方法的實(shí)時(shí)性。

5 結(jié)論

圖7 兩種方法計(jì)算時(shí)間對(duì)比

提出了一種車輛軌跡預(yù)測(cè)的方法。該方法首先接收感知得到的障礙物信息，結(jié)合高精度地圖提供的車道線信息，識(shí)別車輛行為，然后建立s-l坐標(biāo)系，并結(jié)合行為識(shí)別結(jié)果，沿車道橫向與車道縱向建立兩個(gè)多項(xiàng)式方程，分別預(yù)測(cè)車輛的橫向和縱向的運(yùn)動(dòng)；再以車道線曲率為約束條件，篩選出一條最優(yōu)的預(yù)測(cè)軌跡。最后通過城市環(huán)境下的實(shí)車實(shí)驗(yàn)驗(yàn)證了本文算法的有效性、準(zhǔn)確性和實(shí)時(shí)性，實(shí)驗(yàn)結(jié)果表明本文所提方法能夠較為準(zhǔn)確地預(yù)測(cè)運(yùn)動(dòng)車輛未來4 s內(nèi)的軌跡，預(yù)測(cè)軌跡合理，處理過程平均耗時(shí)僅為0.103 ms。

本研究目前適用于結(jié)構(gòu)化道路環(huán)境下運(yùn)動(dòng)車輛的軌跡預(yù)測(cè)，同時(shí)認(rèn)為車輛遵守交通規(guī)則且只做勻速或勻變速運(yùn)動(dòng)，下一步將著重解決環(huán)境車輛做變速運(yùn)動(dòng)時(shí)的軌跡預(yù)測(cè)問題。