基于最小二乘法的場景重構(gòu)目標(biāo)車輛預(yù)測算法

劉衛(wèi)東， 羅華平， 王愛春， 黃少堂

（江鈴汽車股份有限公司， 江西 南昌 330001）

可預(yù)期的未來十余年，高級駕駛員輔助系統(tǒng)ADAS和無人駕駛將是汽車行業(yè)最具創(chuàng)新性和顛覆性的前行方向。精準(zhǔn)、直觀、智能的場景重構(gòu)，是前者演化的重要組成部分，并為持續(xù)豐富的行駕體驗(yàn)提供支撐。

目前而言，傳感器（雷達(dá)、攝像頭等） 由于自身感知能力的局限性，無法保證全天候有效地提供真實(shí)的外部環(huán)境數(shù)據(jù)，如，因自車顛簸，造成傳感器視場角偏轉(zhuǎn)，從而丟失前車目標(biāo)。基于此情形的場景重構(gòu)，必會出現(xiàn)目標(biāo)車輛在儀表/顯示屏上短暫消失，干擾駕駛員判斷，影響駕駛體驗(yàn)，更嚴(yán)重者導(dǎo)致安全事故。

本文基于某公司的中距離毫米波雷達(dá) （MRR） 感知&探測方案，以最小二乘法為核心，考量正常道路行駛的場景進(jìn)行條件約束，設(shè)計出一種簡潔、高效的追蹤&預(yù)測算法，旨在持續(xù)、穩(wěn)定地追蹤目標(biāo)，從而支撐場景重構(gòu)中目標(biāo)車輛可靠、有效的顯示，達(dá)到最優(yōu)行車體驗(yàn)。

1 最小二乘法介紹

最小二乘法（又稱最小平方法） 一般形式可如下式：

觀測值就是通過實(shí)驗(yàn)或者測量得到的多組樣本，理論值就是假設(shè)的擬合函數(shù)。最小二乘法核心思想是求解未知參數(shù)，使得理論值與觀測值之差 （殘差） 的平方和 （損失函數(shù)） 達(dá)到最小，此時擬合函數(shù)的曲線可最大精準(zhǔn)地反映觀測值的基本趨勢。由高斯-馬爾可夫定理可證明，在給定經(jīng)典線性回歸的假定下，最小二乘估計量是具有最小方差的線性無偏估計量。

基于上述，最小二乘法已成為目標(biāo)追蹤&預(yù)測的常用方法之一，其相比于卡爾曼濾波、α-β-γ濾波等方法，原理簡單，易于編程實(shí)現(xiàn)，在一定條件下具有良好的統(tǒng)計性，更具有工程實(shí)現(xiàn)意義。

在現(xiàn)實(shí)中，物體的運(yùn)動軌跡按一定規(guī)律隨時間變化，假定在N個順序時刻的觀測值為f（ti）（i=1，2，…，N），滿足與之殘差平方和 （損失函數(shù)） 最小二乘多項式 （擬合函數(shù)）p（t）的系數(shù)為a0，a1，…，am（m

通過S對ak求偏導(dǎo)數(shù)，令偏導(dǎo)數(shù)為0，可得到多項式的系數(shù)通解：

事實(shí)上，如用矩陣求解，推導(dǎo)出的多項式系數(shù)通解公式更為簡潔和易理解，簡述如下。

將N點(diǎn)前觀測值和假設(shè)多項式組成超定方程組：

式中：αj（j=0，1，2…m）為多項式系數(shù)；Tij（i=1，2，3…N；j=0，1，2…m）為自變量t在i順序時刻的j次方；Yi（i=1，2，3…N）為在i順序時刻的觀測值。

對式（4） 向量化后為：

因超定方程組固有特征，通常無解，需尋找最合適的α，讓方程組“盡可能”成立，由此引入損失函數(shù)S：

如果矩陣TTT非奇異，則α?有唯一解，即多項式系數(shù)通解

2 算法實(shí)現(xiàn)

2.1 感知&探測方案簡介

本文所選擇傳感器是某公司第4代通用中距離毫米波雷達(dá)，探測感知范圍：縱向最大有效距離120m，橫向最大有效距離左右各10m。通過CAN總線，以20ms為周期，上報位于傳感器左前、前、前前、右前方位，共4個目標(biāo)車輛相對于傳感器的縱向（Dx） 距離和橫向（Dy） 距離。

2.2 運(yùn)動模型建立

基于實(shí)際正常行車場景，可作以下設(shè)定。

