基于改進(jìn)強(qiáng)跟蹤ASCKF算法的SINS/GNSS組合對準(zhǔn)方法

劉 軼，程旭紅，程建華

(1. 海軍駐北京地區(qū)航空軍事代表室, 北京 100073；2. 煙臺廣電信息網(wǎng)絡(luò)中心 廣電技術(shù)部, 煙臺 264000；3. 哈爾濱工程大學(xué) 自動化學(xué)院, 哈爾濱 150001)

0 引言

初始對準(zhǔn)是捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System，SINS)的關(guān)鍵技術(shù)之一，對準(zhǔn)精度直接影響系統(tǒng)的導(dǎo)航定位精度[1-2]。隨著組合導(dǎo)航系統(tǒng)的快速發(fā)展，組合對準(zhǔn)成為初始對準(zhǔn)技術(shù)發(fā)展的重要方向之一。其中，SINS與全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)的組合對準(zhǔn)應(yīng)用最為廣泛。在實際應(yīng)用中，受限于系統(tǒng)慣性器件自身的測量誤差、系統(tǒng)建模誤差以及海上環(huán)境的惡劣性等因素，SINS/GNSS組合對準(zhǔn)系統(tǒng)經(jīng)慣性系粗對準(zhǔn)后方位失準(zhǔn)角較大，系統(tǒng)具有較強(qiáng)的非線性、時變性和模型不確定性[3]。因此，在精對準(zhǔn)階段需要建立基于大方位失準(zhǔn)角的非線性誤差模型，并采用非線性濾波方法實現(xiàn)初始對準(zhǔn)。

其中，容積卡爾曼濾波[4-6](Cubature Kalman Filter，CKF)是一種利用三階球面—徑向容積規(guī)則對非線性函數(shù)進(jìn)行近似高斯積分的非線性濾波方法。相比于擴(kuò)展卡爾曼濾波、無跡卡爾曼濾波等，CKF無需計算雅可比矩陣，計算過程相對簡單；在濾波過程中不依賴非線性函數(shù)的具體形式，適用于幾乎任何非線性系統(tǒng)；當(dāng)系統(tǒng)維數(shù)較高時，采用CKF能有效改善濾波精度及穩(wěn)定性[7]。基于以上優(yōu)點(diǎn)，采用CKF進(jìn)行SINS大方位失準(zhǔn)角下的初始對準(zhǔn)，能夠基本解決對準(zhǔn)過程中的非線性問題，提高系統(tǒng)的對準(zhǔn)精度[8-11]。

海上啟動狀態(tài)下，受各種機(jī)動條件的約束以及高海況的干擾，初始對準(zhǔn)過程很難在短時間內(nèi)達(dá)到收斂。對此，采用強(qiáng)跟蹤濾波(Strong Tracking Filter，STF)技術(shù)能夠有效改善對準(zhǔn)系統(tǒng)的魯棒性，加強(qiáng)系統(tǒng)對突發(fā)狀況的跟蹤能力[12]。在實際對準(zhǔn)過程中，由于外界環(huán)境的干擾，系統(tǒng)的噪聲統(tǒng)計特性不斷變化，通過引入Sage-Husa自適應(yīng)濾波算法能夠?qū)崟r獲取噪聲的統(tǒng)計特性，提高系統(tǒng)的自適應(yīng)能力[13-14]。

綜合考慮，提出了一種基于改進(jìn)強(qiáng)跟蹤自適應(yīng)平方根CKF(改進(jìn)的強(qiáng)跟蹤ASCKF)算法的SINS/GNSS組合對準(zhǔn)方法，該方法能夠有效改善非線性系統(tǒng)在大方位失準(zhǔn)角情況下的對準(zhǔn)性能。

1 SINS/GNSS組合對準(zhǔn)數(shù)學(xué)建模

初始對準(zhǔn)的關(guān)鍵在于如何獲得準(zhǔn)確的初始捷聯(lián)矩陣，其基本原理是利用慣性器件的敏感信息，經(jīng)粗對準(zhǔn)得到一個滿足要求的粗略姿態(tài)矩陣，在此基礎(chǔ)上通過精對準(zhǔn)精確估計出實際解算的地理坐標(biāo)系n′系與理想的地理坐標(biāo)系n系之間的失準(zhǔn)角，并使之趨于0，從而得到精確的捷聯(lián)矩陣。在SINS/GNSS組合對準(zhǔn)過程中，水平對準(zhǔn)速度較快、精度較高，系統(tǒng)經(jīng)粗對準(zhǔn)后得到的水平失準(zhǔn)角一般為小角度，而方位失準(zhǔn)角的收斂速度相對緩慢，粗對準(zhǔn)結(jié)束時刻方位失準(zhǔn)角仍較大，因此需要在精對準(zhǔn)階段建立基于大方位失準(zhǔn)角的非線性誤差模型。

(1)

其中：

(2)

捷聯(lián)慣導(dǎo)系統(tǒng)的速度誤差方程、姿態(tài)誤差方程為[15]：

