基于數據關聯和改進統計模型的激光雷達目標跟蹤研究?

黃　晶，李　勇，2，胡　林

前言

無人駕駛系統和高級輔助駕駛系統被認為是解決交通便利和安全問題的最具價值與潛力的方案，而環境感知是其中重要的組成部分[1-2]。由各種車載傳感器構成的環境感知系統為車輛的決策控制提供必要的信息，系統的優劣性將直接影響車輛的行駛安全。而系統的實時性和準確性是實現車輛自動駕駛的重要基礎，它要求在特定時間段內完成車輛與環境信息數據的獲取和保證所獲得數據的可靠性。

目標跟蹤是智能車輛環境感知中的關鍵問題之一[3]，在本文中表現為基于車載激光雷達通過聚類獲取智能車前方目標相對于測量車的位置、尺寸等信息后，通過目標關聯匹配算法來處理得到對雷達量程范圍內各個目標在時間序列上的量測值，然后通過目標運動狀態估計算法有效地抑制測量過程中引入的量測噪聲，實時、準確地辨識出智能車輛前方目標的運動狀態(包括位置、速度和加速度)，進而為目標航跡預測提供依據。因此目標關聯匹配和運動狀態估計是影響目標跟蹤的重要因素，國內外專家學者對此做了相關研究。最近鄰法通過計算各量測位置距跟蹤門中心的距離來實現數據關聯[4]，但當在目標密集或存在雜波干擾的情況下，目標關聯正確率較低；概率數據關聯算法[5]和聯合概率數據關聯算法[6]都是根據相關計算得出互聯概率，并以此作為權值對跟蹤門內的所有量測的狀態濾波估計值進行加權求和作為最終的目標狀態估計值，該算法的關聯準確率較高，但當測量范圍內目標較多時，會導致計算量暴增，從而導致系統實時性較差。本文中出于實時性的考慮，結合單線激光雷達的實際使用提出了一種改進的最近鄰關聯算法，將目標的位置、尺寸大小和反射強度信息通過“關聯函數”結合起來以判定量測與目標之間的關聯性。針對目標的運動狀態估計，文獻[7]和文獻[8]中分別提出了基于勻速(CV)模型的2階卡爾曼濾波器和基于勻加速(CA)模型的3階卡爾曼濾波器，假定目標相對傳感器保持恒定速度或恒定加速度行駛，將相對加速度作為均值為0的高斯白噪聲隨機干擾，濾波器并沒有根據目標運動狀態的變化對系統模型進行相應調整。當目標勻速或勻加速運動時，目標位置和速度估計誤差較小，而當目標做變速運動時，由于系統模型與實際系統變化規律之間存在差異，會產生較大的估計誤差。文獻[9]中提出的Singer模型跟蹤算法認為機動模型是相關噪聲模型，而不是通常假定的白噪聲模型，且對目標的加速度作為具有指數自相關的零均值隨機過程建模，使系統模型與目標實際加速度變化規律接近了一步，但Singer模型算法本質上畢竟是一種先驗模型算法，期望基于先驗模型來有效描述目標的機動是不現實的。文獻[10]中提出了目前已廣泛應用于軍事航天領域的當前統計(CS)模型，該模型與Singer模型中的近似均勻分布假設不同的是該算法采用修正瑞利分布來描述目標加速度的統計特性，加速度分布隨均值變化而變化。本文中構造了基于當前統計模型的自適應卡爾曼濾波跟蹤算法，并針對當前統計模型對于弱機動目標(加速度變化范圍較小)跟蹤效率較低和過度依賴人為設定的加速度極值等缺陷，采用了基于濾波殘差的隸屬度函數來對加速度極值進行實時修正，進而調整加速度方差。

1　目標跟蹤

本文中所進行的基于單線激光雷達的車輛前方目標跟蹤研究主要流程如圖1所示，其中目標運動狀態識別和目標關聯匹配是整個跟蹤過程的重要環節。

圖1　目標跟蹤算法運行流程圖

1.1　改進的運動狀態識別算法

