一種基于多級(jí)Kalman濾波的高精度距離估計(jì)方法

唐 玲，杜雨洺

(成都信息工程學(xué)院電子工程學(xué)院，四川成都610225)

0 引言

在對(duì)慢速運(yùn)動(dòng)目標(biāo)進(jìn)行距離估計(jì)時(shí)，由于目標(biāo)運(yùn)動(dòng)速度過慢，短時(shí)間內(nèi)可以認(rèn)為其處于靜止?fàn)顟B(tài)，在強(qiáng)干擾環(huán)境中，目標(biāo)測(cè)量誤差可能遠(yuǎn)遠(yuǎn)大于其實(shí)際運(yùn)動(dòng)的距離，在這一情況下，測(cè)量值可能很難落在誤差允許范圍之內(nèi)，從而造成數(shù)據(jù)無法真實(shí)反應(yīng)目標(biāo)運(yùn)動(dòng)軌跡[1]。在真實(shí)復(fù)雜環(huán)境下，由于障礙物和多徑干擾的原因，測(cè)量值中可能出現(xiàn)大量野值點(diǎn)，直接對(duì)其進(jìn)行濾波會(huì)導(dǎo)致濾波估計(jì)值明顯地偏離系統(tǒng)真實(shí)狀態(tài)，導(dǎo)致目標(biāo)運(yùn)動(dòng)軌跡失真[2]。因此研究對(duì)慢速目標(biāo)的高精度距離估計(jì)具有重要的實(shí)際意義。

通過對(duì)慢速運(yùn)動(dòng)目標(biāo)的特點(diǎn)進(jìn)行分析，提出一種兩級(jí)Kalman濾波的距離估計(jì)方法，以被測(cè)目標(biāo)的當(dāng)前時(shí)刻距離測(cè)量值為輸入，對(duì)一分鐘內(nèi)的測(cè)量值取均值進(jìn)行差值檢測(cè)，上一時(shí)刻最優(yōu)估計(jì)與當(dāng)前時(shí)刻測(cè)量值的絕對(duì)差作為差值門限檢測(cè)的標(biāo)準(zhǔn)距離差以判斷下一時(shí)刻數(shù)據(jù)點(diǎn)是否為野值點(diǎn)，計(jì)算當(dāng)前時(shí)刻與標(biāo)準(zhǔn)距離的差值，對(duì)當(dāng)前時(shí)刻落在門限范圍外的值進(jìn)行修正，以修正后的數(shù)據(jù)作為第一級(jí)Kalman濾波的初始狀態(tài)向量，這樣避免了野值點(diǎn)對(duì)濾波效果的不利影響，同時(shí)不會(huì)對(duì)距離變化響應(yīng)時(shí)間有較大影響。

1 卡爾曼濾波

1.1 Kalman濾波理論

Kalman濾波器是線性、無偏、最小方差估計(jì)器，是一種通過迭代算法實(shí)現(xiàn)的估算器[3]，原理是從觀測(cè)到的向量中對(duì)隨著時(shí)間不斷變化的狀態(tài)向量進(jìn)行估計(jì)，并且希望估計(jì)值與狀態(tài)向量的真實(shí)值的誤差越小越好[4]，Kalman濾波在處理動(dòng)態(tài)觀測(cè)數(shù)據(jù)時(shí)以其計(jì)算量少及實(shí)時(shí)性在很多方面得到了廣泛應(yīng)用[5]。在跟蹤運(yùn)動(dòng)目標(biāo)的系統(tǒng)中，可以把運(yùn)動(dòng)目標(biāo)的位置信號(hào)看成一個(gè)離散動(dòng)態(tài)系統(tǒng)，用k+1時(shí)刻的目標(biāo)狀態(tài)方程X(k+1)和測(cè)量方程Z(k)描述該系統(tǒng)。

