何柳 李宇寂

（中國第一汽車股份有限公司研發(fā)總院，長春 130013）

主題詞：自動(dòng)駕駛 障礙車輛 軌跡預(yù)測 誤差評估

1 前言

在復(fù)雜的交通環(huán)境中，為達(dá)到行駛安全性和舒適性，車輛不僅要規(guī)劃好行駛路徑，還要根據(jù)周圍障礙物的實(shí)時(shí)變化，動(dòng)態(tài)規(guī)劃接下來的駕駛動(dòng)作，因此準(zhǔn)確預(yù)測周圍障礙物的未來行駛軌跡極為重要。有經(jīng)驗(yàn)的駕駛員會(huì)根據(jù)周圍車輛的一系列動(dòng)作準(zhǔn)確預(yù)測出其接下來的行為，比如直行、換道、轉(zhuǎn)彎和掉頭，并能預(yù)估其未來行駛軌跡的位置，如遠(yuǎn)離或者接近本車，從而避免與周圍車輛發(fā)生碰撞，進(jìn)一步規(guī)劃出更高效的行車路線。自動(dòng)駕駛應(yīng)用中，自動(dòng)駕駛車輛也需要根據(jù)周圍車輛的未來行駛軌跡來準(zhǔn)確規(guī)劃自身車輛接下來的動(dòng)作，自動(dòng)駕駛車輛配有各類傳感器，如視覺攝像頭，毫米波雷達(dá)，激光雷達(dá)等，這使得自動(dòng)駕駛車輛具備遠(yuǎn)超人類的感知能力，并且可以做到不間斷、全方位地檢測周圍環(huán)境，同時(shí)結(jié)合高精地圖豐富的車道信息，就可以對障礙車輛進(jìn)行駕駛行為判斷和行駛軌跡預(yù)測，最終用于自身車輛行為決策和軌跡規(guī)劃的依據(jù)。Lin 等[1]提出了一種障礙物軌跡預(yù)測方法，通過對障礙物軌跡的預(yù)測，實(shí)現(xiàn)了參考軌跡的動(dòng)態(tài)求解。王少博[2]結(jié)合駕駛場景中交通中車輛與自動(dòng)駕駛車輛的交互作用，設(shè)計(jì)并實(shí)現(xiàn)了基于交互性預(yù)測的軌跡規(guī)劃系統(tǒng)。張金旺等[3]以高速公路上的車輛為研究對象，對其運(yùn)動(dòng)進(jìn)行簡化建模,采集大量數(shù)據(jù)作為訓(xùn)練樣本對模型進(jìn)行訓(xùn)練，并分別采用統(tǒng)計(jì)距離和馬爾科夫鏈對其橫向運(yùn)動(dòng)和縱向運(yùn)動(dòng)軌跡進(jìn)行預(yù)測。

本文通過自動(dòng)駕駛車輛周圍傳感器感知障礙車輛的位置和速度信息，并結(jié)合高精地圖的車道信息，在已知障礙物車輛行為預(yù)測的前提下，提出了一種有效的障礙物軌跡預(yù)測方法，同時(shí)提出了一種對預(yù)測軌跡進(jìn)行多維度誤差計(jì)算的方法。

2 軌跡預(yù)測的軟件架構(gòu)

在高級(jí)別的自動(dòng)駕駛系統(tǒng)中，障礙物軌跡預(yù)測模塊起著承上啟下的重要作用，該模塊接收上層感知融合模塊的數(shù)據(jù)、高精地圖和高精定位的數(shù)據(jù)，一方面優(yōu)化障礙物當(dāng)前時(shí)刻的位置、速度信息，另一方面預(yù)測其未來一段時(shí)間的位置、速度信息[4]，最終將其輸出給下游規(guī)劃決策模塊，輔助整個(gè)自動(dòng)駕駛系統(tǒng)安全高效運(yùn)行,自動(dòng)駕駛系統(tǒng)架構(gòu)圖如圖1所示。

圖1 自動(dòng)駕駛系統(tǒng)架構(gòu)

高精地圖包含豐富的道路信息，所有行駛在道路范圍內(nèi)的車輛通常都會(huì)遵循車道約束，從而可以根據(jù)這些先驗(yàn)信息對地圖內(nèi)的障礙物做更準(zhǔn)確的軌跡預(yù)測[5]。由于不同類型的障礙物（機(jī)動(dòng)車、非機(jī)動(dòng)車和行人）的行駛特性有著很大差別，機(jī)動(dòng)車通常會(huì)沿車道中心線行駛，非機(jī)動(dòng)車行駛有一定的隨機(jī)性，但通常會(huì)保持在車道內(nèi)，行人的行駛隨機(jī)性更大，因此對不同類型的障礙物分別用不同的算法是非常必要的。

