許昊天，趙春柳，邵?？?，繆玲娟

（1.北京理工大學(xué)自動化學(xué)院，北京100081；2.國家國防科技工業(yè)局軍工項目審核中心，北京100039）

0 引言

由于隱蔽性好、自主性強，捷聯(lián)慣導(dǎo)系統(tǒng)（Strapdown Inertial Navigation System，SINS）可在各種復(fù)雜環(huán)境下工作，已成為了一種被廣泛應(yīng)用的系統(tǒng)［1?2］。由于SINS的導(dǎo)航誤差會隨時間增加而積累，因此SINS在長時間條件下的導(dǎo)航精度會有所降低。目前，許多先進的SINS均采用了旋轉(zhuǎn)調(diào)制技術(shù)來補償慣性器件的輸出誤差［3］。為了解決單軸旋轉(zhuǎn)調(diào)制技術(shù)無法調(diào)制旋轉(zhuǎn)軸方向上器件誤差的問題，許多學(xué)者對雙軸旋轉(zhuǎn)調(diào)制技術(shù)展開了研究［4?5］。

初始對準(zhǔn)是慣性導(dǎo)航的核心技術(shù)之一，其對準(zhǔn)精度直接決定了后續(xù)導(dǎo)航的精度［6?7］。在SINS初始對準(zhǔn)的研究中，通常將誤差模型看作經(jīng)典的線性模型，這種線性模型是在慣導(dǎo)系統(tǒng)粗對準(zhǔn)后姿態(tài)誤差角較小的情況下獲得的。在工程實際中，由于基座晃動及各種干擾的存在，粗對準(zhǔn)后的姿態(tài)誤差角并非小角度，基于小失準(zhǔn)角的線性誤差模型無法滿足現(xiàn)實應(yīng)用的要求，基于大失準(zhǔn)角捷聯(lián)慣導(dǎo)系統(tǒng)的誤差模型和非線性估計方法得到了許多學(xué)者的關(guān)注［8?9］。

近年來，平方根容積Kalman濾波算法（Square?root Cubature Kalman Filter，SCKF）已可以很好地解決SINS大失準(zhǔn)角初始對準(zhǔn)問題，但其需要對噪聲的統(tǒng)計特性有準(zhǔn)確了解，否則可能出現(xiàn)很大的狀態(tài)估計誤差，甚至導(dǎo)致濾波發(fā)散［10］。Sage?Husa算法能夠?qū)崟r在線估計系統(tǒng)的噪聲統(tǒng)計特性，從而顯著提高濾波算法的自適應(yīng)能力［11?13］，但其只能被應(yīng)用于線性系統(tǒng)中。因此，本文將SCKF算法與Sage?Husa算法相結(jié)合，提出了采用QR分解來完成對噪聲協(xié)方差的平方根矩陣進行估計的ASCKF算法，從而提高了非線性濾波算法的自適應(yīng)性，同時避免了傳統(tǒng)Sage?Husa SCKF算法在非線性較強的情況下易出現(xiàn)系統(tǒng)過程噪聲協(xié)方差矩陣與量測噪聲協(xié)方差矩陣不正定的情況。

1 坐標(biāo)系定義

以下為本文所涉及到的坐標(biāo)系：

（1）地心慣性坐標(biāo)系（i系）

原點選為地心，zi軸沿地球自轉(zhuǎn)方向，xi軸、yi軸位于赤道平面內(nèi)，xi軸從地心指向春分點，yi軸與xi軸、zi軸構(gòu)成右手坐標(biāo)系。

（2）地球坐標(biāo)系（e 系）

原點位于地心，ze軸沿地球自轉(zhuǎn)方向，xe軸指向赤道平面與格林尼治經(jīng)線的交點，ye軸與 xe軸、ze軸構(gòu)成右手坐標(biāo)系。

（3）地理坐標(biāo)系（t系）

原點位于載體的質(zhì)心處，xt、yt、zt分別指向東向、北向、天向。

（4）導(dǎo)航坐標(biāo)系（n 系）

本文選取地理坐標(biāo)系為導(dǎo)航坐標(biāo)系。

（5）載體坐標(biāo)系（b 系）

與載體固連，其原點位于載體重心，xb軸、yb軸、zb軸分別指向載體的右、前、上方向。

（6）慣性測量單元坐標(biāo)系（s系）

