基于卡爾曼濾波的動態定位優化*

陳　剛　王　威　狄　鵬

(1.海軍工程大學管理工程系　武漢　430033)(2.海軍工程大學艦船工程系　武漢　430033)

?

基于卡爾曼濾波的動態定位優化*

陳剛1王威2狄鵬1

(1.海軍工程大學管理工程系武漢430033)(2.海軍工程大學艦船工程系武漢430033)

摘要針對動態定位中存在很多隨機因素影響定位精度的問題，本文將卡爾曼濾波技術應用于動態定位中，設計出了卡爾曼濾波器，對隨機因素造成的誤差進行濾波以減小定位誤差，提高了定位精度?？柭鼮V波技術是一種線性最小方差濾波估計，它只需要通過上一時刻狀態向量的最優估計和現在時刻的觀測量就可以得到現在時刻狀態向量的最優估計值。論文首先以物體在二維空間運動為例，建立系統的狀態轉移方程以及測量方程，然后采用濾波器對測量值進行濾波處理，最后采用算例進行仿真。仿真結果表明,將卡爾曼濾波技術應用于動態定位中，能有效地優化動態定位，提高定位精度。

關鍵詞卡爾曼濾波; 動態定位; 隨機因素; 狀態; 最優估計

Class NumberP228

1引言

如何獲得準確的位置和時間信息對于目前的社會活動極其重要，在實際動態定位中，會存在很多隨機干擾因素影響定位的精度，例如使用GPS定位時，衛星的星歷誤差、衛星鐘差、多路徑效應、接收機本身的噪聲、電離層及對流層等都會對定位產生干擾。隨著時代發展，定位技術也不斷成熟，傳輸時延差TDOA[1～4]、差分GPS[5～7]等定位技術使定位精度不斷提高，但因需要建立差分GPS基準站以及受到差分GPS所面臨的信號作用范圍限制，在很多特殊場合滿足不了定位精度要求，迫切需要新的技術來提高定位精度。卡爾曼博士在1961年提出了一種線性濾波估計理論，即卡爾曼濾波[9～10]，這種濾波引入了控制理論狀態空間的概念，只需要通過上一時刻狀態向量的最優估計和現在時刻的觀測量，然后在時域內進行線性遞推計算，就可將真實的狀態從各種干擾中實時最優地估計出來?？柭鼮V波已成功應用于動態數據處理[11]、慣性導航[12～14]中，使用卡爾曼濾波和其它算法相結合能有效地減少動態定位中的誤差，提高定位的精度。

2卡爾曼濾波算法

卡爾曼濾波算法是一種遞推的線性最小方差估計方法，它通過建立狀態方程和量測方程來描述系統的動態變化過程。由前一時刻的估計值和新獲得的觀測數據，依據濾波增益矩陣的變化，遞推計算出新的估計值，最后得到系統狀態的最優估計。無需存儲不同時刻的觀測數據，便于實時數據處理。因此被廣泛應用于船舶、雷達以及導航等動態定位的數據處理中。

假設系統的狀態向量為X，觀測向量為Z，觀測向量Z可由狀態向量X進行表示，觀測方程為

Z(k)=H(k)X(k)+V(k)

其中H(k)表示觀測矩陣，V(k)表示觀測的噪聲。

不考慮控制項，系統由k-1時刻狀態轉移k時刻狀態可由狀態轉移方程進行表示:

X(k)=Φ(k|k-1)X(k)+W(k)

其中Φ(k|k-1)表示狀態轉移矩陣，W(k)表示系統過程的噪聲。

關于系統過程噪聲和觀測噪聲的統計特性，假定如下:

其中Qk是系統過程噪聲W(k)的非負定方差矩陣；Rk是系統觀測噪聲V(k)的正定方差矩陣，δkj是Kronecker-δ函數。

狀態一步預測

狀態估計

^X(k|k) =^X(k|k-1)+K(k)

濾波增益矩陣

K(k)=P(k|k-1)H(k)T/H(k)P(k|k-1)H(k)T

一步預測誤差方差矩陣

P(k|k-1)=Φ(k|k-1)P(k-1|k-1)

Φ(k|k-1)T+Q(k)

估計誤差方差矩陣

P(k|k)=[1-K(k)H(k)]P(k|k-1)

上述方程即為線性離散系統卡爾曼濾波方程。

圖1　隨機線性離散系統卡爾曼濾波器流程圖

3卡爾曼濾波器模型的構建

W(k)= T22wxTwxwxT22wyTwywyé?êêù?úúT

,

Z(k)=H(k)X(k)+V(k)

其中

對于系統的過程噪聲wx、wy以及系統的測量噪聲v1、v2均為不相關的白噪聲過程， 其特性如下:

令系統過程噪聲方差矩陣為Q(k),系統測量噪聲方差矩陣為R(k)，可表示為

4仿真實驗

