戴邵武,鄭智翔,戴洪德,曹亮杰
(海軍航空工程學院 a.控制工程系;b.研究生管理大隊,山東 煙臺 264001)
初始對準作為慣性導航系統(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 具有更理想的濾波效果。
合理、準確的慣導系統(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為量測噪聲,假設為獨立高斯分布。
擴展卡爾曼濾波應用于非線性系統(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)方程和量程方程線性化。
粒子濾波[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)估計量。
對某捷聯(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卡爾曼濾波對準的精度要高。
對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.