超球體平方根平淡卡爾曼濾波及其應(yīng)用

王躍鋼，蔚 躍，雷堰龍，陳蘇邑

（解放軍第二炮兵工程大學(xué)自動化系，陜西 西安 710025）

0 引言

初始對準(zhǔn)對于慣導(dǎo)系統(tǒng)的精度與啟動準(zhǔn)備時間有著直接的關(guān)系，所以一直被看作慣性導(dǎo)航技術(shù)領(lǐng)域的重要問題之一［1］。捷聯(lián)慣導(dǎo)系統(tǒng)（strap－down inertial navigation system，SINS）初始對準(zhǔn)的實(shí)質(zhì)就是確定載體坐標(biāo)系到真實(shí)導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣。由于初始對準(zhǔn)誤差將嚴(yán)重影響系統(tǒng)的整體誤差，因此要求初始對準(zhǔn)的對準(zhǔn)精度要高，對準(zhǔn)時間要短。采用卡爾曼濾波是實(shí)現(xiàn)慣導(dǎo)系統(tǒng)自對準(zhǔn)的有效途徑之一，但卡爾曼濾波使用的條件是只能用于線性系統(tǒng)，對于非線性系統(tǒng)常用的方法是采用擴(kuò)展卡爾曼濾波（extended kalman filter，EKF），而在大方位失準(zhǔn)角情況下，這種近似線性化的方法舍棄了非線性函數(shù)的高階項(xiàng)，引入了誤差，使濾波結(jié)果為次優(yōu)，甚至導(dǎo)致濾波發(fā)散［2］。Julier［3］等人從非線性均值和方差傳播的角度提出平淡卡爾曼濾波（unscented kalman filter，UKF），這種方法直接利用非線性模型進(jìn)行遞推估計(jì)，避免了線性化誤差的引入且遞推估計(jì)過程不需計(jì)算矩陣，但隨著狀態(tài)空間維數(shù)的增加，計(jì)算量也會急劇增大，加重系統(tǒng)計(jì)算負(fù)擔(dān)，同時無法保證在濾波更新階段生成的狀態(tài)協(xié)方差陣非負(fù)定［4］，從而直接影響采樣點(diǎn)生成時進(jìn)行的Cholesky分解。為此，本文提出超球體平方根平淡卡爾曼濾波方法，解決上述問題。

文章第一、二章為基礎(chǔ)鋪墊，第三章將超球體采樣及平方根濾波結(jié)合提出一種新的濾波方法，并應(yīng)用到捷聯(lián)慣導(dǎo)系統(tǒng)大失準(zhǔn)角下的非線性誤差模型中，第四章通過matlab數(shù)值仿真，分別與EKF和UKF進(jìn)行了對比，說明了新方法的可行性。

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

1.1 速度誤差方程

式中，W為量測噪聲，B為噪聲驅(qū)動陣。

大失準(zhǔn)角下的速度誤差方程為［5］：

令歐拉失準(zhǔn)角φx，φy，φz的轉(zhuǎn)動順序?yàn)棣誾、φx、φy。則可求得C由式（2）確定：

1.2 姿態(tài)誤差方程

大失準(zhǔn)角下姿態(tài)誤差方程為［5］：

1.3 非線性對準(zhǔn)濾波模型

假設(shè)陀螺和加速度計(jì)誤差僅由常值漂移εb＝［εbxεbyεbz］T，▽b＝ ［▽bx▽by▽bz］T和觀測噪聲εg＝ ［εgxεgyεgz］T，▽r＝ ［▽rx▽ry▽rz］T組成，忽略其他因素的影響，可以表示為：

選取狀態(tài)變量為：

式（1）、式（3）、式（5）組成了大失準(zhǔn)角下SINS誤差狀態(tài)方程。

取速度誤差為觀測量得到SINS觀測方程為：

式（8）中，ζ為觀測噪聲。

式（6）、式（8）構(gòu)成了非線性對準(zhǔn)的濾波模型。

2 超球體采樣及平方根濾波

2.1 超球體分布采樣點(diǎn)變換方法

