基于SRUKF的汽車毫米波雷達(dá)目標(biāo)跟蹤方法

梁波，郭劍鷹

（華域汽車系統(tǒng)股份有限公司，上海 200041）

基于SRUKF的汽車毫米波雷達(dá)目標(biāo)跟蹤方法

梁波，郭劍鷹

（華域汽車系統(tǒng)股份有限公司，上海 200041）

SRUKF濾波器能夠解決濾波過程非線性問題，以及在更新步驟中誤差協(xié)方差矩陣奇異導(dǎo)致的發(fā)散問題，在目標(biāo)跟蹤算法中得到了廣泛應(yīng)用。同時(shí)，針對汽車毫米波雷達(dá)目標(biāo)跟蹤時(shí)狀態(tài)方程線性和測量方程非線性的特點(diǎn)，提出了一種改進(jìn)的快速SRUKF算法。相對于常規(guī)SRUKF，該方法減少了計(jì)算量，且濾波精度沒有下降，適用于嵌入式汽車?yán)走_(dá)模塊的實(shí)時(shí)信號處理。

SRUKF濾波器；汽車?yán)走_(dá)；目標(biāo)跟蹤；雷達(dá)數(shù)據(jù)處理

隨著人們對汽車舒適性和安全性要求越來越高，能夠?qū)崿F(xiàn)危險(xiǎn)預(yù)警和主動控制的先進(jìn)駕駛輔助系統(tǒng)正成為眾多主機(jī)廠和一級供應(yīng)商研究的焦點(diǎn)。汽車用毫米波雷達(dá)傳感器因其具備測速準(zhǔn)確、測量距離遠(yuǎn)、環(huán)境適應(yīng)能力強(qiáng)的優(yōu)點(diǎn)，而被廣泛研究和應(yīng)用。國外眾多一級供應(yīng)商如德國博世、德國海拉等已經(jīng)有成熟的77GHz／24GHz汽車?yán)走_(dá)產(chǎn)品面世。近年來，國內(nèi)的一些高校和企業(yè)，針對汽車毫米波雷達(dá)的天線設(shè)計(jì)和算法設(shè)計(jì)展開研究，取得了較大的進(jìn)展。

常用的基于LMCW的汽車?yán)走_(dá)利用多普勒現(xiàn)象進(jìn)行測速和測距，信號處理后可直接得到目標(biāo)的徑向速度、徑向距離和方位角，后續(xù)的數(shù)據(jù)處理算法是在笛卡爾坐標(biāo)系下進(jìn)行，從而需要通過非線性轉(zhuǎn)換實(shí)現(xiàn)狀態(tài)向量到觀測向量的變換。因此，汽車毫米波雷達(dá)的目標(biāo)跟蹤大多采用非線性濾波器。常用的濾波方法有α-β濾波器、卡爾曼濾波器（KF）、擴(kuò)展卡爾曼濾波器（EKF）、無跡卡爾曼濾波器（UKF）、容積卡爾曼濾波器（CKF）等［1］。擴(kuò)展卡爾曼濾波（EKF）［2］通過將非線性函數(shù)一階泰勒近似并忽略高階項(xiàng)，將非線性問題線性化，這種方法會導(dǎo)致系統(tǒng)產(chǎn)生較大的誤差，甚至不穩(wěn)定。無跡卡爾曼濾波（UKF）［2］采用確定性采樣的形式來逼近狀態(tài)的后驗(yàn)分布，從而對系統(tǒng)狀態(tài)的均值和協(xié)方差的估計(jì)更為準(zhǔn)確。UKF在更新步驟執(zhí)行的狀態(tài)誤差協(xié)方差矩陣的Cholesky分解，導(dǎo)致濾波器在協(xié)方差矩陣非正定時(shí)無法分解。SRUKF［3-4］通過將協(xié)方差矩陣在算法迭代執(zhí)行之前進(jìn)行Cholesky分解，獲得平方根矩陣，進(jìn)而在算法迭代過程中，將平方根矩陣進(jìn)行非線性迭代，自動更新，大幅度降低了矩陣非正定的風(fēng)險(xiǎn)。

本文提出一種SRUKF的改進(jìn)算法，在濾波器預(yù)測階段，利用目標(biāo)運(yùn)動線性方程和Kalman濾波器進(jìn)行狀態(tài)和誤差協(xié)方差預(yù)測；在更新階段，利用預(yù)測值構(gòu)造Sigma點(diǎn)，并以SRUKF濾波器方程更新目標(biāo)狀態(tài)和誤差協(xié)方差，降低了計(jì)算量［5］。

1 系統(tǒng)模型

由于車輛運(yùn)行的非突變性，可假設(shè)目標(biāo)車相對于本車作常加速運(yùn)動，對于觀測采樣周期為d t的離散時(shí)間動態(tài)系統(tǒng)，其線性運(yùn)動狀態(tài)方程和非線性觀測方程分別為

