基于高精地圖的障礙物軌跡預(yù)測與誤差評估

何柳 李宇寂

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

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

1 前言

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

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

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

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

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

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

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

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

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

3 障礙物信息存儲與加速度估計

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

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

4 障礙物的多維分類

4.1 障礙物所處位置的分類

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

4.2 障礙物類型的分類

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

4.3 障礙物的車道級分類

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

圖3 障礙物多維分類

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

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

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

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

5.1 機動車和非機動車軌跡預(yù)測

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

圖4 地圖外機動車和非機動車預(yù)測軌跡

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

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

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

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

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

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

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

6.1 機動車軌跡預(yù)測

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

圖6 機動車直行工況下的預(yù)測軌跡

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

圖7 機動車變道工況下的預(yù)測軌跡

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

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

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

6.2 非機動車軌跡預(yù)測

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

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

圖8 非機動車的預(yù)測軌跡

6.3 行人軌跡預(yù)測

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

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

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

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

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

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

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

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

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

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

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

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

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

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

表1 橫向距離誤差

表2 縱向距離誤差

表3 歐氏距離誤差

表4 航向角誤差

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

（1）預(yù)測時長越長距離誤差和角度誤差越大，因為隨著時間推移，不確定性更大。

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

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

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

9 結(jié)束語

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

致謝：

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