在真實交通環境中，車載雷達測量值中帶有來自前方路面與道路隔離護欄等干擾源和雷達自身震顫所造成的量測噪聲，且激光雷達只能通過直接測量得到目標相對雷達的距離和方位信息。因此需要設計目標運動狀態識別算法，從帶有量測噪聲的雷達量測信號中來較準確地辨識出目標的位置、速度和加速度信息。

在汽車、軍事和航海航空等研究領域中，采用的目標運動狀態估計方法通常是先建立目標運動狀態模型，然后基于這個模型進行濾波估計。目前基于車載傳感器的目標運動狀態估計多是假定目標做勻速或者勻加速運動，將相對加速度作為均值為0的高斯白噪聲隨機干擾。這樣的假設給運動狀態建模及建立濾波估計算法帶來了極大的便利，但在實際的交通環境中，車輛經常面臨換道超車、紅燈減速停車等多變的交通狀況，車速也不可能長時間穩定在某一車速下。因此定速度模型和定加速度模型均不能真實反映目標實際的運動狀態變化，造成目標運動狀態估計精度下降，甚至發散。

本文中基于軍事航天領域中的當前統計運動模型并對其進行了改進，用于運動狀態識別。當前統計模型與目前車輛領域常用的Singer模型中的近似均勻分布假設不同[11]，該算法采用修正瑞利分布來描述機動加速度的統計特性，使目標下一時刻的加速度變化范圍只能在當前加速度的某一鄰域內，因而當前統計模型能較真實地反映目標機動范圍和強度的變化，故基于當前統計模型的自適應卡爾曼濾波估計算法在估計目標狀態的同時，還可辨識出機動加速度均值，從而實時地修正加速度分布，并通過方差反饋到下一時刻的濾波增益中，實現了閉環自適應跟蹤。在當前統計模型中有:

式中:a (k)為加速度均值；amax為目標最大加速度。

本文中設計了基于當前統計模型的自適應卡爾曼濾波估計算法[12]，算法流程如下。

(1)狀態預測

(2)協方差預測

(3)濾波殘差(新息)

(4)濾波殘差的協方差矩陣

(5)濾波增益

(6)狀態估計更新

(7)協方差更新

然而，當前統計模型對于弱機動目標(加速度變化較小)跟蹤效率較低，過度依賴人為設定的加速度極值，且本文測定的是目標對雷達安裝車輛的相對加速度，加速度極值難以確定，因此本文中將濾波殘差以及高斯型隸屬度函數引入到對加速度極值的實時調整過程中:

由式(11)和式(12)可知:調節因子uk(k)可根據當前的濾波殘差和協方差在[0，1]之間變動，通過間接調整加速度極值來改變加速度方差，進而調整濾波增益，改善跟蹤精度。

1.2　基于“關聯函數”的目標關聯匹配算法

目標運動狀態估計的實現僅僅依靠目標在一幀中的量測信息顯然是不夠的，需要在連續多幀傳感器數據中找到該目標對應的量測值。通常在雷達量程范圍內會存在多個目標，而目標關聯匹配模塊的作用就是將雷達探測得到的多幀中的量測值按照它們所歸屬的目標源匹配起來，即找到每個目標量測信息在時間序列上的分布。目標關聯匹配需要解決跟蹤門、數據關聯和目標運動狀態預測3大問題。

根據跟蹤門的形狀和大小等來初步篩選出與每個目標可能形成配對關系的量測數據，形成候選量測集合。當某個量測值未落入上一時刻的目標序列中的任何一個的跟蹤門內時，即可判定該量測值可能來自一個本時刻新出現的障礙物或干擾雜波；當上一時刻的目標序列中的某個目標在本時刻的跟蹤門內不存在任何一個量測值，則認為這個目標可能超出量程范圍或被其他障礙物遮擋等。跟蹤門的選擇應盡量平衡相互矛盾的兩方面因素:一是使目標的真實量測值落入它的跟蹤門內的可能性足夠大；二是盡可能減少目標跟蹤門內的無關量測值，以減小計算量。在本文中出于實時性的考慮，選擇使用計算量相對較小的矩形跟蹤門，建立矩形跟蹤門為:如果X和Y方向的量測值與目標位置的一步預測值滿足式(13)，則認為此量測值是該目標的候選量測值。

