梁波，郭劍鷹

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

基于SRUKF的汽車毫米波雷達目標跟蹤方法

梁波，郭劍鷹

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

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

SRUKF濾波器；汽車雷達；目標跟蹤；雷達數(shù)據(jù)處理

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

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

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

1 系統(tǒng)模型

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

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

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

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

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

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

2 SRUKF算法的改進

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

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

1）初始化

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

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

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

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

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

5）計算卡爾曼增益

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

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

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

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

圖1 仿真場景

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

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

圖2 濾波誤差分布曲線

表1 SRUKF與改進SRUKF試驗誤差統(tǒng)計表

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

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

4 結(jié)論

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

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

差協(xié)方差預(yù)測，在更新階段，利用預(yù)測值構(gòu)造Sigma點，并以SRUKF濾波器方程更新目標狀態(tài)和誤差協(xié)方差，減少了1次UT變換。MATLAB的算法仿真和數(shù)據(jù)統(tǒng)計表明，該算法運算效率得到了提高，同時保持原有的濾波精度。算法移植到DSP平臺上后，實測中有較好的跟蹤效果和適應(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ā).汽車毫米波雷達目標跟蹤的快速平方根CKF算法［J］.南京理工大學學報，2016，40（1）：56-60.

［6］肖雷，劉高峰，魏建仁.幾種動目標運動模型的跟蹤性能對比［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è)計和數(shù)字信號處理；郭劍鷹（1973-），男，江蘇張家港人，工學博士，長期從事計算機輔助制造、機器人及自動化、汽車電子產(chǎn)品開發(fā)、規(guī)劃及管理。