基于強跟蹤濾波器的SINS大失準角初始對準新方法

彭 卓，張 嶸，郭美鳳，劉 剛，羅壽紅

（1. 清華大學 精密儀器系，北京 100084；2. 中國航天員科研訓練中心，北京 100094）

基于強跟蹤濾波器的SINS大失準角初始對準新方法

彭 卓1,2，張 嶸1，郭美鳳1，劉 剛1，羅壽紅1

（1. 清華大學 精密儀器系，北京 100084；2. 中國航天員科研訓練中心，北京 100094）

捷聯慣性導航系統靜基座初始對準時一般先進行粗對準，使失準角縮小到一定范圍內從而滿足小失準角假設下的線性誤差模型，然后再進行精對準。在不進行粗對準時失準角一般為大角度，需要采用復雜的非線性誤差模型和非線性濾波方法。研究發現通過設置合理的誤差協方差矩陣初值，采用反饋校正濾波結構，并引入強跟蹤濾波算法可以在大失準角情況下既無需粗對準，又無需采用非線性模型來實現精對準。仿真結果表明，該方法可以實現大失準角初始對準，魯棒性好，在任意姿態初值下都可以使航向角在300 s內收斂到0.05°的理論極限精度，與小失準角精對準方法的速度和精度相當但省去了粗對準因而耗時更短，與無跡卡爾曼濾波在600 s時才收斂到0.5°的速度相比大為改善。

捷聯慣性導航；初始對準；大失準角；強跟蹤濾波器

捷聯慣性導航系統（Strapdown Inertial Navigation System，SINS）本身是一個積分系統，積分初值對導航結果的影響巨大。初始對準就是在導航開始前確定載體初始姿態的過程，對導航精度至關重要。SINS靜基座初始對準通常采用標準卡爾曼濾波器（Kalman Filter，KF），濾波模型采用小失準角假設下的線性誤差模型。在實際使用中，為了滿足小失準角假設，必須先進行粗對準，使姿態誤差縮小到一定范圍內，然后才能采用KF進行精對準。常用的粗對準方法為解析式粗對準[1]，為了提高精度通常需要取一段時間的平均值，這不僅增加了步驟，還增加了時間。如果不進行粗對準，失準角一般為大角度，此時線性誤差模型已經失效，需要采用非線性誤差模型和非線性濾波方法。例如劉海鵬采用擴展卡爾曼濾波（ExtendedKalman Filter，EKF）和無跡卡爾曼濾波（Unscented Kalman Filter，UKF）進行大失準角初始對準并仿真驗證了隨著失準角的增大UKF比EKF估計精度更高[2]；龍瑞采用中心差分卡爾曼濾波器（Central Difference Kalman Filter，CDKF）并仿真驗證了在大失準角條件下CDKF比EKF和UKF具有更高的精度[3]；Jamshaid提出二階分離插值濾波器（Second-order Divided Difference filter，DDF2）在航向大失準角初始對準中能夠獲得比EKF和UKF更高的姿態精度[4]；Wu采用基于奇異值分解的容積卡爾曼濾波器（Cubature Kalman Filter，CKF）進行大失準角初始對準獲得了比UKF更高的精度和更快的速度[5]。但是，以上這些非線性濾波方法都存在兩個問題：一是算法復雜，計算量比標準卡爾曼濾波大的多；二是非線性濾波都存在近似系統模型或者近似概率分布的問題，不是最優濾波器，特別是在失準角已經滿足小角度假設時非線性濾波不如標準卡爾曼濾波精度高。針對以上問題，本文提出一種大失準角初始對準新方法，該方法仍然采用線性模型，通過設置合理的濾波初值，采用反饋校正并創新性的引入強跟蹤濾波器來實現大失準角情況下的直接精對準。該方法與傳統粗對準+精對準方法相比不僅省去了粗對準，節省了時間，還保持了標準卡爾曼濾波器能達到的精度，與一般非線性濾波方法相比不僅不需要進行復雜的非線性濾波計算，還具有更高的精度、更快的速度和更小的計算量，是一種非常實用的大失準角初始對準解決方案。

1 系統模型

