陳 帥，王婉晨，王逸辰，朱 瑋

（1.遼寧石油化工大學(xué)信息與控制工程學(xué)院，撫順 113001；2.北京航天控制儀器研究所， 北京 100039）

0 引言

捷聯(lián)慣導(dǎo)系統(tǒng)因其自主性強(qiáng)、隱蔽性好、抗干擾能力強(qiáng)以及其全信息服務(wù)等優(yōu)點(diǎn)被廣泛應(yīng)用于航空、航天、航海等各個(gè)領(lǐng)域［1］。初始對(duì)準(zhǔn)技術(shù)是捷聯(lián)慣導(dǎo)系統(tǒng)的關(guān)鍵技術(shù)之一，用于求解慣導(dǎo)系統(tǒng)導(dǎo)航解算過(guò)程所需的初始方向余弦矩陣。初始方向余弦矩陣是由初始對(duì)準(zhǔn)過(guò)程得到的初始姿態(tài)角和方位角確定的，初始對(duì)準(zhǔn)過(guò)程的時(shí)間和對(duì)準(zhǔn)結(jié)果的精度是影響后續(xù)導(dǎo)航解算過(guò)程的兩個(gè)非常重要的因素。然而，初始對(duì)準(zhǔn)時(shí)間的減小與對(duì)準(zhǔn)精度的提高兩者之間是矛盾的關(guān)系。在一般情況下，初始對(duì)準(zhǔn)時(shí)間越長(zhǎng)，初始對(duì)準(zhǔn)結(jié)果的精度才會(huì)越高。為了解決這一矛盾，很多學(xué)者對(duì)此進(jìn)行了不同方式與方法的改進(jìn)研究。

為了提高初始對(duì)準(zhǔn)過(guò)程中方位失準(zhǔn)角的收斂速度，文獻(xiàn)［2］提出了一種快速初始對(duì)準(zhǔn)方法，該方法采用水平失準(zhǔn)角的估計(jì)值來(lái)計(jì)算方位失準(zhǔn)角。而為了增加初始對(duì)準(zhǔn)過(guò)程的對(duì)準(zhǔn)精度，文獻(xiàn)［3］建立了東向陀螺漂移的估計(jì)模型用于補(bǔ)償方位失準(zhǔn)角的估計(jì)精度。但是，這兩種方法都依賴于低通濾波器的設(shè)計(jì)與使用，方位失準(zhǔn)角的估計(jì)結(jié)果對(duì)慣性傳感器的噪聲很敏感［4］。為了解決慣性傳感器的未知噪聲特性和穩(wěn)定性問(wèn)題，很多學(xué)者對(duì)此做了大量的研究工作，提出了很多新穎的濾波算法，如KF/UPF［5］、CKF［6］、五階CKF［7］等。即便如此，在精對(duì)準(zhǔn)過(guò)程當(dāng)中，假設(shè)量測(cè)噪聲滿足量測(cè)噪聲和過(guò)程噪聲方差值已知的Gauss分布，那么Kalman濾波方法始終是最優(yōu)的線性估計(jì)器［8］。

然而，Kalman濾波對(duì)準(zhǔn)方法是一種線性濾波方法，其不適用于非線性初始對(duì)準(zhǔn)過(guò)程。二階平滑變結(jié)構(gòu)濾波方法是一種不需要誤差協(xié)方差矩陣的預(yù)測(cè)—校正估計(jì)方法［9］，其能夠克服噪聲和干擾影響，是一種簡(jiǎn)單且實(shí)用的魯棒濾波方法。為了改善捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)過(guò)程中方位失準(zhǔn)角的對(duì)準(zhǔn)精度，本文提出了一種基于組合對(duì)準(zhǔn)結(jié)構(gòu)的二階平滑變結(jié)構(gòu)濾波對(duì)準(zhǔn)方法。該組合對(duì)準(zhǔn)結(jié)構(gòu)將Kalman濾波對(duì)準(zhǔn)方法和羅經(jīng)對(duì)準(zhǔn)方法相結(jié)合，構(gòu)建了一個(gè)狀態(tài)變量與量測(cè)變量維數(shù)完全相同的初始對(duì)準(zhǔn)模型，由二階SVSF來(lái)完成對(duì)此組合對(duì)準(zhǔn)模型的狀態(tài)估計(jì)過(guò)程。實(shí)驗(yàn)仿真結(jié)果表明，該方法對(duì)準(zhǔn)得到的方位失準(zhǔn)角對(duì)準(zhǔn)結(jié)果優(yōu)于一些目前較為常見(jiàn)的初始對(duì)準(zhǔn)方法。