式中:PG為目標的正確量測值落入其跟蹤門內的參考概率；σri2為觀測噪聲方差陣中第i個對角線元素。為使目標的真實量測值以較大的概率落入目標的跟蹤門內，須使PG≥3.5，本文取PG＝3.5。

跟蹤門的中心由目標運動狀態的一步預測來確定，即在得到本時刻目標的位置估計之后，預測下一時刻目標的位置，然后以此位置為中心建立跟蹤門來篩選得到目標的候選量測值，為下一時刻目標關聯匹配做準備。本文中利用目標運動狀態濾波估計算法中的一步預測結果中的位置分量作為目標位置的一步預測，且該算法中的預測協方差可用于計算目標跟蹤門的門限值。

當目標的跟蹤門內落入了多個量測值時，則須通過數據關聯算法依據一定的判定準則計算某個量測值屬于該目標的可能性，最后從目標跟蹤門內的所有候選量測值中找到目標的真實量測值。本文中出于實時性的考慮，并結合單線激光雷達的實際使用提出了一種改進的最近鄰關聯算法。由于傳統最近鄰算法只考慮量測值與目標一步預測位置之間的距離，在單一判據下匹配準確性較差，故在改進的最近鄰匹配算法中將目標的位置、尺寸和激光反射強度信息通過“關聯函數”結合起來判定量測值與目標之間的關聯。“關聯函數”為

式中:Cor為關聯函數值；ak，bk分別為k時刻量測的長和寬(即聚類結果的矩形框尺寸)；(xk，yk)表示量測的位置；(xk｜k-1，yk｜k-1)表示目標一步預測位置(即跟蹤門中心位置)；Ik表示量測的反射強度；k1，k2，k3分別為尺寸、位置、反射強度差異的加權系數，且k1＋k2＋k3＝1。加權系數的取值對數據關聯匹配算法的實際效果具有直接的影響，目標位置是其中最重要的特征，因此所占權重較大，即k2取值應在0.5-0.6之間；由于激光雷達的激光束成扇形的掃描特性，且為固定的角度分辨率，所以相鄰兩激光反射點間的距離是隨目標距離遠近而變化，使當目標距離較遠時所返回的云點數減少從而導致目標尺寸信息精度下降，故當目標距離較遠時，目標尺寸特征所占的權重應相應減小；而激光反射強度在雷達量程范圍內隨目標距離遠近變化而引起的衰減程度較弱，故反射強度特征在“關聯函數”中所占權重較為穩定。加權系數的取值須在真實環境中通過大量的試驗反復調試來確定，以獲得更準確可靠的目標關聯效果。

當目標的跟蹤門內有多個候選量測值而使關聯函數值Cor最大時，則認為此量測值與目標的關聯性最大，即該量測值為目標在本幀中對應的量測值。

2　仿真與試驗結果

2.1　目標運動狀態估計仿真

在Matlab/Simulink中分別建立勻速、勻加速、Singer、標準當前統計(CS)和改進的當前統計(NCS)等運動模型的自適應卡爾曼濾波估計算法模型[13]，分別進行以下工況的目標運動狀態估計。

工況一:目標在全局坐標系下沿X，Y方向分別作勻速運動，觀測量為目標位置，仿真周期0.1s，仿真時間30s。

工況二:目標在全局坐標系下沿X方向作勻加速運動，沿Y方向做勻速運動，觀測量為目標位置，仿真周期0.1s，仿真時間30s。

工況三:目標在全局坐標系下沿X方向作變加速運動，沿Y方向做勻速運動，觀測量為目標位置，仿真周期0.1s，仿真時間30s。

各工況下仿真結果估計誤差如圖2所示(其中勻速、勻加速模型誤差較大，圖中忽略)，均方根誤差如表1～表3所示。

