大失準(zhǔn)角SINS初始對準(zhǔn)的UPF算法

王磊,萬超（導(dǎo)航技術(shù)研究所,西安710068）

大失準(zhǔn)角SINS初始對準(zhǔn)的UPF算法

王磊,萬超
（導(dǎo)航技術(shù)研究所,西安710068）

摘要：針對大失準(zhǔn)角條件下捷聯(lián)慣導(dǎo)系統(tǒng)（SINS）初始對準(zhǔn)誤差模型呈嚴(yán)重非線性的特點，設(shè)計了大失準(zhǔn)角SINS的無跡粒子濾波（UPF）初始對準(zhǔn)算法；并針對UPF算法實時性不強(qiáng)的特點，采用殘差系統(tǒng)重采樣對其重采樣方法進(jìn)行改進(jìn)；再結(jié)合三軸轉(zhuǎn)臺靜基座采集得到的實測數(shù)據(jù)，分別進(jìn)行了UPF和無跡卡爾曼濾波（UKF）算法仿真，仿真結(jié)果表明：UPF算法比UKF算法具有更高的對準(zhǔn)精度和更快的對準(zhǔn)速度；改進(jìn)后的UPF算法在對準(zhǔn)精度上略差一些，但比原算法具有更強(qiáng)的實時性。

關(guān)鍵詞：捷聯(lián)慣導(dǎo)系統(tǒng);大失準(zhǔn)角;初始對準(zhǔn);UPF濾波;重采樣

大失準(zhǔn)角條件下，SINS初始對準(zhǔn)誤差模型呈線性化假設(shè)不再成立，線性姿態(tài)誤差模型不再可用，于是使用傳統(tǒng)的濾波方法進(jìn)行初始對準(zhǔn)狀態(tài)估計受到局限。

目前，針對大失準(zhǔn)角非線性誤差模型，初始對準(zhǔn)濾波方法主要有擴(kuò)展卡爾曼濾波（EKF）、UKF、粒子濾波（PF）。EKF通過對非線性函數(shù)的泰勒級數(shù)展開式進(jìn)行一階線性化截斷，從而將非線性問題轉(zhuǎn)化為線性問題。但當(dāng)非線性函數(shù)泰勒級數(shù)展開式的高階項無法忽略時，EKF將導(dǎo)致很大的線性化誤差，造成濾波器精度變低。文獻(xiàn)[3]在大失準(zhǔn)角SINS初始對準(zhǔn)條件下推導(dǎo)了簡化UKF算法，并通過仿真試驗證明了算法的有效性。粒子濾波是英國學(xué)者Gorden等于1933年提出的。該算法能得到基于物理模型的近似最優(yōu)數(shù)值解，而且簡單實用，對系統(tǒng)的動態(tài)模型概率分布形式具有良好的魯棒性，受濾波初始誤差影響小，可適用于任何非線性系統(tǒng)模型，但隨著算法的迭代會出現(xiàn)權(quán)值退化現(xiàn)象。文獻(xiàn)[2]將EKF作為粒子濾波的重要性函數(shù)，證明了EPF算法在一定程度上解決了權(quán)值退化現(xiàn)象，而且具有比傳統(tǒng)的EKF算法對準(zhǔn)時間快，對準(zhǔn)精度高的特點。

本文在粒子濾波的基礎(chǔ)上通過將UKF作為其重要性分布函數(shù)來解決粒子退化問題，結(jié)合實測數(shù)據(jù)，研究比較UPF算法與UKF算法在SINS大失準(zhǔn)角初始對準(zhǔn)條件下的優(yōu)劣性，并針對UPF算法實時性差的缺陷，使用殘差系統(tǒng)重采樣對其重采樣方法進(jìn)行改進(jìn)，以達(dá)到提高實時性的目的。

1SINS大失準(zhǔn)角誤差模型

1.1SINS大失準(zhǔn)角非線性狀態(tài)誤差方程??