1 組合對(duì)準(zhǔn)模型

1.1 誤差傳遞模型

在大方位失準(zhǔn)角的非線性模型中，方位失準(zhǔn)角的估計(jì)誤差仍舊是影響導(dǎo)航精度的主要因素。因此，為了改進(jìn)精對(duì)準(zhǔn)過(guò)程中失準(zhǔn)角的估計(jì)性能，本文采用了基于Kalman濾波對(duì)準(zhǔn)方法的非線性誤差傳遞模型。選取當(dāng)?shù)氐臇|北天坐標(biāo)系作為導(dǎo)航坐標(biāo)系，即n系［10］。在n系中，大方位失準(zhǔn)角和速度誤差方程表示如下

式（1）和式（2）中，φE、φN和φU分別為俯仰失準(zhǔn)角、橫滾失準(zhǔn)角和方位失準(zhǔn)角；ωie為地球自轉(zhuǎn)速率；ε和▽為陀螺的常值漂移率和加速度計(jì)的常值零偏，下標(biāo)E、N和U分別代表n系的東北天三個(gè)軸的指向；δV為速度誤差；g為當(dāng)?shù)氐闹亓铀俣?；L為當(dāng)?shù)氐木暥?；Re為地球的半徑。

為了縮減狀態(tài)變量、增加量測(cè)變量從而使?fàn)顟B(tài)變量維數(shù)與量測(cè)變量維數(shù)得到統(tǒng)一，本文實(shí)際采用的非線性對(duì)準(zhǔn)模型為上述對(duì)準(zhǔn)模型的降階形式，僅失準(zhǔn)角和速度誤差作為狀態(tài)變量，由其構(gòu)成的狀態(tài)方程表示如下

常見(jiàn)的初始對(duì)準(zhǔn)過(guò)程中的零速量測(cè)模型表示如下

式（4）中，Z＝［δVEδVN］T；V＝［vEvN］T，V為滿足Gauss分布的量測(cè)噪聲變量，vE和vN分別為東向和北向速度變量的隨機(jī)誤差；H＝［I2×2O2×3］為量測(cè)轉(zhuǎn)移矩陣。

1.2 羅經(jīng)對(duì)準(zhǔn)方法

為了改善一定時(shí)間區(qū)間內(nèi)的對(duì)準(zhǔn)精度，本文采用了變參數(shù)羅經(jīng)對(duì)準(zhǔn)方法。在靜基座的情況下，姿態(tài)方程和速度方程可以表示為

水平羅經(jīng)環(huán)路和方位羅經(jīng)環(huán)路的示意圖分別如圖1和圖2所示。

圖1 水平羅經(jīng)環(huán)路示意圖Fig.1 Diagram of horizontal compass loop

圖2 方位羅經(jīng)環(huán)路示意圖Fig.2 Diagram of azimuth compass loop

由圖1和圖2可知，變量KEi、KNi和KUj（i＝1，2，3；j＝1，2，3，4）是控制參數(shù)，其值可通過(guò)式（7）、式（8）計(jì)算得到

式（9）中，HE＝HN＝1，東向速度誤差和北向速度誤差同時(shí)用于計(jì)算方位軸的控制角速率，這樣圖2才能夠用于大方位失準(zhǔn)角的對(duì)準(zhǔn)過(guò)程。為了縮短對(duì)準(zhǔn)過(guò)程的收斂時(shí)間，方位對(duì)準(zhǔn)環(huán)路中需采用一種變參數(shù)方法，即

式（10）中，Tbegin為方位羅經(jīng)對(duì)準(zhǔn)的起始時(shí)間，Tend為確保方位對(duì)準(zhǔn)角收斂到小角時(shí)所需的最長(zhǎng)時(shí)間節(jié)點(diǎn)。當(dāng)Tz＞Tend時(shí)，控制參數(shù)Ke（s）為零。

1.3 組合對(duì)準(zhǔn)機(jī)制示意圖

