基于自適應(yīng)無(wú)跡粒子濾波的SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)

孫 進(jìn)，徐曉蘇，劉義亭，張利斌

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院 微慣性?xún)x表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2. 常州信息職業(yè)技術(shù)學(xué)院 電子與電氣工程學(xué)院，常州 213164）

基于自適應(yīng)無(wú)跡粒子濾波的SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)

孫 進(jìn)1，徐曉蘇1，劉義亭1，張利斌2

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院 微慣性?xún)x表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；2. 常州信息職業(yè)技術(shù)學(xué)院 電子與電氣工程學(xué)院，常州 213164）

針對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)大方位失準(zhǔn)的情況進(jìn)行分析，提出了采用了自適應(yīng)UPF濾波進(jìn)行初始對(duì)準(zhǔn)的方法。該濾波器基于強(qiáng)跟蹤濾波器的思想，通過(guò)引入衰減記憶因子有效增強(qiáng)當(dāng)前信息殘差對(duì)系統(tǒng)的修正作用，在一定程度上降低了由于系統(tǒng)簡(jiǎn)化、噪聲統(tǒng)計(jì)特性不確定對(duì)系統(tǒng)造成的影響，同時(shí)較好地克服了UPF中粒子退化的現(xiàn)象。當(dāng)系統(tǒng)噪聲統(tǒng)計(jì)特性確定時(shí)，自適應(yīng)UPF水平對(duì)準(zhǔn)時(shí)間比UPF對(duì)準(zhǔn)時(shí)間大約少200 s，方位對(duì)準(zhǔn)時(shí)間大約少150 s，對(duì)準(zhǔn)結(jié)束后，自適應(yīng)UPF對(duì)準(zhǔn)的縱搖、橫搖以及航向誤差均值分別為-0.018°、0.0074°、-0.8609°，UPF對(duì)準(zhǔn)對(duì)應(yīng)的值分別為0.0209°、0.0189°、1.0983°。當(dāng)系統(tǒng)噪聲統(tǒng)計(jì)特性不確定時(shí)，自適應(yīng)UPF水平對(duì)準(zhǔn)時(shí)間比UPF對(duì)準(zhǔn)時(shí)間大約少400 s，方位對(duì)準(zhǔn)時(shí)間大約少450 s，對(duì)準(zhǔn)結(jié)束后，自適應(yīng)UPF對(duì)準(zhǔn)的縱搖、橫搖以及航向誤差均值分別為0.0105°、0.0122°、-0.7304°，UPF對(duì)準(zhǔn)對(duì)應(yīng)的值分別為0.0454°、0.0278°、2.8174°。仿真結(jié)果表明：在系統(tǒng)噪聲統(tǒng)計(jì)特性確定或不確定的情況下，自適應(yīng)UPF能在一定程度上提高大方位失準(zhǔn)捷聯(lián)慣導(dǎo)系統(tǒng)的初始對(duì)準(zhǔn)速度和對(duì)準(zhǔn)精度。

捷聯(lián)慣導(dǎo)系統(tǒng)；大失準(zhǔn)角；初始對(duì)準(zhǔn)；無(wú)跡粒子濾波；自適應(yīng)無(wú)跡粒子濾波

初始對(duì)準(zhǔn)是捷聯(lián)慣導(dǎo)系統(tǒng)中的關(guān)鍵技術(shù)之一，其對(duì)準(zhǔn)時(shí)間和精度直接影響慣導(dǎo)系統(tǒng)的工作性能[1]。