(3)

(4)

基于此，選取合適的狀態(tài)變量建立如下形式的SINS/GNSS組合對準(zhǔn)系統(tǒng)模型：

(5)

忽略垂向，選取狀態(tài)變量X為

其中，δVE、δVN分別為東向、北向速度誤差；δφ、δλ分別為經(jīng)度、緯度誤差；φE、φN、φU為3個方向的姿態(tài)誤差；εE、εN、εU為3個方向的陀螺漂移。

系統(tǒng)噪聲ω為

選取觀測量Z為SINS與GNSS二者位置誤差的差值，v為量測噪聲，h為量測矩陣，由于在此系統(tǒng)中，量測方程為線性，故量測矩陣為

對上述SINS/GNSS組合對準(zhǔn)模型進(jìn)行離散化，得到如下的非線性系統(tǒng)：

(6)

式中，ωk-1是零均值、方差為Qk-1的系統(tǒng)噪聲，vk是零均值、方差為Rk的量測噪聲，二者為相互獨(dú)立的高斯白噪聲。

2 改進(jìn)強(qiáng)跟蹤ASCKF算法設(shè)計

傳統(tǒng)的CKF算法是在三階球面—徑向容積規(guī)則的基礎(chǔ)上，利用一組權(quán)值相等的容積點(diǎn)對非線性函數(shù)進(jìn)行近似高斯積分運(yùn)算，從而得到隨機(jī)變量的均值和方差[4-5]。

當(dāng)系統(tǒng)模型不準(zhǔn)確或噪聲統(tǒng)計特性未知時，采用CKF進(jìn)行大方位失準(zhǔn)角下的初始對準(zhǔn)會帶來較大的對準(zhǔn)誤差，甚至引起濾波發(fā)散。針對上述情況，提出了一種改進(jìn)強(qiáng)跟蹤ASCKF算法。該算法通過引入改進(jìn)的強(qiáng)跟蹤濾波器和時變噪聲統(tǒng)計估計器，能夠抑制濾波發(fā)散，提高大方位失準(zhǔn)角下的對準(zhǔn)精度。

2.1 改進(jìn)的強(qiáng)跟蹤濾波器

強(qiáng)跟蹤濾波要求輸出殘差序列時刻保持正交，滿足以下要求[16]：

當(dāng)系統(tǒng)中存在的各種不確定誤差較大時，無法保證殘差序列γk時時正交，通過在狀態(tài)協(xié)方差陣中引入多重漸消因子矩陣，可以提高新數(shù)據(jù)在濾波過程中的比重，在線調(diào)整濾波增益Kk，以提高系統(tǒng)魯棒性和對突發(fā)狀態(tài)的跟蹤能力。多重漸消因子矩陣λk的求解如下[17-18]：

λk=diag[λ1,k,λ2,k,…,λn,k]

(7)

(8)

其中：

(9)

(10)

(11)

式中，αi可根據(jù)先驗知識確定，ρ為遺忘因子，0<ρ≤1，通常ρ取0.95；β為弱化因子，取為3。

式(11)中涉及雅可比矩陣的計算，當(dāng)系統(tǒng)維數(shù)較高時，計算過程復(fù)雜。因此，本文基于強(qiáng)跟蹤濾波的等效描述，采用CKF迭代過程中的狀態(tài)協(xié)方差和量測協(xié)方差表示Nk、Mk。

(12)

由式(12)得

(13)

將式(13)代入式(9)，則Nk可表示為

Nk=Vk-βRk-

(14)

對于Mk有

(15)

結(jié)合式(9)、式(15)，將Mk進(jìn)一步表示為

(16)

(17)

改進(jìn)的強(qiáng)跟蹤濾波器通過在狀態(tài)預(yù)測方差陣的平方根更新中引入多重漸消因子調(diào)整濾波增益，避免求取雅可比矩陣，計算更加簡便，改善了系統(tǒng)的魯棒性。

2.2 改進(jìn)的時變噪聲統(tǒng)計估計器

在應(yīng)用CKF進(jìn)行組合對準(zhǔn)的過程中，涉及系統(tǒng)噪聲和量測噪聲的協(xié)方差矩陣，而在實際情況中是很難準(zhǔn)確獲得噪聲的統(tǒng)計特性。對此，文獻(xiàn)[19]中提出一種基于CKF的時變噪聲估計器，能夠?qū)崟r估計噪聲的統(tǒng)計特性，詳見文獻(xiàn)[19]中式(45)、式(46)：

(18)

(19)

2.3 濾波發(fā)散判斷

針對新息異常易影響噪聲統(tǒng)計特性的問題，基于協(xié)方差匹配的原則，引入濾波發(fā)散判斷

(20)

式中，tr()為求矩陣的跡。若式(20)成立，則式(19)改寫為：

(21)

3 改進(jìn)強(qiáng)跟蹤ASCKF算法實現(xiàn)

