改進(jìn)的強(qiáng)跟蹤自適應(yīng)UKF算法及其在大方位失準(zhǔn)角對(duì)準(zhǔn)中的應(yīng)用

李 明，柴洪洲，靳凱迪，王 敏，宋開(kāi)放

改進(jìn)的強(qiáng)跟蹤自適應(yīng)UKF算法及其在大方位失準(zhǔn)角對(duì)準(zhǔn)中的應(yīng)用

李 明，柴洪洲，靳凱迪，王 敏，宋開(kāi)放

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

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

強(qiáng)跟蹤濾波；自適應(yīng)；無(wú)跡卡爾曼濾波；多重漸消因子；組合導(dǎo)航

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)（strapdown inertial navigation system, SINS）具有無(wú)源、隱蔽性高、短時(shí)高精度等優(yōu)點(diǎn)，但導(dǎo)航誤差隨時(shí)間累積較快，需要其他傳感器輔助導(dǎo)航[1]。全球衛(wèi)星導(dǎo)航系統(tǒng)（global navigation satellite system，GNSS）定位精度高，有助于提高導(dǎo)航精度、完好性和可用性[2]。SINS/GNSS組合導(dǎo)航被廣泛用于海洋航行、航空、汽車導(dǎo)航等領(lǐng)域[3]。

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

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

1 SINS/GNSS動(dòng)態(tài)對(duì)準(zhǔn)誤差模型

1.1 坐標(biāo)系定義

1.2 動(dòng)態(tài)對(duì)準(zhǔn)誤差模型

位置誤差模型可表示為

2 改進(jìn)的強(qiáng)跟蹤自適應(yīng)無(wú)跡卡爾曼濾波

2.1 無(wú)跡卡爾曼濾波

根據(jù)式（1）、式（2）和式（6）誤差模型，并增廣陀螺漂移和加速度計(jì)偏置構(gòu)建15維非線性狀態(tài)方程和6維線性量測(cè)方程為：

UKF濾波通過(guò)無(wú)跡變換傳播和更新非線性模型的狀態(tài)均值和誤差協(xié)方差陣，選取有限個(gè)加權(quán)Sigma采樣點(diǎn)近似系統(tǒng)狀態(tài)的先驗(yàn)統(tǒng)計(jì)特性，再通過(guò)非線性方程得到系統(tǒng)狀態(tài)的后驗(yàn)概率密度分布特性[14]。UKF濾波計(jì)算過(guò)程為：

狀態(tài)預(yù)測(cè)值及其均方差陣更新為：

2）量測(cè)更新。量測(cè)方程為線性系統(tǒng)，得量測(cè)更新過(guò)程[16]為：

2.2 強(qiáng)跟蹤濾波基本原理

STF需要滿足的條件為

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

2.3 改進(jìn)的強(qiáng)跟蹤自適應(yīng)無(wú)跡卡爾曼濾波

結(jié)合式（18）至式（22）及式（25）至式（34）得改進(jìn)的強(qiáng)跟蹤自適應(yīng)無(wú)跡卡爾曼濾波量測(cè)更新過(guò)程為：

3 實(shí)驗(yàn)與結(jié)果分析

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

圖1 舟山船載海上實(shí)驗(yàn)軌跡

圖2 船載實(shí)驗(yàn)設(shè)備

表1 海上實(shí)驗(yàn)SINS主要參數(shù)

載體運(yùn)動(dòng)中SINS參數(shù)一般不會(huì)與標(biāo)稱參數(shù)完全契合，且一般會(huì)隨時(shí)間變化，真實(shí)的SINS參數(shù)較難獲得。為驗(yàn)證本文算法對(duì)于系統(tǒng)模型參數(shù)失配的魯棒性，SINS設(shè)備參數(shù)采用表1數(shù)據(jù)。初始方位角加入1 °橫滾角誤差和俯仰角誤差，以及90 °方位角誤差。為方便實(shí)驗(yàn)結(jié)果對(duì)比和分析，定義UKF濾波為算法1，融合強(qiáng)跟蹤濾波的STUKF為算法2，本文所提算法為算法3。

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