Julier等人［3］從非線性均值和方差傳播的角度提出UKF，這種方法直接利用非線性模型進(jìn)行遞推估計(jì)，避免了線性化誤差的引入，且遞推估計(jì)過程不需計(jì)算矩陣，比EKF更為簡單，后來又提出了超球體分 布 采 樣 點(diǎn) 變 換 （spherical simplex unscented transformation，SSUT）方法［6］，減少采樣點(diǎn)的求取，將采樣點(diǎn)數(shù)目由2n＋1減小到n＋2，當(dāng)系統(tǒng)維數(shù)很大時可以極大地降低計(jì)算量［7］。同時SSUT中采樣點(diǎn)具有與狀態(tài)相同的前二階矩。SSUT采樣點(diǎn)的選取步驟如下：

1）選擇第0個權(quán)值W0，并且0≤W0≤1

2）計(jì)算其他權(quán)值

3）初始化一維化狀態(tài)序列

4）空間維數(shù)j＝2，3，…，n時，遞推公式為：

2.2 平方根平淡卡爾曼濾波算法

由于在標(biāo)準(zhǔn)UKF濾波狀態(tài)更新階段，P陣很容易失去正定性［8］。造成協(xié)方差陣P 無法進(jìn)行Cholesky分解，不能生成采樣點(diǎn)。采用平方根平淡卡爾曼濾波解決此問題，該方法在UKF濾波的時間更新和量測更新階段采用平方根濾波將狀態(tài)估計(jì)誤差協(xié)方差陣的Cholesky因子形式Sa直接傳遞，避免了在采樣點(diǎn)計(jì)算時對狀態(tài)估計(jì)誤差協(xié)方差陣的Cholesky分解，從而解決了UKF數(shù)值不穩(wěn)定性的問題，減小了計(jì)算量。

3 超球體平方根平淡卡爾曼濾波算法

改進(jìn)的平淡卡爾曼濾波算法是SSUT方法與平方根UKF結(jié)合的方法，這里稱之為超球體平方根平淡卡爾曼濾波算法，取超球體采樣的首字母以及平方根的第二個字母，該算法英文縮寫為SRUKF（spherical root unscented kalman filter）。

不妨設(shè)非線性系統(tǒng)的模型如式（13）：

式中，過程噪聲wk是均值為0，方差為Q的高斯白噪聲。同樣，量測噪聲vk為均值為0，方差為R的高斯白噪聲。

1）初始化

2）計(jì)算采樣點(diǎn)

3）時間更新

4）量測更新

4 仿真分析

仿真初值按照如下選?。簴|向、北向、天向失準(zhǔn)角分別取2°，2°，15°；陀螺常值漂移取0.02 （°）／h，隨 機(jī) 漂 移 為 0.01 （°）／h；加 速 度 計(jì) 常 值 漂 移 為1×10－4g，緯度取45°，當(dāng)?shù)馗叨?50m，仿真時間400s，SINS采樣周期取0.05s。

按照以上仿真條件分別進(jìn)行EKF，SRUKF，UKF濾波，仿真結(jié)果如表1和圖1—圖3所示。

表1 捷聯(lián)慣導(dǎo)系統(tǒng)的初始對準(zhǔn)穩(wěn)態(tài)均值Tab.1 Steady state mean of SINS initial alignment

其中，表1穩(wěn)態(tài)均值是250～350s仿真結(jié)果取絕對值后的均值。這段時間內(nèi)，濾波進(jìn)入穩(wěn)態(tài)沒有了符號的變化，為了比較穩(wěn)態(tài)值偏移0的距離，因此都取絕對值，100s時間段內(nèi)求均值的目的是為了有效地克服單個奇異點(diǎn)帶來的影響，不會對仿真結(jié)果造成影響。

圖1 東向誤差角估計(jì)值Fig.1 Error estimation of east angle

圖2 北向誤差角估計(jì)值Fig.2 Error estimation of north angle

圖3 天向誤差角估計(jì)值Fig.3 Error estimation of attitude angld

