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

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

0 引言

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

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

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

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

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

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

速度誤差方程可表示為：

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

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

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

式中：w為量測噪聲，假設(shè)為獨(dú)立高斯分布。

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

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

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

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

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

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

1）計(jì)算Sigma點(diǎn)：

2）時(shí)間更新：

3）量測更新：

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

3 Unscented粒子濾波

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

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

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

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

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

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

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

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

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

2）i=1,2,…,N時(shí)刻，在每個時(shí)刻，對每個粒子進(jìn)行如下運(yùn)算：

3）利用重采樣算法，對加權(quán)粒子進(jìn)行重采樣；

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

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

4 仿真

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

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

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

圖1 水平東向誤差角

圖2 水平北向誤差角

圖3 方位誤差角

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

5 結(jié)論

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

[1]趙瑞.捷聯(lián)慣導(dǎo)系統(tǒng)自動標(biāo)定和初始對準(zhǔn)的理論和實(shí)驗(yàn)研究[D].北京∶清華大學(xué),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]朱志宇.粒子濾波算法及其應(yīng)用[M].北京∶科學(xué)出版社,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]秦永元.慣性導(dǎo)航[M].北京∶科學(xué)出版社,2006∶7-8.

[8]雄凱,張洪鉞.粒子濾波在慣導(dǎo)系統(tǒng)非線性對準(zhǔn)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào),2003,11(6)∶20-22.