在捷聯(lián)慣導(dǎo)系統(tǒng)的初始對(duì)準(zhǔn)過(guò)程中，系統(tǒng)是不完全可觀和可控的，因?yàn)镵alman濾波方法不能夠準(zhǔn)確估計(jì)慣性傳感器的常值漂移，這也就導(dǎo)致了失準(zhǔn)角會(huì)存在一定的理論誤差值。通常，這些傳感器的常值漂移可以通過(guò)旋轉(zhuǎn)調(diào)制技術(shù)進(jìn)行一定的補(bǔ)償，但旋轉(zhuǎn)調(diào)制技術(shù)的引入會(huì)加大慣導(dǎo)系統(tǒng)的復(fù)雜度，而且會(huì)引入額外的誤差。為了簡(jiǎn)化系統(tǒng)的復(fù)雜程度，減少額外誤差的引入，本文提出了一種組合對(duì)準(zhǔn)方法用于解決因慣導(dǎo)系統(tǒng)不完全可觀可控而導(dǎo)致的狀態(tài)變量可觀測(cè)度低的問(wèn)題。該方法在羅經(jīng)對(duì)準(zhǔn)進(jìn)入理論誤差范圍內(nèi)后將Kalman濾波模型中的速度量測(cè)模型切換到組合過(guò)程中的增量模型，該增量模型的量測(cè)狀態(tài)由速度誤差、姿態(tài)角誤差和方位角誤差構(gòu)成。

為了實(shí)現(xiàn)這一組合對(duì)準(zhǔn)過(guò)程，有兩點(diǎn)是需要明確知道的。第一點(diǎn)就是增量量測(cè)模型的建立是為了實(shí)現(xiàn)完全可觀測(cè)模型的構(gòu)建，增量量測(cè)模型采用三個(gè)失準(zhǔn)角和水平速度誤差作為量測(cè)狀態(tài)，從而構(gòu)成了五維量測(cè)模型。其中，三個(gè)失準(zhǔn)角可以通過(guò)在同一時(shí)間節(jié)點(diǎn)下求得的Kalman濾波方法中未補(bǔ)償?shù)姆较蛴嘞揖仃嚺c羅經(jīng)對(duì)準(zhǔn)方法中補(bǔ)償后的方向余弦矩陣乘積并求解得到。失準(zhǔn)角的計(jì)算過(guò)程可以按式（11）和式（12）來(lái)進(jìn)行求解

因此，增量量測(cè)模型可以用式（14）來(lái)表示

式（14）中，Z?＝［δVEδVNφ′Eφ′Nφ′U］T，V?＝［vEvN0 0 0］T，H?為一個(gè)5×5的單位矩陣。所以，由式（5）和式（14）便可構(gòu)成完全可觀測(cè)的組合對(duì)準(zhǔn)模型。由于在大方位失準(zhǔn)角對(duì)準(zhǔn)過(guò)程中，方位失準(zhǔn)角不能同小失準(zhǔn)角一樣進(jìn)行線性等效，因而需要通過(guò)姿態(tài)矩陣來(lái)進(jìn)行姿態(tài)的計(jì)算。

實(shí)現(xiàn)組合過(guò)程的第二點(diǎn)就是切換時(shí)間的選擇。Kalman濾波對(duì)準(zhǔn)方法收斂到理論精度的速度要比羅經(jīng)對(duì)準(zhǔn)方法快，為了得到更加準(zhǔn)確的量測(cè)值就需要選擇羅經(jīng)對(duì)準(zhǔn)結(jié)果進(jìn)入理論精度區(qū)域后的時(shí)間作為兩種方法的組合時(shí)間。組合對(duì)準(zhǔn)機(jī)制流程圖如圖3所示。

在圖3中，Kalman濾波對(duì)準(zhǔn)方法和羅經(jīng)對(duì)準(zhǔn)方法同時(shí)運(yùn)行。t1為水平角對(duì)準(zhǔn)時(shí)間，t2與t3之和為方位角對(duì)準(zhǔn)時(shí)間。t2時(shí)間段為方位角穩(wěn)定階段，即羅經(jīng)對(duì)準(zhǔn)結(jié)果收斂到理論精度范圍內(nèi)的階段。在方位角穩(wěn)定之后，式（4）切換到式（14），由式（3）和式（14）來(lái)完成后續(xù)的組合對(duì)準(zhǔn)過(guò)程。

