一種基于多級Kalman濾波的高精度距離估計方法

2015-01-05 07:51:26杜雨洺

成都信息工程大學學報 2015年2期

唐玲，杜雨洺

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

0 引言

在對慢速運動目標進行距離估計時，由于目標運動速度過慢，短時間內可以認為其處于靜止狀態，在強干擾環境中，目標測量誤差可能遠遠大于其實際運動的距離，在這一情況下，測量值可能很難落在誤差允許范圍之內，從而造成數據無法真實反應目標運動軌跡[1]。在真實復雜環境下，由于障礙物和多徑干擾的原因，測量值中可能出現大量野值點，直接對其進行濾波會導致濾波估計值明顯地偏離系統真實狀態，導致目標運動軌跡失真[2]。因此研究對慢速目標的高精度距離估計具有重要的實際意義。

通過對慢速運動目標的特點進行分析，提出一種兩級Kalman濾波的距離估計方法，以被測目標的當前時刻距離測量值為輸入，對一分鐘內的測量值取均值進行差值檢測，上一時刻最優估計與當前時刻測量值的絕對差作為差值門限檢測的標準距離差以判斷下一時刻數據點是否為野值點，計算當前時刻與標準距離的差值，對當前時刻落在門限范圍外的值進行修正，以修正后的數據作為第一級Kalman濾波的初始狀態向量，這樣避免了野值點對濾波效果的不利影響，同時不會對距離變化響應時間有較大影響。

1 卡爾曼濾波

1.1 Kalman濾波理論

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

式中，X(k)為k時刻的目標狀態向量，W(k)為k時刻目標運動過程噪聲，U(k)為k時刻對系統的控制量，A、B、H為系統參數，Z(k)為k時刻觀測值，V(k)為k時刻測量噪聲。在測距系統中有

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

(2)狀態向量、觀測噪聲、過程噪聲互不相關，即

式中，k，j均表示時刻，上角標T表示矩陣的轉置。

Kalman濾波是結合當前時刻的測量值，對上一時刻的預測進行校正，得到校正后的最優估計，該估計具有最小方差。一般來說，Kalman濾波分為兩個步驟:預測、校正。預測即對時間狀態向量進行更新，校正即對測量狀態向量進行更新[6]。當Q、R值確定時，由下列遞推公式得到目標的狀態估計值:

1.2 多級Kalman濾波方法應用于慢速運動目標距離估計

在對慢速運動目標距離估計數據處理時，經典Kalman濾波存在不足。

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

(2)慢速運動目標的特點是目標與觀測點之間的相對運動速度很慢，若觀測點處于靜止狀態，短時間內可以忽略目標運動速度，把運動目標看成是靜止狀態建模，這樣系統具有可觀測性[11]，但是當目標運動到一定距離時，那么測量值偏離真實值的誤差有可能遠遠大于目標實際運動的距離，因此目標的運動給測量帶來了誤差[12]。在這種情況下，經過Kalman濾波后卻不能立即在數據上反應出來，即不能在實時對距離變化做出響應，因此不能判斷目標狀態是否發生改變。

(3)對于慢速運動目標而言，當改變濾波參數提高測距精度時，相鄰估計值之間的差值受濾波精度影響，如果被測目標位置發生變化，一個數據率時間內變化的距離值為厘米級以下，濾波精度過高時被平滑處理，不能在數據上體現出距離的變化。因此，濾波精度越高系統反應時間越長，可以看出系統反應時間和濾波精度相互影響并且相互矛盾。

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

圖1 多級Kalman濾波流程圖

2 實驗仿真及結果分析

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

2.1 濾波模型建立

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

2.2 主要參數選取

在實際工程中，濾波初始值和估計誤差方差初值都會影響系統的穩定性，且精確的初始值可以獲得最優的濾波估計值。如果初始值選取不恰當，長時間之后，可能導致濾波發散，所以選取適合的濾波參數對濾波精度起著至關重要的作用。

2.2.1 濾波初始值選定

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

2.2.2 過程噪聲方差Q值選取

過程噪聲方差Q反應連續兩個時刻的距離方差，它的好壞對濾波精度有直接的影響，Q值選取的一個基本原則是使Q值大小與動態模型的精度相匹配。由于距離測量系統的目標不斷運動，外界環境也在不斷發生變化，所以采用自適應的方法估計過程噪聲方差[15]。

通過對實驗數據的分析可知，一級Kalman濾波中Q值應該比二級Kalman濾波中Q值小一個數量級，通過先進行較低精度濾波對距離數據中差距較大的值進行粗略的估計，在進行較高精度的濾波獲取較好的估計，這樣達到對濾波精度的較好控制。最終選取Q1=0.85 × 10－3，Q2=10－4。

2.2.3 測量噪聲方差R值選取

測量噪聲方差R同樣是很重要的一個參數，反應目標距離的測量精度，通過對測量數據和距離測量系統動力學方程建模得知，當R=3時，系統的收斂性和測量精度能達到較好的平衡。

2.3 仿真實驗及數據分析

2.3.1 濾波精度

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

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

圖2 原始距離信息

圖3 差值檢測后的值

圖4 一級Kalman濾波后的結果

圖5 二級Kalman濾波后的結果

表1 各階段濾波數據精度值比較

2.3.2 反應時間

對慢速運動目標而言，系統反應時間是很重要的指標，決定了系統是否能及時的對目標位置變化做出反應。以初始距離12.5 m為例，系統處于穩定狀態時移動一段距離(測得移動距離為0.8 m)，若反應時間在10分鐘以內則認為符合要求，對測量誤差和系統反應時間進行分析。圖6為原始距離信息，在距離變化前測量誤差為2.12 m，經過5.42分鐘系統對該變化做出反應，并且距離變化后的誤差為3.23 m?？梢钥闯鰷y量值偏離真實值的誤差遠大于目標實際運動的距離，對于慢速運動目標來說，不能達到測量要求。經過多級Kalman濾波處理后，測量誤差越來越小，系統反應時間也能控制在規定時間內。最終結果如圖9所示，在距離變化前測量誤差為0.47 m，經過9.89分鐘對距離變化做出反應，距離變化后的誤差為0.37 m，可以看出測量值偏離真實值的誤差小于目標實際運動的距離。對于慢速運動目標來說，測量精度和對距離變化的反應時間都符合系統測量要求。