隨著導(dǎo)航系統(tǒng)應(yīng)用領(lǐng)域的不斷拓展，大多數(shù)應(yīng)用環(huán)境不能滿(mǎn)足初始失準(zhǔn)角為大角度和噪聲為高斯白噪聲的條件，此時(shí)繼續(xù)使用傳統(tǒng)導(dǎo)航系統(tǒng)線(xiàn)性化模型和卡爾曼濾波（Kalman Filter，KF）將會(huì)產(chǎn)生較大的模型誤差和估計(jì)誤差，使得導(dǎo)航參數(shù)不可信[1]。針對(duì)這種情況，國(guó)內(nèi)外的研究主要分為兩個(gè)方面：一是慣性導(dǎo)航系統(tǒng)的非線(xiàn)性模型的研究[2-6]。由于捷聯(lián)慣性導(dǎo)航系統(tǒng)的嚴(yán)格數(shù)學(xué)誤差模型是一組非線(xiàn)性微分方程，以線(xiàn)性模型去逼近非線(xiàn)性模型，必然存在一定的建模誤差。小失準(zhǔn)角線(xiàn)性模型只有在假設(shè)各種誤差源較小的條件下才成立，而實(shí)際中粗對(duì)準(zhǔn)的失準(zhǔn)角在很多情況下為大角度，因此直接采用非線(xiàn)性模型更能真實(shí)地反映誤差傳播特性。二是非線(xiàn)性濾波器的研究[7-10]，常用的非線(xiàn)性濾波方法有擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）、無(wú)跡卡爾曼濾波（Unscented Kalman Filter，UKF）、粒子濾波（Particle Filter，PF）、EKF-PF、UKF-PF（UPF），在一定程度上UPF的使用限制更少，濾波效果要優(yōu)于其他幾種。本文針對(duì)UPF的不足進(jìn)行改進(jìn)，基于強(qiáng)跟蹤濾波器的思想對(duì)UPF進(jìn)行改進(jìn)使 UPF具有一定的自適應(yīng)能力，并將自適應(yīng)UPF應(yīng)用于捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System，SINS）大方位失準(zhǔn)的初始對(duì)準(zhǔn)中，通過(guò)引入衰減記憶因子有效增強(qiáng)當(dāng)前信息殘差對(duì)系統(tǒng)修正作用，在一定程度上降低了由于系統(tǒng)簡(jiǎn)化、噪聲統(tǒng)計(jì)特性不確定對(duì)系統(tǒng)造成的影響，同時(shí)較好地克服了UPF中粒子退化的現(xiàn)象。將UPF和自適應(yīng)UPF兩種非線(xiàn)性濾波器在初始對(duì)準(zhǔn)中的濾波效果進(jìn)行了比較，結(jié)果表明自適應(yīng)UPF比UPF具有更快的收斂速度并且在一定程度上提高對(duì)準(zhǔn)精度。

1 SINS非線(xiàn)性誤差模型

對(duì)于捷聯(lián)系統(tǒng)大失準(zhǔn)角情況，轉(zhuǎn)動(dòng)次序引起的誤差無(wú)法忽略，必須針對(duì)大失準(zhǔn)的情況重新建立 SINS的誤差模型。使用歐拉平臺(tái)誤差角來(lái)表示理想導(dǎo)航坐標(biāo)系與計(jì)算導(dǎo)航坐標(biāo)系之間的失準(zhǔn)角，且該組誤差角需要考慮轉(zhuǎn)動(dòng)的先后順序。建立相應(yīng)的 SINS非線(xiàn)性誤差模型。

本文坐標(biāo)系選取如下：

i系——地心慣性坐標(biāo)系；

e系——選取“東-北-天（E-N-U）”地球坐標(biāo)系；

n系——選取地理坐標(biāo)系為導(dǎo)航坐標(biāo)系；

b系——“右-前-上”坐標(biāo)系為捷聯(lián)慣導(dǎo)坐標(biāo)系。

n系先后依次經(jīng)過(guò)三次歐拉角轉(zhuǎn)動(dòng)至b系，這三個(gè)歐拉角分別記為航向角俯仰角和橫滾角系與b系之間的旋轉(zhuǎn)變換關(guān)系可用姿態(tài)矩陣描述[6]。

1.1 姿態(tài)誤差方程

實(shí)際工作的導(dǎo)航系統(tǒng)由于存在各種干擾和量測(cè)誤差，SINS的計(jì)算平臺(tái)坐標(biāo)系(n′系)通常與理想導(dǎo)航坐標(biāo)系(n系)之間存在轉(zhuǎn)動(dòng)誤差。n系需按照一定的順序依次轉(zhuǎn)過(guò)三個(gè)角度才能與n′系重合，現(xiàn)假設(shè)這三次轉(zhuǎn)動(dòng)依次繞z軸、x軸、y軸旋轉(zhuǎn)，且轉(zhuǎn)過(guò)的角度分別記為φz、φx、φy，其矢量表示形式為三次旋轉(zhuǎn)對(duì)應(yīng)的姿態(tài)變換陣依次為Czφ、Cxφ、Cyφ，故n系至n′系的變換矩陣為

