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

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

0 引言

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

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

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

1 坐標(biāo)系定義

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

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

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

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

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

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

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

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

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

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

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

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

原點(diǎn)位于慣性測量單元（IMU）的重心，3個(gè)坐標(biāo)軸的指向?yàn)閼T性傳感器的敏感軸，初始時(shí)刻s系坐標(biāo)軸指向與b系一致。對(duì)于單軸旋轉(zhuǎn)式慣導(dǎo)系統(tǒng)而言，轉(zhuǎn)動(dòng)方向?yàn)槔@方位軸旋轉(zhuǎn)；對(duì)于雙軸旋轉(zhuǎn)式慣導(dǎo)系統(tǒng)而言，轉(zhuǎn)動(dòng)方向?yàn)槔@方位軸和橫滾軸旋轉(zhuǎn)。

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

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

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

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

假設(shè)在任意時(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)中，慣性器件安裝于平臺(tái)上，測量的是s系相對(duì)于i系的運(yùn)動(dòng)。因此，慣性器件的信號(hào)輸出可以表示為

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

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

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

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

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

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

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

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

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

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

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

4 ASCKF算法

4.1 非線性誤差模型

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

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

4.2 SCKF算法

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

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

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

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

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

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

（2）時(shí)間更新

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

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

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

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

（1）初始化

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

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

按照式（21）～式（38）進(jìn)行 SCKF 濾波估計(jì)。

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

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

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

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

4.4 ASCKF算法

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

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

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

2）按照式（21）～式（38）進(jìn)行 SCKF 濾波估計(jì)。

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

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

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

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

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

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

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

分別對(duì)三種慣導(dǎo)系統(tǒng)的初始對(duì)準(zhǔn)結(jié)果進(jìn)行20次Monte Carlo仿真實(shí)驗(yàn)，取各次實(shí)驗(yàn)的姿態(tài)角誤差的算數(shù)平均值作為對(duì)準(zhǔn)精度的評(píng)價(jià)指標(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算法的初始對(duì)準(zhǔn)結(jié)果Fig.3 Initial alignment of SCKF algorithm for biaxial rotary SINS

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

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

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

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

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

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

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

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

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

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

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

6 結(jié)論

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