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

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

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

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

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

1 最小二乘法介紹

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

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

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

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

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

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

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

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

對(duì)式（4） 向量化后為：

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

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

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

2.1 感知&探測(cè)方案簡(jiǎn)介

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

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

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

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

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

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

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

2.3 算法詳解

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

可測(cè)量或已知，由矩陣解可得：

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

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

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

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

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

圖1 算法基本流程圖

2.5 算法驗(yàn)證

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

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

3 總結(jié)

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