假設(shè)過程噪聲和觀測噪聲為加性零均值高斯白噪聲。目標(biāo)車輛運(yùn)動狀態(tài)可以用一個(gè)6維向量表示，Xk=其中，x是目標(biāo)的橫向距離，vx=是橫向速度，ax=是橫向加速度，y是縱向距離，vy=是縱向速度，ay=是縱向加速度。

運(yùn)動模型采用常加速（CA）模型［6］，過程函數(shù)使用F矩陣表示，兩次測量的時(shí)間間隔d t作為運(yùn)動方程的唯一參數(shù)，目標(biāo)運(yùn)動方程如下［5］

過程噪聲協(xié)方差矩陣Q為對角陣

而汽車毫米波雷達(dá)單個(gè)周期可測得的目標(biāo)信息是zk=［r，α，v］T，包含目標(biāo)距離r，目標(biāo)角度α，目標(biāo)徑向速度v。測量模型是一個(gè)非線性函數(shù)，無法用矩陣形式表示，狀態(tài)空間到測量值空間的映射表示如下

測量噪聲協(xié)方差矩陣R為對角陣

2 SRUKF算法的改進(jìn)

UKF濾波器的核心是UT變換，其主要作用是對均值為u，誤差協(xié)方差矩陣為P的服從高斯分布的隨機(jī)變量x，生成若干固定采樣sigma點(diǎn)，經(jīng)過非線性變換后，可以得到變換后逼近高斯分布的隨機(jī)變量均值和誤差協(xié)方差矩陣，且變換函數(shù)線性度越強(qiáng)，則變換后的分布越接近高斯分布，其逼近的均值和方差越接近實(shí)際分布函數(shù)的參數(shù)。

傳統(tǒng)UKF濾波器、SRUKF濾波器一般在時(shí)間更新和測量更新階段都是用了UT變換。由于汽車毫米波雷達(dá)目標(biāo)跟蹤中運(yùn)動方程的線性，改進(jìn)的快速SRUKF濾波器基本思想是［5］：在預(yù)測階段使用線性卡爾曼濾波器的預(yù)測方程，在更新階段采用SRUKF濾波器的更新方程計(jì)算增益、更新狀態(tài)和誤差協(xié)方差矩陣。改進(jìn)的快速SRUKF濾波器算法步驟如下［3］。

1）初始化

式中：X0均值為，平方根方差為SX，0。

2）狀態(tài)及誤差平方根一步預(yù)測

3）產(chǎn)生Sigma點(diǎn)并計(jì)算權(quán)值

4）量測值及協(xié)方差預(yù)測

由步驟3）中得到的Wm和測量值預(yù)測值σy，k+1可得到測量值的預(yù)測值

5）計(jì)算卡爾曼增益

卡爾曼增益Kk+1的計(jì)算是卡爾曼濾波器的核心步驟，它決定了狀態(tài)預(yù)測值與傳感器測量值的可信度大小。

6）狀態(tài)及協(xié)方差更新

3 仿真、試驗(yàn)及數(shù)據(jù)分析

仿真在MATLAB下進(jìn)行，雷達(dá)模塊處于靜止?fàn)顟B(tài)，單個(gè)目標(biāo)車輛相對于雷達(dá)的運(yùn)動軌跡如圖1所示，比較了常規(guī)SRUKF與改進(jìn)SRUKF的濾波精度和運(yùn)算耗時(shí)。仿真時(shí)，2種方法采用添加相同高斯白噪聲的測量誤差數(shù)據(jù)，根據(jù)實(shí)際使用的24GHz雷達(dá)前端性能，所添加的測量距離誤差均方根為1.0m，角度均方根值為1°，速度均方根值為0.05m／s，刷新周期50ms。

圖1 仿真場景

進(jìn)行了10 000次蒙特卡羅試驗(yàn)。試驗(yàn)分析了單次濾波前后的誤差分布，統(tǒng)計(jì)SRUKF和改進(jìn)SRUKF 2種濾波器在濾波前后的誤差均值和誤差均方根值，并統(tǒng)計(jì)了2種濾波算法的平均耗時(shí)。

圖2中藍(lán)色標(biāo)識為測量誤差分布曲線，紅色標(biāo)識為濾波后誤差分布曲線。由誤差分布曲線可見，濾波收斂速度較快，且誤差分布比較均勻，基本保持在零均值附近，符合高斯分布的特點(diǎn)。試驗(yàn)統(tǒng)計(jì)了SRUKF和改進(jìn)SRUKF 2種濾波器10 000次蒙特卡羅試驗(yàn)的濾波誤差值，統(tǒng)計(jì)結(jié)果如表1所示。從表1中可見，2種方法誤差統(tǒng)計(jì)值一致。試驗(yàn)表明，狀態(tài)和誤差協(xié)方差矩陣預(yù)測部分采用線性方程，與采用SRUKF的預(yù)測方程具有同等效果。