式中，X(k)為k時(shí)刻的目標(biāo)狀態(tài)向量，W(k)為k時(shí)刻目標(biāo)運(yùn)動(dòng)過程噪聲，U(k)為k時(shí)刻對(duì)系統(tǒng)的控制量，A、B、H為系統(tǒng)參數(shù)，Z(k)為k時(shí)刻觀測(cè)值，V(k)為k時(shí)刻測(cè)量噪聲。在測(cè)距系統(tǒng)中有

(1){W(k)}、{V(k)}是均值為零的高斯白噪聲隨機(jī)序列，即

(2)狀態(tài)向量、觀測(cè)噪聲、過程噪聲互不相關(guān)，即

式中，k，j均表示時(shí)刻，上角標(biāo)T表示矩陣的轉(zhuǎn)置。

Kalman濾波是結(jié)合當(dāng)前時(shí)刻的測(cè)量值，對(duì)上一時(shí)刻的預(yù)測(cè)進(jìn)行校正，得到校正后的最優(yōu)估計(jì)，該估計(jì)具有最小方差。一般來說，Kalman濾波分為兩個(gè)步驟:預(yù)測(cè)、校正。預(yù)測(cè)即對(duì)時(shí)間狀態(tài)向量進(jìn)行更新，校正即對(duì)測(cè)量狀態(tài)向量進(jìn)行更新[6]。當(dāng)Q、R值確定時(shí)，由下列遞推公式得到目標(biāo)的狀態(tài)估計(jì)值:

1.2 多級(jí)Kalman濾波方法應(yīng)用于慢速運(yùn)動(dòng)目標(biāo)距離估計(jì)

在對(duì)慢速運(yùn)動(dòng)目標(biāo)距離估計(jì)數(shù)據(jù)處理時(shí)，經(jīng)典Kalman濾波存在不足。

(1)在應(yīng)用Kalman對(duì)數(shù)據(jù)進(jìn)行濾波處理時(shí)，只需要知道被研究對(duì)象的數(shù)學(xué)模型和噪聲統(tǒng)計(jì)的先驗(yàn)知識(shí)，并不考慮測(cè)量值是否有現(xiàn)實(shí)意義，即只需要根據(jù)驗(yàn)前確定的系統(tǒng)參數(shù)和噪聲方差的初始值，按照濾波公式遞推下一時(shí)刻的估計(jì)值[8]。當(dāng)目標(biāo)運(yùn)動(dòng)狀態(tài)量中有野值點(diǎn)出現(xiàn)時(shí)，Kalman濾波也會(huì)跟蹤野值，這對(duì)估計(jì)結(jié)果有很大影響[9]。因此，直接對(duì)測(cè)量值進(jìn)行Kalman濾波可以剔除孤立型野值點(diǎn)，但是對(duì)連續(xù)型野值點(diǎn)進(jìn)行剔除時(shí)[10]，由于Kalman濾波也會(huì)對(duì)每一個(gè)野值點(diǎn)按權(quán)重累積，這對(duì)濾波結(jié)果影響很大，可能嚴(yán)重降低濾波精度。

(2)慢速運(yùn)動(dòng)目標(biāo)的特點(diǎn)是目標(biāo)與觀測(cè)點(diǎn)之間的相對(duì)運(yùn)動(dòng)速度很慢，若觀測(cè)點(diǎn)處于靜止?fàn)顟B(tài)，短時(shí)間內(nèi)可以忽略目標(biāo)運(yùn)動(dòng)速度，把運(yùn)動(dòng)目標(biāo)看成是靜止?fàn)顟B(tài)建模，這樣系統(tǒng)具有可觀測(cè)性[11]，但是當(dāng)目標(biāo)運(yùn)動(dòng)到一定距離時(shí)，那么測(cè)量值偏離真實(shí)值的誤差有可能遠(yuǎn)遠(yuǎn)大于目標(biāo)實(shí)際運(yùn)動(dòng)的距離，因此目標(biāo)的運(yùn)動(dòng)給測(cè)量帶來了誤差[12]。在這種情況下，經(jīng)過Kalman濾波后卻不能立即在數(shù)據(jù)上反應(yīng)出來，即不能在實(shí)時(shí)對(duì)距離變化做出響應(yīng)，因此不能判斷目標(biāo)狀態(tài)是否發(fā)生改變。