由文獻(xiàn)[6]可以得到SINS姿態(tài)誤差方程為

1.2 速度誤差方程

在導(dǎo)航坐標(biāo)系中，SINS速度微分方程[6]為

但在實(shí)際系統(tǒng)中該速度微分方程含有誤差，此時(shí)SINS速度微分方程應(yīng)為

1.3 SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)誤差模型

狀態(tài)向量取

噪聲向量取

2 UPF和自適應(yīng)UPF

2.1 UPF算法

1）初始化：k=0。從初始的前驗(yàn)概率分布p(x0)中進(jìn)行N個(gè)粒子的采樣，即

2）加權(quán)粒子的預(yù)測(cè)、采樣：k = 1, 2, …。利用無(wú)跡粒子濾波對(duì)粒子進(jìn)行預(yù)測(cè)更新并計(jì)算 Sigma點(diǎn)，

時(shí)間更新為

量測(cè)更新為

3）根據(jù)權(quán)值更新公式

對(duì)N個(gè)粒子相應(yīng)的權(quán)值進(jìn)行計(jì)算并做歸一化處理。

2.2 本文自適應(yīng)UPF算法

基于強(qiáng)跟蹤濾波的的思想，采用時(shí)變漸消因子削弱陳舊數(shù)據(jù)對(duì)當(dāng)前濾波值得影響， 實(shí)時(shí)調(diào)整狀態(tài)預(yù)測(cè)誤差的協(xié)方差以及相應(yīng)的增益矩陣來(lái)達(dá)到。這在一定程度上能夠減緩UPF粒子退化的現(xiàn)象，并且加快粒子濾波的收斂速度。

本文采取的自適應(yīng)措施是對(duì)濾波器的協(xié)方差進(jìn)行判斷，具體判斷公式如下[10]：式中：S為設(shè)定的調(diào)整系數(shù)，一般取為系統(tǒng)的殘差序列

修正公式為

式中，

本文自適應(yīng)UPF的實(shí)現(xiàn)步驟如下：

1）初始化：k=0。從初始的前驗(yàn)概率分布p(x0)中進(jìn)行N個(gè)粒子的采樣，即

2）預(yù)測(cè)更新

3） 判斷式(13)是否成立，如果成立則跳至步驟5，否則按照式(15)對(duì)進(jìn)行修正；

4）量測(cè)更新

5）根據(jù)權(quán)值更新公式

對(duì)N個(gè)粒子相應(yīng)的權(quán)值進(jìn)行計(jì)算并做歸一化處理。

6） 通過(guò)重采樣算法計(jì)算重采樣的粒子及其權(quán)值，

3 UPF濾波影響因素分析——重要性概率密度函數(shù)對(duì)UPF精度的影響

重要性概率密度函數(shù)的選擇集中體現(xiàn)在粒子的權(quán)重更新部分，即[10]

4 仿真及分析

4.1 仿真條件

在靜基座條件下：陀螺儀的常值漂移為0.01 (°)/h，隨機(jī)漂移為；加速度計(jì)零偏為100 μg (g = 9.8 m2/s)，隨機(jī)偏差為50 μg；當(dāng)?shù)氐乩砭暥葹?2.37°，經(jīng)度為118.22°，仿真時(shí)間為2000 s。

4.2 仿真結(jié)果與分析

1）實(shí)驗(yàn)1

按照建立的方位大失準(zhǔn)角誤差模型，在噪聲統(tǒng)計(jì)特性確定的情況下分別采用兩種濾波算法進(jìn)行了仿真實(shí)驗(yàn)?，F(xiàn)選取初始失準(zhǔn)角在使用兩種濾波算法的情況下。仿真過(guò)程中不進(jìn)行反饋修正，對(duì)準(zhǔn)誤差的仿真結(jié)果如圖1、圖2所示。

圖1 大方位失準(zhǔn)角對(duì)準(zhǔn)誤差Fig.1 Alignment errors of large azimuth misalignment angles

圖2 大失準(zhǔn)角對(duì)準(zhǔn)誤差Fig.2 Alignment errors of large misalignments angles

