組合導航系統(tǒng)中自適應迭代粒子濾波

黃春梅，　鄒慧瑩，　王　杰

(長春工業(yè)大學 計算機科學與工程學院， 吉林 長春　130012)

?

組合導航系統(tǒng)中自適應迭代粒子濾波

黃春梅，鄒慧瑩*，王杰

(長春工業(yè)大學 計算機科學與工程學院， 吉林 長春130012)

摘要：提出一種自適應迭代粒子濾波算法，利用自身迭代實時更新重要密度函數(shù)，快速將粒子推到高似然區(qū)，讓粒子權重分散均勻，提高采樣效率。運用模擬退火算法來解決粒子權重很小、不易歸一化的問題,仿真結果可知，自適應迭代粒子濾波算法在SINS/GPS組合導航中有更好的實用性。

關鍵詞：SINS/GPS組合導航； 標準粒子濾波； 迭代粒子濾波； 退火算法

0引言

在SINS/GPS組合導航系統(tǒng)中，解算精度與高精度非線性的濾波算法有很大關系[1]。主要的非線性濾波算法有擴展卡爾曼濾波(EKF)、Unscented卡爾曼濾波(UKF)、粒子濾波等。EKF利用Taylor公式對組合導航系統(tǒng)模型進行線性化，忽略掉所有誤差的二次項及高于二次項的誤差項，當系統(tǒng)模型非線性化很強的時候，濾波效果明顯下降[2]；UKF利用非線性系統(tǒng)的已知數(shù)學模型和初始狀態(tài)的均值、方差來獲得一組權值函數(shù)以及一組樣本點，不需要對系統(tǒng)模型進行線性化變換，但是其需要假設噪聲滿足高斯分布，而且系統(tǒng)維數(shù)較高就增加其計算量[3]；標準粒子濾波基于蒙特卡洛模擬和貝葉斯估計理論，直接利用非線性系統(tǒng)完成非線性變換，避免了線性化的系統(tǒng)高階誤差影響，但是粒子濾波會出現(xiàn)重要性密度函數(shù)難以選取和粒子退化等問題[4]。

針對上述標準粒子濾波出現(xiàn)的問題進行改進，提出了一種利用自身迭代來更新重要性密度函數(shù)的自適應迭代粒子濾波方法。當重要性密度函數(shù)的高斯分布超出了預定的閾值，自適應迭代粒子濾波算法就利用修正項自身迭代實時更新重要性密度函數(shù)，然后再利用模擬退火算法和自適應退火參數(shù)來解決粒子歸一化權重相同的問題，將粒子快速推入高似然區(qū)域，以提高采樣效率[5]。

1SINS/GPS系統(tǒng)的非線性模型[6]

SINS/GPS系統(tǒng)結構如圖1所示。

圖1SINS/GPS系統(tǒng)結構圖

在SINS/GPS組合導航中采用的是東-北-天坐標系， GPS接收機得到的位置、速度和SINS解算出來的位置、速度相減，差值作為組合濾波器的輸入，然后組合濾波器的輸出采用反饋校正，陀螺儀和加速度計漂移誤差的校正在SINS中進行，而位置和速度信息直接對SINS的解算結果進行校正。

1.1SINS/GPS狀態(tài)方程

誤差狀態(tài)向量：

由位置誤差、速度誤差、姿態(tài)誤差角、加速度計零偏誤差以及陀螺儀漂移誤差等組成。

1)位置誤差方程：

(1)

2)速度誤差方程：

(2)

3)平臺誤差方程：

(3)

式中----載體坐標系到導航坐標系的轉換矩陣；

εb----載體坐標系中的陀螺儀漂移。

將式(1)～式(3)綜合，得到狀態(tài)方程：

(4)

式中：FI----系統(tǒng)狀態(tài)矩陣；

WI----系統(tǒng)噪聲；

GI----系統(tǒng)噪聲矩陣。

1.2SINS/GPS量測方程

組合系統(tǒng)的量測信息是SINS解算的位置、速度與GPS測量到的位置、速度之差。

1)位置量測誤差：

(5)

式中：λ、L、R----分別為經(jīng)度、緯度、地球半徑；

nx，ny，nz----分別為GPS 接收機沿地理坐標系x、y、z三個軸方向上的位置誤差噪聲。

2)速度量測誤差：

(6)

式中：nvx，nvy，nvz----分別為GPS 接收機沿地理坐標系x、y、z三個軸方向上的速度誤差噪聲。

將式(5)與式(6)合并，得量測方程：

(7)

2標準粒子濾波算法[7]

假設離散化系統(tǒng)狀態(tài)方程和量測方程：

(8)

(9)

vk-1和wk是過程噪聲和量測噪聲，相互獨立。f(·)和h(·)是已知的函數(shù)。

標準粒子濾波步驟如下：

1)初始化。

2)更新。

在k時刻，更新粒子的權值

(10)

并且歸一化

(11)

則k時刻x的最小均方估計為

(12)

3)重采樣。

得到新的粒子集合

4)預測。

5)時刻k=k+1，轉到步驟2)。

在標準粒子濾波算法中，權重的方差隨時間而隨機遞增，使得少數(shù)粒子擁有較大權重，最后很可能導致大量的計算工作浪費，粒子集無法表達實際的后驗概率，產(chǎn)生了粒子退化的問題。

3自適應迭代粒子濾波算法

自適應迭代粒子濾波的基本思想就是利用粒子濾波自身迭代修正，產(chǎn)生新的IDF，并在模擬退火算法的幫助下將粒子推入高似然區(qū)。

假設已知

且有過程噪聲和量測噪聲的協(xié)方差分別為：

