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

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

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

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

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

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

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

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

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

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

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

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

其中有：??

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

速度誤差方程: ??

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

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

2.1UPF算法??

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

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





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

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

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

如下：?

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

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

forn=1:N ?

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

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

end ??

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

3仿真分析

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

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

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

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

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

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

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

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

4結(jié)論

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

參考文獻(xiàn)：

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

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

[3]嚴(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(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é)報(bào),2010,31(01):120-124.