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

梁波，郭劍鷹

（華域汽車系統股份有限公司，上海 200041）

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

梁波，郭劍鷹

（華域汽車系統股份有限公司，上海 200041）

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

SRUKF濾波器；汽車雷達；目標跟蹤；雷達數據處理

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

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

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

1 系統模型

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

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

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

過程噪聲協方差矩陣Q為對角陣

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

測量噪聲協方差矩陣R為對角陣

2 SRUKF算法的改進

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

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

1）初始化

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

2）狀態及誤差平方根一步預測

3）產生Sigma點并計算權值

4）量測值及協方差預測

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

5）計算卡爾曼增益

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

6）狀態及協方差更新

3 仿真、試驗及數據分析

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

圖1 仿真場景

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

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

圖2 濾波誤差分布曲線

表1 SRUKF與改進SRUKF試驗誤差統計表

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

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

4 結論

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

圖3 雷達實測數據濾波效果圖

差協方差預測，在更新階段，利用預測值構造Sigma點，并以SRUKF濾波器方程更新目標狀態和誤差協方差，減少了1次UT變換。MATLAB的算法仿真和數據統計表明，該算法運算效率得到了提高，同時保持原有的濾波精度。算法移植到DSP平臺上后，實測中有較好的跟蹤效果和適應能力。

［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］劉華軍，賴少發.汽車毫米波雷達目標跟蹤的快速平方根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-），男，安徽巢湖人，碩士，長期從事汽車電子產品開發、嵌入式軟件設計和數字信號處理；郭劍鷹（1973-），男，江蘇張家港人，工學博士，長期從事計算機輔助制造、機器人及自動化、汽車電子產品開發、規劃及管理。