原點位于慣性測量單元（IMU）的重心，3個坐標(biāo)軸的指向為慣性傳感器的敏感軸，初始時刻s系坐標(biāo)軸指向與b系一致。對于單軸旋轉(zhuǎn)式慣導(dǎo)系統(tǒng)而言，轉(zhuǎn)動方向為繞方位軸旋轉(zhuǎn)；對于雙軸旋轉(zhuǎn)式慣導(dǎo)系統(tǒng)而言，轉(zhuǎn)動方向為繞方位軸和橫滾軸旋轉(zhuǎn)。

2 旋轉(zhuǎn)調(diào)制機理及誤差分析

旋轉(zhuǎn)式SINS采用傳統(tǒng)SINS的解算算法對慣性測量單元的輸出信息進行導(dǎo)航解算，從而得到導(dǎo)航結(jié)果。導(dǎo)航解算過程如圖1所示。

旋轉(zhuǎn)式SINS的基本工作原理與傳統(tǒng)SINS一致，只是在SINS的基礎(chǔ)上添加了旋轉(zhuǎn)平臺。旋轉(zhuǎn)式SINS的位置誤差方程與傳統(tǒng)SINS一致，在此不再列出。其姿態(tài)和速度誤差模型為

圖1 導(dǎo)航解算方案Fig.1 Scheme of navigation solution

假設(shè)在任意時刻t，IMU繞任意旋轉(zhuǎn)軸的旋轉(zhuǎn)角度為α（t），則s系到b系的變換陣為

圖2 b系與單軸旋轉(zhuǎn)s系之間的關(guān)系Fig.2 Relationship between b-system and uniaxial rotation s-system

在旋轉(zhuǎn)式捷聯(lián)慣導(dǎo)系統(tǒng)中，慣性器件安裝于平臺上，測量的是s系相對于i系的運動。因此，慣性器件的信號輸出可以表示為

忽略二階及二階以上小量，由式（4）、式（5）可以推出，陀螺和加速度計的輸出誤差為

當(dāng)系統(tǒng)進行旋轉(zhuǎn)調(diào)制時，與旋轉(zhuǎn)軸垂直的兩個軸上的慣性器件誤差會被調(diào)制為正余弦形式，而在旋轉(zhuǎn)軸方向上的誤差則不被調(diào)制。將式（8）、式（9）在一個旋轉(zhuǎn)周期內(nèi)進行積分，由于與旋轉(zhuǎn)軸垂直方向的誤差是正余弦形式，因此其積分結(jié)果為0，從而可抑制系統(tǒng)誤差，而在旋轉(zhuǎn)軸方向上的誤差則按照原來的規(guī)律傳播。因此，單軸旋轉(zhuǎn)式捷聯(lián)慣導(dǎo)系統(tǒng)只能抑制與旋轉(zhuǎn)軸相垂直的兩個軸向上的誤差，而無法減少旋轉(zhuǎn)軸上的誤差。若想抑制三個軸上的誤差，可采用雙軸旋轉(zhuǎn)調(diào)制方案。

3 雙軸旋轉(zhuǎn)調(diào)制方案設(shè)計

本文采用十六次序雙軸連續(xù)旋轉(zhuǎn)方案，具體轉(zhuǎn)位方案與文獻［14］相同。

設(shè)慣性測量單元的旋轉(zhuǎn)速率為Ω，次序1、3、6、8繞方位軸轉(zhuǎn)動，在載體坐標(biāo)系O?xbyb平面內(nèi)呈現(xiàn)出正、反一周的變化規(guī)律。設(shè)每一次序轉(zhuǎn)動時間為T/2，則在轉(zhuǎn)動過程中陀螺常值漂移在載體坐標(biāo)系x軸的投影積分為

由式（10）和式（11）可知，次序 1、3、6、8 在轉(zhuǎn)動過程中累積的常值誤差為0，即

根據(jù)類似的計算方式，次序2、4、5、7繞橫滾軸轉(zhuǎn)動，在轉(zhuǎn)動過程中在載體坐標(biāo)系O?xbzb平面內(nèi)同樣呈現(xiàn)出正、反一周的變化規(guī)律，累積的常值誤差為0，即

次序9、11、14、16繞方位軸轉(zhuǎn)動，在轉(zhuǎn)動過程中在載體坐標(biāo)系O?xbyb平面內(nèi)呈現(xiàn)出正、反一周的變化規(guī)律，累積的常值誤差為0，即

