本文詳細描述了卡爾曼濾波器在汽車毫米波距離測量應用中的工作原理，詳細介紹了卡爾曼的工作原理，實際測量中的噪聲來源，并在搭建好模型后運用MATLAB分析驗證了模型的有效性。

毫米波雷達對待測目標進行間隔時間為T的一系列測量，獲得距離，速度和角度信息，這些測量信息中因為包含測量噪聲，所以我們不能完全采信這些測量結果。同時，我們假設待測目標按照勻加速度模型進行運動，那么當前狀態下的待測目標距離可以按照待測目標上一狀態的距離，速度信息依據牛頓線性模型計算得出，但是這種牛頓線性理想模型未能考慮其他能破壞模型的外力作用，比如待測目標的動力輸出不可能是恒定不變的，地面阻力以及風的阻力也不是恒定不變的，因此按照牛頓線性模型推算的結果是缺損的，未能考慮其他外力作用。卡爾曼濾波器可以很好的平衡含冗余噪聲的測量結果和缺少信息的過于理想的計算結果，并實現對待測目標的準確跟蹤。

1、卡爾曼濾波介紹

卡爾曼早期最成功的應用案例是在阿波羅登月計劃中成功解決了軌道預測問題。由于卡爾曼濾波是一種時域濾波方法，其算法采用遞歸形式，便于在計算機上實現，目前，卡爾曼濾波被廣泛應用在慣性制導，地圖導航，雷達測量等各個領域。總結起來，卡爾曼能解決的問題特征為：一是不能對待測目標進行準確建模，外力作用不穩定，但是掌握外力作用效果的統計值；二是雖然可進行一系列的測量，但是測量結果因含有噪聲而不能完全采信。

毫米波雷達測量示意圖如圖1所示，假設雷達的測量周期T，時刻nT的測量向量記為u(n),

    (1)

r(n)代表目標相對于雷達的徑向距離，φ(n)代表目標相對于雷達角度，代表目標相對于雷達徑向速度。由于毫米波雷達測量得到的距離速度等信息都是徑向的，因此測量向量u(n)是在極坐標系下獲得的。

圖1、雷達目標測量示意圖

但是為了便于利用簡單的牛頓線性預測模型對目標進行跟蹤，也即對待測目標連續兩次時間間隔為T的兩個狀態進行關聯，我們需要獲得笛卡爾坐標系下待測目標的距離，速度等信息。由于測量是在極坐標系中而狀態跟蹤是在笛卡爾坐標系，因此需要進行坐標系轉換。由于這種坐標系的轉換是非線性的，因此需要使用擴展卡爾曼濾波器。      

假設笛卡爾坐標系下待測目標在時刻nT的狀態向量記為S(n):

    (2)

其中x(n),,分別代表nT時刻待測目標在x軸方向上的位置，速度和加速度。y(n),,分別代表nT時刻待測目標在y軸方向上的位置，速度和加速度。

卡爾曼濾波器算法是按照遞歸工作的，假設已知待測目標的狀態向量S(n-1)，待測目標按照勻加速度進行運動。利用牛頓線性模型得到的從狀態S(n-1)到狀態S(n)的轉移矩陣定義為F：

    (3)

基于轉移矩陣F和(n-1)T時刻待測目標的狀態向量S(n-1)，我們可以得到nT時刻的待測目標的狀態估計向量S_apr (n)如下：

S_apr (n)=F*S(n-1)    (4)

由于勻加速度模型過于理想，不符合待測目標的實際運行狀態，也即和無法保持恒定不變，因此S_apr (n)獲得的距離信息是缺失的，未能考慮其他破壞勻加速度模型的外力作用，我們假設這些外力導致待測目標在x軸和y軸上加速度變化為w_x(n)和w_y(n)，那么考慮外力作用后的待測目標的狀態向量S(n)如下：

    (5)

    (6)

    (7)

定義w(n)引入的狀態變量變化的協方差矩陣Q(n)，其中w_x，w_y為省略下標n的簡寫，

    (8)

S_apr (n)中完全不含有τw(n-1)導致的狀態變化，由于無法準確量化w(n-1)，也可以理解S(n)始終不能完全涵蓋τw(n-1)，實際上S(n)中還未考慮狀態向量S(n-1)中缺失的信息，我們定義S(n)中缺少的信息總和為d(n)，由于同樣無法準確計算d(n)，我們期望通過遞歸收斂的方法解決d(n)。定義d(n)的協方差矩陣為P(n)=E[d(n) d(n)^T ]，P(n)矩陣大小為6*6。假設我們已知P(n-1)，那么可以推算出P_apr (n)，公式如下：

P_apr (n)=F*P(n-1)*F^T+Q(n-1)    (9)

F*P(n-1)* F^T可以理解為S(n-1)狀態缺少的信息通過轉移矩陣F到狀態S(n)時產生的缺少信息的協方差矩陣，Q(n-1)為外力在時刻(n-1)T到時刻nT時間內引入的狀態協方差矩陣。

測試向量u(n)和我們要求解的狀態向量S(n)描述的是同一個待測目標，其關系式如下：

u(n)=H(S_apr (n) )+J_H (S_apr (n) )[S(n)-S_apr (n) ]+v(n)    (10)

    (11)

其中H(S_apr (n))表示將笛卡爾坐標系下的向量S_apr (n)轉換到極坐標系中，在上述公式中，測量向量u(n)通過非線性關系與狀態向量S(n)相關。因此，我們使用擴展卡爾曼濾波器，通過僅保留泰勒級數展開式中的第一項，簡化了u(n)和S(n)之間的關系，偏導數矩陣J_H (S(n) )定義如下：

    (12)

