黃春梅，　鄒慧瑩，　王　杰

(長(zhǎng)春工業(yè)大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院， 吉林 長(zhǎng)春　130012)

?

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

黃春梅，鄒慧瑩*，王杰

(長(zhǎng)春工業(yè)大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院， 吉林 長(zhǎng)春130012)

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

關(guān)鍵詞：SINS/GPS組合導(dǎo)航； 標(biāo)準(zhǔn)粒子濾波； 迭代粒子濾波； 退火算法

0引言

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

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

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

SINS/GPS系統(tǒng)結(jié)構(gòu)如圖1所示。

圖1SINS/GPS系統(tǒng)結(jié)構(gòu)圖

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

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

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

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

1)位置誤差方程：

(1)

2)速度誤差方程：

(2)

3)平臺(tái)誤差方程：

(3)

式中----載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣；

εb----載體坐標(biāo)系中的陀螺儀漂移。

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

(4)

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

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

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

1.2SINS/GPS量測(cè)方程

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

1)位置量測(cè)誤差：

(5)

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

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

2)速度量測(cè)誤差：

(6)

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

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

(7)

2標(biāo)準(zhǔn)粒子濾波算法[7]

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

(8)

(9)

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

標(biāo)準(zhǔn)粒子濾波步驟如下：

1)初始化。

2)更新。

在k時(shí)刻，更新粒子的權(quán)值

(10)

并且歸一化

(11)

則k時(shí)刻x的最小均方估計(jì)為

(12)

3)重采樣。

得到新的粒子集合

4)預(yù)測(cè)。

5)時(shí)刻k=k+1，轉(zhuǎn)到步驟2)。

在標(biāo)準(zhǔn)粒子濾波算法中，權(quán)重的方差隨時(shí)間而隨機(jī)遞增，使得少數(shù)粒子擁有較大權(quán)重，最后很可能導(dǎo)致大量的計(jì)算工作浪費(fèi)，粒子集無(wú)法表達(dá)實(shí)際的后驗(yàn)概率，產(chǎn)生了粒子退化的問(wèn)題。

3自適應(yīng)迭代粒子濾波算法

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

假設(shè)已知

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

設(shè)定閾值ρ，如果

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

(13)

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

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

(14)

(15)

其中

(16)

使

(17)

使

得：

(18)

自適應(yīng)迭代粒子濾波步驟如下：

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

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

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

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

然后歸一化

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

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

4仿真分析

標(biāo)準(zhǔn)粒子濾波速度誤差和自適應(yīng)迭代粒子濾波速度誤差分別如圖2和圖3所示。

對(duì)上述兩種方法進(jìn)行仿真分析。通過(guò)圖2與圖3分析比較可以得出，標(biāo)準(zhǔn)粒子濾波東-北-天3個(gè)方向的速度誤差范圍分別為-0.85～ +1.00 m/s、-0.95 ～ 0.91 m/s、-1.00～ 0.78 m/s，自適應(yīng)迭代粒子濾波東-北-天3個(gè)方向的速度誤差范圍分別為-0.32～ +0.51 m/s、-0.29 ～ +0.73 m/s、-0.46～ +0.34 m/s。

標(biāo)準(zhǔn)粒子濾波位置誤差和自適應(yīng)迭代粒子濾波位置誤差分別如圖4和圖5所示。

通過(guò)圖4與圖5的分析比較得出，標(biāo)準(zhǔn)粒子濾波東-北-天3個(gè)方向的位置誤差范圍為-8.93～ +9.12 m、-10.00～ +9.53 m、-10.00～ +9.47 m，自適應(yīng)迭代粒子濾波東-北-天3個(gè)方向的位置誤差范圍為-4.67～ +6.03 m、-3.65～ +5.21 m、-4.86～ +5.07 m。

圖2標(biāo)準(zhǔn)粒子濾波速度誤差

圖3　自適應(yīng)迭代粒子濾波速度誤差

5結(jié)語(yǔ)

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

圖4標(biāo)準(zhǔn)粒子濾波位置誤差

圖5　自適應(yīng)迭代粒子濾波位置誤差

參考文獻(xiàn)：

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

[2]劉毛毛，秦品樂(lè)，呂國(guó)宏,等.基于多新息理論的EKF改進(jìn)算法[J].計(jì)算機(jī)應(yīng)用研究，2015,32:1568-1571.

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

[4]王法勝，魯明羽，趙清杰,等.粒子濾波算法[J].計(jì)算機(jī)學(xué)報(bào)，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]王新龍，李亞峰，紀(jì)新春.SINS/GPS組合導(dǎo)航技術(shù)[M].北京：北京航空航天大學(xué)出版社，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.

中圖分類(lèi)號(hào)：TP 274

文獻(xiàn)標(biāo)志碼：A

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

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

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

收稿日期：2015-11-23