改進的強跟蹤自適應UKF算法及其在大方位失準角對準中的應用

李 明，柴洪洲，靳凱迪，王 敏，宋開放

改進的強跟蹤自適應UKF算法及其在大方位失準角對準中的應用

李 明，柴洪洲，靳凱迪，王 敏，宋開放

（信息工程大學地理空間信息學院，鄭州 450001）

針對無跡卡爾曼濾波（UKF）易受系統模型參數失配、狀態變化情況影響，導致濾波精度下降甚至發散問題，提出一種改進的強跟蹤自適應無跡卡爾曼濾波（STAUKF）。將強跟蹤濾波（STF）與UKF濾波結合，并引入多重漸消因子，有針對性地自動調節狀態估計均方誤差陣。根據新息向量構造檢驗門限函數，提高了濾波對有用歷史信息的利用率。進一步引入簡化的薩格-胡薩（Sage-Husa）濾波，自適應調節量測噪聲方差，較傳統Sage-Husa算法減少了計算量，提高了算法的魯棒性。最后采用海上實測數據進行實驗驗證，并與UKF濾波、強跟蹤UKF濾波（STUKF）比較。結果表明，該算法優勢明顯，有效縮短了大方位失準角誤差收斂時間，提高了組合導航精度。較UKF濾波方位角收斂時間縮短了93%，東、北、天方向速度均方根誤差分別降低89%、93%和82%，位置均方根誤差分別降低98%、94%和97%。

強跟蹤濾波；自適應；無跡卡爾曼濾波；多重漸消因子；組合導航

0 引言

捷聯慣性導航系統（strapdown inertial navigation system, SINS）具有無源、隱蔽性高、短時高精度等優點，但導航誤差隨時間累積較快，需要其他傳感器輔助導航[1]。全球衛星導航系統（global navigation satellite system，GNSS）定位精度高，有助于提高導航精度、完好性和可用性[2]。SINS/GNSS組合導航被廣泛用于海洋航行、航空、汽車導航等領域[3]。

為處理組合導航非線性系統問題，文獻[5]采用擴展卡爾曼濾波（extended Kalman filter，EKF）處理大方位失準角下組合導航系統的非線性問題，但EKF通過泰勒（Taylor）展開對非線性系統進行一階線性化近似，忽略二階以上誤差項，引入了截斷誤差，且需要解算復雜的雅克比矩陣，易導致濾波性能次優甚至發散[6]。文獻[7]通過仿真實驗證明了無跡卡爾曼濾波（unscented Kalman filter, UKF）較EKF具有更高的估計精度，是一種處理非線性誤差模型的實用方法。文獻[8]基于粒子濾波，通過計算高斯核的最優寬帶參數，提高了粒子濾波的計算效率。粒子濾波與UKF濾波核心思想均為通過選擇西格馬（Sigma）樣本點近似概率密度分布，但UKF只需要確定性選擇Sigma點，計算量遠小于粒子濾波。當粒子濾波近似概率密度遠離真實概率密度時，濾波效果變差甚至發散[9]。文獻[10]針對衛星信號受到遮擋等導致觀測質量下降的情況，提出基于QR分解的容積卡爾曼濾波（cubature Kalman filter，CKF），并應用于SINS/GNSS組合導航非線性系統，減少了計算量，提高了濾波精度。但CKF濾波對先驗信息統計特性較為敏感，不準確的先驗信息將導致CKF濾波嚴重震蕩，降低組合導航精度[11]。文獻[12]通過蒙特卡羅仿真實驗，表明CKF濾波只對線運動敏感，對角運動并不敏感，而UKF濾波可同時敏感線運動和角運動，2種運動方向的激勵作用均可縮短誤差收斂時間。綜合上述分析，UKF濾波用于非線性組合導航系統優勢明顯，但UKF濾波容易受系統模型參數失配影響，且對系統突變狀態較為敏感，降低濾波魯棒性。

針對UKF濾波在系統模型參數失配情況下魯棒性降低，系統狀態變化跟蹤能力不足的問題，本文提出一種改進的強跟蹤自適應無跡卡爾曼濾波。首先，將UKF濾波與強跟蹤濾波結合，提高了濾波對于系統模型參數失配問題的魯棒性；然后，相對于傳統單漸消因子強跟蹤濾波，本文引入多重漸消因子，更具針對性的自動調節狀態估計均方誤差陣；進一步根據新息序列構造了檢驗門限函數，相較于傳統強跟蹤濾波持續引入漸消因子放大狀態均方誤差陣，提高了有效歷史信息的利用率；為進一步提高算法對于時變量測噪聲的自適應能力，加入簡化的薩格-胡薩（Sage-Husa）濾波，并構造了指數漸消遺忘因子，較傳統Sage-Husa濾波降低了計算量，提高了濾波自適應能力。