(3)對(duì)于慢速運(yùn)動(dòng)目標(biāo)而言，當(dāng)改變?yōu)V波參數(shù)提高測(cè)距精度時(shí)，相鄰估計(jì)值之間的差值受濾波精度影響，如果被測(cè)目標(biāo)位置發(fā)生變化，一個(gè)數(shù)據(jù)率時(shí)間內(nèi)變化的距離值為厘米級(jí)以下，濾波精度過高時(shí)被平滑處理，不能在數(shù)據(jù)上體現(xiàn)出距離的變化。因此，濾波精度越高系統(tǒng)反應(yīng)時(shí)間越長，可以看出系統(tǒng)反應(yīng)時(shí)間和濾波精度相互影響并且相互矛盾。

考慮到以上幾個(gè)問題，采用多級(jí)Kalman濾波來對(duì)慢速運(yùn)動(dòng)目標(biāo)距離估計(jì)數(shù)據(jù)進(jìn)行處理，通過改變第一級(jí)和第二級(jí)濾波參數(shù)來調(diào)和系統(tǒng)反應(yīng)時(shí)間和濾波精度不能同時(shí)優(yōu)化的矛盾。Kalman濾波前對(duì)初始值進(jìn)行預(yù)處理，取一組數(shù)據(jù)的均值進(jìn)行野值點(diǎn)的判定，當(dāng)數(shù)據(jù)超過判定門限時(shí)，用一個(gè)分段函數(shù)加權(quán)改變新舊數(shù)據(jù)比重，降低漏判和誤判的概率;否則，進(jìn)行第一次濾波。取第一級(jí)Kalman濾波結(jié)果的最小值作為第二級(jí)濾波的先驗(yàn)估計(jì)值，從而得到最優(yōu)估計(jì)，這樣既減小了濾波誤差，同時(shí)在不同場(chǎng)合將反應(yīng)時(shí)間控制在允許范圍內(nèi)。多級(jí)Kalman濾波流程如圖1所示。

圖1 多級(jí)Kalman濾波流程圖

2 實(shí)驗(yàn)仿真及結(jié)果分析

以德國Nanotron公司的nanoLOC通訊測(cè)距模塊[13]為例，利用實(shí)驗(yàn)數(shù)據(jù)仿真驗(yàn)證多級(jí)Kalman濾波方法的有效性。理論上，nanoLOC距離估計(jì)精度在3 m左右[14]，這在對(duì)精度要求很高的環(huán)境下不能滿足要求。為了驗(yàn)證多級(jí)Kalman濾波在提高慢速目標(biāo)距離估計(jì)精度的效果，以15 m實(shí)際觀測(cè)值作為一級(jí)Kalman濾波的先驗(yàn)估計(jì)值，在MATLAB上進(jìn)行仿真實(shí)驗(yàn)。該模塊的數(shù)據(jù)率為0.02 s，目標(biāo)的速度0.001 m/s，在二維平面里，目標(biāo)的運(yùn)動(dòng)距離為2×10－5m，而測(cè)量值偏離真實(shí)值的距離(測(cè)量誤差)為0.6～3 m，測(cè)量誤差遠(yuǎn)遠(yuǎn)大于目標(biāo)的運(yùn)動(dòng)距離。

2.1 濾波模型建立