因?yàn)閮煞N對(duì)準(zhǔn)方法的方位失準(zhǔn)角的理論精度計(jì)算不盡相同，作為量測(cè)狀態(tài)變量的方位失準(zhǔn)角誤差與狀態(tài)變量的方位失準(zhǔn)角誤差在組合過(guò)程中會(huì)部分抵消。因此，組合對(duì)準(zhǔn)得到的方位失準(zhǔn)角估計(jì)值略優(yōu)于常規(guī)對(duì)準(zhǔn)方法。

2 五階容積-二階平滑變結(jié)構(gòu)方法

二階平滑變結(jié)構(gòu)濾波方法在其預(yù)測(cè)更新過(guò)程中是一個(gè)預(yù)測(cè)-校正的形式。在預(yù)測(cè)過(guò)程中，先驗(yàn)狀態(tài)估計(jì)通過(guò)k時(shí)刻的系統(tǒng)先驗(yàn)知識(shí)來(lái)進(jìn)行計(jì)算。在更新過(guò)程中，計(jì)算得到的先驗(yàn)估計(jì)值用來(lái)產(chǎn)生后驗(yàn)狀態(tài)估計(jì)值。在這種方法中，校正增益用于將估計(jì)的狀態(tài)和其一階導(dǎo)數(shù)限制在滑動(dòng)超平面的臨近區(qū)域，這個(gè)臨近區(qū)域也就是其存在子空間。為了便于描述二階平滑變結(jié)構(gòu)濾波過(guò)程，假設(shè)通過(guò)離散時(shí)間狀態(tài)轉(zhuǎn)移模型表示的一類非線性系統(tǒng)為

圖3 組合對(duì)準(zhǔn)過(guò)程示意圖Fig.3 Schematic diagram of integrated alignment process

式（15）中，F(xiàn)：Rn+p→Rn為非線性狀態(tài)轉(zhuǎn)移函數(shù)，xk+1∈Rn×1為狀態(tài)變量，uk∈Rp×1為控制變量，wk∈Rn×1為過(guò)程不確定變量。假設(shè)量測(cè)方程為線性或分段線性的，那么其可具體表示如下

式（16）中，zk+1∈Rm×1為量測(cè)變量，vk+1∈Rm×1為量測(cè)噪聲，H∈Rm×n為一個(gè)已知的正對(duì)角線或偽對(duì)角線測(cè)量矩陣。

二階平滑變結(jié)構(gòu)濾波算法是針對(duì)線性系統(tǒng)進(jìn)行設(shè)計(jì)的，為了解決該系統(tǒng)的非線性問(wèn)題，本文提出采用五階容積規(guī)則進(jìn)行一步預(yù)測(cè)，從而構(gòu)建成了五階容積-二階平滑變結(jié)構(gòu)濾波算法，該算法在非線性系統(tǒng)中的應(yīng)用主要可歸納為以下5個(gè)步驟［10］：

1）由系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣得到的先驗(yàn)狀態(tài)估計(jì)向量的預(yù)測(cè)為

其中，容積點(diǎn)和權(quán)重表示如下

式（25）中，為準(zhǔn)確的量測(cè)模型H的估計(jì)。

3）二階平滑變結(jié)構(gòu)濾波算法校正增益向量Kk+1∈Rn×1可通過(guò)一個(gè)包含了先驗(yàn)量測(cè)誤差、后驗(yàn)量測(cè)誤差和前一時(shí)刻的后驗(yàn)量測(cè)誤差的函數(shù)來(lái)表示

式（28）中，為量測(cè)矩陣的偽逆。假設(shè)初始時(shí)刻所有狀態(tài)是可測(cè)的，那么和H在初始時(shí)刻就是方陣，。γ＝diag（γii）∈Rm×m為一個(gè)對(duì)角矩陣，其對(duì)角元素滿足0＜γii＜1。

5）在每個(gè)采樣時(shí)間點(diǎn)進(jìn)行步驟1～步驟4的迭代重復(fù)就是整個(gè)算法的具體實(shí)現(xiàn)過(guò)程，需要特別說(shuō)明的是后續(xù)的迭代過(guò)程中，需要將作為來(lái)使用。

3 仿真驗(yàn)證

3.1 仿真參數(shù)

初始對(duì)準(zhǔn)過(guò)程的仿真參數(shù)如表1所示。一般地，大方位失準(zhǔn)角不會(huì)超過(guò)15°，因此將初始方位失準(zhǔn)角設(shè)為15°，水平失準(zhǔn)角設(shè)為1°。

