本文詳細(xì)描述了卡爾曼濾波器在汽車毫米波距離測(cè)量應(yīng)用中的工作原理，詳細(xì)介紹了卡爾曼的工作原理，實(shí)際測(cè)量中的噪聲來(lái)源，并在搭建好模型后運(yùn)用MATLAB分析驗(yàn)證了模型的有效性。

毫米波雷達(dá)對(duì)待測(cè)目標(biāo)進(jìn)行間隔時(shí)間為T的一系列測(cè)量，獲得距離，速度和角度信息，這些測(cè)量信息中因?yàn)榘瑴y(cè)量噪聲，所以我們不能完全采信這些測(cè)量結(jié)果。同時(shí)，我們假設(shè)待測(cè)目標(biāo)按照勻加速度模型進(jìn)行運(yùn)動(dòng)，那么當(dāng)前狀態(tài)下的待測(cè)目標(biāo)距離可以按照待測(cè)目標(biāo)上一狀態(tài)的距離，速度信息依據(jù)牛頓線性模型計(jì)算得出，但是這種牛頓線性理想模型未能考慮其他能破壞模型的外力作用，比如待測(cè)目標(biāo)的動(dòng)力輸出不可能是恒定不變的，地面阻力以及風(fēng)的阻力也不是恒定不變的，因此按照牛頓線性模型推算的結(jié)果是缺損的，未能考慮其他外力作用。卡爾曼濾波器可以很好的平衡含冗余噪聲的測(cè)量結(jié)果和缺少信息的過(guò)于理想的計(jì)算結(jié)果，并實(shí)現(xiàn)對(duì)待測(cè)目標(biāo)的準(zhǔn)確跟蹤。

1、卡爾曼濾波介紹

卡爾曼早期最成功的應(yīng)用案例是在阿波羅登月計(jì)劃中成功解決了軌道預(yù)測(cè)問(wèn)題。由于卡爾曼濾波是一種時(shí)域?yàn)V波方法，其算法采用遞歸形式，便于在計(jì)算機(jī)上實(shí)現(xiàn)，目前，卡爾曼濾波被廣泛應(yīng)用在慣性制導(dǎo)，地圖導(dǎo)航，雷達(dá)測(cè)量等各個(gè)領(lǐng)域。總結(jié)起來(lái)，卡爾曼能解決的問(wèn)題特征為：一是不能對(duì)待測(cè)目標(biāo)進(jìn)行準(zhǔn)確建模，外力作用不穩(wěn)定，但是掌握外力作用效果的統(tǒng)計(jì)值；二是雖然可進(jìn)行一系列的測(cè)量，但是測(cè)量結(jié)果因含有噪聲而不能完全采信。

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

    (1)

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

圖1、雷達(dá)目標(biāo)測(cè)量示意圖

但是為了便于利用簡(jiǎn)單的牛頓線性預(yù)測(cè)模型對(duì)目標(biāo)進(jìn)行跟蹤，也即對(duì)待測(cè)目標(biāo)連續(xù)兩次時(shí)間間隔為T的兩個(gè)狀態(tài)進(jìn)行關(guān)聯(lián)，我們需要獲得笛卡爾坐標(biāo)系下待測(cè)目標(biāo)的距離，速度等信息。由于測(cè)量是在極坐標(biāo)系中而狀態(tài)跟蹤是在笛卡爾坐標(biāo)系，因此需要進(jìn)行坐標(biāo)系轉(zhuǎn)換。由于這種坐標(biāo)系的轉(zhuǎn)換是非線性的，因此需要使用擴(kuò)展卡爾曼濾波器。      

假設(shè)笛卡爾坐標(biāo)系下待測(cè)目標(biāo)在時(shí)刻nT的狀態(tài)向量記為S(n):

    (2)

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

卡爾曼濾波器算法是按照遞歸工作的，假設(shè)已知待測(cè)目標(biāo)的狀態(tài)向量S(n-1)，待測(cè)目標(biāo)按照勻加速度進(jìn)行運(yùn)動(dòng)。利用牛頓線性模型得到的從狀態(tài)S(n-1)到狀態(tài)S(n)的轉(zhuǎn)移矩陣定義為F：

    (3)

基于轉(zhuǎn)移矩陣F和(n-1)T時(shí)刻待測(cè)目標(biāo)的狀態(tài)向量S(n-1)，我們可以得到nT時(shí)刻的待測(cè)目標(biāo)的狀態(tài)估計(jì)向量S_apr (n)如下：

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

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

    (5)

    (6)

    (7)