Kalman濾波最主要的一步就是建立系統(tǒng)的狀態(tài)模型，在實(shí)際應(yīng)用中很難對(duì)系統(tǒng)進(jìn)行精確的描述，因此在實(shí)際中常用近似模型代替。為了數(shù)據(jù)精度得到較大改善的同時(shí)不影響響應(yīng)時(shí)間，設(shè)計(jì)了兩級(jí)Kalman濾波，第一級(jí)濾波對(duì)系統(tǒng)測(cè)量得到的距離計(jì)數(shù)據(jù)進(jìn)行第一次估計(jì)，得到改進(jìn)后的估計(jì)值，這個(gè)估計(jì)值作為第二級(jí)濾波的先驗(yàn)估計(jì)值，進(jìn)行第二級(jí)濾波，最終完成對(duì)距離信息的完整估計(jì)。

2.2 主要參數(shù)選取

在實(shí)際工程中，濾波初始值和估計(jì)誤差方差初值都會(huì)影響系統(tǒng)的穩(wěn)定性，且精確的初始值可以獲得最優(yōu)的濾波估計(jì)值。如果初始值選取不恰當(dāng)，長時(shí)間之后，可能導(dǎo)致濾波發(fā)散，所以選取適合的濾波參數(shù)對(duì)濾波精度起著至關(guān)重要的作用。

2.2.1 濾波初始值選定

在距離測(cè)量系統(tǒng)中，由于噪聲的不確定性，濾波初始值對(duì)整體濾波效果的影響很大，Kalman濾波器中初始值為后續(xù)估計(jì)的起始參考值，唯一地確定了估計(jì)模型的起始估計(jì)點(diǎn)，的誤差將直接影響濾波器的收斂速度，誤差越大則濾波器的收斂速度越慢。因此，為了確保數(shù)據(jù)較快的收斂初始值應(yīng)盡可能的準(zhǔn)確，取系統(tǒng)開啟后前5分鐘的數(shù)據(jù)均值，即=，其中N為5分鐘內(nèi)數(shù)據(jù)樣本量。

2.2.2 過程噪聲方差Q值選取

過程噪聲方差Q反應(yīng)連續(xù)兩個(gè)時(shí)刻的距離方差，它的好壞對(duì)濾波精度有直接的影響，Q值選取的一個(gè)基本原則是使Q值大小與動(dòng)態(tài)模型的精度相匹配。由于距離測(cè)量系統(tǒng)的目標(biāo)不斷運(yùn)動(dòng)，外界環(huán)境也在不斷發(fā)生變化，所以采用自適應(yīng)的方法估計(jì)過程噪聲方差[15]。

通過對(duì)實(shí)驗(yàn)數(shù)據(jù)的分析可知，一級(jí)Kalman濾波中Q值應(yīng)該比二級(jí)Kalman濾波中Q值小一個(gè)數(shù)量級(jí)，通過先進(jìn)行較低精度濾波對(duì)距離數(shù)據(jù)中差距較大的值進(jìn)行粗略的估計(jì)，在進(jìn)行較高精度的濾波獲取較好的估計(jì)，這樣達(dá)到對(duì)濾波精度的較好控制。最終選取Q1=0.85 × 10－3，Q2=10－4。

2.2.3 測(cè)量噪聲方差R值選取

測(cè)量噪聲方差R同樣是很重要的一個(gè)參數(shù)，反應(yīng)目標(biāo)距離的測(cè)量精度，通過對(duì)測(cè)量數(shù)據(jù)和距離測(cè)量系統(tǒng)動(dòng)力學(xué)方程建模得知，當(dāng)R=3時(shí)，系統(tǒng)的收斂性和測(cè)量精度能達(dá)到較好的平衡。

2.3 仿真實(shí)驗(yàn)及數(shù)據(jù)分析

2.3.1 濾波精度

