大失準角SINS初始對準的UPF算法

王磊,萬超（導航技術研究所,西安710068）

大失準角SINS初始對準的UPF算法

王磊,萬超
（導航技術研究所,西安710068）

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

關鍵詞：捷聯慣導系統;大失準角;初始對準;UPF濾波;重采樣

大失準角條件下，SINS初始對準誤差模型呈線性化假設不再成立，線性姿態誤差模型不再可用，于是使用傳統的濾波方法進行初始對準狀態估計受到局限。

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

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

1SINS大失準角誤差模型

1.1SINS大失準角非線性狀態誤差方程??

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

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

其中有：??

由（4）式，根據文獻[3]，以歐拉平臺誤差角表示的SINS非線性姿態誤差方程為: ??

速度誤差方程: ??

1.2靜基座初始對準誤差模型??

2UPF算法及其改進

2.1UPF算法??

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

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





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

2.2UPF重采樣方法及其改進??

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

如下：?

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

產生一個隨機數U~u[0,1/N]；?

forn=1:N ?

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

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

end ??

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

3仿真分析

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

圖1大失準角初始對準誤差

圖2改進后的UPF在對準精度上和其他兩種算法對比

圖3改進前后UPF算法在實時性上的對比

表1失準角估計的均方誤差值

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

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

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

4結論

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

參考文獻：

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

[2]王志剛,陳良友,邊少峰.EPF算法在慣導非線性初始對準中的應用[J].中國慣性技術學報,2007,15(02):165-166.

[3]嚴恭敏,嚴衛生,徐德民.簡化UKF濾波在SINS大失準角初始對準中的應用[J].中國慣性技術學報,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].哈爾濱工程大學學報,2010,31(01):120-124.