從仿真結果中可以看出，基于勻速模型的卡爾曼濾波估計適用范圍較窄，不能直接得到目標的加速度濾波估計，且效果一般；基于勻加速模型的卡爾曼濾波估計在勻速和勻加速工況下所得到的對目標位置和加速度的濾波結果比基于Singer、當前統計和新當前統計模型的濾波估計結果更接近于真實值，這是因為在卡爾曼濾波估計過程中，當系統過程噪聲協方差Q較大時，濾波估計結果會更偏向于觀測值，導致對噪聲濾波效果變差，對基于當前統計模型的自適應卡爾曼濾波算法而言，當最大加速度amax為確定值，且目標在較小的加速度值范圍變動時，跟蹤性能會隨著系統過程噪聲協方差變大而降低；而在改進的新當前統計模型中，由于引入了加速度調節因子，可根據濾波殘差和協方差動態調整加速度值，既能保證目標做勻速和勻加速運動時的濾波估計精度，也能避免目標加速度超過選值范圍導致濾波估計效果變差甚至出現發散，基于改進的新當前統計模型的卡爾曼濾波估計算法在3種工況中都表現出對觀測噪聲良好的濾除效果，且能獲得更貼近實際的濾波估計結果。

圖2　各工況下仿真結果估計誤差

表1　工況一下的仿真結果均方根誤差

表2　工況二下的仿真結果均方根誤差

2.2　目標關聯匹配實車試驗驗證

本文中以力帆620作為試驗車平臺，在試驗車上裝有激光雷達、慣性導航系統和工控機等，如圖3所示。本文中使用HOKUYO公司的二維激光雷達，基本參數見表4。實車試驗的相關算法程序和上位機界面都是基于Visual Studio C/C＋＋和Qt編寫的，主要功能有雷達傳感器測量數據實時讀取、目標聚類、關聯匹配和運動狀態估計，并將測量數據和算法處理結果保存，以便離線驗證和分析處理。

表3　工況三下的仿真結果均方根誤差

圖3　實車試驗平臺

表4　激光雷達基本技術參數

在目標關聯匹配的實車試驗過程中，出于對試驗設備保護的考慮和避免復雜的坐標換算，以便于得到更準確的測量信息驗證算法的準確性，試驗車平臺保持靜止狀態，試驗車前方有行人、車輛和靜止的障礙物共計6～10個，利用試驗車上的激光雷達探測車輛前方目標并進行關聯匹配。數據關聯匹配算法中加權系數ki(i＝1，2，3)的取值對于算法的實際效果具有直接的影響，需要在試驗中反復調試來確定取值，因此在實車試驗過程中通過上位機界面反復修正“關聯函數”中尺寸、位置、反射強度差異的加權系數，一共進行了30次在線測試，系統采樣周期為50ms，采樣時間為20s，每次測試得到400幀數據，試驗過程如圖4所示。

圖4　目標關聯匹配試驗過程

根據目標關聯匹配的結果進行分析可得出:當加權系數設置為 k1＝0.3，k2＝0.5，k3＝0.2 時，在測量車前方30m范圍內的目標關聯匹配正確率達到95%以上，但在30～60m范圍內時，目標關聯匹配正確率只達到40%左右；而當加權系數調整為k1＝0.1，k2＝0.6，k3＝0.3 時，匹配正確率又達到了 80%以上。故算法中采取兩組加權系數，將30m設定為距離閾值，根據目標位置配置相應的目標“關聯函數”加權系數再次進行試驗。圖5為同一時段內被觀測到的3個目標在慣性坐標系下(以激光發射器為原點，X軸定義為水平向右，Y軸定義為水平向前)的運動軌跡。試驗過程中，目標1和目標2相隔較近，且在雷達視野中發生過相互遮擋，但由圖可見，跟蹤得到的目標軌跡具有良好的連續性，并沒有出現目標的丟失和位置突變，即表明系統成功實現了車輛前方目標的關聯匹配。

