基于EPEA的SINS大失準角非線性初始對準方法

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

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

0 引 言

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

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

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

1 SINS的非線性誤差模型

1.1 EPEA微分方程

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

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

其中

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

在歐拉誤差角均為小角度的小失準角情況下，有

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

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

于是，得

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

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

因此，在大方位失準角或小失準角情況下，歐拉平臺誤差角微分方程可簡化為

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

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

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

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

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

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

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

整理得

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

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

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

1.3 速度誤差模型

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

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

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

小失準角情況下，將式（5）代入上式得

1.4 SINS靜基座初始對準方程

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

大方位失準角情況下SINS初始對準方程為

小失準角情況下SINS初始對準方程為

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

2 EKF算法和UKF算法

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

2.1 EKF算法

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

預(yù)測：

更新：

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

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

2.2 UKF算法

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

構(gòu)造采樣點：

預(yù)測：

更新同EKF算法的更新.

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

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

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

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

3 仿真實驗

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

圖1 不同失準角估計誤差Fig.1 Estimation errors for different misalignment angles

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

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

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

圖2 失準角的估計均方根誤差分布Fig.2 ermsdistribution for misalignment angles

4 結(jié) 語

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

實際上，沒有一種濾波算法可以被證明明顯地優(yōu)于其他濾波算法.選擇哪一種濾波算法或者哪些濾波算法的組合，最終取決于實際的應(yīng)用場合和應(yīng)用目標.在系統(tǒng)線性誤差不大，且系統(tǒng)的線性化模型比較容易獲得的情況下，比較適合采用EKF算法；而在系統(tǒng)線性誤差比較大的情況下，EKF已無法保證良好的估計性能，此時適合采用UKF算法.采用EKF算法和UKF算法的主要目的是為了迅速辨識失準角大致范圍并降低初始對準誤差，對準過程中當失準角滿足小角度假設(shè)時，進一步切換到經(jīng)典小失準角Kalman濾波方法，能在提高對準精度的同時進一步降低計算量，從而獲得更為準確的初始姿態(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è)大學出版社，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)大方位失準角下的非線性對準[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] 嚴恭敏，嚴衛(wèi)生，徐德民.簡化UKF濾波在SINS大失準角初始對準中的應(yīng)用[J].中國慣性技術(shù)學報，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].北京：科學出版社，2006.QIN Yong－yuan.Inertial Navigation[M].Beijing：Science Press，2006.（in Chinese）

[14] 劉 也，余安喜，朱炬波，等.加性噪聲條件下的UKF算法 [J].中國科學：技術(shù)科學，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）