障礙物軌跡預(yù)測的輸出為一系列跟時(shí)間相關(guān)的位置坐標(biāo)點(diǎn)[6]。由于障礙物未來行駛的不確定性，為了更精準(zhǔn)地計(jì)算其未來行駛位置，首先需要對其未來的駕駛行為進(jìn)行預(yù)測，從而輔助軌跡位置預(yù)測輸出更精確的軌跡點(diǎn)信息[7]。障礙物的行為預(yù)測非常復(fù)雜，有著很多算法來解決，本文不在此做深入研究。

由于地圖中車道連接的多樣性[8]，尤其是路口場景，車輛未來的行駛車道有多種可能，但在通常情況下車輛的行駛需要符合車輛動(dòng)力學(xué)特性（如不會(huì)以非常大的速度轉(zhuǎn)彎），以此對于車輛多個(gè)候選預(yù)測軌跡可以用車輛動(dòng)力學(xué)特性規(guī)則進(jìn)行篩選，從而輸出更精準(zhǔn)更合理的預(yù)測軌跡，軌跡預(yù)測模塊的算法流程如圖2所示[9-10]。

圖2 軌跡預(yù)測模塊的算法流程

3 障礙物信息存儲(chǔ)與加速度估計(jì)

感知融合模塊會(huì)提供車輛周圍障礙物的實(shí)時(shí)信息，由于障礙物的行駛軌跡有一定的延續(xù)性，因此需要將障礙物的當(dāng)前信息進(jìn)行存儲(chǔ)，并對歷史信息進(jìn)行更新，該信息包括車輛的坐標(biāo)位置和航向角，同時(shí)采用循環(huán)存儲(chǔ)的方式可以在保證數(shù)據(jù)量足夠的前提下，減少不必要的存儲(chǔ)空間浪費(fèi)[11]。對障礙物歷史信息的分析，有助于優(yōu)化車輛當(dāng)前感知信息的誤識(shí)別及異常數(shù)據(jù)的修復(fù)。

單純依賴傳感器（如激光雷達(dá)，攝像頭等）對障礙物的加速度識(shí)別不夠精確，而加速度對障礙物未來行駛軌跡的預(yù)測又非常重要，因此對障礙物加速度的預(yù)測是非常必要的，可以根據(jù)障礙物一段時(shí)間的歷史運(yùn)動(dòng)數(shù)據(jù)來對其加速度進(jìn)行估計(jì)，具體方法為通過一定時(shí)間內(nèi)障礙物的行駛距離，初始速度和終止速度，根據(jù)牛頓運(yùn)動(dòng)學(xué)定律對加速度進(jìn)行估計(jì)[12-13]。

4 障礙物的多維分類

4.1 障礙物所處位置的分類

高精地圖為自動(dòng)駕駛系統(tǒng)提供了豐富的道路信息[14]，按地圖(地圖內(nèi))行駛的障礙物車輛其行駛軌跡通常會(huì)符合地圖車道的約束；但是不按照地圖（地圖外）行駛的障礙物（如在路沿以外的非機(jī)動(dòng)車和行人等）運(yùn)動(dòng)隨機(jī)性很大，無法得到高精地圖的信息參考[15]，因此需要將障礙物根據(jù)所處位置區(qū)分為地圖內(nèi)和地圖外。

4.2 障礙物類型的分類

不同類型的障礙物其運(yùn)動(dòng)特性有很大差別[16]，如機(jī)動(dòng)車行駛速度較快，通常會(huì)沿著車道中心線行駛；非機(jī)動(dòng)車行駛速度較慢，通常不會(huì)沿著車道中心線，但會(huì)在車道內(nèi)行駛；行人行駛的速度很慢，但其隨機(jī)性非常大，地圖車道對其約束性很小。因此針對不同類型的障礙物設(shè)計(jì)不同的軌跡預(yù)測算法是非常必要的。

4.3 障礙物的車道級(jí)分類

對于地圖范圍內(nèi)的障礙物會(huì)有更多的地圖信息可供參考，對于機(jī)動(dòng)車和非機(jī)動(dòng)車通常有車道的約束作用，因此還需要將其更準(zhǔn)確的分類到所屬車道[17]，障礙物多維分類如圖3所示。

圖3 障礙物多維分類

高精地圖包含車道的坐標(biāo)位置、車道寬度和車道航向信息，因此可以根據(jù)障礙物的當(dāng)前時(shí)刻位置和航向角來計(jì)算其所屬車道，從而根據(jù)該車道的信息來約束障礙物的未來行駛軌跡[18]。

