基于EPEA的SINS大失準(zhǔn)角非線性初始對(duì)準(zhǔn)方法

趙 紅 宇， 王 哲 龍， 姜 鳴， 宮 少 奇， 尚 紅

（1.大連理工大學(xué) 控制科學(xué)與工程學(xué)院，遼寧 大連 116024；2.中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所 機(jī)器人學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，遼寧 沈陽(yáng) 100080；3.浙江中控技術(shù)股份有限公司，浙江 杭州 310052；4.中國(guó)地震應(yīng)急搜救中心，北京 100049）

0 引 言

實(shí)際應(yīng)用時(shí)，捷聯(lián)慣導(dǎo)系統(tǒng)（SINS）由于受工作環(huán)境和陀螺儀精度的影響，地球自轉(zhuǎn)角速率甚至?xí)煌勇菰肼曆蜎](méi).粗對(duì)準(zhǔn)結(jié)束后[1]，可能出現(xiàn)大方位失準(zhǔn)角或大失準(zhǔn)角的情況，此時(shí)采用小失準(zhǔn)角誤差模型和線性Kalman濾波技術(shù)不能準(zhǔn)確地描述系統(tǒng)誤差的傳播特性[2].因此，研究大失準(zhǔn)角下的初始對(duì)準(zhǔn)技術(shù)對(duì)于SINS具有十分重要的意義.通常，當(dāng)粗對(duì)準(zhǔn)精度無(wú)法滿足小失準(zhǔn)角假設(shè)、不進(jìn)行或不便進(jìn)行粗對(duì)準(zhǔn)（如空中對(duì)準(zhǔn)）時(shí)，都需要考慮在大失準(zhǔn)角情況下的初始對(duì)準(zhǔn)問(wèn)題[3，4].