圖2 濾波誤差分布曲線

表1 SRUKF與改進(jìn)SRUKF試驗(yàn)誤差統(tǒng)計(jì)表

在MATLAB運(yùn)行環(huán)境下對SRUKF和改進(jìn)SRUKF 2種濾波器進(jìn)行了10000次蒙特卡羅的試驗(yàn)，統(tǒng)計(jì)了2種濾波器的平均單次運(yùn)算時(shí)間開銷，在SRUKF算法下平均耗時(shí)0.0553s，應(yīng)用改進(jìn)SRUKF算法平均耗時(shí)0.0431s。由統(tǒng)計(jì)試驗(yàn)可見，改進(jìn)算法的效率提高了20%左右。

將改進(jìn)的SRUKF算法移植應(yīng)用到DSP雷達(dá)信號處理平臺上，結(jié)合雷達(dá)前端模塊進(jìn)行了行人的檢測和跟蹤試驗(yàn)，結(jié)果如圖3所示。試驗(yàn)結(jié)果表明，改進(jìn)的SRUKF算法對毫米波雷達(dá)實(shí)測數(shù)據(jù)具有較強(qiáng)的適應(yīng)能力和較好的隨機(jī)誤差抑制效果。

4 結(jié)論

本文描述了SRUKF濾波器在汽車毫米波雷達(dá)目標(biāo)跟蹤上的應(yīng)用，同時(shí)，為了降低計(jì)算量和提高系統(tǒng)實(shí)時(shí)性，提出了一種改進(jìn)的快速SRUKF濾波器。該濾波器在預(yù)測階段，利用Kalman濾波器方程進(jìn)行狀態(tài)和誤

圖3 雷達(dá)實(shí)測數(shù)據(jù)濾波效果圖

差協(xié)方差預(yù)測，在更新階段，利用預(yù)測值構(gòu)造Sigma點(diǎn)，并以SRUKF濾波器方程更新目標(biāo)狀態(tài)和誤差協(xié)方差，減少了1次UT變換。MATLAB的算法仿真和數(shù)據(jù)統(tǒng)計(jì)表明，該算法運(yùn)算效率得到了提高，同時(shí)保持原有的濾波精度。算法移植到DSP平臺上后，實(shí)測中有較好的跟蹤效果和適應(yīng)能力。

［1］Michael Sachse，HAF Radar development，IAV，2014.

［2］Gustafsson F，Hendeby G.Some relations between extended and unscented Kalman filters［J］.IEEE Transactions on Signal Processing，2012，60（2）：545-555.

［3］Merwe R V D，Wan E A.The square-root unscented Kalman filter for state and parameter-estimation［C］. Icassp.2001：3 461-3 464.

［4］Heuer M，Al-Hamadi A，Rain A，et al.Pedestrian tracking with occlusion using a 24 GHz automotive radar［C］.International Radar Symposium.2014：1-4.

［5］劉華軍，賴少發(fā).汽車毫米波雷達(dá)目標(biāo)跟蹤的快速平方根CKF算法［J］.南京理工大學(xué)學(xué)報(bào)，2016，40（1）：56-60.

［6］肖雷，劉高峰，魏建仁.幾種動目標(biāo)運(yùn)動模型的跟蹤性能對比［J］.火力與指揮控制，2005，32（5）：106-109.

（編輯 陳程）

Target Tracking of Automotive M illimeter-W ave Radar Using SRUKF Filter

LIANG Bo，GUO Jian-ying
（Huayu Automotive Systems Co.，Ltd.，Shanghai 200041，China）

Square root unscented kalman filter（SRUKF）could solve the nonlinear estimation problem in filtering，and guarantees state covariance to be positive semi-definite during update process，thus is widely used in target tracking algorithm.This paper proposes a faster SRUKF algorithm which reduces computation than regular SRUKF without accuracy sacrifice，which is suitable for real-time signal processing on built-in vehicle radar module.

SRUKF filter；automotive radar；target tracking；radar data processing

463.675

A

1003－8639（2016）08－0042－03

2016-06-02；

2016-07-03

梁波（1982-），男，安徽巢湖人，碩士，長期從事汽車電子產(chǎn)品開發(fā)、嵌入式軟件設(shè)計(jì)和數(shù)字信號處理；郭劍鷹（1973-），男，江蘇張家港人，工學(xué)博士，長期從事計(jì)算機(jī)輔助制造、機(jī)器人及自動化、汽車電子產(chǎn)品開發(fā)、規(guī)劃及管理。