5 地圖外障礙物的軌跡預(yù)測

通常情況下，障礙物的運(yùn)動(dòng)有一定的規(guī)律性和延續(xù)性，機(jī)動(dòng)車和非機(jī)動(dòng)車會(huì)沿著歷史軌跡繼續(xù)向前，行人的運(yùn)動(dòng)隨機(jī)性很大，但其短時(shí)間內(nèi)通常會(huì)以當(dāng)前面部朝向方向運(yùn)動(dòng)。因此可以通過對障礙物一段時(shí)間歷史軌跡的統(tǒng)計(jì)來預(yù)測其未來短時(shí)間的軌跡。

預(yù)測時(shí)間是軌跡預(yù)測一個(gè)很重要的指標(biāo)，自動(dòng)駕駛系統(tǒng)中路徑規(guī)劃模塊需要規(guī)劃未來7 s 的行駛路徑，因此軌跡預(yù)測模塊需要預(yù)測障礙物未來7 s 的軌跡，但在地圖外的障礙物由于其隨機(jī)性較大，通常情況下只會(huì)預(yù)測未來3 s 的軌跡，地圖內(nèi)的障礙物由于有車道的約束，會(huì)預(yù)測未來7 s的軌跡。

5.1 機(jī)動(dòng)車和非機(jī)動(dòng)車軌跡預(yù)測

對于地圖外的機(jī)動(dòng)車和非機(jī)動(dòng)車，采用歷史軌跡數(shù)據(jù)曲線擬合的方法來獲得其運(yùn)動(dòng)軌跡方程，最終結(jié)合當(dāng)前時(shí)刻的速度信息、航向角信息和加速度信息對擬合的軌跡方程進(jìn)行數(shù)據(jù)外推插值來獲得未來的可能軌跡，地圖外的機(jī)動(dòng)車和非機(jī)動(dòng)車的預(yù)測軌跡如圖4所示。

圖4 地圖外機(jī)動(dòng)車和非機(jī)動(dòng)車預(yù)測軌跡

地圖外的軌跡方程采用5次多項(xiàng)式，公式如下：

式中，yt為障礙物的歷史t時(shí)刻的軌跡點(diǎn)的縱坐標(biāo)；x1t為障礙物的歷史t時(shí)刻的橫坐標(biāo)；C0、C1、C2、C3、C4和C5為方程系數(shù)。

5.2 行人的軌跡預(yù)測

對于行人，由于其運(yùn)動(dòng)的隨機(jī)性很大，對歷史軌跡的擬合參考意義不大，因此只能結(jié)合當(dāng)前時(shí)刻的速度信息，在航向角的方向上進(jìn)行直線數(shù)據(jù)外推來獲得其預(yù)測軌跡，行人的預(yù)測軌跡如圖5所示。

圖5 行人的預(yù)測軌跡

6 地圖內(nèi)障礙物的軌跡預(yù)測

地圖范圍內(nèi)的障礙物通常情況下不會(huì)雜亂無章地運(yùn)動(dòng)，車道信息、停止線信息和紅綠燈信息都對其有約束，因此可以結(jié)合高精地圖中的這些信息來獲得更精確的預(yù)測軌跡。

6.1 機(jī)動(dòng)車軌跡預(yù)測

機(jī)動(dòng)車在車道內(nèi)通常沿著車道中心線行駛，即使當(dāng)前時(shí)刻不處于車道中心線，未來的趨勢一定是沿著車道中心線，因此車輛行駛的目標(biāo)車道中心線信息有很大的參考意義，機(jī)動(dòng)車直行工況下的預(yù)測軌跡如圖6所示。

圖6 機(jī)動(dòng)車直行工況下的預(yù)測軌跡

機(jī)動(dòng)車在車道保持的工況下，目標(biāo)車道中心線為當(dāng)前所處車道中心線；當(dāng)處于變道工況時(shí)，目標(biāo)車道中心線1 為當(dāng)前所處車道中心線，目標(biāo)車道中心線2為待變車道的中心線。機(jī)動(dòng)車變道工況下的軌跡預(yù)測如圖7所示。

圖7 機(jī)動(dòng)車變道工況下的預(yù)測軌跡

生成地圖內(nèi)的軌跡預(yù)測方法如下：以當(dāng)前時(shí)刻定位提供的車輛坐標(biāo)位置，感知提供的車輛航向角，速度和加速度信息，結(jié)合目標(biāo)車道中心線信息，通過3階貝塞爾曲線擬合方法生成平滑連接曲線，最終的軌跡坐標(biāo)點(diǎn)在目標(biāo)車道的中心線上。