圖5　目標關聯匹配試驗結果

3　結論

本文中以單線激光雷達為主要傳感器，從目標運動狀態估計和數據關聯匹配兩個方面對車輛前方的目標跟蹤進行了相關研究，得出結論如下:

(1)針對當前統計模型對于弱機動目標跟蹤效率較低和過度依賴人為設定的加速度極值的缺陷，設計了基于改進的當前統計模型的自適應卡爾曼濾波估計算法，利用高斯型隸屬度模糊函數實現對加速度極值的實時調整，進而提高對不同狀態下的運動目標的跟蹤精度；

(2)結合激光雷達的特性提出了一種通過構造距離、尺寸和反射強度的“關聯函數”來進行量測值與目標之間匹配的數據關聯算法，通過在試驗中不斷調整各加權系數的權值，確定了兩套加權系數值以適應目標與雷達的距離遠近，提高了目標關聯匹配的準確性。

本文中所提出的基于數據關聯和改進統計模型的激光雷達目標跟蹤算法能基本滿足智能車輛在有效范圍內的環境感知，為路徑規劃和自主導航奠定基礎，同時本文中所設計的車輛前方機動目標運動狀態濾波估計算法也同樣適用于基于機器視覺和紅外傳感等傳感器的障礙物跟蹤識別系統。但由于試驗場地限制，和缺乏多輛配備慣性導航的車輛作為目標車，所以難以開展相關試驗以進一步驗證目標跟蹤算法的效果。這將作為今后的研究內容，通過搭建更符合真實交通狀況的試驗場景來測試和改善目標跟蹤算法。

[1] 戴斌，聶一鳴，孫振平，等.地面無人駕駛技術現狀及應用[J].汽車與安全，2012(3):46-49.

[2] 王肖，李克強，王建強，等.基于三維激光雷達的智能車輛目標參數辨識[J].汽車工程，2016，38(9):1146-1152.

[3] 周俊靜，段建民，楊光祖，等.基于雷達測距的車輛識別與跟蹤方法[J].汽車工程，2014，36(11):1415-1420.

[4] ZHANG Y，LEUNGH，LOT，et al.Distributed sequential nearest neighbor multi-target tracking algorithm[J].IEE Proceedings-Radar， Sonar and Navigation，1996，143(4):255-260.

[5] ABOLMAESUMI P，SIROUSPOUR M R.An interacting multiple model probabilistic data association filter for cavity boundary extraction from ultrasound images[J].IEEE Transactions on Medical Imaging，2004，23(6):772-84.

[6] VERMAAK J， GODSILL SJ， PEREZ P.Monte carlo filtering for multi target tracking and data association[J].Aerospace&Electronic Systems IEEE Transactions on，2005，41(1):309-332.

[7] LI X R，JILKOV V P.Survey of maneuvering target tracking.Part I.Dynamic models[J].IEEE Transactions on Aerospace&Electronic Systems，2004，39(4):1333-1364.

[8] PETROVSKAYA A，THRUN S.Efficient techniques for dynamic vehicle detection[J].Springer Tracts in Advanced Robotics，2009，54:79-91.

[9] 邵俊偉.一種參數自適應調整的Singer模型跟蹤算法[J].科技信息，2014(13):111-112.

[10] SHI Z G，HONG SH，CHEN K S.Tracking airborne targets hidden in blind doppler using current statistical model particle filter[J].Progress in Electromagnetics Research，2008，82:227-240.

[11] MA X，LIU Q，HE Z，et al.Visual tracking via exemplar regression model[J].Knowledge-Based Systems，2016，106(C):26-37.

[12] 段建民，鄭凱華，李龍杰，等.基于多層激光雷達的道路信息提取算法[J].控制工程，2016，23(4):468-473.

[13] HU L，GU Z Q，HUANGJ，et al.Research and realization of optimum route planning in vehicle navigation systems based on a hybrid genetic algorithm[J].Proceedings of the Institution of Mechanical Engineers， Part D:Journal of Automobile Engineering，2008，222(5):757-763.