捷聯(lián)慣導(dǎo)系統(tǒng)在該初始條件下水平（東向與北向）誤差角的理論穩(wěn)態(tài)誤差值為18.73″，天向理論穩(wěn)態(tài)誤差值為4.46′。仿真結(jié)果表明：水平誤差角仿真結(jié)果與理論分析的效果相符，三種濾波方法水平失準(zhǔn)角誤差很小，收斂時間也很快，這是因?yàn)樗绞?zhǔn)角是小角度。其次，對于初始對準(zhǔn)的核心部分方位角的對準(zhǔn)精度而言，表1指出SRUKF與UKF的濾波結(jié)果相對于EKF來講與理論值更為接近，這從圖3也可以直接看出，這是因?yàn)镾RUKF和UKF在處理非線性系統(tǒng)的精度能達(dá)到二階，而EKF在處理非線性問題上是按照泰勒級數(shù)展開取一階的線性化方法，其精度只能達(dá)到一階，要想達(dá)到更高精度就必須得求復(fù)雜的雅克比矩陣。因此，雅克比矩陣的求取是限制EKF精度的主要因素。最后，圖3中SRUKF的收斂時間要比UKF快，原因是采樣點(diǎn)數(shù)目由2n＋1減小到n＋2，系統(tǒng)維數(shù)較大時可以減小計(jì)算量。綜上可知，SRUKF相對于EKF提高了濾波精度，相對于UKF縮短了對準(zhǔn)時間，這種改進(jìn)的方法提高了對準(zhǔn)效率。

5 結(jié)論

提出了超球體平方根平淡卡爾曼方法，該方法是通過SSUT與平方根濾波結(jié)合，解決了UKF數(shù)值不穩(wěn)定性的問題，同時當(dāng)系統(tǒng)維數(shù)較大時降低了計(jì)算量。將超球體平方根平淡卡爾曼濾波方法應(yīng)用到SINS大失準(zhǔn)角下的初始對準(zhǔn)當(dāng)中，仿真結(jié)果表明：SRUKF對于EKF能有效提高方位失準(zhǔn)角對準(zhǔn)精度，由18.75′提高到了3.85′；相對于 UKF雖然精度差不多，但是減少了采樣點(diǎn)，減小了計(jì)算量，提高了計(jì)算效率，是一種理想的濾波方法。

［1］鄧正隆.慣性導(dǎo)航原理［M］.哈爾濱：哈爾濱工業(yè)大學(xué)出版社，1994.

［2］李濤.非線性濾波方法存導(dǎo)航系統(tǒng)中的應(yīng)用研究［D］.長沙：國防科技大學(xué)，2003.

［3］Hovland G E，Von Hoff T P，Gallestey E A.Nonlinear estimation methods for parameter tracking in power plants［J］.Control Engineering Practice，2005（13）：1 341－1 345.

［4］NING Xiaolin，F(xiàn)ANG Jiangcheng.An autonomous celestial navigation for LEO salellite based on unscented kalman filter and information fusion［J］.Aerospace Science and Technology，2007（11）：222－228.

［5］龍瑞，秦永元，趙長山.簡化SPKF在捷聯(lián)慣導(dǎo)系統(tǒng)非線性對準(zhǔn)中的應(yīng)用［J］.壓電與聲光，2010，32（5）：746－749.LONG Rui，QIN Yongyuan，ZHAO Changshan.Application of simplified SPKF in non－linear alignment of SINS［J］.Piezoelectrics ＆ Acoustooptics，2010，32（5）：746－749.

［6］Julier S J，Uhlmann J K.Reduced sigma point filters for the propagation of means and covariance through nonlinear transformations［C］／／AACC Proceedings of American Control Conference，2002：887－892

［7］Simon J Julier.The spherical simplex unscented transformation［J］.Proceddings of the IEEE American Control Conference，Denver Colorado，2003：2 430－2 434

［8］SONG Qi，HAN Jianda.An adaptive UKF algorithm for the state and parameter estimations of a mobile robot［J］.Acta Automatica Sinica，2008，34（1）：72－79.