3階貝塞爾曲線參數(shù)方程如下：

式中，每一段3階貝塞爾曲線均由2個(gè)端點(diǎn)和2個(gè)柄點(diǎn)，由4個(gè)控制點(diǎn)進(jìn)行控制，對于其中每個(gè)控制點(diǎn)的改變，均會(huì)影響這段曲線所有部分，其中A、B、C、D為4個(gè)控制點(diǎn)坐標(biāo)，P(t)表示曲線上的每一點(diǎn)。公式中把A和D稱為端點(diǎn)，B和C稱為柄點(diǎn)，參數(shù)t為0.00 ～1.00，步長取0.01。根據(jù)車輛當(dāng)前位置航向角和目標(biāo)車道，通過3階貝塞爾曲線擬合即可得到最終的軌跡坐標(biāo)點(diǎn)。

6.2 非機(jī)動(dòng)車軌跡預(yù)測

非機(jī)動(dòng)車的行駛有一定的隨機(jī)性，通常不會(huì)沿著車道中心線行駛，但會(huì)在當(dāng)前車道內(nèi)行駛，因此所屬車道中心線可以對軌跡預(yù)測提供參考信息。

生成軌跡的方法如下：以當(dāng)前時(shí)刻車輛坐標(biāo)位置、航向角、速度和加速度信息，結(jié)合所屬車道中心線信息，在保持與車道中心線橫向偏差不變的前提下，通過3階貝塞爾曲線擬合方法生成平滑連接曲線，最終的軌跡坐標(biāo)點(diǎn)處于所屬車道中心線的橫向偏移位置，地圖內(nèi)的非機(jī)動(dòng)車的軌跡預(yù)測如圖8所示。

圖8 非機(jī)動(dòng)車的預(yù)測軌跡

6.3 行人軌跡預(yù)測

由于行人運(yùn)動(dòng)的隨機(jī)性，高精地圖無法提供有效的參考信息，因此地圖內(nèi)的行人軌跡預(yù)測與地圖外的行人軌跡預(yù)測采用相同的方法。

7 預(yù)測軌跡的輸出和篩選

障礙車輛某一時(shí)刻可能存在多條可通行的道路，尤其在路口處可能存在多條候選的預(yù)測軌跡，因此需要盡可能地排除不可能或者概率很低的預(yù)測軌跡，以減少誤識(shí)別。

根據(jù)車輛動(dòng)力學(xué)的特性，行駛中的車輛為防止側(cè)滑通常不會(huì)以很快的速度轉(zhuǎn)彎，既不會(huì)產(chǎn)生過大的側(cè)向加速度。當(dāng)車輛在路口處存在多條候選預(yù)測軌跡時(shí)，結(jié)合車輛的速度和不同預(yù)測軌跡的曲率，可計(jì)算得到不同軌跡下的側(cè)向加速度，根據(jù)經(jīng)驗(yàn)設(shè)定側(cè)向加速度的閾值即可排除會(huì)使車輛產(chǎn)生過大側(cè)向加速度的候選軌跡，最終輸出合理的預(yù)測軌跡。

側(cè)向加速度的計(jì)算公式如下：

式中，a表示側(cè)向加速度；v表示車輛速度；c表示軌跡的曲率。

8 預(yù)測軌跡的誤差評估

目前汽車行業(yè)對障礙物軌跡預(yù)測結(jié)果進(jìn)行誤差評估的主要方法是：對預(yù)測軌跡坐標(biāo)點(diǎn)序列和真實(shí)軌跡坐標(biāo)點(diǎn)序列求歐氏距離并作統(tǒng)計(jì)，這種方法只能粗略反映預(yù)測軌跡與真實(shí)軌跡的整體距離差，而汽車的運(yùn)動(dòng)通?？煞纸鉃闄M向運(yùn)動(dòng)和縱向運(yùn)動(dòng)，不同的軌跡預(yù)測方法對汽車的橫向運(yùn)動(dòng)和縱向運(yùn)動(dòng)有不同的預(yù)測效果，因此將誤差評估分解為橫向評估和縱向評估是非常必要的；另一方面車輛的狀態(tài)在某一時(shí)刻不只由坐標(biāo)位置決定，還跟航向角相關(guān)，因此對軌跡預(yù)測的航向角進(jìn)行誤差評估也非常有必要。