定義w(n)引入的狀態(tài)變量變化的協(xié)方差矩陣Q(n)，其中w_x，w_y為省略下標(biāo)n的簡(jiǎn)寫(xiě)，

    (8)

S_apr (n)中完全不含有τw(n-1)導(dǎo)致的狀態(tài)變化，由于無(wú)法準(zhǔn)確量化w(n-1)，也可以理解S(n)始終不能完全涵蓋τw(n-1)，實(shí)際上S(n)中還未考慮狀態(tài)向量S(n-1)中缺失的信息，我們定義S(n)中缺少的信息總和為d(n)，由于同樣無(wú)法準(zhǔn)確計(jì)算d(n)，我們期望通過(guò)遞歸收斂的方法解決d(n)。定義d(n)的協(xié)方差矩陣為P(n)=E[d(n) d(n)^T ]，P(n)矩陣大小為6*6。假設(shè)我們已知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)狀態(tài)缺少的信息通過(guò)轉(zhuǎn)移矩陣F到狀態(tài)S(n)時(shí)產(chǎn)生的缺少信息的協(xié)方差矩陣，Q(n-1)為外力在時(shí)刻(n-1)T到時(shí)刻nT時(shí)間內(nèi)引入的狀態(tài)協(xié)方差矩陣。

測(cè)試向量u(n)和我們要求解的狀態(tài)向量S(n)描述的是同一個(gè)待測(cè)目標(biāo)，其關(guā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))表示將笛卡爾坐標(biāo)系下的向量S_apr (n)轉(zhuǎn)換到極坐標(biāo)系中，在上述公式中，測(cè)量向量u(n)通過(guò)非線性關(guān)系與狀態(tài)向量S(n)相關(guān)。因此，我們使用擴(kuò)展卡爾曼濾波器，通過(guò)僅保留泰勒級(jí)數(shù)展開(kāi)式中的第一項(xiàng)，簡(jiǎn)化了u(n)和S(n)之間的關(guān)系，偏導(dǎo)數(shù)矩陣J_H (S(n) )定義如下：

    (12)

定義y(n)=u(n)-H(S_apr (n))，y(n)有兩部分組成，一是測(cè)量噪聲v(n)，定義測(cè)量誤差v(n)的協(xié)方差矩陣為R(n)=E[v(n)*v(n)^T]，R(n)矩陣大小為3*3。二是J_H (S_apr (n) )[S(n)-S_apr (n) ]這是因?yàn)槟Ｐ瓦^(guò)于理想而導(dǎo)致的信息缺失。因此我們可以求得y(n)的協(xié)方差矩陣為

    (13)

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

    (14)

矩陣運(yùn)算會(huì)讓卡爾曼增益的理解變得困難，如果是標(biāo)量運(yùn)算，卡爾曼增益將變得簡(jiǎn)單，C(n)可以理解為缺失信息和測(cè)量誤差總和，K(n)就是缺失信息占C(n)的比例。獲得卡爾曼增益后更新?tīng)顟B(tài)向量S(n)：

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

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

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)中已經(jīng)通過(guò)K(n)y(n)獲得信息，那么P(n)就代表狀態(tài)向量S(n)中殘留缺失信息，用于下一次迭代。也可以認(rèn)為我們通過(guò)卡爾曼濾波器從新獲得了部分缺失信息K(n)J_H (S_apr (n))P_apr (n)，更新后的P(n)代表S(n)尚缺少的信息，將被用于S(n+1)次的狀態(tài)迭代。

2、實(shí)踐中的修正

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

圖2、測(cè)量點(diǎn)漂移示意圖

3、雷達(dá)簡(jiǎn)介

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

表1、77GHz毫米波長(zhǎng)距雷達(dá)參數(shù)配置表

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

4、仿真驗(yàn)證

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

圖3、卡爾曼濾波效果對(duì)比

5、結(jié)論

本文詳細(xì)介紹了卡爾曼濾波器在毫米波雷達(dá)測(cè)量領(lǐng)域中的應(yīng)用，分析了毫米波雷達(dá)測(cè)量中的誤差來(lái)源，最后通過(guò)matlab仿真驗(yàn)證，結(jié)果也證明卡爾曼濾波器可大幅降低毫米波雷達(dá)的測(cè)量誤差。

6、參考

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

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

本文MWRF.NET獨(dú)家版權(quán)所有，未經(jīng)允許不得轉(zhuǎn)載！