一般所謂的失準角指的是n系到n′系的小角度旋轉矢量，與轉動次序無關。而在大失準角情況下，根據有限轉動的不可交換性，需要定義大失準角的轉動次序。本文定義大失準角的轉動次序為：n系繞z軸轉?z角到n1系，n1系再繞x1軸轉?x角到n2系，n2系再繞y2軸轉?y角到n′系。記大失準角可以推出大失準角情況下 SINS姿態誤差和速度誤差方程分別如式(1)和式(2)所示[6]：

2 算法原理

2.1 反饋校正技術

以上卡爾曼濾波模型是在小失準角假設下推導出來的，要用它來解決大失準角非線性模型的初始對準問題，首要解決的問題是如何讓系統迅速收斂到小失準角范圍內。聯想到在羅經法對準中，雖然羅經對準算法是在小失準角假設下推導出來的，但是在大失準角時依然適用[7]，究其原因在于在大失準角情況下，雖然線性模型中的羅經項與真實的非線性羅經項不同，但是它們反饋之后對系統的校正方向是相同的，因此通過反饋可以逐漸將系統“拉回”到小失準角。受此啟發，我們采用帶反饋校正的卡爾曼濾波器來輔助大失準角初始對準。反饋校正是一種在動基座初始對準和組合導航中經常被采用的系統編排方式[8]，它是指在由卡爾曼濾波器得到系統誤差的估計值后，將估計結果反饋回系統中，校正系統的力學編排方程。

在反饋校正時如果不加區分的反饋所有狀態會由于某些狀態的估計效果不佳而影響對準的精度。有學者提出通過可觀測度給反饋狀態加權決定其反饋程度可以改善反饋校正的效果[9]。因此，在大失準角初始對準中，我們只反饋校正我們希望迅速收斂的失準角?和觀測量δVn，而不反饋可觀測性較差且對模型不確定性影響較小的狀態εb和▽b。此外，反饋系數對系統輸出也有一定的影響，反饋系數小系統輸出會比較平滑，但是收斂時間較長，反饋系數大系統在初始階段波動較劇烈，但是收斂較快，為了使大失準角迅速收斂這里選取反饋系數為最大值1。

2.2 P0陣的設置

標準卡爾曼濾波器的公式如式(7)所示：

當卡爾曼濾波器運行一段時間后，Pk-1矩陣將趨于一個小量，一般情況下R＞＞Q，由式(7)可以看出此時Pk/k-1＜＜R，Kk也趨近于一個小量，這將導致新息很難進入到估計結果中去，濾波估計對系統的修正變得微乎其微，如果這時系統還未收斂到小失準角，那么就很難再繼續收斂了，將導致反饋校正失敗。從這個角度來講，P矩陣反映了反饋校正的動力，P越大反饋校正越充分。為了使反饋校正有效的將系統由大失準角“拉回”到小失準角，就必須提供足夠大的P矩陣，一個最直接的辦法是選用足夠大的P0。考慮到系統12維狀態的可觀測性以及對反饋校正的貢獻各不相同，需要進一步分析P0中各分量的設置。

δV是觀測量，在濾波開始階段主要依靠觀測量來提供信息，所以P0中的δV應該設置的盡量大一些，當δV足夠大時有即P1會迅速收斂到與P0無關的R水平，所以δV大了是沒有壞處的。在濾波初始階段?的誤差很大，主要依靠觀測量來估計?，所以P0中的?也應設置為較大的值，同時為了避免隨著濾波進行?迅速變小，從而失去對大失準角反饋校正的“動力”，P0中的?應設置的足夠大。P0中的ε和▽不宜設置過大，過大會導致過分依賴量測來估計ε和▽，但是因為ε和▽不完全可觀測，依靠量測估計出來的ε和▽是發散的，可能引起很大誤差，同時ε和▽也不宜太小，太小會影響ε和▽中可觀測分量的收斂速度。高精度陀螺和加表的常值零偏通常不大，ε和▽的初值設為零已經很接近真實值了，所以P0中的ε和▽可根據先驗知識適當設置的小一些，例如（0.1～1）倍的經驗值，▽的可觀測性更弱一些所以可以設置的更小一些。綜合考慮，推薦P0中的δV設置為10 m/s，?設置為1000o，ε設置為先驗知識的0.5倍，▽設置為先驗知識的0.1倍。

2.3 強跟蹤濾波器