本文選取城市開放道路采集的軌跡預(yù)測結(jié)果進(jìn)行誤差評估，錄制時(shí)間為1 h，采樣頻率為10 Hz，分別對不同工況下的數(shù)據(jù)進(jìn)行評估，最終取平均值作為預(yù)測的統(tǒng)計(jì)結(jié)果。圖9～圖11 為某一時(shí)刻單條預(yù)測軌跡與真實(shí)軌跡的坐標(biāo)點(diǎn)序列，圖9為直行工況下的預(yù)測軌跡與真實(shí)軌跡的坐標(biāo)點(diǎn)。圖10 為變道工況下的預(yù)測軌跡與真實(shí)軌跡的坐標(biāo)點(diǎn)，圖11為路口轉(zhuǎn)彎工況下的預(yù)測軌跡與真實(shí)軌跡的坐標(biāo)點(diǎn)。

圖9 直行工況軌跡預(yù)測與實(shí)測

圖10 變道工況軌跡預(yù)測

圖11 路口轉(zhuǎn)彎工況軌跡預(yù)測

為了對預(yù)測軌跡有更全面的評估，計(jì)算了預(yù)測時(shí)長為3 s和7 s的平均距離誤差與末點(diǎn)距離誤差，并根據(jù)直行、變道、路口轉(zhuǎn)彎3種工況分別計(jì)算了預(yù)測軌跡與真實(shí)軌跡的橫向距離誤差、縱向距離誤差、歐氏距離誤差和航向角誤差，最終根據(jù)所有誤差結(jié)果求平均值得到距離均方根差（Root Mean Distance Squared Error,RMDSE）。

其中，（Px，Py）為t時(shí)刻預(yù)測行駛軌跡點(diǎn)，（Tx，Ty）為相同時(shí)間t所對應(yīng)的真實(shí)行駛軌跡點(diǎn)。Diff_end為預(yù)測軌跡最末點(diǎn)與對應(yīng)的t時(shí)刻的真實(shí)行駛軌跡的歐式距離，即為末點(diǎn)綜合誤差。t從0～N，N為人為定義的預(yù)測時(shí)間，如表1～表4所示。

表1 橫向距離誤差

表2 縱向距離誤差

表3 歐氏距離誤差

表4 航向角誤差

通過統(tǒng)計(jì)結(jié)果可得出如下結(jié)論：

（1）預(yù)測時(shí)長越長距離誤差和角度誤差越大，因?yàn)殡S著時(shí)間推移，不確定性更大。

（2）不同工況預(yù)測效果不同，直行工況預(yù)測效果好于變道和路口轉(zhuǎn)彎，主要由于變道和轉(zhuǎn)彎工況下車輛的橫向移動(dòng)較大。

（3）橫向距離誤差明顯小于縱向距離誤差，因?yàn)楸疚牟捎昧烁呔貓D車道信息作為預(yù)測的先驗(yàn)信息，為預(yù)測算法提供了豐富的橫向參考數(shù)據(jù)，因此可以得到較好的橫向預(yù)測效果。然而，在縱向上由于不確定性較大，又沒有準(zhǔn)確的參考信息，對車輛速度和加速度很難準(zhǔn)確預(yù)測，從而導(dǎo)致了縱向誤差較大，這也證明了不能只通過歐氏距離誤差來對預(yù)測結(jié)果進(jìn)行評估。

（4）航向角誤差評估很必要，因?yàn)檐囕v在同一個(gè)位置可能處于不同的航向，極端情況下甚至可能處于完全相反的行駛方向，通過數(shù)據(jù)可見本文提出的方法對航向角的預(yù)測比較準(zhǔn)確，同時(shí)可以看出在轉(zhuǎn)彎工況下的航向角預(yù)測效果差于直行工況和變道工況。

9 結(jié)束語

通過試驗(yàn)數(shù)據(jù)可以看出，本文提出的障礙物軌跡預(yù)測方法可以有效地對常規(guī)運(yùn)動(dòng)的障礙物進(jìn)行軌跡預(yù)測，橫向誤差的評估結(jié)果較好，說明此方法可以有效預(yù)測障礙物未來的橫向運(yùn)動(dòng)，主要是由于結(jié)合了高精地圖的車道信息，可以更好地預(yù)測障礙車輛所處車道?？v向誤差的評估結(jié)果較差，主要由于對車輛未來速度和加速度的預(yù)測不夠準(zhǔn)確，尤其是當(dāng)車輛處于急加速或急減速的工況，很難做到對車輛縱向行駛距離的精準(zhǔn)預(yù)測。除此之外，對于運(yùn)動(dòng)隨機(jī)性很大的行人，目前的算法還無法精確預(yù)測其軌跡，這也是未來需要改進(jìn)的方向。

致謝：

感謝中國第一汽車集團(tuán)有限公司研發(fā)總院智能網(wǎng)聯(lián)開發(fā)院人工智能研究所同事的支持與幫助。