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

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

0 引 言

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

近年來，針對SINS大失準(zhǔn)角下初始對準(zhǔn)的誤差模型和非線性估計方法不斷涌現(xiàn)，Bucy等[5，6]等提出并研究了適用于非線性系統(tǒng)和非線性量測情況下的擴展卡爾曼濾波（extended Kalman filter，EKF）算法.但EKF引入了高階項截斷誤差，必然會降低模型的準(zhǔn)確性，隨著時間的延長，估計精度難以保證，甚至使濾波器難以穩(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ù)載體在初始對準(zhǔn)時的運動狀態(tài)可將初始對準(zhǔn)分為兩類，即靜基座初始對準(zhǔn)和動基座初始對準(zhǔn).通常，捷聯(lián)慣導(dǎo)系統(tǒng)的可觀測性較差，尤其在靜基座情況下其可觀測性最弱；在動基座情況下，通過使基座有目的地機動可以提高捷聯(lián)慣導(dǎo)系統(tǒng)的可觀測性，從而提高捷聯(lián)慣導(dǎo)系統(tǒng)初始對準(zhǔn)的收斂速度及估計精度.本文主要研究靜基座情況下，SINS的初始對準(zhǔn)問題.基于歐拉平臺誤差角（EPEA）的概念描述理論導(dǎo)航坐標(biāo)系到計算導(dǎo)航坐標(biāo)系之間的失準(zhǔn)角[3，11]，摒棄經(jīng)典小失準(zhǔn)角誤差模型中無限轉(zhuǎn)動與旋轉(zhuǎn)次序無關(guān)的做法.在此基礎(chǔ)上推導(dǎo)適用于SINS初始對準(zhǔn)的非線性誤差模型，該模型對姿態(tài)誤差和相對姿態(tài)不作任何線性化假設(shè)，能準(zhǔn)確描述SINS的誤差傳播規(guī)律.在系統(tǒng)噪聲和量測噪聲均為復(fù)雜加性噪聲并且量測方程為線性方程時，詳細(xì)分析大失準(zhǔn)角、大方位失準(zhǔn)角與小失準(zhǔn)角情況下初始對準(zhǔn)過程的異同，給出帶阻尼解算的簡化EKF算法和簡化UKF算法，并對兩種濾波算法在靜基座狀態(tài)下的對準(zhǔn)效果進行Monte Carlo[12]仿真比較.

1 SINS的非線性誤差模型

1.1 EPEA微分方程

首先定義文中所用到的坐標(biāo)系：地心慣性坐標(biāo)系記為i系；地球坐標(biāo)系記為e系；導(dǎo)航坐標(biāo)系選取“東 －北 －天”地理坐標(biāo)系，記為n系；機體坐標(biāo)系選取“右 －前 －上”坐標(biāo)系，記為b系[13].n系依次繞航向軸、俯仰軸、橫滾軸作3次歐拉角旋轉(zhuǎn)可至b系，且n系到b系的旋轉(zhuǎn)變換關(guān)系可用姿態(tài)矩陣Cbn描述.實際上，帶誤差的計算導(dǎo)航坐標(biāo)系n′系與理想導(dǎo)航坐標(biāo)系n系之間存在失準(zhǔn)角.類似于n系到b系的轉(zhuǎn)動過程，n系依次經(jīng)過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)造的反對稱矩陣，則有

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

于是，得

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

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

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

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)誤差方程

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

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

1.3 速度誤差模型

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

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

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

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

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

靜基座狀態(tài)下SINS的對準(zhǔn)過程中，通常假定當(dāng)?shù)匚恢靡阎夜潭ú蛔?，可以不考慮位置誤差的影響，即則SINS大失準(zhǔn)角情況下初始對準(zhǔn)的非線性方程為

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

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

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

2 EKF算法和UKF算法

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

2.1 EKF算法

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

預(yù)測：

更新：

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

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

2.2 UKF算法

當(dāng)系統(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一樣，都采用標(biāo)準(zhǔn)Kalman濾波器“預(yù)測－更新”的算法框架.當(dāng)系統(tǒng)狀態(tài)方程為非線性方程而量測方程為

線性方程時，在預(yù)測階段，EKF通過計算Jacobian矩陣進行狀態(tài)及其均方誤差預(yù)測，而UKF通過使用UT變換進行狀態(tài)及其均方誤差預(yù)測，但二者在更新階段的濾波步驟與標(biāo)準(zhǔn)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種比較典型的初始失準(zhǔn)角，即大失準(zhǔn)角情況α（0）＝ （10° 20° 60°）T，大方位失準(zhǔn)角情況α（0）＝ （1° 2° 60°）T，小失準(zhǔn)角情況α（0）＝（10′ 20′ 60′）T.為了比較初始對準(zhǔn)算法的性能，在相同條件下分別將EKF算法和UKF算法用于精對準(zhǔn)過程，3種情況下Monte Carlo仿真得到的失準(zhǔn)角估計誤差如圖1所示.

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

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

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

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

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

4 結(jié) 語

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

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