僅僅依靠調節 P0陣來解決大失準角初始對準問題是不可靠的，有時候并不能將系統“拉回”到小失準角，這一點在后面的仿真中可以看到，因此需要一種能夠在線調節P矩陣的方法，強跟蹤濾波器（Strong Tracking Filter，STF）是一個很好的解決方案。強跟蹤濾波器最早由周東華教授提出，強跟蹤只是指它的性能，它實際上是一種自適應的指數漸消記憶卡爾曼濾波器。指數漸消是指通過引入漸消因子來使濾波器的記憶長度按照指數速度衰減，以提高對新近量測的利用程度。強跟蹤濾波器通過新息正交原理來自適應的調節漸消因子，使得濾波器具有更強的關于模型不確定性的魯棒性，更強的關于突變狀態的跟蹤能力，在故障檢測、自適應控制等領域被廣泛應用[10-11]。

早期使用的強跟蹤濾波器中對所有的狀態采用相同的漸消因子[12]，稱之為單一漸消因子強跟蹤濾波器（Single Fading Strong Tracking Filter，SFSTF）。仿真表明在初始對準中采用SFSTF會帶來嚴重錯誤，究其原因在于并不是12個狀態都存在嚴重“突變”，不同狀態對模型不確定性的貢獻差異巨大，因此必須對不同狀態施加不同的漸消因子，這也是復雜多變量系統中經常采用的手段[13-14]。對不同變量采用不同漸消因子的強跟蹤濾波器稱為多重漸消因子強跟蹤濾波器（Multiple Fading Strong Tracking Filter，MFSTF），MFSTF的公式與標準卡爾曼濾波器基本相同，只是在計算Pk/k-1時引入漸消因子矩陣，如式(8)所示：

Dk為漸消因子矩陣，是一個對角陣，對角元素是對不同狀態分量施加的漸消因子。多重漸消因子的求解方法可以參考文獻[14]，計算量并不大，而且當漸消因子取1時退化為標準卡爾曼濾波。此處，為了保證P矩陣的對稱性，創新性的將式(8)改進為式(9)的形式，仿真顯示這樣做可以提高對準精度。

在 MFSTF中需要事先根據先驗知識確定多重漸消因子的比例，考慮到大失準角初始對準中主要模型誤差是失準角，因此確定漸消因子比例時突出對失準角的跟蹤，選用

3 仿真驗證

為了驗證以上結論，進行了一系列仿真驗證。仿真參數為：陀螺常值零偏0.01 (°)/h，角度隨機游走系數0.001 (°)/√h，加表常值零偏100 μg，速度隨機游走系數10 μg/√Hz；載體真實姿態[0°, 0°, 10°]，真實位置[40°, 116°, 50m]，真實速度0 m/s；卡爾曼濾波器狀態初值X0=0，R=diag([0.01,0.01,0.01] m/s)2，Q根據陀螺角度隨機游走系數和加表速度隨機游走系數設置，P0= diag(k×[1°, 1°, 1°], [10, 10, 10] m/s, 0.5×[0.01, 0.01, 0.01] (°)/h, 0.1×[100, 100, 100] μg)2，其中k用于調節P0陣中?的大小。

首先，驗證了P0對大失準角初始對準的影響。仿真中采用帶反饋校正的標準卡爾曼濾波器，初始姿態設為[1°, 1°, -170°]（對應航向角誤差180°，是大失準角中最惡劣的情況），調節P0中的k從10-2～106變化，每次仿真時長3600 s，得到的航向角估計值如圖1所示，圖中紅色虛線是真實航向角，黑色圓圈是仿真結果，右下角小圖是局部放大圖。由圖 1可以看出 P0很小時得不到正確的航向估計，隨著P0的增大，逐漸可以得到正確的航向估計，說明調節P0確實可以解決大失準角情況下的初始對準問題；當 P0設置為 100°以上時大多數情況下都可以得到正確的航向角，但是在約 390°時出現了一個“突變”，這說明并不是 P0越大，系統就一定能夠收斂到正確的航向角，單靠調節P0的這種方法確實可靠性較差。

圖1 P0對航向角估計的影響（KF）Fig.1 Effect of P0on the estimate of heading in KF

其次，驗證了采用強跟蹤濾波器對系統可靠性的提高。采用與前次仿真相同的參數，只是濾波算法由KF變為MFSTF，得到的航向角估計值如圖2所示。對比圖2和圖1可以看出：采用MFSTF后，P0選用范圍大大放寬了，選用任意值時都可以得到足夠精確的航向估計，經過多次仿真也沒有發現“突變”現象，說明采用強跟蹤跟濾波器后大大提高了系統的可靠性，完美解決了大失準角初始對準問題。