近年來(lái)，針對(duì)SINS大失準(zhǔn)角下初始對(duì)準(zhǔn)的誤差模型和非線性估計(jì)方法不斷涌現(xiàn)，Bucy等[5，6]等提出并研究了適用于非線性系統(tǒng)和非線性量測(cè)情況下的擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）算法.但EKF引入了高階項(xiàng)截?cái)嗾`差，必然會(huì)降低模型的準(zhǔn)確性，隨著時(shí)間的延長(zhǎng)，估計(jì)精度難以保證，甚至使濾波器難以穩(wěn)定.此外，在使用EKF算法前必須知道非線性函數(shù)的具體展開(kāi)形式，才能計(jì)算非線性函數(shù)的Jacobian矩陣，且此過(guò)程非常繁瑣并容易出錯(cuò).Julier等提出了處理非線性問(wèn)題的無(wú)跡卡爾曼濾波 （unscented Kalman filter，UKF）算 法[7，8].UKF的獨(dú)特之處在于采用確定性采樣策略近似非線性分布，取代EKF對(duì)非線性模型的線性化處理，避免了求取Jacobian矩陣，能取得更好的濾波性能[9，10].

捷聯(lián)慣導(dǎo)系統(tǒng)依據(jù)載體在初始對(duì)準(zhǔn)時(shí)的運(yùn)動(dòng)狀態(tài)可將初始對(duì)準(zhǔn)分為兩類，即靜基座初始對(duì)準(zhǔn)和動(dòng)基座初始對(duì)準(zhǔn).通常，捷聯(lián)慣導(dǎo)系統(tǒng)的可觀測(cè)性較差，尤其在靜基座情況下其可觀測(cè)性最弱；在動(dòng)基座情況下，通過(guò)使基座有目的地機(jī)動(dòng)可以提高捷聯(lián)慣導(dǎo)系統(tǒng)的可觀測(cè)性，從而提高捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)的收斂速度及估計(jì)精度.本文主要研究靜基座情況下，SINS的初始對(duì)準(zhǔn)問(wèn)題.基于歐拉平臺(tái)誤差角（EPEA）的概念描述理論導(dǎo)航坐標(biāo)系到計(jì)算導(dǎo)航坐標(biāo)系之間的失準(zhǔn)角[3，11]，摒棄經(jīng)典小失準(zhǔn)角誤差模型中無(wú)限轉(zhuǎn)動(dòng)與旋轉(zhuǎn)次序無(wú)關(guān)的做法.在此基礎(chǔ)上推導(dǎo)適用于SINS初始對(duì)準(zhǔn)的非線性誤差模型，該模型對(duì)姿態(tài)誤差和相對(duì)姿態(tài)不作任何線性化假設(shè)，能準(zhǔn)確描述SINS的誤差傳播規(guī)律.在系統(tǒng)噪聲和量測(cè)噪聲均為復(fù)雜加性噪聲并且量測(cè)方程為線性方程時(shí)，詳細(xì)分析大失準(zhǔn)角、大方位失準(zhǔn)角與小失準(zhǔn)角情況下初始對(duì)準(zhǔn)過(guò)程的異同，給出帶阻尼解算的簡(jiǎn)化EKF算法和簡(jiǎn)化UKF算法，并對(duì)兩種濾波算法在靜基座狀態(tài)下的對(duì)準(zhǔn)效果進(jìn)行Monte Carlo[12]仿真比較.

1 SINS的非線性誤差模型

1.1 EPEA微分方程

首先定義文中所用到的坐標(biāo)系：地心慣性坐標(biāo)系記為i系；地球坐標(biāo)系記為e系；導(dǎo)航坐標(biāo)系選取“東 －北 －天”地理坐標(biāo)系，記為n系；機(jī)體坐標(biāo)系選取“右 －前 －上”坐標(biāo)系，記為b系[13].n系依次繞航向軸、俯仰軸、橫滾軸作3次歐拉角旋轉(zhuǎn)可至b系，且n系到b系的旋轉(zhuǎn)變換關(guān)系可用姿態(tài)矩陣Cbn描述.實(shí)際上，帶誤差的計(jì)算導(dǎo)航坐標(biāo)系n′系與理想導(dǎo)航坐標(biāo)系n系之間存在失準(zhǔn)角.類似于n系到b系的轉(zhuǎn)動(dòng)過(guò)程，n系依次經(jīng)過(guò)3次基本旋轉(zhuǎn)可至n′系，記這3次旋轉(zhuǎn)的歐拉誤差角分別為αz、αx和αy，則其確定的坐標(biāo)變換矩陣如下：

根據(jù)有限次基本旋轉(zhuǎn)的復(fù)合原理，n系到n′系的姿態(tài)矩陣為

其中

在水平誤差角αx和αy較小而方位誤差角αz較大的大方位失準(zhǔn)角情況下，有

在歐拉誤差角均為小角度的小失準(zhǔn)角情況下，有

記矢量α＝ （αxαyαz）T，設(shè)（Φ×）為由α構(gòu)造的反對(duì)稱矩陣，則有

設(shè)n′系相對(duì)于n系的角速度為

于是，得

式（7）即為歐拉平臺(tái)誤差角微分方程，它描述了歐拉平臺(tái)誤差角α與n′系角速度ωn′nn′之間的關(guān)系，若能推導(dǎo)出ωn′nn′的變化規(guī)律，則可建立起SINS基于歐拉平臺(tái)誤差角的誤差模型.

若水平誤差角αx和αy為小角度，ωn′nn′可近似為

因此，在大方位失準(zhǔn)角或小失準(zhǔn)角情況下，歐拉平臺(tái)誤差角微分方程可簡(jiǎn)化為

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

理論上，SINS在n系的姿態(tài)矩陣微分方程為

其中（ω×）表示由向量ω構(gòu)成的反對(duì)稱矩陣，且

實(shí)際上，SINS含誤差的姿態(tài)矩陣微分方程為

定義姿態(tài)矩陣的計(jì)算誤差

對(duì)式（12）求微分，并將式（10）、（11）代入，整理得

根據(jù)反對(duì)稱陣的相似變換及其與矢量之間的關(guān)系，上式的矢量等價(jià)形式為

整理得

最后，將式（17）代入式（7），得SINS基于EPEA的非線性姿態(tài)誤差方程

大方位失準(zhǔn)角或小失準(zhǔn)角情況下，根據(jù)式（9）得

小失準(zhǔn)角情況下，根據(jù)式（5）得

1.3 速度誤差模型

理論上，SINS在n系的速度微分方程為

實(shí)際上，SINS含誤差的速度微分方程為

將式（22）和（21）相減，可得SINS速度誤差方程

小失準(zhǔn)角情況下，將式（5）代入上式得

1.4 SINS靜基座初始對(duì)準(zhǔn)方程

靜基座狀態(tài)下SINS的對(duì)準(zhǔn)過(guò)程中，通常假定當(dāng)?shù)匚恢靡阎夜潭ú蛔儯梢圆豢紤]位置誤差的影響，即則SINS大失準(zhǔn)角情況下初始對(duì)準(zhǔn)的非線性方程為

大方位失準(zhǔn)角情況下SINS初始對(duì)準(zhǔn)方程為

小失準(zhǔn)角情況下SINS初始對(duì)準(zhǔn)方程為

其中非線性函數(shù)f（·）、g（·）的具體形式根據(jù)失準(zhǔn)角的大小情況從式（25）～（27）中解算，量測(cè)矩陣Hk＝ （03×3I3×3），vk為量測(cè)噪聲.

2 EKF算法和UKF算法

EKF與UKF使用的都是標(biāo)準(zhǔn)Kalman濾波器的框架，是回歸最小均方誤差估計(jì)器，但二者實(shí)現(xiàn)原理不同.本文研究在系統(tǒng)噪聲和量測(cè)噪聲均為加性噪聲并且量測(cè)方程為線性方程時(shí)，SINS的非線性誤差模型.當(dāng)量測(cè)方程是線性方程時(shí)，EKF和UKF的濾波遞推過(guò)程都可得到進(jìn)一步的簡(jiǎn)化，從而有利于降低濾波計(jì)算量和減小濾波發(fā)散可能性.

2.1 EKF算法

與標(biāo)準(zhǔn)Kalman濾波一樣，EKF也采用“預(yù)測(cè)－更新”的算法框架，針對(duì)式（28）所描述的非線性系統(tǒng)，其標(biāo)準(zhǔn)遞推過(guò)程如下.

預(yù)測(cè)：

更新：

以上是基于非線性離散系統(tǒng)模型進(jìn)行的EKF遞推過(guò)程描述，即采用先離散化后線性化的

推導(dǎo)方法.在實(shí)際程序中，考慮到系統(tǒng)離散化以及離散化系數(shù)矩陣計(jì)算的方便，采用先線性化后離散化的推導(dǎo)方法.在Kalman濾波算法的遞推過(guò)程中，系統(tǒng)均方誤差矩陣Pk和Pk｜k－1要求是非負(fù)定的.而實(shí)際濾波過(guò)程中，估計(jì)的均方誤差矩陣可能會(huì)逐漸失去非負(fù)定性甚至失去對(duì)稱性，導(dǎo)致濾波發(fā)散，因此需要在數(shù)值穩(wěn)定性方面對(duì)標(biāo)準(zhǔn)遞推過(guò)程做進(jìn)一步的改進(jìn).

2.2 UKF算法

當(dāng)系統(tǒng)噪聲和量測(cè)噪聲均為加性噪聲時(shí)，為了降低濾波計(jì)算量，無(wú)需對(duì)其進(jìn)行狀態(tài)增廣處理[14]，針對(duì)式（28）所描述的非線性系統(tǒng)，采用對(duì)稱采樣點(diǎn)策略，簡(jiǎn)化的UKF遞推過(guò)程如下.

構(gòu)造采樣點(diǎn)：

預(yù)測(cè)：

更新同EKF算法的更新.

相應(yīng)的權(quán)重計(jì)算如下：

其中n是狀態(tài)向量xk的維數(shù)；λ＝α2（n＋κ）－n是一個(gè)比例參數(shù)；α控制采樣點(diǎn)的分布狀態(tài)，決定采樣點(diǎn)與均值的離散程度，通常取為0到1之間很小的正值，如1×10－3；κ是一個(gè)比例因子，在狀態(tài)估計(jì)時(shí)通常取為0；β也是一個(gè)比例因子，在狀態(tài)滿足Gauss分布時(shí)通常取為表示矩陣P的平方根，滿足矩陣方程P＝AAT，A可以通過(guò)奇異值分解、Cholesky分解、特征根分解等方法求得.

由以上遞推過(guò)程可知，UKF與EKF一樣，都采用標(biāo)準(zhǔn)Kalman濾波器“預(yù)測(cè)－更新”的算法框架.當(dāng)系統(tǒng)狀態(tài)方程為非線性方程而量測(cè)方程為

線性方程時(shí)，在預(yù)測(cè)階段，EKF通過(guò)計(jì)算Jacobian矩陣進(jìn)行狀態(tài)及其均方誤差預(yù)測(cè)，而UKF通過(guò)使用UT變換進(jìn)行狀態(tài)及其均方誤差預(yù)測(cè)，但二者在更新階段的濾波步驟與標(biāo)準(zhǔn)Kalman濾波算法完全相同.UKF不對(duì)非線性系統(tǒng)方程和量測(cè)方程進(jìn)行線性化，而是對(duì)狀態(tài)向量的概率密度函數(shù)進(jìn)行近似，因此不依賴于非線性系統(tǒng)方程的具體形式，算法相對(duì)獨(dú)立，適用于任何形式的非線性模型.

3 仿真實(shí)驗(yàn)

假設(shè)SINS所處位置的地理緯度為45°，選取較低精度陀螺的對(duì)應(yīng)值，陀螺儀的常值漂移為0.1°/h，隨機(jī)漂移為0.01°/h；加速度計(jì)的常值偏差為100×10－6g，隨機(jī)偏差為50×10－6g.選擇3種比較典型的初始失準(zhǔn)角，即大失準(zhǔn)角情況α（0）＝ （10° 20° 60°）T，大方位失準(zhǔn)角情況α（0）＝ （1° 2° 60°）T，小失準(zhǔn)角情況α（0）＝（10′ 20′ 60′）T.為了比較初始對(duì)準(zhǔn)算法的性能，在相同條件下分別將EKF算法和UKF算法用于精對(duì)準(zhǔn)過(guò)程，3種情況下Monte Carlo仿真得到的失準(zhǔn)角估計(jì)誤差如圖1所示.

圖1 不同失準(zhǔn)角估計(jì)誤差Fig.1 Estimation errors for different misalignment angles

仿真結(jié)果表明，不同失準(zhǔn)角情況下，采用本文給出的非線性初始對(duì)準(zhǔn)模型，EKF和UKF算法都能滿足對(duì)準(zhǔn)要求，且初始失準(zhǔn)角越小，對(duì)準(zhǔn)時(shí)間越短，對(duì)準(zhǔn)精度越高.大失準(zhǔn)角、大方位失準(zhǔn)角情況下，UKF算法較EKF算法具有對(duì)準(zhǔn)時(shí)間更快、對(duì)準(zhǔn)精度更高和適用范圍更廣的優(yōu)點(diǎn)，但UKF算法的計(jì)算量比EKF大；而在小失準(zhǔn)角情況下，由于系統(tǒng)的線性化誤差小，二者的對(duì)準(zhǔn)時(shí)間和對(duì)準(zhǔn)精度基本相同.

為了更清楚地比較上述兩種方法的對(duì)準(zhǔn)效果，相同條件下分別進(jìn)行100次Monte Carlo仿真.表1中給出了對(duì)準(zhǔn)結(jié)束前100s內(nèi)各失準(zhǔn)角估計(jì)均方根誤差對(duì)時(shí)間的平均值，圖2給出了不同失準(zhǔn)角下估計(jì)均方根誤差的分布情況.

表1 失準(zhǔn)角的估計(jì)均方根誤差穩(wěn)態(tài)值Tab.1 Steady－state RMSE of estimated misalignment angles

圖2 失準(zhǔn)角的估計(jì)均方根誤差分布Fig.2 ermsdistribution for misalignment angles

4 結(jié) 語(yǔ)

本文基于歐拉平臺(tái)誤差角的概念建立了SINS在大失準(zhǔn)角、大方位失準(zhǔn)角與小失準(zhǔn)角情況下的初始對(duì)準(zhǔn)誤差模型.在量測(cè)方程為線性時(shí)，推導(dǎo)了簡(jiǎn)化的EKF算法和UKF算法，分析了不同失準(zhǔn)角情況下初始對(duì)準(zhǔn)過(guò)程的異同.靜基座狀態(tài)下的Monte Carlo仿真結(jié)果，驗(yàn)證了基于3種失準(zhǔn)角所建立的初始對(duì)準(zhǔn)誤差模型的準(zhǔn)確性和兩種非線性初始對(duì)準(zhǔn)算法的有效性，并對(duì)兩種濾波算法的性能做了定性和定量的評(píng)估.

實(shí)際上，沒(méi)有一種濾波算法可以被證明明顯地優(yōu)于其他濾波算法.選擇哪一種濾波算法或者哪些濾波算法的組合，最終取決于實(shí)際的應(yīng)用場(chǎng)合和應(yīng)用目標(biāo).在系統(tǒng)線性誤差不大，且系統(tǒng)的線性化模型比較容易獲得的情況下，比較適合采用EKF算法；而在系統(tǒng)線性誤差比較大的情況下，EKF已無(wú)法保證良好的估計(jì)性能，此時(shí)適合采用UKF算法.采用EKF算法和UKF算法的主要目的是為了迅速辨識(shí)失準(zhǔn)角大致范圍并降低初始對(duì)準(zhǔn)誤差，對(duì)準(zhǔn)過(guò)程中當(dāng)失準(zhǔn)角滿足小角度假設(shè)時(shí)，進(jìn)一步切換到經(jīng)典小失準(zhǔn)角Kalman濾波方法，能在提高對(duì)準(zhǔn)精度的同時(shí)進(jìn)一步降低計(jì)算量，從而獲得更為準(zhǔn)確的初始姿態(tài)矩陣.

[1] JIANG Y F. Error analysis of analytic coarse alignment methods [J].IEEE Transactions on Aerospace and Electronic System，1998，34（1）：334－337.

[2] 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社，1998.QIN Yong－yuan，ZHANG Hong－yue，WANG Shuhua.Kalman Filter and Integrated Navigation Principle [M].Xi′an：Northwestern Polytechnical University Press，1998.（in Chinese）

[3] 魏春嶺，張洪鉞，郝曙光.捷聯(lián)慣導(dǎo)系統(tǒng)大方位失準(zhǔn)角下的非線性對(duì)準(zhǔn)[J].航天控制，2003（4）：25－34.WEI Chun－ling，ZHANG Hong－yue， HAO Shuguang.SINS nonlinear alignment with large azimuth misalignment angles [J]. Aerospace Control，2003（4）：25－34.（in Chinese）

[4] Kim K， Park C G. Non－symmetric unscented transformation with application to in－flight alignment[J].International Journal of Control，Automation and Systems，2010，8（4）：776－781.

[5] Bucy R S，Senne K D.Digital synthesis of non－linear filters[J].Automatica，1971，7（3）：287－298.

[6] Sunahara Y. An approximate method of state estimation for nonlinear dynamical systems [J].Transactions of the ASME.Series D，Journal of Basic Engineering，1970，92（2）：385－393.

[7] Julier S J，Uhlmann J K，Durrant－Whyte H F.A new method for the nonlinear transformation of means and covariance in filters and estimators [J].IEEE Transactions on Automatic Control，2000，45（3）：477－482.

[8] 潘 泉，楊 峰，葉 亮，等.一類非線性濾波器——UKF綜述[J].控制與決策，2005，20（5）：481－489.PAN Quan，YANG Feng，YE Liang，etal.Survey of a kind of nonlinear filters— UKF[J].Control and Decision，2005，20（5）：481－489.（in Chinese）

[9] Romanenko A，Castro J.The unscented filter as an alternative to the EKF for nonlinear state estimation：a simulation case study[J].Computers and Chemical Engineering，2004，28（3）：347－355.

[10] Kol s S，F(xiàn)oss B A，Schei T S.Constrained nonlinear state estimation based on the UKF approach [J].Computers and Chemical Engineering，2009，33（8）：1386－1401.

[11] 嚴(yán)恭敏，嚴(yán)衛(wèi)生，徐德民.簡(jiǎn)化UKF濾波在SINS大失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2008，16（3）：253－264.YAN Gong－min，YAN Wei－sheng，XU De－min.Application of simplified UKF in SINS initial alignment for large misalignment angles[J].Journal of Chinese Inertial Technology， 2008，16（3）：253－264.（in Chinese）

[12] Doucet A，De Freitas N，Gordon N.Sequential Monte Carlo Methods in Practice[M].New York：Springer－Verlag，2001.

[13] 秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，2006.QIN Yong－yuan.Inertial Navigation[M].Beijing：Science Press，2006.（in Chinese）

[14] 劉 也，余安喜，朱炬波，等.加性噪聲條件下的UKF算法 [J].中國(guó)科學(xué)：技術(shù)科學(xué)，2010，40（11）：1286－1299.LIU Ye，YU An－xi，ZHU Ju－bo，etal.Unscented Kalman filtering in the additive noise case [J].Science China－Technological Sciences， 2010，40（11）：1286－1299.（in Chinese）