戴邵武，鄭智翔，戴洪德，曹亮杰

（海軍航空工程學院 a.控制工程系；b.研究生管理大隊，山東 煙臺 264001）

0 引言

初始對準作為慣性導航系統(tǒng)的關鍵技術之一，對準精度直接影響導航系統(tǒng)的精度。當失準角為小角度時，捷聯(lián)慣導系統(tǒng)（SINS）的誤差方程可近似為線性方程，并可采用卡爾曼濾波估計SINS 失準角；當運載體遭遇惡劣環(huán)境，姿態(tài)變化劇烈，此時利用粗對準得到的初始姿態(tài)誤差將非常大，基于小失準角的系統(tǒng)誤差模型已經不能準確地描述慣性導航系統(tǒng)的誤差傳播特性，則需引入大失準角誤差方程，以減少初始對準中的非線性誤差。

本文主要研究SINS 初始對準中大失準角的濾波問題。文中首先建立基于大失準角的SINS 誤差模型，接著介紹2種非線性濾波——Unscented卡爾曼濾波（UKF）和Unscented粒子濾波（UPF），然后對誤差模型進行仿真分析。仿真結果表明，在大失準角下的初始對準中，UPF較 UKF 具有更理想的濾波效果。

1 捷聯(lián)慣導系統(tǒng)非線性誤差模型

合理、準確的慣導系統(tǒng)誤差模型，可以提高初始對準精度，縮短對準時間。慣性導航系統(tǒng)的誤差模型本質上是非線性的，與線性誤差模型相比，非線性誤差模型不僅能更準確地描述誤差的傳播特性，而且由于不需要很多約束條件，可以進一步擴展誤差模型的應用范圍。下面將建立大方位失準角情況下的非線性初始對準模型。

首先，通過加速度計和陀螺敏感地球重力g和地球自轉角速率ωie的信息，估算載體坐標系到地理坐標系的變換陣；然后，再精確估計計算地理坐標系與真實地理坐標系之間的失準角?，從而建立起準確的初始變換矩陣，使計算地理坐標系與實際地理坐標系重合，完成捷聯(lián)慣導系統(tǒng)的初始對準。對準中，從地理坐標系 (t)到計算平臺系 (p)的轉換矩陣為：

因此，姿態(tài)誤差方程可表示為[1]：

速度誤差方程可表示為：

由于垂直通道與兩個水平通道的耦合很小，因此將垂直通道略去，即不考慮 Uvδ的影響。不考慮位置誤差，因此可得到靜基座大方位失準角時的速度誤差方程和姿態(tài)誤差方程為[1]：

則根據(jù)式(4)以及(2)和(3)可得到初始對準的非線性系統(tǒng)方程：

式中：v為系統(tǒng)噪聲，假設為獨立高斯分布。

式中：w為量測噪聲，假設為獨立高斯分布。

2 UKF算法及實現(xiàn)

擴展卡爾曼濾波應用于非線性系統(tǒng)的狀態(tài)估計得到了學術界和工程界的普遍認可，但由于EKF 將動力學模型在當前狀態(tài)估計值處進行Taylor 展開線性化，并將量測模型在狀態(tài)一步預測處進行Taylor展開線性化，僅近似到非線性函數(shù)Taylor 展開式的一次項，在估計狀態(tài)后驗分布的統(tǒng)計特性時會產生較大誤差。

為改善非線性濾波效果，S.J.Juliear 等提出了基于UT（Unscented Transformation）采樣的卡爾曼濾波方法（UKF）。[2-3]

在UKF中，狀態(tài)分布同樣為高斯分布，其特性由一組確定選擇的采樣點給出。這些采樣點能完全捕獲高斯分布變量的均值和方差，通過真實非線性系統(tǒng)的傳播后，其捕獲的均值和方差能精確到任意非線性的Taylor 展開的二次項。

設隨機非線性系統(tǒng)的狀態(tài)空間模型為：

該系統(tǒng)的UKF算法為：

1）計算Sigma點：

2）時間更新：

3）量測更新：

顯然，實現(xiàn)UKF 不需要計算Jacobian 矩陣，也不需要對系統(tǒng)方程和量程方程線性化。

3 Unscented粒子濾波

粒子濾波[4]（PF）是一種序列蒙特卡羅濾波方法，它通過對一組隨機采樣的加權粒子演化與傳播來遞推近似狀態(tài)的后驗概率密度函數(shù)，從而獲得其他關于狀態(tài)的統(tǒng)計量。從理論上來說粒子濾波可以應用于任何動態(tài)狀態(tài)空間模型，是一種最優(yōu)的非線性濾波器。

對于服從p(x)分布的非線性系統(tǒng)f(x)，從中采樣一定數(shù)量的粒子（N為粒子數(shù)量）來表示概率密度函數(shù)，有

式中：δ (?)為Dirac delta函數(shù)；1/N為對應的權值，故可得f(x)的期望估計值為

對狀態(tài)方程是非線性，觀測方程是線性的非線性系統(tǒng)離散化，可寫成