次序10、12、13、15繞橫滾軸轉(zhuǎn)動，在轉(zhuǎn)動過程中在載體坐標(biāo)系O?xbzb平面內(nèi)呈現(xiàn)出正、反一周的變化規(guī)律，累積的常值誤差為0，即

由式（10）～式（15）可知，陀螺的常值漂移被完全調(diào)制。加速度計的常值偏值調(diào)制過程與陀螺類似，因此本文選擇的十六次序雙軸連續(xù)旋轉(zhuǎn)方案可將慣性器件的常值誤差完全調(diào)制，進而避免了誤差的積累。

文獻［14］表明，在采用十六次序雙軸旋轉(zhuǎn)調(diào)制方案后，慣性器件標(biāo)度因數(shù)誤差的影響與傳統(tǒng)SINS一致，標(biāo)度因數(shù)誤差不會引起導(dǎo)航坐標(biāo)系下東向角誤差，但會由于標(biāo)度因數(shù)誤差與地球自轉(zhuǎn)角速度分量耦合而引起北向和天向角誤差，并隨時間積累。安裝誤差會與地球自轉(zhuǎn)角速度分量耦合引起隨時間積累的姿態(tài)角誤差，本文限于篇幅不再對這兩項誤差進行計算。

4 ASCKF算法

4.1 非線性誤差模型

在靜基座初始對準(zhǔn)中，由于在實際的工程環(huán)境中存在基座晃動及各種干擾，粗對準(zhǔn)后的姿態(tài)誤差角比較大，此時無法使用傳統(tǒng)的線性濾波方程進行精對準(zhǔn)。嚴(yán)恭敏［9］針對這一問題研究了捷聯(lián)慣導(dǎo)系統(tǒng)非線性誤差模型，設(shè)導(dǎo)航坐標(biāo)系（n系）經(jīng)歷三次轉(zhuǎn)動到達計算導(dǎo)航坐標(biāo)系（n′系），三次轉(zhuǎn)動的角度分別為 αx，αy，αz。令 α＝［αxαyαz］T，有

該狀態(tài)方程和量測方程為慣導(dǎo)系統(tǒng)誤差模型的通用表達式，后文運用該表達式進行了濾波對準(zhǔn)。

4.2 SCKF算法

平方根容積Kalman濾波算法（SCKF）由于濾波精度高、穩(wěn)定性強，在非線性系統(tǒng)中的應(yīng)用較為廣泛。因此，本文選擇該算法在靜基座條件下進行初始對準(zhǔn)。

設(shè)非線性系統(tǒng)離散化的狀態(tài)方程與量測方程為

式（18）中，f為非線性狀態(tài)轉(zhuǎn)移矩陣，h為線性量測矩陣，wk-1、vk為互不相關(guān)的零均值Gauss白噪聲，方差分別為Qk-1、Rk。SCKF算法的計算步驟如下所示：

（1）初始化，給定均值和協(xié)方差

計算球容積點和權(quán)值，即有

式（20）中，［1］＝［I，-I］，I為 n 維單位方陣，［1］j表示?。?］的第 j列。

（2）時間更新

SCKF算法借鑒了平方根濾波的思想，避免了復(fù)雜的矩陣求逆和分解運算，直接以協(xié)方差平方根矩陣的形式在相鄰時刻間進行遞推更新，能夠有效提高濾波的計算效率和數(shù)值穩(wěn)定性。

4.3 傳統(tǒng)的Sage-Husa SCKF算法

SCKF算法在用于大失準(zhǔn)角初始對準(zhǔn)時需要對噪聲的統(tǒng)計特性有準(zhǔn)確了解，否則可能出現(xiàn)很大的狀態(tài)估計誤差，甚至導(dǎo)致濾波發(fā)散。王思思［10］提出的Sage?Husa SCKF算法利用Sage?Husa噪聲估計器對系統(tǒng)噪聲的均值qk及觀測噪聲的均值rk進行了在線估計，并對系統(tǒng)過程噪聲協(xié)方差矩陣Qk、觀測噪聲方差矩陣Rk進行了實時修正，從而使得SCKF算法具有了自適應(yīng)性。