表1 確定性噪聲下濾波性能對(duì)比Tab.1 Comparison on filtering performances when with deterministic noise

表2 不確定性噪聲下濾波性能對(duì)比Tab.2 Comparison on filtering performances when with uncertain noise

通過(guò)圖 1 可以看出，基于大方位失準(zhǔn)角的 SINS誤差模型，當(dāng)初始失準(zhǔn)角為時(shí)，采用自適應(yīng)UPF，水平對(duì)準(zhǔn)所需時(shí)間小于200 s，方為對(duì)準(zhǔn)所需時(shí)間小于450 s。采用UPF，水平對(duì)準(zhǔn)所需時(shí)間約為400 s，方位對(duì)準(zhǔn)約為600 s。自適應(yīng)UPF的對(duì)準(zhǔn)時(shí)間明顯優(yōu)于UPF，但兩者的對(duì)準(zhǔn)精度相當(dāng)。通過(guò)表1的統(tǒng)計(jì)結(jié)果可以看出，在一定程度上采用自適應(yīng)UPF比UPF的對(duì)準(zhǔn)精度更高。

2）實(shí)驗(yàn)二

為了驗(yàn)證兩種濾波方法在具有不確定噪聲情況下時(shí)的濾波性能，特意在三個(gè)方向的加速度上增加了均值為 0.02，方差為 0.02的噪聲，進(jìn)行實(shí)驗(yàn)分析。取

通過(guò)圖 2可以看出，當(dāng)增加觀測(cè)噪聲后，采用UPF，水平方向上的對(duì)準(zhǔn)會(huì)有一個(gè)大幅度的震蕩，對(duì)系統(tǒng)可能造成一定的影響。水平對(duì)準(zhǔn)時(shí)間大約600 s，方位對(duì)準(zhǔn)時(shí)間大約900 s。采用自適應(yīng)UPF后，水平對(duì)準(zhǔn)時(shí)間明顯優(yōu)于UPF，約200 s，且誤差曲線(xiàn)平滑。方位對(duì)準(zhǔn)需要450 s，對(duì)準(zhǔn)時(shí)間明顯優(yōu)于UPF。通過(guò)表2的統(tǒng)計(jì)結(jié)果表明，采用自適應(yīng)UPF的對(duì)準(zhǔn)精度明顯優(yōu)于UPF。

5 結(jié) 論

本文通過(guò)仿真比較了UPF和自適應(yīng)UPF在捷聯(lián)慣導(dǎo)系統(tǒng)大方位失準(zhǔn)情況下噪聲統(tǒng)計(jì)特性固定及噪聲統(tǒng)計(jì)特性不固定的初始對(duì)準(zhǔn)，在噪聲統(tǒng)計(jì)特性確定時(shí)使用自適應(yīng)UPF比普通UPF具有更快的對(duì)準(zhǔn)速度，對(duì)準(zhǔn)精度優(yōu)勢(shì)不明顯。當(dāng)系統(tǒng)噪聲的統(tǒng)計(jì)特性不確定時(shí)，采用自適應(yīng)UPF與普通UPF相比，對(duì)準(zhǔn)速度和對(duì)準(zhǔn)精度具有明顯的優(yōu)勢(shì)。

（References）：

[1] 孫楓, 唐李軍. 基于CKF的SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)技術(shù)[J]. 儀器儀表學(xué)報(bào), 2012, 33(2): 327-335. Sun Feng, Tang Li-jun. Initial alignment of large azimuth misalignment angle in SINS based on CKF[J]. Chinese Journal of Scientific Instrument, 2012, 33(2): 327-335.

[2] Dini D H, Mandic D P, Julier S J. A widely linear complex unscented Kalman filter[J]. IEEE Signal Processing Letters, 2011, 18(11): 623-626.

[3] Donovan G T. Position error correction for an autonomous underwater vehicle inertial navigation system (INS) using a particle filter[J]. IEEE Journal of Oceanic Engineering, 2012, 37(3): 431-445.

[4] Rigatos G G. Nonlinear Kalman filters and particle filters for integrated navigation of unmanned aerial vehicles[J]. Robotics and Autonomous Systems, 2012, 60(7): 978-995.