為了驗證所設計的卡爾曼濾波模型的可行性和有效性,進行仿真試驗。假設某物體沿x軸方向以速度vx0=5m/s的速度做勻速運動，沿y軸做初速度vy0=5m/s，ay=1m/s2的勻加速運動，采樣周期T=0.1s，采樣時間為10s。通過仿真模擬出測量噪聲為R(k)的觀測值Z(k)。設置相關仿真參數。

系統初始化位置及協方差矩陣為

系統過程方差矩陣及測量方差矩陣

采用卡爾曼濾波器對系統觀測值Z(k)進行濾波處理，濾波前后結果如圖2。

圖2　卡爾曼濾波結果

將最優估計值、測量值與真實值進行對比:

=370.04

=40.08

對濾波前后x軸，y軸方向做誤差分析如圖3和圖4。

圖3　卡爾曼濾波前后x方向的誤差圖

圖4　卡爾曼濾波前后y方向的誤差圖

可以看出測量值經卡爾曼濾波器濾波后得到的最優估計更加接近真實值，卡爾曼濾波器能更加有效地減小定位誤差。

5結語

通過建立系統運動的狀態轉移方程以及測量方程，采用卡爾曼濾波器對系統定位的測量值進行優化處理，得到系統狀態的最優估計。最后的仿真算例結果表明，卡爾曼濾波在動態濾波精度上有明顯改善，對動態定位有明顯優化效果，從而說明卡爾曼濾波算法具有一定的有效性和實用性。

參 考 文 獻

[1] 彭宇,王丹.無線傳感器網絡定位技術綜述[J].電子測量與儀器學報,2011,25(5):389-396.

[2] 殷桂華,王小輝,雷毅談.基于TDOA的室內運動目標雙曲線RFID定位方法[J].計算機應用,2014，34(S2):52-54.

[3] 劉華辰,劉麗莉,王奉章,等.基于TDOA測距模型的SOCP和Taylor混合定位方案[J].激光雜志,2015,32(2),107-112.

[4] 劉繼斌,劉培國,李高升,等.三維AOA /TDOA被動定位及其迭代融合算法[J].全球定位系統,2006(4):5-9.

[5] 張怡,張亮,廉保旺.差分GPS定位系統的衛星位置算法研究[J].彈箭與制導學報,2003(1):85-87.

[6] 牟聰,王偉,張明. 差分 GPS 應用于機載雷達精度統計的方法研究火控雷達技術[J].2012(3):25-28.

[7] 楊世學,劉宇明.高精度差分GPS技術在海洋資源調查中的應用[J].南海地質研究,2002:92-96.

[8] 王甫紅.高精度星載GPS實時定軌卡爾曼濾波模型[J].武漢大學學報,2010(6):653-656.

[9] Kalman R.E.A new approach to linear filtering and Predietion Problem[J].Jornal of Basic Eng(ASME)，1960,82D:95-108.

[10] Jens G.Balchen,Nils A.Jenssen etc.A dynamic positioning system based on Kalman filtering and optimal contral[J].Identification and control,1980(3):135-163.

[11] 梁民贊,陸揚,周新鵬.一種抑制卡爾曼濾波發散的實時數據處理方法 [J].聲學技術,2008(5):761-765.

[12] 戴革林,李珺.基于混合卡爾曼濾波的組合導航算法研究[J].網絡安全技術與應用,2014(10):60-63.

[13] 崔平遠,鄭黎方,裴?？?等.基于卡爾曼/粒子組合濾波器的組合導航方法研究[J].系統仿真學報,2009(1):220-223.

[14] 劉愛元,戴洪德,盧建華,等.組合導航系統模擬器中的卡爾曼濾波設計[J].計算機與數字工程,2014(9):1649-1652.

Optimization of Dynamic Positioning Based on Kalman Filter

CHEN Gang1WANG Wei2DI Peng1

(1.Department of Management Science, Naval University of Engineering, Wuhan430033)(2.Department of Naval Architecture, Naval University of Engineering, Wuhan430033)

AbstractAiming at problems in the process of dynamic positioning，there are many random factors influencing positioning accuracy, Kalman filtering is applied to dynamic positioning, a Kalman filter is designed and the deviation caused by random factors is filtered to reduce the positioning deviation, the positioning accuracy is improved. Kalman filtering is one kind of linear minimum variance estimation methods. It can give the current optimal estimation of state vector with the current observation vector and the optimal estimation of last time. First, a model that objects move in the two-dimensional space is created and the transfer equation and measurement equation of the system state are given. And then the measured values is filtered. Last, the model is simulated. The results show that the Kalman filtering can effectively optimize the dynamic positioning, improve the positioning accuracy.

Key WordsKalman filter, dynamic positioning, random factors, state, optimal estimation

* 收稿日期：2015年11月11日,修回日期：2015年12月27日

作者簡介：陳剛，男，碩士，研究方向：系統管理。王威，男，博士，博士生導師，研究方向：系統工程。狄鵬，男，博士，碩士生導師，研究方向：裝備保障。

中圖分類號P228

DOI：10.3969/j.issn.1672-9730.2016.05.015