常規(guī)非線性Kalman濾波方法（UKF、CKF和五階CKF）的初始參數(shù)如下所示：

表1 仿真參數(shù)Table 1 Simulation parameters

五階容積-二階平滑變結(jié)構(gòu)濾波算法的初始參數(shù)表示如下：

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

仿真過(guò)程持續(xù)600s，五階容積-二階平滑變結(jié)構(gòu)濾波算法可以通過(guò)圖3中的組合對(duì)準(zhǔn)方法來(lái)實(shí)現(xiàn)。組合對(duì)準(zhǔn)過(guò)程開(kāi)始于200s之后，失準(zhǔn)角誤差曲線如圖4～圖6所示。

在圖4中，很明顯俯仰失準(zhǔn)角誤差不是最小的，但卻是穩(wěn)定的。同樣的現(xiàn)象在圖6中也可以看到，而且在所有估計(jì)方法中，其方位失準(zhǔn)角誤差是最小的，方位失準(zhǔn)角誤差是精對(duì)準(zhǔn)過(guò)程的關(guān)鍵。因此，五階容積-二階平滑變結(jié)構(gòu)濾波對(duì)準(zhǔn)方法能夠較好地解決大方位失準(zhǔn)角對(duì)準(zhǔn)問(wèn)題。

圖4 俯仰失準(zhǔn)角估計(jì)誤差曲線Fig.4 Estimation error curves of pitch misalignment angle

圖5 橫滾失準(zhǔn)角估計(jì)誤差曲線Fig.5 Estimation error curves of roll misalignment angle

圖6 方位失準(zhǔn)角估計(jì)誤差曲線Fig.6 Estimation error curves of azimuth misalignment angle

為了表明五階容積-二階平滑變結(jié)構(gòu)濾波算法的有效性，在誤差測(cè)量條件下進(jìn)行了100次Monte Carlo仿真模擬，均值和標(biāo)準(zhǔn)差如表2和表3所示。

表2 估計(jì)誤差均值Table 2 Mean of estimation errors

表3 估計(jì)誤差標(biāo)準(zhǔn)差Table 3 Standard deviation of estimation errors

從表2可以看出，五階容積-二階平滑變結(jié)構(gòu)濾波方法的水平對(duì)準(zhǔn)誤差不比其他方法的結(jié)果更好，但是其方位失準(zhǔn)角估計(jì)精度明顯比其他幾種方法高。表3中的標(biāo)準(zhǔn)差表明了估計(jì)結(jié)果的穩(wěn)定性，同時(shí)也說(shuō)明了本文提出的方法具有更好的重現(xiàn)性?？傊?，五階容積-二階平滑變結(jié)構(gòu)濾波方法能夠用于捷聯(lián)慣導(dǎo)初始對(duì)準(zhǔn)過(guò)程中，并且能夠得到很好的估計(jì)精度和穩(wěn)定的估計(jì)結(jié)果。

4 結(jié)論

本文提出了一種五階容積-二階平滑變結(jié)構(gòu)濾波對(duì)準(zhǔn)方法，該方法提高了大方位失準(zhǔn)角對(duì)準(zhǔn)過(guò)程中方位失準(zhǔn)角的對(duì)準(zhǔn)精度。五階容積-二階平滑變結(jié)構(gòu)濾波方法需通過(guò)Kalman 濾波對(duì)準(zhǔn)方法和羅經(jīng)對(duì)準(zhǔn)方法組合的方式才能得以實(shí)現(xiàn)。五階容積-二階平滑變結(jié)構(gòu)濾波是一個(gè)魯棒估計(jì)方法，仿真結(jié)果表明該方法比其他常規(guī)對(duì)準(zhǔn)方法有更好的估計(jì)精度。此外，需要特別指出的是，五階容積-二階平滑變結(jié)構(gòu)濾波對(duì)系統(tǒng)構(gòu)成還有其特殊要求，即系統(tǒng)狀態(tài)變量維數(shù)與量測(cè)變量維數(shù)要相同，且要求系統(tǒng)完全可觀和可控。本文通過(guò)組合對(duì)準(zhǔn)方法和量測(cè)模型的切換，很好地解決了其在捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)中的應(yīng)用這一問(wèn)題。