[5] 薛麗, 高社生, 趙巖. 權(quán)值自適應(yīng)調(diào)整 Unscented 粒子濾波及其在組合導(dǎo)航中的應(yīng)用 [J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2012, 20(4): 459-463. Xue Li, Gao She-sheng, Zhao Yan. Unscented particle filtering with adaptive adjusted weight and its application in integrated navigation[J]. Journal of Chinese Inertial Technology, 2012, 20(4): 459-463.

[6] 嚴(yán)恭敏, 嚴(yán)衛(wèi)生, 徐德民. 簡(jiǎn)化UKF濾波在SINS大失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2008, 16(3): 253-264. Yan Gong-min, Yan Wei-sheng, Xu De-min. Application of simplified UKF in SINS initial alignment for large misalignment angles[J]. Journal of Chinese Inertial Technology, 2008, 16(3): 253-264.

[7] Gustafsson F, Hendeby G. Some relations between extended and unscented Kalman filters[J]. IEEE Transactions on Signal Processing, 2012, 60(2): 545-555.

[8] Vaccarella A, De Momi E, Enquobahrie A, et al. Unscented Kalman filter based sensor fusion for robust optical and electromagnetic tracking in surgical navigation[J]. IEEE Transactions on Instrumentation and Measurement, 2013, 62(7): 2067-2081.

[9] Zhou J, Yang Y, Zhang J, et al. Applying quaternionbased unscented particle filter on INS/GPS with field experiments [C]//Proceedings of the ION GNSS. Portland, 2011: 1-14.

[10] 李昱辰, 李戰(zhàn)明. 噪聲未知情況下的自適應(yīng)無(wú)跡粒子濾波算法[J]. 吉林大學(xué)學(xué)報(bào)(工學(xué)版), 2013, 43(4): 1439-1445. Li Yu-chen, Li Zhan-ming. Adaptive unscented particle algorithm under unknown noise[J]. Journal of Jilin University (Engineering and Technology Edition), 2013, 43(4): 1439-1445.

Initial alignment of large azimuth misalignment in SINS based on adaptive unscented particle filter

SUN Jin1, XU Xiao-su1, LIU Yi-ting1, ZHANG Li-bin2

(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology of Ministry of Education, School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China; 2. School of Electronic and Electrical Engineering, Changzhou College of Information Technology, Changzhou 213164, China)

An initial alignment method for Strapdown inertial navigation system (SINS) when with large azimuth misalignment angle is proposed based on an adaptive unscented particle filter (UPF). Inspired by strong tracking filter, a memory attenuation factor is introduced in this adaptive UPF to effectively correct the system by the current information residual. This adaptive UPF can help reduce the influence of the simplified model and the uncertain statistical properties’ noise on the system. Meanwhile, this filter could retard the particle degenerating which often occurs in the particle filter. When the statistics of the system noise is certain, the level alignment time of adaptive UPF is about 200 s less than that of UPF, the azimuth alignment time is about 150 s less; while after the alignment, the pitching, rolling and heading error mean of the adaptive UPF are -0.018°, 0.0074° and -0.8609° respectively, and the corresponding values of UPF are 0.0209°, 0.0189° and 1.0983° respectively. When the statistics of the system noise is uncertain, the level alignment time of adaptive UPF is about 400 s less than that of UPF, the azimuth alignment time is about 450 s less; while after the alignment, the pitching, rolling and heading error mean of the adaptive UPF are 0.0105°, 0.0122° and -0.7304° respectively, and the corresponding values of UPF are 0.0454°, 0.0278° and 2.8174° respectively. Simulation results show that the adaptive UPF can improvethe alignment accuracy and speed of the SINS, no matter the statistical properties of the noise be sure or not.

strapdown inertial navigation system; large misalignment angle; initial alignment; unscented particle filter; adaptive UPF

U666.1

A

1005-6734(2016)02-0154-06

10.13695/j.cnki.12-1222/o3.2016.02.003

2015-12-28；

2016-03-17

國(guó)家自然科學(xué)基金項(xiàng)目（61473085，51175082，51375088，61273056）

孫進(jìn)（1988—），男，博士研究生，從事組合導(dǎo)航研究。E-mail: sunjin8607986@126.com

聯(lián) 系 人：徐曉蘇（1961—），男，教授，博士生導(dǎo)師。E-mail: xxs@seu.edu.cn