1 SINS/GNSS動態對準誤差模型

1.1 坐標系定義

1.2 動態對準誤差模型

位置誤差模型可表示為

2 改進的強跟蹤自適應無跡卡爾曼濾波

2.1 無跡卡爾曼濾波

根據式（1）、式（2）和式（6）誤差模型，并增廣陀螺漂移和加速度計偏置構建15維非線性狀態方程和6維線性量測方程為：

UKF濾波通過無跡變換傳播和更新非線性模型的狀態均值和誤差協方差陣，選取有限個加權Sigma采樣點近似系統狀態的先驗統計特性，再通過非線性方程得到系統狀態的后驗概率密度分布特性[14]。UKF濾波計算過程為：

狀態預測值及其均方差陣更新為：

2）量測更新。量測方程為線性系統，得量測更新過程[16]為：

2.2 強跟蹤濾波基本原理

STF需要滿足的條件為

式（24）使STF滿足最小方差估計的基礎上，使其殘差序列時刻保持正交，最大化利用其中有用信息，加強對模型參數失配情況的魯棒性，提高對狀態變化的實時跟蹤能力[17]。

2.3 改進的強跟蹤自適應無跡卡爾曼濾波

結合式（18）至式（22）及式（25）至式（34）得改進的強跟蹤自適應無跡卡爾曼濾波量測更新過程為：

3 實驗與結果分析

為驗證本文所提算法的有效性，采用浙江省舟山市海上船載實測數據進行實驗驗證，實測軌跡如圖1所示，船載實驗設備如圖2所示。數據采集于2021年11月15日，GNSS設備采用華測P5型接收機，輸出頻率1 Hz，可輸出GNSS三維速度和三維位置信息。SINS設備采用Novatel Span ISA-100C光纖捷聯慣導系統，輸出頻率200 Hz，SINS標稱參數詳見表1。采用高精度數據處理軟件（Novatel inertial explorer, IE）的GNSS/SINS平滑組合解算結果作為參考值。

圖1 舟山船載海上實驗軌跡

圖2 船載實驗設備

表1 海上實驗SINS主要參數

載體運動中SINS參數一般不會與標稱參數完全契合，且一般會隨時間變化，真實的SINS參數較難獲得。為驗證本文算法對于系統模型參數失配的魯棒性，SINS設備參數采用表1數據。初始方位角加入1 °橫滾角誤差和俯仰角誤差，以及90 °方位角誤差。為方便實驗結果對比和分析，定義UKF濾波為算法1，融合強跟蹤濾波的STUKF為算法2，本文所提算法為算法3。

橫滾角和俯仰角對比參考值并不明顯，只給出方位角對比參考值曲線，如圖3所示。由圖3可知，算法3只需要約40 s可以跟蹤到載體實際方位角變化，之后與參考值曲線吻合較好；算法2次之，需要約200 s；而算法1所需時間最長，直到1400 s時才能跟蹤到載體實際方位角變化。表明系統模型參數失配情況下，算法2和算法3較算法1具有更強跟蹤載體真實姿態變化的能力，且算法3略優于算法2。圖4～圖6給出了4種算法橫滾角、俯仰角和方位角誤差的對比曲線，相應均方根誤差（100 s后）對比情況如表2所示。

圖3 方位角對比參考值曲線

圖4 橫滾角誤差對比曲線

圖5 俯仰角誤差對比曲線

圖6 方位角誤差對比曲線

表2 姿態均方根誤差對比表 (°)

由圖4和圖5可知：算法1的橫滾角誤差和俯仰角誤差曲線均出現較大幅度波動，俯仰角誤差曲線波動更為明顯；而算法2只出現較小幅度波動，是因為強跟蹤UKF濾波對于系統模型參數失配情況具有較強的魯棒性；算法3波動幅度最小，尤其橫滾角和方位角誤差曲線幾乎未出現明顯波動，算法3魯棒性最強。算法3需要100 s方位角誤差收斂到0°；算法2次之，需要650 s；而算法1只能在1400 s時方位角誤差收斂至1 °，最終收斂精度為0.5°。算法3較算法2收斂時間縮短85%，較算法1收斂時間縮短93%，最終收斂精度提高0.5 °。從表2可知，算法3較算法1和算法2的均方根誤差俯仰角分別降低73%和48%，橫滾角分別降低78%和17%，方位角分別降低98%和90%。證明系統模型參數失配情況下，算法3姿態角收斂性能優勢明顯，所需收斂時間短，誤差收斂精度高；算法2次之；算法1最差。為進一步對比和分析3種算法組合導航系統三維速度和三維位置，圖7給出了參考值東向和北向速度，圖8至圖13給出了3種算法速度和位置誤差對比曲線，相應的均方根誤差對比情況如表3所示。