1） 在正常行車過程中，所有的目標(biāo)車輛，都可視為在以傳感器或者本車為原點(diǎn)的二維平面內(nèi)，沿X正軸 （縱向）和Y軸（橫向） 連續(xù)（性線） 運(yùn)動。

2） 在較短的時間段內(nèi)，可視為目標(biāo)車輛相對自車作勻速直線運(yùn)動。

由上，目標(biāo)車輛與本車的距離變化可等效為一次函數(shù)：

式中：D——在第N個順序時刻目標(biāo)車輛相對于本車的位置；D0——開始觀測時目標(biāo)車輛相對于本車的初始位置；v——目標(biāo)車輛相對本車的速度；T——從開始觀測到N個順序時刻的持續(xù)時間。

2.3 算法詳解

1） 本車和目標(biāo)車輛運(yùn)動變化是相互獨(dú)立的，為保證運(yùn)動模型有效，本算法在實(shí)現(xiàn)中，只對最近6組測量數(shù)據(jù)（N=6），即100ms時間段內(nèi)進(jìn)行一次函數(shù)（擬合函數(shù)） 求解，可得：

可測量或已知，由矩陣解可得：

2） 為保證預(yù)測準(zhǔn)確性，當(dāng)目標(biāo)丟失時，基于式 （10）和式（12） 的結(jié)果α?，僅計算（預(yù)測） 不超過500ms的目標(biāo)運(yùn)動軌跡，預(yù)測時間達(dá)到設(shè)定閥值（如500ms），將對目標(biāo)停止預(yù)測。

3） 本算法僅預(yù)測目標(biāo)車輛的縱向距離，并設(shè)定縱向預(yù)測的有效范圍［DXNEthr，DXFEthr］（如近端DXNEthr=10m，遠(yuǎn)端DXFEthr=100m）。當(dāng)預(yù)測距離不在設(shè)定范圍內(nèi)時，可認(rèn)為目標(biāo)永久丟失，將對目標(biāo)停止預(yù)測。

4） 本算法不對目標(biāo)車輛的橫向距離預(yù)測，僅對最近6組橫向測量數(shù)據(jù)進(jìn)行加權(quán)平均wi+1>0，且wi為正整數(shù)。當(dāng)目標(biāo)丟失時，判斷DYmean是否在有效范圍內(nèi)，如不在，可認(rèn)為目標(biāo)車輛駛離本車道，即目標(biāo)永久丟失。基于實(shí)現(xiàn)行車場景，需預(yù)設(shè)兩類有效范圍值，分別適用于左&右目標(biāo)，前&前前目標(biāo)。

5） 因?qū)嶋H道路行駛場景，目標(biāo)車輛相對本車通常最大速度為240km/h （約66.67m/s），即兩車以120km/h相對而行，進(jìn)而可得一個報文周期 （20ms），兩車最大位移距離約為1.3m。由此可認(rèn)定：當(dāng)最近兩組測量數(shù)據(jù)Dt-1，Dt之差絕對值|Dt-Dt-1|＞ΔD （本算法設(shè)定為ΔD=4m） 時，追蹤的目標(biāo)已切換，從而需重新求解一次函數(shù) （擬合函數(shù)），以匹配新目標(biāo)的運(yùn)動軌跡。

2.4 算法基本流程圖（圖1）

圖1 算法基本流程圖

2.5 算法驗(yàn)證

本算法經(jīng)實(shí)際道路行車測試，可取得預(yù)期效果。如圖2所示，在兩車相對速度約為40m/s情況下，目標(biāo)車輛丟失500ms （1.32～1.82s；51.31～67.95m），通過擬合函數(shù)計算（預(yù)測） 的目標(biāo)車輛位置和真實(shí)位置僅相差約4m，已滿足在場景重構(gòu)中，平滑地示意目標(biāo)車輛位置。

圖2 實(shí)際道路行車測試

3 總結(jié)

本文所介紹以最小二乘法為核心的目標(biāo)車輛預(yù)測算法，原理易懂，計算量小，編程簡單，可很好地實(shí)現(xiàn)目標(biāo)車輛平滑地動態(tài)顯示，提升和豐富行車體驗(yàn)。由于最小二乘法固有特征，在機(jī)動性很強(qiáng)的系統(tǒng)中，很難保證對目標(biāo)的穩(wěn)定跟蹤，因此不適合于對目標(biāo)車輛長時間預(yù)測，從而導(dǎo)致誤差偏大，失去應(yīng)用的意義。