粒子濾波中，權值較小的粒子隨運算的進行而很快消失，權值大的粒子得以保留，這種粒子衰竭的現(xiàn)象嚴重影響了粒子濾波的性能。

采用重采樣可有效遏制粒子的衰竭，保持粒子的平衡，重采樣后粒子對應的權值wk,i均被重新定義為1/N。

確定了k時刻的粒子值與對應的歸一化權值，則系統(tǒng)狀態(tài)最優(yōu)估計值為

設非線性系統(tǒng)過程噪聲的方差為Q，觀測噪聲的方差為R，綜合Unscented卡爾曼濾波和粒子濾波，Unscented粒子濾波算法如下[6-7]：

1）在t=0時刻，從 x0～ p(x0)中采樣N個粒子x0,i(i=1,2,…,N)對非線性系統(tǒng)初始化：

2）i=1,2,…,N時刻，在每個時刻，對每個粒子進行如下運算：

3）利用重采樣算法，對加權粒子進行重采樣；

4）計算非線性狀態(tài)量估計值；

5）返回第2）步，按新觀測量遞歸計算下一個時刻的狀態(tài)估計量。

4 仿真

對某捷聯(lián)慣導系統(tǒng)進行初始對準，狀態(tài)矢量x的初始值X(0)均取0，初始失準角 φE=φN=1°，φU=10°，陀螺常值漂移為0.02 (°)/h，隨機漂移為(1.0×10?3(°)/)2，加 速 度 計的初 始 偏 差為1.0×10?4m/s2，隨機偏差為(1.0×10?5m/s3/2)2，速度測量誤差為0.01 m/s，安裝誤差為10′，當?shù)氐乩砭暥葹楸本?40 °，初始狀態(tài)采樣的粒子服從p(x(0))～N(x(0),P0)。則P0、Q、R為

給定初始狀態(tài)為

從重要性函數(shù)中采樣的粒子數(shù)N=100，分別用Unscented卡爾曼濾波和Unscented粒子濾波進行對準，結果如圖1～3所示：

圖1 水平東向誤差角

圖2 水平北向誤差角

圖3 方位誤差角

從圖1、圖2中可以看出，用Unscented卡爾曼濾波和Unscented粒子濾波對水平角的對準均能快速收斂至穩(wěn)態(tài)值，對準誤差也快速收斂到了穩(wěn)態(tài)值。顯然，在大方位失準角對準中，利用Unscented粒子濾波處理達到穩(wěn)態(tài)值的速度比用Unscented卡爾曼濾波處理達到穩(wěn)態(tài)值的速度要快。在圖3方位角對準誤差圖中可以看出Unscented粒子濾波誤差減少速率比Unscented卡爾曼濾波減少速率快，其中Unscented粒子濾波達到的穩(wěn)定對準誤差是2.05′，而 Unscented卡爾曼濾波達到的穩(wěn)定對準誤差是2.17′，理論的穩(wěn)定對準精度是1.97′。由此比較可以看出，應用Unscented粒子濾波對準的精度比Unscented卡爾曼濾波對準的精度要高。

5 結論

對Unscented卡爾曼濾波和Unscented卡爾曼粒子濾波在捷聯(lián)慣導系統(tǒng)大方位失準角初始對準進行了研究，利用Unscented卡爾曼濾波和Unscented卡爾曼粒子濾波對靜基座大方位失準角捷聯(lián)慣導系統(tǒng)初始對準進行分析和仿真。結果證實了兩種非線性濾波的可行性，且Unscented卡爾曼粒子濾波在初始對準中效果優(yōu)于Unscented卡爾曼濾波。

[1]趙瑞.捷聯(lián)慣導系統(tǒng)自動標定和初始對準的理論和實驗研究[D].北京∶清華大學,2001∶58-118.

[2]JULIER S J,UHLMAN J K.A new approach for filtering nonlinear systems[C]//Proceedings of the 1995 American Control Conference.Seattle,W A∶[s.n.],1995∶1628-1632.

[3]JULIER S J,UHLMANNN J K.A new method for the nonlinear transformations of means and covariances in filters and estimator[J].IEEE Trans A C,2000∶45(3)∶477-482.

[4]GORDON N J,SALMOND D J,SMITH A F M.Novel approach to nonlinear/non-gaussian bayesian state estimation[J].IEEE Proceedings of Radar and Signal Processing,1993∶140(2)∶107-113.

[5]朱志宇.粒子濾波算法及其應用[M].北京∶科學出版社,2010∶25-26.

[6]MERWE R V,DOUCET A,DE FREITAS N,et al.The unscented particle filter[R]//Technical Report CUED/F-INPENG/TR 380.Cambridge University Engineering Department,2000.

[7]秦永元.慣性導航[M].北京∶科學出版社,2006∶7-8.

[8]雄凱,張洪鉞.粒子濾波在慣導系統(tǒng)非線性對準中的應用[J].中國慣性技術學報,2003,11(6)∶20-22.