計算機(jī)的截斷效應(yīng)會導(dǎo)致傳統(tǒng)CKF在求取容積點(diǎn)的過程中產(chǎn)生較大的舍入誤差，影響濾波穩(wěn)定性。為了降低二次求取過程中舍入誤差的影響，采用矩陣QR分解的形式直接求取狀態(tài)預(yù)測陣的分解因子，并結(jié)合上述改進(jìn)的強(qiáng)跟蹤濾波器、時變噪聲統(tǒng)計估計器，設(shè)計了一種改進(jìn)的強(qiáng)跟蹤ASCKF算法，具體實現(xiàn)過程如下：

(22)

2) 利用上一時刻的狀態(tài)估計誤差協(xié)方差陣的分解因子Sk-1/k-1計算容積點(diǎn)：

(23)

(24)

(25)

(26)

(27)

6) 重新采樣：

(28)

(29)

(30)

(31)

(32)

(33)

式中：

(34)

(35)

12) 重復(fù)上述步驟6)～10)，重新計算容積點(diǎn)，并進(jìn)行量測更新，得到新的Szz,k/k-1、Pzz,k/k-1、Pxz,k/k-1。

13) 計算濾波增益Kk：

(36)

14) 進(jìn)行狀態(tài)更新：

(37)

15) 計算狀態(tài)估計誤差協(xié)方差矩陣的分解因子：

(38)

16) 基于以上改進(jìn)的CKF算法，將式(19)進(jìn)一步簡化為：

(39)

18)k=k+1，依次重復(fù)上述步驟，進(jìn)行下一次濾波。

4 仿真分析

4.1 仿真條件設(shè)置

1) 仿真條件初始化：

初始位置：緯度φ=45.7796°，經(jīng)度λ=126.6705°；精對準(zhǔn)過程中，做勻加速運(yùn)動，初始速度5n mile/h、加速度為0.1g；航向角為45°。載體搖擺運(yùn)動參數(shù)如下：

陀螺儀常值漂移0.003(°)/h，噪聲0.0005(°)/h；加速度計零偏3.0×10-5g，噪聲5.0×10-6g；仿真時間0.4h。

2) 濾波器參數(shù)設(shè)置：

狀態(tài)誤差協(xié)方差陣初始化：

P0/0= diag{(0.1m/s)2,(0.1m/s)2,(2m)2,

(2m)2,(0.017°)2,(0.017°)2,(0.033°)2,

(0.003(°)/h)2,(0.003(°)/h)2,(0.003(°)/h)2}

系統(tǒng)噪聲方差陣初始化：

0,0,(0.0005(°)/h)2,(0.0005(°)/h)2,

(0.0005(°)/h)2,0,0,0}

量測噪聲方差陣初始化：

4.2 仿真結(jié)果分析

針對以下兩種不同大方位失準(zhǔn)角情況下的SINS/GNSS組合對準(zhǔn)，分別采用傳統(tǒng)的CKF和改進(jìn)的強(qiáng)跟蹤ASCKF算法進(jìn)行初始對準(zhǔn)仿真。

情況1：當(dāng)初始失準(zhǔn)角為(0.03°,0.03°,0.35°)時，東向失準(zhǔn)角誤差、北向失準(zhǔn)角誤差和方位失準(zhǔn)角誤差仿真結(jié)果分別如圖1、圖2和圖3所示。

情況2：當(dāng)初始失準(zhǔn)角為(0.06°,0.06°,0.95°)時，初始方位失準(zhǔn)角明顯增大情況下，得到東向失準(zhǔn)角誤差、北向失準(zhǔn)角誤差和方位失準(zhǔn)角誤差仿真結(jié)果分別如圖4、圖5和圖6所示。

兩種不同失準(zhǔn)角情況下，初始對準(zhǔn)結(jié)束時刻方位失準(zhǔn)角誤差的大小如表1所示。

Tab.1 Alignment results comparison between CKF and improved CKF(′)

由圖1、圖2、圖4、圖5可知，在兩種不同的大方位失準(zhǔn)角情況下，分別采用傳統(tǒng)的CKF和改進(jìn)的強(qiáng)跟蹤ASCKF算法進(jìn)行組合對準(zhǔn)，水平失準(zhǔn)角精度差異并不明顯。由圖3、圖6以及表1分析可得，采用改進(jìn)的CKF算法可將方位失準(zhǔn)角的對準(zhǔn)精度提高70%以上，對準(zhǔn)性能明顯改善。

5 結(jié)論

針對大方位失準(zhǔn)角下的初始對準(zhǔn)問題，建立了非線性對準(zhǔn)誤差模型，提出了一種基于改進(jìn)強(qiáng)跟蹤ASCKF算法的SINS/GNSS組合對準(zhǔn)方法。相比較傳統(tǒng)的CKF算法，該方法能夠明顯提高大方位失準(zhǔn)角下的對準(zhǔn)精度，對后續(xù)組合導(dǎo)航系統(tǒng)長航時、高精度導(dǎo)航定位具有重要的實際意義。