圖2 P0對航向角估計的影響（MFSTF）Fig.2 Effect of P0on the estimate of heading in MFSTF

最后，綜合比較了小失準角KF精對準方法、本文提出的 MFSTF方法和基于非線性模型的 UKF方法。仿真數據同前，KF方法中沒有進行粗對準而直接將初始失準角設置為小角度（初始姿態[0.1°, 0.1°, 5°]），MFSTF和UKF的初始姿態都選為[1°, 1°, -170°]。三種方法的仿真結果如圖3所示，圖中的兩幅小圖是局部放大圖。由圖3可以看出MFSTF的收斂速度要明顯快于 UKF，而且精度也比UKF高；MFSTF和KF幾乎都在300 s左右收斂，速度和精度不相上下。

為了進一步比較三種算法的精度、速度和計算量，將一些有代表性的仿真結果列在表1中。從精度上看，零偏 0.01 (°)/h的陀螺對應的航向誤差極限精度約為0.05°，比較300 s時的航向誤差可以看出MFSTF和KF的都可以達到這個精度，而UKF由于尚未收斂而精度較差；從速度上看，KF和MFSTF都在300 s左右收斂到極限精度10.05°附近，收斂速度相差無幾，而UKF在600 s時才收斂到10.5°附近，收斂速度遠遠慢于前兩者；從計算量上看，比較各種算法處理3600 s數據的耗時可以看出 KF最短，MFSTF約是KF的1.1倍，而UKF約是KF的3.8倍，說明MFSTF的計算量與KF在同一水平，遠小于UKF。通過以上比較可以看出，MFSTF可以很好地處理大失準角初始對準問題，與UKF相比精度、速度和計算量都有明顯優勢，與KF相比精度、速度和計算量均在同一水平，但省去了粗對準步驟，因而總耗時更少。

表1 KF、MFSTF和UKF比較Tab.1 Comparison among KF, MFSTF and UKF

圖3 KF、MFSTF和UKF航向角估計結果對比Fig.3 Comparison on heading estimates among KF, MFSTF and UKF

4 結 論

本文提出一種適用于靜基座大失準角初始對準的新方法，無需進行粗對準，也無需采用非線性模型和非線性濾波，只須在傳統的線性模型基礎上選擇合適的P0陣，并采用反饋校正濾波結構和強跟蹤濾波器即可。仿真結果表明這種方法能夠很好的解決大失準角初始對準問題，魯棒性好，對準精度、收斂速度和計算量都明顯優于基于非線性模型的UKF方法，與只適用于小失準角情況的標準卡爾曼濾波精對準方法在同一水平，但因無需粗對準而總耗時更短。這種新方法在大失準角初始對準領域非常實用。

（References）：

[1] Jiang Y F. Error analysis of analytic coarse alignment methods[J]. IEEE Transactions on Aerospace and Electronic Systems, 1998, 34(1): 334-337.

[2] 劉海鵬, 張科, 李恒年, 等. 捷聯慣導系統大失準角下的初始對準研究[J]. 航空計算技術, 2012, 42(04): 39-42. Liu Hai-peng, Zhang Ke, Li Heng-nian, et al. Research on initial alignment of SINS with large misalignment angles[J]. Aeronautical Computing Technique, 2012, 42(4): 39-42.

[3] 龍瑞, 秦永元, 夏家和. CDKF在捷聯慣導系統大失準角初始對準的應用[J]. 西北工業大學學報, 2010, 28(03): 364-368. Long Rui, Qin Yong-yuan, Xia Jia-he. Applying CDKF (Central Difference Kalman Filter) to initial alignment of sins for large misalignment angles[J]. Journal of Northwestern Polytechnical University, 2010, 28(3): 364-368.

[4] Jamshaid A, Rasheeq M. Initial orientation of inertial navigation system realized through nonlinear modeling and filtering[J]. Measurement, 2011, 44(5): 793-801.

[5] Wu Xu, Sun Feng. Simulation study for FOG strapdown inertial navigation nonlinear alignment based on SVD-cubature Kalman filter[C]//Symposium on Photonics and Optoelectronics. Shanghai, 2012: 1-4.