圖3 方位角對(duì)比參考值曲線

圖4 橫滾角誤差對(duì)比曲線

圖5 俯仰角誤差對(duì)比曲線

圖6 方位角誤差對(duì)比曲線

表2 姿態(tài)均方根誤差對(duì)比表 (°)

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

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

圖8 東向速度誤差對(duì)比曲線

圖9 北向速度誤差對(duì)比曲線

圖10 天向速度誤差對(duì)比曲線

圖11 北向位置誤差對(duì)比曲線

圖12 東向位置誤差對(duì)比曲線

圖13 高度誤差對(duì)比曲線

圖14 GNSS速度誤差對(duì)比曲線

表3 組合導(dǎo)航系統(tǒng)速度和位置均方根誤差對(duì)比

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

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

4 結(jié)束語(yǔ)

根據(jù)SINS/GNSS組合導(dǎo)航濾波過(guò)程的新息向量構(gòu)造檢驗(yàn)門限函數(shù)，將多漸消因子強(qiáng)跟蹤濾波與UKF濾波結(jié)合，并引入簡(jiǎn)化的Sage-Husa濾波，提出一種改進(jìn)的強(qiáng)跟蹤自適應(yīng)無(wú)跡卡爾曼濾波算法。采用海上船載數(shù)據(jù)進(jìn)行實(shí)驗(yàn)驗(yàn)證，結(jié)果表明本文所提算法在模型參數(shù)失配情況下具有較強(qiáng)的魯棒性和更好的跟蹤系統(tǒng)狀態(tài)變化的能力?？偟膩?lái)說(shuō)，縮短了失準(zhǔn)角誤差的收斂時(shí)間，提高了組合導(dǎo)航精度，性能優(yōu)勢(shì)明顯，可以為遠(yuǎn)海航行提供參考。

[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] 劉海鵬, 張科, 李恒年, 等. 捷聯(lián)慣導(dǎo)系統(tǒng)大失準(zhǔn)角下的初始對(duì)準(zhǔn)研究[J]. 航空計(jì)算技術(shù), 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] 李曉明, 趙長(zhǎng)勝, 譚興龍. 改進(jìn)的容積卡爾曼濾波的組合導(dǎo)航定位算法[J]. 測(cè)繪科學(xué), 2020, 45(9): 25-30.

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

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

[13] 嚴(yán)恭敏, 嚴(yán)衛(wèi)生, 徐德民. 基于歐拉平臺(tái)誤差角的SINS非線性誤差模型研究[J]. 西北工業(yè)大學(xué)學(xué)報(bào), 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] 王維, 胡高歌, 高社生, 等. 基于極大似然準(zhǔn)則的INS/GNSS組合導(dǎo)航自適應(yīng)UKF濾波算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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] 嚴(yán)恭敏, 翁浚. 捷聯(lián)慣導(dǎo)算法與組合導(dǎo)航原理[M]. 西安：西北工業(yè)大學(xué)出版社, 2019: 163-165.

[19] 薛海建, 王解, 郭曉松, 等. SINS非線性自對(duì)準(zhǔn)中的強(qiáng)跟蹤UKF算法設(shè)計(jì)[J]. 上海交通大學(xué)學(xué)報(bào), 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

李明，柴洪洲，靳凱迪，等. 改進(jìn)的強(qiáng)跟蹤自適應(yīng)UKF算法及其在大方位失準(zhǔn)角對(duì)準(zhǔn)中的應(yīng)用[J]. 導(dǎo)航定位學(xué)報(bào), 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

國(guó)家自然科學(xué)基金項(xiàng)目（42074014）。

李明（1990—），男，河北唐山人，碩士研究生，工程師，研究方向?yàn)镚NSS/SINS組合導(dǎo)航。

柴洪洲（1969—），男，河北滄州人，博士，教授，研究方向?yàn)榇蟮販y(cè)量數(shù)據(jù)處理與水下導(dǎo)航定位。