基于改進強跟蹤ASCKF算法的SINS/GNSS組合對準方法

劉 軼，程旭紅，程建華

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

0 引言

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

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

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

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

1 SINS/GNSS組合對準數學建模

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

(1)

其中：

(2)

捷聯慣導系統的速度誤差方程、姿態誤差方程為[15]：

(3)

(4)

基于此，選取合適的狀態變量建立如下形式的SINS/GNSS組合對準系統模型：

(5)

忽略垂向，選取狀態變量X為

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

系統噪聲ω為

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

對上述SINS/GNSS組合對準模型進行離散化，得到如下的非線性系統：

(6)

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

2 改進強跟蹤ASCKF算法設計

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

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

2.1 改進的強跟蹤濾波器

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

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

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

(7)

(8)

其中：

(9)

(10)

(11)

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

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

(12)

由式(12)得

(13)

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

Nk=Vk-βRk-

(14)

對于Mk有

(15)

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

(16)

(17)

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

2.2 改進的時變噪聲統計估計器

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

(18)

(19)

2.3 濾波發散判斷

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

(20)

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

(21)

3 改進強跟蹤ASCKF算法實現

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

(22)

2) 利用上一時刻的狀態估計誤差協方差陣的分解因子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) 進行狀態更新：

(37)

15) 計算狀態估計誤差協方差矩陣的分解因子：

(38)

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

(39)

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

4 仿真分析

4.1 仿真條件設置

1) 仿真條件初始化：

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

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

2) 濾波器參數設置：

狀態誤差協方差陣初始化：

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}

系統噪聲方差陣初始化：

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

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

量測噪聲方差陣初始化：

4.2 仿真結果分析

針對以下兩種不同大方位失準角情況下的SINS/GNSS組合對準，分別采用傳統的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可知，在兩種不同的大方位失準角情況下，分別采用傳統的CKF和改進的強跟蹤ASCKF算法進行組合對準，水平失準角精度差異并不明顯。由圖3、圖6以及表1分析可得，采用改進的CKF算法可將方位失準角的對準精度提高70%以上，對準性能明顯改善。

5 結論

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