定義y(n)=u(n)-H(S_apr (n))，y(n)有兩部分組成，一是測量噪聲v(n)，定義測量誤差v(n)的協方差矩陣為R(n)=E[v(n)*v(n)^T]，R(n)矩陣大小為3*3。二是J_H (S_apr (n) )[S(n)-S_apr (n) ]這是因為模型過于理想而導致的信息缺失。因此我們可以求得y(n)的協方差矩陣為

    (13)

其中和R(n)在統計特性上相互獨立，因此可以定義卡爾曼增益為

    (14)

矩陣運算會讓卡爾曼增益的理解變得困難，如果是標量運算，卡爾曼增益將變得簡單，C(n)可以理解為缺失信息和測量誤差總和，K(n)就是缺失信息占C(n)的比例。獲得卡爾曼增益后更新狀態向量S(n)：

S(n)=S_apr (n)+K(n)y(n)   (15)

K(n)y(n)是根據卡爾曼增益，從y(n)中得到的缺失信息，由于y(n)中既混合測量噪聲又包括缺失信息，因此并不能從K(n)y(n)中獲得缺失信息并屏蔽測量噪聲，實際上缺失信息和測量噪聲按照同樣的比例關系（卡爾曼增益）被更新到狀態向量S(n)中。所以S(n)中混合了部分測量噪聲而且仍然是信息缺失的，缺失的信息更新如下：

P(n)=P_apr (n)-K(n)J_H (S_apr (n))P_apr (n)   (16)

K(n)J_H (S_apr (n))P_apr (n)代表S(n)中已經通過K(n)y(n)獲得信息，那么P(n)就代表狀態向量S(n)中殘留缺失信息，用于下一次迭代。也可以認為我們通過卡爾曼濾波器從新獲得了部分缺失信息K(n)J_H (S_apr (n))P_apr (n)，更新后的P(n)代表S(n)尚缺少的信息，將被用于S(n+1)次的狀態迭代。

2、實踐中的修正

在圖2中，虛線圓是我們要跟蹤的圓形待測目標，實心小圓點是待測目標上的雷達反射點，實心方形小點是實心圓的平均值，在圖2中圓形待測目標按照虛直線運動，而由于我們待測目標上測量點的漂移，實際得到的待測目標的運行軌跡是實折線。因此公式y(n)=u(n)-H(S_apr (n))將不僅僅包含測量噪聲，在實際測量中還需要考慮測量點漂移帶來的噪聲。解決辦法是將測量點聚類后，動態估計待測目標的大小，因為測量點只能在待測目標上漂移，可以將待測目標大小乘以0.25作為測量點漂移誤差。

圖2、測量點漂移示意圖

3、雷達簡介

首先采用NXP公司的TEF8102和S32R274實現了一款77GHz毫米波長距雷達，采用FMCW調制方式，具體設計參數配置如表1所示，一次測量序列由128個啁啾組成，每個啁啾內采樣512點，啁啾內模數轉換器采樣速率為20MSPS，一個啁啾持續時間為45us，128個啁啾組成一個序列完成一次測量，128個啁啾持續時間為45us*128=5.76毫秒，加上額外的處理時間，本雷達設計完成一次距離，速度和角度的測量周期為30ms。也即每30ms雷達會上報一次待測目標的距離，速度和角度信息，由于一次測量周期30ms相對較短，我們假設在30ms之內，待測目標按照勻加速度進行運動。

表1、77GHz毫米波長距雷達參數配置表

參數	數值
載波頻率（Hz）	7.70E+10
載波波長（米）	3.90E-03
采樣間隔（秒）	5.00E-08
啁啾周期（秒）	4.50E-05
啁啾采樣周期（秒）	2.56E-05
單啁啾采樣點數	512
啁啾個數	128
掃頻帶寬（Hz）	2.00E+08
距離分辨率（米）	7.50E-01
速度分辨率（米/秒）	3.38E-01
無模糊最大速度（米/秒）	2.16E+01
固定單元格最大速度（米/秒）	6.51E+01
無模糊最大距離（米）	1.92E+02

4、仿真驗證

為了驗證算法的效果，需要將算法輸出結果和基準事實進行對比，在實際測試中只能獲得實測結果而無法獲得基準數據，因此算法的驗證通過生成仿真數據的方法。假設待測目標長10米，寬4米，高2米的矩形，按照距離，角度和速度生成1000點測試點，測試點均勻分布于待測目標內，在距離，角度和速度上加高斯白噪，從1000點鐘隨機挑選10個測試點作為模型的輸入數據，待測目標按照勻加速運動。圖3橫軸代表測量序列，每30ms生成一次仿真數據輸入模型，縱軸代表相對誤差，單位是百分比，為了直觀顯示數據，對縱軸數據進行放大，縱軸數據2.5對應誤差為100%。由圖可知真實測量誤差（矩形測量點）遠遠高于卡爾曼濾波后的誤差（圖中圓形測量點），顯示模型能夠有效的對待測目標進行跟蹤測量，顯示出卡爾曼濾波器在毫米波雷達測量中發揮了重要作用。

圖3、卡爾曼濾波效果對比

5、結論

本文詳細介紹了卡爾曼濾波器在毫米波雷達測量領域中的應用，分析了毫米波雷達測量中的誤差來源，最后通過matlab仿真驗證，結果也證明卡爾曼濾波器可大幅降低毫米波雷達的測量誤差。

6、參考

黃小平王巖, 卡爾曼濾波原理及應用, pp. 85, 電子工業出版社

作者：王洪勝、王堯青、張秋英
安富利電子元件（北京射頻與微波實驗室）

本文MWRF.NET獨家版權所有，未經允許不得轉載！