假設(shè)非線性系統(tǒng)中的時變噪聲wk、vk分別為互不相關(guān)的Gauss白噪聲，過程噪聲的協(xié)方差Qk為非負(fù)定矩陣，量測噪聲的協(xié)方差Rk為正定矩陣。在估計時變噪聲的統(tǒng)計特性時，應(yīng)當(dāng)注重最新數(shù)據(jù)對系統(tǒng)的影響，逐漸遺忘陳舊數(shù)據(jù)。傳統(tǒng)的Sage?Husa SCKF算法可總結(jié)如下：

（1）初始化

按照式（19）、式（20）完成參數(shù)初始化過程，并對Q0、R0、q0、r0賦予初始值。

（2）狀態(tài)估計

按照式（21）～式（38）進行 SCKF 濾波估計。

（3）噪聲參數(shù)估計

按照式（39）～式（42）對噪聲參數(shù)進行更新。

系統(tǒng)過程噪聲的均值為

通過在SCKF中加入Sage?Husa時變噪聲統(tǒng)計估計器，可以有效估計出當(dāng)前時刻的系統(tǒng)過程噪聲與量測噪聲，減少陳舊數(shù)據(jù)對濾波過程的干擾，從而提高濾波算法的自適應(yīng)性。

4.4 ASCKF算法

由于傳統(tǒng)的Sage?Husa SCKF算法在系統(tǒng)階次較高或系統(tǒng)非線性較強的情況下易出現(xiàn)系統(tǒng)過程噪聲協(xié)方差矩陣Qk與量測噪聲協(xié)方差矩陣Rk不正定的情況，進而易導(dǎo)致濾波算法無法繼續(xù)進行計算。因此，本文提出了改進Sage?Husa SCKF算法，即AS?CKF算法。該算法利用QR分解來完成對噪聲協(xié)方差平方根矩陣的估計，避免了傳統(tǒng)Sage?Husa SCKF算法在非線性較強的情況下易出現(xiàn)系統(tǒng)過程噪聲協(xié)方差矩陣與量測噪聲協(xié)方差矩陣不正定的情況，從而可配合SCKF算法完成自適應(yīng)濾波過程。

針對式（18）所描述的系統(tǒng)，本文提出的ASCKF算法的具體過程如下所示：

1）設(shè)定初值，并設(shè)定 SQ，0＝chol（Q0）、SR，0＝chol（R0）。其中，chol為矩陣的 Cholesky 分解。

2）按照式（21）～式（38）進行 SCKF 濾波估計。

3）對噪聲統(tǒng)計特性進行估計。

觀測噪聲協(xié)方差的平方根為

系統(tǒng)過程噪聲協(xié)方差的平方根為

將改進的 ASCKF 算法通過式（43）、式（44）計算噪聲協(xié)方差的平方根矩陣，并直接將式（43）、式（44）分別帶入式（31）、式（25）中，從而避免了傳統(tǒng) Sage?Husa SCKF 算法使用式（41）、式（42）進行計算時易出現(xiàn)的矩陣非正定問題，以及由此導(dǎo)致的式（27）、式（33）無法進行計算的問題。在下文的仿真分析中，將運用ASCKF濾波算法對雙軸旋轉(zhuǎn)式捷聯(lián)慣導(dǎo)系統(tǒng)進行非線性初始精對準(zhǔn)。

5 仿真結(jié)果與分析

5.1 SINS與旋轉(zhuǎn)式SINS初始對準(zhǔn)結(jié)果對比

雙軸旋轉(zhuǎn)式SINS靜基座的初始對準(zhǔn)結(jié)果如圖3所示，單軸旋轉(zhuǎn)式SINS靜基座的初始對準(zhǔn)結(jié)果如圖4所示，傳統(tǒng)SINS靜基座的初始對準(zhǔn)結(jié)果如圖5所示。

分別對三種慣導(dǎo)系統(tǒng)的初始對準(zhǔn)結(jié)果進行20次Monte Carlo仿真實驗，取各次實驗的姿態(tài)角誤差的算數(shù)平均值作為對準(zhǔn)精度的評價指標(biāo)，其結(jié)果如下：

1）雙軸旋轉(zhuǎn)式SINS的方位角誤差為0.177°，俯仰角誤差為0.006°，橫滾角誤差為-0.012°；

2）單軸旋轉(zhuǎn)式SINS的方位角誤差為0.219°，俯仰角誤差為0.011°，橫滾角誤差為-0.014°；3）SINS的方位角誤差為 0.374°，俯仰角誤差為0.016°，橫滾角誤差為-0.019°。