圖7 參考值東向速度和北向速度

圖8 東向速度誤差對比曲線

圖9 北向速度誤差對比曲線

圖10 天向速度誤差對比曲線

圖11 北向位置誤差對比曲線

圖12 東向位置誤差對比曲線

圖13 高度誤差對比曲線

圖14 GNSS速度誤差對比曲線

表3 組合導航系統速度和位置均方根誤差對比

由圖3和圖7可知，載體在200 ～300、600～ 1000、1200 ～1300 s之間多次出現較大幅度的方位角和速度變化，可由此進一步驗證本文算法的強跟蹤能力。由圖8至圖13可知：在組合導航系統狀態變化期間，算法1三維速度和位置誤差曲線均出現較大幅度波動，魯棒性較差；而算法2只出現較小波動，體現了強跟蹤濾波狀態變化下的魯棒性；對比算法1和算法2，算法3幾乎未出現波動情況，誤差曲線基本保持平滑，說明算法3進一步提高了系統狀態變化情況下的魯棒性。

由表3可知，算法3較算法1和算法2均方根誤差的東向速度分別降低89%和42%，北向速度分別降低93%和60%，天向速度分別降低82%和50%，東向位置分別降低98%和80%，北向位置分別降低94%和50%，高度分別降低97%和90%。由以上分析可得出結論，本文算法較算法1和算法2具有系統模型參數失配情況下更好的魯棒性，以及狀態變化情況下更強的跟蹤能力，組合導航性能優勢明顯。強跟蹤濾波精度提升明顯，但穩定性稍有不足，作者將進一步研究和提升其穩定性。

4 結束語

根據SINS/GNSS組合導航濾波過程的新息向量構造檢驗門限函數，將多漸消因子強跟蹤濾波與UKF濾波結合，并引入簡化的Sage-Husa濾波，提出一種改進的強跟蹤自適應無跡卡爾曼濾波算法。采用海上船載數據進行實驗驗證，結果表明本文所提算法在模型參數失配情況下具有較強的魯棒性和更好的跟蹤系統狀態變化的能力。總的來說，縮短了失準角誤差的收斂時間，提高了組合導航精度，性能優勢明顯，可以為遠海航行提供參考。

[1] XU Xiaofei, NIE Zhixi, WANG Zhengjie, et al. Performance assessment of BDS-3 PPP-B2b/INS loosely coupled integration[J]. Remote Sensing, 2022, 14(13): 2957.

[2] DU Zhenqiang, CHAI Hongzhou, XIAO Guorui, et al. The realization and evaluation of PPP ambiguity resolution with INS aiding in marine survey[J]. Marine Geodesy, 2020, 44(2): 136-156.

[3] NING Yipeng, WANG Jian, HAN Houzeng, et al. An optimal radial basis function neural network enhanced adaptive robust Kalman filter for GNSS/INS integrated systems in complex urban areas[J]. Sensors, 2018, 18(9): 3091.

[4] HAN Songlai, WANG Jinling. A novel initial alignment method for GPS/INS integration with large initial heading error[C]//The Institute of Navigation. Proceedings of the 22nd International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS 2009). Manassas, Virginia: The Institute of Navigation, Inc., 2009: 935-945[2022-08-16].

[5] LIM J N, CHAN G P. A rapid and adaptive alignment under mooring condition using adaptive EKF and CNN-based learning[J]. Sensors, 2020, 20(15): 4069.

[6] WAN E A, MERWE R. The unscented Kalman filter for nonlinear estimation[C]//The Institute of Electrical and Electronic Engineers（IEEE）. Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium. Palms Springs：IEEE, 2000: 153-158[2022-08-16].

[7] 劉海鵬, 張科, 李恒年, 等. 捷聯慣導系統大失準角下的初始對準研究[J]. 航空計算技術, 2012, 42(4): 39-42.

[8] SHAO Haijun, MIAO Lingjuan, GAO Weixi, et al. Ensemble particle filter based on KLD and its application to initial alignment of the SINS in large misalignment angles[J]. IEEE Transactions on Industrial Electronics, 2018, 65(11): 8946-8955.