設定閾值ρ，如果

該粒子有效，用重采樣技術估計xk；否則，就用更新等式產(chǎn)生新的IDF。更新的等式如下：

(13)

式中：c(zk)----修正項。

粒子濾波利用其自身迭代修正項c(zk)。用cm(zk)表示第m次迭代，則有：

(14)

(15)

其中

(16)

使

(17)

使

得：

(18)

自適應迭代粒子濾波步驟如下：

2)更新IDF。更新公式為：

第m次迭代產(chǎn)生新的IDF為:

3)歸一化權值。引入退火參數(shù)αm(αm≥1)，協(xié)方差為αmRk。粒子的權值越大越好，則需要成本函數(shù):

最大化。退火參數(shù)αm=max{1,1/βm}，而粒子權值

然后歸一化

4)重采樣。從新等到的IDF中得到粒子集合。

6)判斷是否采樣結束，否則繼續(xù)以上步驟采樣。

4仿真分析

標準粒子濾波速度誤差和自適應迭代粒子濾波速度誤差分別如圖2和圖3所示。

對上述兩種方法進行仿真分析。通過圖2與圖3分析比較可以得出，標準粒子濾波東-北-天3個方向的速度誤差范圍分別為-0.85～ +1.00 m/s、-0.95 ～ 0.91 m/s、-1.00～ 0.78 m/s，自適應迭代粒子濾波東-北-天3個方向的速度誤差范圍分別為-0.32～ +0.51 m/s、-0.29 ～ +0.73 m/s、-0.46～ +0.34 m/s。

標準粒子濾波位置誤差和自適應迭代粒子濾波位置誤差分別如圖4和圖5所示。

通過圖4與圖5的分析比較得出，標準粒子濾波東-北-天3個方向的位置誤差范圍為-8.93～ +9.12 m、-10.00～ +9.53 m、-10.00～ +9.47 m，自適應迭代粒子濾波東-北-天3個方向的位置誤差范圍為-4.67～ +6.03 m、-3.65～ +5.21 m、-4.86～ +5.07 m。

圖2標準粒子濾波速度誤差

圖3　自適應迭代粒子濾波速度誤差

5結語

自適應迭代粒子濾波算法在粒子濾波算法重采樣的基礎上進行改進，利用濾波本身來進行自適應迭代，從而使粒子快速進入高似然區(qū)域，又基于粒子權重逐漸集中于少數(shù)粒子上，并且自適應的修正項慢慢趨近于零值的問題，利用模擬退火參數(shù)來解決，讓成本函數(shù)最大化，得到的最大解就是所需的參數(shù)，從而解決了粒子濾波中粒子退化問題，通過相關性能比較，所提出的改進算法的有效性得以驗證。

圖4標準粒子濾波位置誤差

圖5　自適應迭代粒子濾波位置誤差

參考文獻：

[1]王健.提高SINS/GPS組合導航系統(tǒng)定位精度的方法研究[J].現(xiàn)代導航，2014，2(1)：7-10.

[2]劉毛毛，秦品樂，呂國宏,等.基于多新息理論的EKF改進算法[J].計算機應用研究，2015,32:1568-1571.

[3]汲清波，馮馳，呂曉鳳.UKF、PF與UPF跟蹤性能的比較[J].計算機工程與應用，2008，44(32)：60-63.

[4]王法勝，魯明羽，趙清杰,等.粒子濾波算法[J].計算機學報，2014，37(8):1679-1694.

[5]Zhouhua Duan， Zixing Cai， Jinxia Yu. Adaptive particle filter for unknown fault detection of wheeled mobile robots[J]. Proc. of IEEE/RSJ International Conference on Intelligent Robots and Systems， Beijing， China， Oct.，2006：1312-1315.

[6]王新龍，李亞峰，紀新春.SINS/GPS組合導航技術[M].北京：北京航空航天大學出版社，2014.

[7]J Y Zuo， Y N Jia， Y Z Zhang， et al. Adaptive iterated particle filter[J]. Electronics Letters，2013,49(12)：724-744.

[8]A Rahni， E Lewis， M Guy， et al. A particle filter approach to respiratory motion estimation in nuclear medicine imaging[J]. IEEE Transactions on Nuclear Science，2011,58(5)：2276-2285.

Adaptive iterated particle filter in integrated navigation system

HUANG Chunmei,ZOU Huiying*,WANG Jie

(School of Computer Science & Engineering, Changchun University of Technology, Changchun 130012， China)

Abstract：An adaptive iterated particle filter (AIPF)is put forward, in which self-update is applied to update the importance density function in real time for pushing the particles to the high likelihood region quickly, so the particle weight can be evenly distributed to improve the sample efficiency. An annealing algorithm is used to solve the problem that the particle weight is too small to normalize. Simulation results demonstrate that the adaptive iterated particle filter is practical for SINS/GPS integrated navigation system.

Key words：SINS/GPS integrated navigation system; particle filter; iterated particle filter; annealing algorithm.

中圖分類號：TP 274

文獻標志碼：A

文章編號：1674-1374(2016)01-0030-06

DOI:10.15923/j.cnki.cn22-1382/t.2016.1.07

作者簡介：黃春梅(1965-)，女，漢族，吉林長春人，長春工業(yè)大學副教授，博士，主要從事嵌入式控制方向研究,E-mail:huangchunmei@ccut.edu.cn. *通訊作者：鄒慧瑩(1990-)，女，漢族，湖北荊門人，長春工業(yè)大學碩士研究生，主要從事嵌入式控制方向研究,E-mail:ssmilezou@163.com.

收稿日期：2015-11-23