基于自適應無跡粒子濾波的SINS大方位失準角初始對準

孫 進，徐曉蘇，劉義亭，張利斌

（1. 東南大學 儀器科學與工程學院 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；2. 常州信息職業技術學院 電子與電氣工程學院，常州 213164）

基于自適應無跡粒子濾波的SINS大方位失準角初始對準

孫 進1，徐曉蘇1，劉義亭1，張利斌2

（1. 東南大學 儀器科學與工程學院 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；2. 常州信息職業技術學院 電子與電氣工程學院，常州 213164）

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

捷聯慣導系統；大失準角；初始對準；無跡粒子濾波；自適應無跡粒子濾波

初始對準是捷聯慣導系統中的關鍵技術之一，其對準時間和精度直接影響慣導系統的工作性能[1]。

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

1 SINS非線性誤差模型

對于捷聯系統大失準角情況，轉動次序引起的誤差無法忽略，必須針對大失準的情況重新建立 SINS的誤差模型。使用歐拉平臺誤差角來表示理想導航坐標系與計算導航坐標系之間的失準角，且該組誤差角需要考慮轉動的先后順序。建立相應的 SINS非線性誤差模型。

本文坐標系選取如下：

i系——地心慣性坐標系；

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

n系——選取地理坐標系為導航坐標系；

b系——“右-前-上”坐標系為捷聯慣導坐標系。

n系先后依次經過三次歐拉角轉動至b系，這三個歐拉角分別記為航向角俯仰角和橫滾角系與b系之間的旋轉變換關系可用姿態矩陣描述[6]。

1.1 姿態誤差方程

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

由文獻[6]可以得到SINS姿態誤差方程為

1.2 速度誤差方程

在導航坐標系中，SINS速度微分方程[6]為

但在實際系統中該速度微分方程含有誤差，此時SINS速度微分方程應為

1.3 SINS大方位失準角初始對準誤差模型

狀態向量取

噪聲向量取

2 UPF和自適應UPF

2.1 UPF算法

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

2）加權粒子的預測、采樣：k = 1, 2, …。利用無跡粒子濾波對粒子進行預測更新并計算 Sigma點，

時間更新為

量測更新為

3）根據權值更新公式

對N個粒子相應的權值進行計算并做歸一化處理。

2.2 本文自適應UPF算法

基于強跟蹤濾波的的思想，采用時變漸消因子削弱陳舊數據對當前濾波值得影響， 實時調整狀態預測誤差的協方差以及相應的增益矩陣來達到。這在一定程度上能夠減緩UPF粒子退化的現象，并且加快粒子濾波的收斂速度。

本文采取的自適應措施是對濾波器的協方差進行判斷，具體判斷公式如下[10]：式中：S為設定的調整系數，一般取為系統的殘差序列

修正公式為

式中，

本文自適應UPF的實現步驟如下：

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

2）預測更新

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

4）量測更新

5）根據權值更新公式

對N個粒子相應的權值進行計算并做歸一化處理。

6） 通過重采樣算法計算重采樣的粒子及其權值，

3 UPF濾波影響因素分析——重要性概率密度函數對UPF精度的影響

重要性概率密度函數的選擇集中體現在粒子的權重更新部分，即[10]

4 仿真及分析

4.1 仿真條件

在靜基座條件下：陀螺儀的常值漂移為0.01 (°)/h，隨機漂移為；加速度計零偏為100 μg (g = 9.8 m2/s)，隨機偏差為50 μg；當地地理緯度為32.37°，經度為118.22°，仿真時間為2000 s。

4.2 仿真結果與分析

1）實驗1

按照建立的方位大失準角誤差模型，在噪聲統計特性確定的情況下分別采用兩種濾波算法進行了仿真實驗?，F選取初始失準角在使用兩種濾波算法的情況下。仿真過程中不進行反饋修正，對準誤差的仿真結果如圖1、圖2所示。

圖1 大方位失準角對準誤差Fig.1 Alignment errors of large azimuth misalignment angles

圖2 大失準角對準誤差Fig.2 Alignment errors of large misalignments angles

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

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

通過圖 1 可以看出，基于大方位失準角的 SINS誤差模型，當初始失準角為時，采用自適應UPF，水平對準所需時間小于200 s，方為對準所需時間小于450 s。采用UPF，水平對準所需時間約為400 s，方位對準約為600 s。自適應UPF的對準時間明顯優于UPF，但兩者的對準精度相當。通過表1的統計結果可以看出，在一定程度上采用自適應UPF比UPF的對準精度更高。

2）實驗二

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

通過圖 2可以看出，當增加觀測噪聲后，采用UPF，水平方向上的對準會有一個大幅度的震蕩，對系統可能造成一定的影響。水平對準時間大約600 s，方位對準時間大約900 s。采用自適應UPF后，水平對準時間明顯優于UPF，約200 s，且誤差曲線平滑。方位對準需要450 s，對準時間明顯優于UPF。通過表2的統計結果表明，采用自適應UPF的對準精度明顯優于UPF。

5 結 論

本文通過仿真比較了UPF和自適應UPF在捷聯慣導系統大方位失準情況下噪聲統計特性固定及噪聲統計特性不固定的初始對準，在噪聲統計特性確定時使用自適應UPF比普通UPF具有更快的對準速度，對準精度優勢不明顯。當系統噪聲的統計特性不確定時，采用自適應UPF與普通UPF相比，對準速度和對準精度具有明顯的優勢。

（References）：

[1] 孫楓, 唐李軍. 基于CKF的SINS大方位失準角初始對準技術[J]. 儀器儀表學報, 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] 薛麗, 高社生, 趙巖. 權值自適應調整 Unscented 粒子濾波及其在組合導航中的應用 [J]. 中國慣性技術學報, 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] 嚴恭敏, 嚴衛生, 徐德民. 簡化UKF濾波在SINS大失準角初始對準中的應用[J]. 中國慣性技術學報, 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] 李昱辰, 李戰明. 噪聲未知情況下的自適應無跡粒子濾波算法[J]. 吉林大學學報(工學版), 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

國家自然科學基金項目（61473085，51175082，51375088，61273056）

孫進（1988—），男，博士研究生，從事組合導航研究。E-mail: sunjin8607986@126.com

聯 系 人：徐曉蘇（1961—），男，教授，博士生導師。E-mail: xxs@seu.edu.cn