[9] SUN Jin, XU Xiaosu, LIU Yiting, et al. Initial alignment of large azimuth misalignment angles in SINS based on adaptive UPF[J]. Sensors, 2015, 15(9): 21807-21823.

[10] 李曉明, 趙長勝, 譚興龍. 改進的容積卡爾曼濾波的組合導航定位算法[J]. 測繪科學, 2020, 45(9): 25-30.

[11] 孫楓, 吳旭, 曹通, 等. 基于變分貝葉斯Cubature KF的SINS海上對準方法[J]. 華中科技大學學報(自然科學版), 2013, 41(1): 80-84.

[12] 徐景碩, 王勇軍, 劉亞. 大失準角情形下UKF與CKF的比較研究[J]. 電光與控制, 2017, 24(9): 42-46.

[13] 嚴恭敏, 嚴衛生, 徐德民. 基于歐拉平臺誤差角的SINS非線性誤差模型研究[J]. 西北工業大學學報, 2009, 27(4): 511-516.

[14] JULIER S J. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE, 2004, 92(3): 401-422.

[15] GAO Bingbing, HU Gaoge, GAO Shesheng, et al. Multi-sensor optimal data fusion for INS/GNSS/CNS integration based on unscented Kalman filter[J]. International Journal of Control, Automation and Systems, 2018, 16(1): 129-140.

[16] 王維, 胡高歌, 高社生, 等. 基于極大似然準則的INS/GNSS組合導航自適應UKF濾波算法[J]. 中國慣性技術學報, 2017, 25(5): 656-663.

[17] WANG Lijun, WU Lifeng, GUAN Yong, et al. Online sensor fault detection based on an improved strong tracking filter[J]. Sensors, 2015, 15(2): 4578-4591.

[18] 嚴恭敏, 翁浚. 捷聯慣導算法與組合導航原理[M]. 西安：西北工業大學出版社, 2019: 163-165.

[19] 薛海建, 王解, 郭曉松, 等. SINS非線性自對準中的強跟蹤UKF算法設計[J]. 上海交通大學學報, 2015, 49(09): 1429-1434.

Improved strong tracking adaptive UKF algorithm and its application in large azimuth misalignment

LI Ming, CHAI Hongzhou, JIN Kaidi, WANG Min, SONG Kaifang

（Institute of Geospatial Information，Information Engineering University，Zhengzhou 450001，China）

Aiming at the problem that the unscented Kalman filter (UKF) is easily affected by the mismatch of system model parameters and state changes, which leads to the decrease of filtering accuracy and even divergence, this paper proposes an improved strong tracking adaptive unscented Kalman filter (STAUKF). Strong tracking filter (STF) and UKF are combined, and multiple fading factors are introduced to automatically adjust the mean square error matrix of state estimation. The test threshold function is constructed according to the innovation vector, which improves the utilization rate of useful historical information by filtering. Furthermore, a simplified Sage-Husa filter is introduced to adaptively adjust the measurement noise variance, which reduces the amount of computation and improves the robustness of the algorithm compared with the traditional Sage-Husa algorithm. Finally, the experimental verification is carried out using the measured data at sea, and compared with UKF and strong tracking UKF (STUKF), the results show that the algorithm has obvious advantages, effectively shortens the convergence time of large azimuth misalignment errors, and improves the accuracy of integrated navigation. Compared with the UKF, the azimuth convergence time is shortened by 93%, the velocity root mean square error in the east, north, and up directions is reduced by 89%, 93%, and 82%, respectively, and the position root mean square error is reduced by 98%, 94%, and 97%, respectively.

strong tracking filter; adaptive; unscented Kalman filter; multiple fading factor; integrated navigation

P228

A

2095-4999(2022)06-0165-08

李明，柴洪洲，靳凱迪，等. 改進的強跟蹤自適應UKF算法及其在大方位失準角對準中的應用[J]. 導航定位學報, 2022, 10(6): 165-172.（LI Ming, CHAI Hongzhou, JIN Kaidi, et al.Improved strong tracking adaptive UKF algorithm and its application in large azimuth misalignment[J]. Journal of Navigation and Positioning, 2022, 10(6): 165-172.）

10.16547/j.cnki.10-1096.20220622.

2022-07-11

國家自然科學基金項目（42074014）。

李明（1990—），男，河北唐山人，碩士研究生，工程師，研究方向為GNSS/SINS組合導航。

柴洪洲（1969—），男，河北滄州人，博士，教授，研究方向為大地測量數據處理與水下導航定位。