[6] 嚴恭敏, 嚴衛生, 徐德民. 基于歐拉平臺誤差角的SINS非線性誤差模型研究[J]. 西北工業大學學報, 2009, 27(4): 511-516. Yan Gong-min, Yan Wei-sheng, Xu De-min. A SINS nonlinear error model reflecting better characteristics of SINS errors[J]. Journal of Northwestern Polytechnical University, 2009, 27(4): 511-516.

[7] 李漢舟, 潘泉, 鄧麟, 等. 極點配置對SINS羅經對準性能影響[J]. 中國慣性技術學報, 2014, 22(06): 711-718. Li Han-zhou, Pan Quan, Deng Lin, et al. Pole assignment’s influence on alignment performance of SINS gyrocompass[J]. Journal of Chinese Inertial Technology, 2014, 22(6): 711-718.

[8] Hu Jie, Cheng Xiang-hong. A new in-motion initial alignment for land-vehicle SINS/OD integrated system[C] //2014 Position, Location and Navigation Symposium. Monterey, CA, 2014: 407-412.

[9] 孫鈺琛, 段鳳陽, 陳鵬, 等. 基于可觀測度分析的捷聯慣導初始對準方法[J]. 壓電與聲光, 2014, 36(03): 380-383. Sun Yu-chen, Duan Feng-yang, Chen Peng, et al. Initial alignment method for SINS based on the analysis of the degree of observability[J]. Piezoelectrics & Acoustooptics, 2014, 36(03): 380-383.

[10] Wang L, Wu L, Guan Y, et al. Online sensor fault detection based on an improved strong tracking filter[J]. Sensors, 2015, 15(2): 4578-4591.

[11] Wang D, Zhou D H, Jin Y H, et al. Adaptive generic model control for a class of nonlinear time-varying processes with input time delay[J]. Journal of Process Control, 2004, 14(5): 517-531.

[12] Xia Qi-jun, Rao Ming, Ying Yi-qun, et al. Adaptive fading Kalman filter with an application[J]. Automatica, 1994, 30(8): 1333-1338.

[13] Gao Wei-xi, Miao Ling-juan, Ni Mao-lin. Multiple fading factors Kalman filter for SINS static alignment application[J]. Chinese Journal of Aeronautics, 2011, 24(4): 476-483.

[14] 趙小明, 趙帥, 郭永剛, 等. 基于強跟蹤濾波的車載行進間對準[J]. 中國慣性技術學報, 2015, 23(2): 141-144. Zhao Xiao-ming, Zhao Shuai, Guo Yong-gang, et al. In-motion alignment based on strong tracking filter[J]. Journal of Chinese Inertial Technology, 2015, 23(2): 141-144.

Strong tracking filter based SINS initial alignment for large misalignment angles

PENG Zhuo1,2, ZHANG Rong1, GUO Mei-feng1, LIU Gang1, LUO Shou-hong1
(1. Department of Precision Instrument, Tsinghua University, Beijing 100084, China; 2. Astronaut Center of China, Beijing 100094, China)

Initial alignment is important for strapdown inertial navigation system. Coarse alignment is usually implemented before fine alignment to make the misalignment angles sufficiently small to meet the linear error model. Without coarse alignment, the misalignment angles will be large enough to need non-linear model and non-linear filter, which are complicated and inaccurate. For this reason, a new method is presented which don’t need coarse alignment or non-linear model. This is achieved by choosing appropriate initial error covariance matrix and using feedback compensation structure and strong tracking filter. Simulation shows the new method is effective and robust for large misalignment angles. The heading error can ultimately converge to 0.05° after 300 s under arbitrary initial attitude, while the traditional fine alignment has almost the same accuracy and speed but needs additional time for coarse alignment, and the UKF only converges to 0.5° after 600 s which is much slower.

SINS; initial alignment; large misalignment angles; strong tracking filter

U666.1

：A

2015-05-07；

：2015-08-28

總裝備部預研項目（20114113019）

彭卓（1986—），男，博士研究生，從事高精度慣性導航系統研究。E-mail：z-peng12@mails.tsinghua.edu.cn

聯 系 人：張嶸（1969—），男，研究員，博士生導師。E-mail：rongzh@mail.tsinghua.edu.cn

1005-6734(2015)05-0565-05

10.13695/j.cnki.12-1222/o3.2015.05.002