將（1）式三個矩陣逆序相乘得系至系的變換矩陣：??

從而由（3）式得歐拉平臺誤差角微分方程：??

其中有：??

由（4）式，根據(jù)文獻(xiàn)[3]，以歐拉平臺誤差角表示的SINS非線性姿態(tài)誤差方程為: ??

速度誤差方程: ??

1.2靜基座初始對準(zhǔn)誤差模型??

2UPF算法及其改進(jìn)

2.1UPF算法??

UPF算法是一種考慮到先驗密度和觀測信息的作用，把UKF和PF相結(jié)合的濾波方法，利用UKF實時的進(jìn)行狀態(tài)的均值和方差估計，以正態(tài)分布作為PF粒子采樣的重要性函數(shù)，從而可以將采樣粒子從高先驗密度區(qū)向高似然密度區(qū)移動。其三個主要步驟是：產(chǎn)生新的粒子、計算粒子權(quán)重和重采樣，解算主要流程如下：??

（7）設(shè)定k=k+1，返回第2步。??





UPF改進(jìn)了一般PF濾波方法中重要性函數(shù)的選擇方法，有利于將采樣粒子從高先驗密度區(qū)向高似然密度區(qū)移動，可以提高濾波精度。相對與一般PF濾波而言，UPF解算計算量有所增加，且同樣需要重采樣過程，所以在它具有良好的對準(zhǔn)精度的同時，卻不可避免的帶來了實時性不強(qiáng)的缺點。

2.2UPF重采樣方法及其改進(jìn)??

重采樣是決定解決粒子權(quán)值退化問題和提高算法實時性的關(guān)鍵步驟。UPF算法中的重采樣方法是系統(tǒng)重采樣法，其算法代碼簡述

如下：?

從上述代碼可以看出，系統(tǒng)重采樣通過循環(huán)比較Wsum與U值大小，來確定原粒子分解的個數(shù)。其中包含了兩層循環(huán)和比較運算，所需要的存儲空間為而殘差系統(tǒng)重采樣算法只需要一層循環(huán)，且無比較運算，其算法代碼簡述如下：?

產(chǎn)生一個隨機(jī)數(shù)U~u[0,1/N]；?

forn=1:N ?

i(n)=[(w(n)-U·n]+1；（[]為取整運算）

U=U+i（n)/N-w(n)；

end ??

殘差系統(tǒng)重采樣的U值隨機(jī)產(chǎn)生后并不進(jìn)行累加運算，而是通過每次粒子分解結(jié)果進(jìn)行調(diào)整，而且沒有比較運算，所需要的存儲空間為而且只需要一層循環(huán)，理論上提高了代碼的運行效率，因此考慮用殘差系統(tǒng)重采樣方法代替原有重采樣方法，可以提高UPF算法的實時性。將改進(jìn)后的UPF稱為IRUPF(improvedresamplingUPF)。

3仿真分析

仿真中實際量測數(shù)據(jù)由三軸轉(zhuǎn)臺靜基座下采集獲得。具體方法是：三軸轉(zhuǎn)臺歸零（“東北天”位置）后，驅(qū)動內(nèi)、中、外三個框都靜止在5，開啟光纖捷聯(lián)系統(tǒng)，預(yù)熱30min，室溫條件下，以100Hz采樣頻率采集30min陀螺和加速度計數(shù)據(jù)。陀螺的隨機(jī)漂移為，加速度計零偏和隨機(jī)偏差都為，速度誤差為0.1m/s，初始位置為[34.24.108.9,380]，狀態(tài)初值x(0)均為0，失準(zhǔn)角為，粒子數(shù)為200，仿真時間為10秒，時刻更新1000次，分別采用UKF和UPF仿真，并使用濾波結(jié)果閉環(huán)對速度和姿態(tài)進(jìn)行修正，得到如下仿真結(jié)果見下圖。

圖1大失準(zhǔn)角初始對準(zhǔn)誤差