圖2為經(jīng)nanoLOC通訊測(cè)距模塊測(cè)量得到的原始測(cè)量信息，可以看出由測(cè)距模塊測(cè)量得到的數(shù)據(jù)中斑點(diǎn)型野值點(diǎn)較多，導(dǎo)致測(cè)量數(shù)據(jù)誤差高達(dá)3.6 m;由前面分析可知，直接進(jìn)行Kalman濾波濾除斑點(diǎn)型野值點(diǎn)可能導(dǎo)致系統(tǒng)對(duì)距離變化的反應(yīng)時(shí)間變慢，因此對(duì)圖2中的數(shù)據(jù)進(jìn)行差值檢測(cè)，圖3經(jīng)差值檢測(cè)后的數(shù)據(jù)即第一級(jí)Kalman濾波的初始狀態(tài)值，從圖3看出剔除原始數(shù)據(jù)中斑點(diǎn)型野值點(diǎn)后誤差從3.6 m減小到1.78 m，誤差下降1.82 m;圖4為經(jīng)第一級(jí)Kalman濾波后的數(shù)據(jù)，誤差從1.78 m減小到0.87 m，下降0.91 m;圖5為經(jīng)過第二級(jí)Kalman濾波后的距離信息，此時(shí)誤差已經(jīng)減小到0.45 m。

對(duì)比分析可以看出，Kalman濾波的方法可以有效地對(duì)數(shù)據(jù)進(jìn)行預(yù)估和校正，使輸出變得更平滑。表1為不同時(shí)期濾波精度值。

圖2 原始距離信息

圖3 差值檢測(cè)后的值

圖4 一級(jí)Kalman濾波后的結(jié)果

圖5 二級(jí)Kalman濾波后的結(jié)果

表1 各階段濾波數(shù)據(jù)精度值比較

2.3.2 反應(yīng)時(shí)間

對(duì)慢速運(yùn)動(dòng)目標(biāo)而言，系統(tǒng)反應(yīng)時(shí)間是很重要的指標(biāo)，決定了系統(tǒng)是否能及時(shí)的對(duì)目標(biāo)位置變化做出反應(yīng)。以初始距離12.5 m為例，系統(tǒng)處于穩(wěn)定狀態(tài)時(shí)移動(dòng)一段距離(測(cè)得移動(dòng)距離為0.8 m)，若反應(yīng)時(shí)間在10分鐘以內(nèi)則認(rèn)為符合要求，對(duì)測(cè)量誤差和系統(tǒng)反應(yīng)時(shí)間進(jìn)行分析。圖6為原始距離信息，在距離變化前測(cè)量誤差為2.12 m，經(jīng)過5.42分鐘系統(tǒng)對(duì)該變化做出反應(yīng)，并且距離變化后的誤差為3.23 m。可以看出測(cè)量值偏離真實(shí)值的誤差遠(yuǎn)大于目標(biāo)實(shí)際運(yùn)動(dòng)的距離，對(duì)于慢速運(yùn)動(dòng)目標(biāo)來說，不能達(dá)到測(cè)量要求。經(jīng)過多級(jí)Kalman濾波處理后，測(cè)量誤差越來越小，系統(tǒng)反應(yīng)時(shí)間也能控制在規(guī)定時(shí)間內(nèi)。最終結(jié)果如圖9所示，在距離變化前測(cè)量誤差為0.47 m，經(jīng)過9.89分鐘對(duì)距離變化做出反應(yīng)，距離變化后的誤差為0.37 m，可以看出測(cè)量值偏離真實(shí)值的誤差小于目標(biāo)實(shí)際運(yùn)動(dòng)的距離。對(duì)于慢速運(yùn)動(dòng)目標(biāo)來說，測(cè)量精度和對(duì)距離變化的反應(yīng)時(shí)間都符合系統(tǒng)測(cè)量要求。

表2 距離變化中各階段數(shù)據(jù)反應(yīng)時(shí)間

圖6 原始距離信息

圖7 差值處理結(jié)果

圖8 第一級(jí)Kalman濾波結(jié)果

圖9 第二級(jí)Kalman濾波結(jié)果

3 結(jié)束語

