劉 軼，程旭紅，程建華

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

0 引言

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

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

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

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

1 SINS/GNSS組合對準數(shù)學建模

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

(1)

其中：

(2)

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

(3)

(4)

基于此，選取合適的狀態(tài)變量建立如下形式的SINS/GNSS組合對準系統(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組合對準模型進行離散化，得到如下的非線性系統(tǒng)：

(6)

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

2 改進強跟蹤ASCKF算法設計

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

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

2.1 改進的強跟蹤濾波器

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

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

(12)

由式(12)得

(13)

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

Nk=Vk-βRk-

(14)

對于Mk有

(15)

結合式(9)、式(15)，將Mk進一步表示為

(16)

(17)

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

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

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

(18)

(19)

2.3 濾波發(fā)散判斷

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

(20)

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

(21)

3 改進強跟蹤ASCKF算法實現(xiàn)

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

(22)

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

(23)

(24)

(25)

(26)

(27)

6) 重新采樣：

(28)

(29)

(30)

(31)

(32)

(33)

式中：

(34)

(35)

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

13) 計算濾波增益Kk：

(36)

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

(37)

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

(38)

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

(39)

18)k=k+1，依次重復上述步驟，進行下一次濾波。

4 仿真分析

4.1 仿真條件設置

1) 仿真條件初始化：

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

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

2) 濾波器參數(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 仿真結果分析

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

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

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

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

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

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

5 結論

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