圖3 雙軸旋轉(zhuǎn)式捷聯(lián)慣導(dǎo)系統(tǒng)SCKF算法的初始對準(zhǔn)結(jié)果Fig.3 Initial alignment of SCKF algorithm for biaxial rotary SINS

圖4 單軸旋轉(zhuǎn)式捷聯(lián)慣導(dǎo)系統(tǒng)SCKF算法的初始對準(zhǔn)結(jié)果Fig.4 Initial alignment of SCKF algorithm for single axis rotary SINS

圖5 捷聯(lián)慣導(dǎo)系統(tǒng)SCKF算法的初始對準(zhǔn)結(jié)果Fig.5 Initial alignment of SCKF algorithm for SINS

顯然，單軸旋轉(zhuǎn)式SINS靜基座的初始對準(zhǔn)精度與雙軸旋轉(zhuǎn)式SINS基本相當(dāng)。但由于對準(zhǔn)時間有限，并且考慮到系統(tǒng)非線性對誤差收斂速度的影響，單軸、雙軸旋轉(zhuǎn)式捷聯(lián)慣導(dǎo)系統(tǒng)均未能讓方位誤差角收斂到一個較小的范圍。傳統(tǒng)SINS靜基座初始對準(zhǔn)精度不如兩種旋轉(zhuǎn)式SINS。

5.2 ASCKF算法的仿真結(jié)果與分析

為了驗證在噪聲模型不確定情況下的ASCKF濾波算法具有良好的自適應(yīng)能力，本節(jié)采用十六次序雙軸連續(xù)旋轉(zhuǎn)方案，并通過實驗?zāi)M了噪聲發(fā)生突變的情況。實驗仿真的初始條件在5.1節(jié)的基礎(chǔ)上，在300s后觀測噪聲的方差增大了100倍。采用SCKF濾波算法的靜基座初始對準(zhǔn)結(jié)果如圖6所示，采用ASCKF濾波算法的靜基座初始對準(zhǔn)結(jié)果如圖7所示。

圖6 雙軸旋轉(zhuǎn)式SINS SCKF濾波算法在噪聲變化環(huán)境下的初始對準(zhǔn)結(jié)果Fig.6 Initial alignment of biaxial rotary SINS SCKF algorithm in noisy changing environment

圖7 雙軸旋轉(zhuǎn)式SINS ASCKF濾波算法在噪聲變化環(huán)境下的初始對準(zhǔn)Fig.7 Initial alignment of biaxial rotary SINS ASCKF algorithm in noisy changing environment

分別對兩種濾波方案進行20次Monte Carlo仿真實驗，取各次實驗的姿態(tài)角誤差的算數(shù)平均值作為對準(zhǔn)精度的評價指標(biāo)，結(jié)果如下：

1）雙軸旋轉(zhuǎn)式SINS采用SCKF算法在噪聲統(tǒng)計特性發(fā)生突變的情況下進行大失準(zhǔn)角初始對準(zhǔn)的方位角誤差均值為-1.528°，俯仰角誤差均值為0.016°，橫滾角誤差均值為-0.018°。

2）雙軸旋轉(zhuǎn)式SINS采用ASCKF算法在噪聲統(tǒng)計特性發(fā)生突變的情況下進行大失準(zhǔn)角初始對準(zhǔn)的方位角誤差均值為0.307°，俯仰角誤差均值為0.011°，橫滾角誤差均值為-0.015°。

顯然，在噪聲模型發(fā)生變化的情況下，采用傳統(tǒng)的SCKF濾波方法會使方位角的對準(zhǔn)精度明顯降低，而ASCKF算法則具有很好的自適應(yīng)性，可以很好地被應(yīng)用于噪

6 結(jié)論

本文采用通過理論推導(dǎo)更好地抑制旋慣導(dǎo)系統(tǒng)的非SCKF算法，并法的基礎(chǔ)上，提出了一種ASCKF濾波算法。該算法采用QR分解來完成對噪聲協(xié)方差的平方根矩陣的估計，從而避免了傳統(tǒng)Sage?Husa SCKF算法所面臨的所估噪聲協(xié)方差矩陣非正定所帶來的問題。最后，通過仿真驗證了在觀測噪聲發(fā)生突變的情況下ASCKF濾波算法具有良好的自適應(yīng)性。