通過對(duì)nanoLOC測(cè)量數(shù)據(jù)的分析，提出對(duì)距離測(cè)量系統(tǒng)的目標(biāo)距離信息進(jìn)行兩級(jí)Kalman濾波處理，建立對(duì)應(yīng)的濾波模型，且對(duì)主要的濾波器參數(shù)的選取進(jìn)行討論。實(shí)驗(yàn)結(jié)果表明，方法簡(jiǎn)單有效，在實(shí)際應(yīng)用中針對(duì)具體要求可以增加Kalman濾波級(jí)數(shù)，具有較強(qiáng)的實(shí)用性。

[1] 易哲，王國宏，張翔宇，陳澤元.一種高數(shù)據(jù)率雷達(dá)跟蹤慢速目標(biāo)的數(shù)據(jù)互聯(lián)算法[J].電學(xué)與控制，2014，21(7):31－35.

[2] 祝轉(zhuǎn)民，秋宏興，李濟(jì)生，等.動(dòng)態(tài)測(cè)量數(shù)據(jù)野值的辨識(shí)與剔除[J].系統(tǒng)工程與電子技術(shù)，2004，26(2):147－149.

[3] 黃鶴，張會(huì)生，許家棟，等.一種基于二級(jí)變維Kalman濾波的跟蹤算法[J].航空計(jì)算技術(shù)，2008，38(3):97－100.

[4] 張國權(quán)，倪重匡.應(yīng)用Kalman濾波提高雷達(dá)測(cè)量精度[J].電子學(xué)報(bào)，1981，3:19－25.

[5] 樊建新，董緒榮，羅定開，等.動(dòng)態(tài)測(cè)量問題數(shù)據(jù)處理的一個(gè)快速濾波算法[J].武漢測(cè)繪科技大學(xué)學(xué)報(bào)，1998，23(2):111－114.

[6] 余翔，馮璐，漆晶.一種組合式的Kalman濾波算法[J].電視技術(shù)，2013，37(9):168－170.

[7] 趙軍.基于C語言的測(cè)量數(shù)據(jù)修正技術(shù)[J].計(jì)算機(jī)與網(wǎng)絡(luò)，2013，15:48－50.

[8] 秦慶強(qiáng)，張曉安，李艾華.基于自適應(yīng)Kalman濾波的加速度計(jì)動(dòng)態(tài)數(shù)據(jù)處理[J].計(jì)算機(jī)工程與設(shè)計(jì)，2009，30(17):3943－3945.

[9] 朱學(xué)鋒，韓榮閣，楊若紅.基于模糊預(yù)測(cè)系統(tǒng)的觀測(cè)數(shù)據(jù)野值剔除方法[J].系統(tǒng)工程與電子技術(shù)，2006，28(3):478－482.

[10] 張帆，盧崢.自適應(yīng)抗野值Kalman濾波[J].電機(jī)與控制學(xué)報(bào)，2007，11(2):188－190.

[11] 張凱，劉洋.慢速目標(biāo)僅測(cè)角無源定位算法研究[J].雷達(dá)科學(xué)與技術(shù)，2014，12(3):291－296.

[12] 劉愛東，倪永強(qiáng)，王建國.一種實(shí)時(shí)剔除雷達(dá)測(cè)量數(shù)據(jù)中野值的方法分析[J].火力與指揮控制，2004，29(S1):17－19.

[13] 余文芳，胡旭科.基于線性調(diào)頻的nanoLOC新技術(shù)與應(yīng)用研究[J].信息通信，2011，2:4－6.

[14] 鞏家昌，張軍.Nanotron技術(shù)在無線測(cè)距系統(tǒng)中的應(yīng)用[J].現(xiàn)代電子技術(shù)，2011，34(3):56－58.

[15] 祝轉(zhuǎn)民，秋宏興，李濟(jì)生，等.Kalman濾波工程應(yīng)用問題分析及改進(jìn)方法研究[J].宇航學(xué)報(bào)，2002，23(3)，44－47.