圖2改進(jìn)后的UPF在對準(zhǔn)精度上和其他兩種算法對比

圖3改進(jìn)前后UPF算法在實時性上的對比

表1失準(zhǔn)角估計的均方誤差值

結(jié)合表1，從圖1可以看出，在三個方向的對準(zhǔn)精度上，UPF明顯要高于UKF，在對準(zhǔn)時間上，UPF在北向和東向失準(zhǔn)角的第30次時刻更新時就已經(jīng)開始進(jìn)入收斂狀態(tài)，而UKF要推遲到第70次時刻更新。即使在一般收斂速度比較慢的天向失準(zhǔn)角上，UPF也保持了這種優(yōu)越性。從圖中還可以看出兩種曲線收斂后，UPF的抗干擾能力要比UKF強(qiáng)，而且不易發(fā)散。

圖2是IRUPF在對準(zhǔn)精度上和其他兩種算法的比較，介于篇幅，只給出東向誤差仿真圖。從表1中可以得出IRUPF在精度上要比UPF略差一些，而且從圖1中可以看到IRUPF的毛刺比較多，但總體效果仍舊比UKF好，同時在圖3中，UPF完成仿真需要近50分鐘，而IRUPF只需要5分鐘，很明顯在實時性上IRUPF要優(yōu)于UPF。在圖3中，橫坐標(biāo)為代碼設(shè)定的仿真時間，縱坐標(biāo)為實際完成仿真所需時間，本文所采用的計算平臺為Intel(R)CPUE65502.33GHZ,內(nèi)存為1G，仿真軟件為Matlab7.8.0。

另外，粒子數(shù)目越多，粒子濾波的精度就會越高。但是隨著粒子數(shù)目的增加，迭代時刻的延長，就造成計算量增大，使得運算時間比較長，反而嚴(yán)重影響了初始對準(zhǔn)的性能。在實際工作中，粒子數(shù)目的選擇需在對準(zhǔn)精度和實時性這兩種性能之間做取舍。

4結(jié)論

文中將UKF作為PF的重要性分布函數(shù)，設(shè)計了大失準(zhǔn)角SINS的無跡粒子濾波（UPF）初始對準(zhǔn)算法。通過實測數(shù)據(jù)的試驗仿真，驗證了在狀態(tài)方程為非線性，量測方程為線性的SINS大失準(zhǔn)角條件的對準(zhǔn)模型中，UPF算法在對準(zhǔn)精度和對準(zhǔn)時間上的有效性；并使用殘差系統(tǒng)重采樣對原始的UPF重采樣方法進(jìn)行改進(jìn)，使得原UPF的重采樣方法由兩層循環(huán)變?yōu)橐粚?，且舍去了比較運算，提高了代碼運行效率，并通過試驗仿真驗證了改進(jìn)后的算法極大的提高了初始對準(zhǔn)的實時性。

參考文獻(xiàn)：

[1]秦永元,張洪鉞,汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西北工業(yè)大學(xué)出版社,1998.

[2]王志剛,陳良友,邊少峰.EPF算法在慣導(dǎo)非線性初始對準(zhǔn)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報,2007,15(02):165-166.

[3]嚴(yán)恭敏,嚴(yán)衛(wèi)生,徐德民.簡化UKF濾波在SINS大失準(zhǔn)角初始對準(zhǔn)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報,2008,16(03):364-368.

[4]JohnL.Crassidis,F.LandisMarkley.UnscentedFilteringforSpacecraftAttitudeEstimation[J].JournalofGuldanceandDynamics,2003,26(04):536-542.

[5]JulierSJ,UlmanJK.Uhlmann.Unscentedfilteringandnonlinearestimation[J].prodeedingsoftheIEEE,2004,92(03):401-422.

[6]馮池,趙娜,王萌.一種改進(jìn)殘差重采樣算法研究[J].哈爾濱工程大學(xué)學(xué)報,2010,31(01):120-124.