改進的自適應(yīng)最大相關(guān)熵UKF 算法在SINS/GNSS 組合導(dǎo)航中的應(yīng)用

黃海舟, 周凌柯, 張永耀, 蔡紫燁, 李 勝

（南京理工大學(xué)自動化學(xué)院, 南京 210094）

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System, SINS）和全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite Systems, GNSS） 在導(dǎo)航任務(wù)中應(yīng)用廣泛, SINS 具有長期穩(wěn)定性差、 短期穩(wěn)定性好、 動態(tài)性能好的特點, 而GNSS 則正好相反, 因此工程上常將兩者通過組合導(dǎo)航技術(shù)結(jié)合起來, 組成SINS/GNSS 組合導(dǎo)航系統(tǒng), 以提升組合導(dǎo)航的整體性能［1］。 SINS/GNSS 組合導(dǎo)航系統(tǒng)是典型的非線性系統(tǒng), 無論是導(dǎo)航參數(shù)解算還是導(dǎo)航方程建立都涉及非線性問題, 且系統(tǒng)中的噪聲多為非高斯噪聲［2］。 當(dāng)前, 擴展卡爾曼濾波（Extended Kalman Filter, EKF）被廣泛應(yīng)用于組合導(dǎo)航系統(tǒng)設(shè)計, 然而當(dāng)系統(tǒng)模型的非線性較強時, EKF 濾波精度會嚴重下降甚至發(fā)散, 魯棒性較差［3］。

為了克服EKF 存在的問題, 有學(xué)者提出了無跡卡爾曼濾波（Unscented Kalmam Filter, UKF）。UKF 利用無跡變換對非線性系統(tǒng)的狀態(tài)后驗概率密度函數(shù)進行近似, 可以從二階以上精度近似高斯線性系統(tǒng)的狀態(tài)后驗均值與方差。 此外, 相比于EKF, UKF 無需計算Jacobian 矩陣, 具有濾波精度高、 計算簡單、 收斂性好等特點［4-5］。 然而,UKF 與EKF 均假設(shè)系統(tǒng)中噪聲服從高斯分布, 當(dāng)系統(tǒng)中噪聲非高斯時, 會存在魯棒性較差、 濾波精度下降的問題［6］。

文獻［7］、 文獻［8］提出了一種基于最大相關(guān)熵準(zhǔn)則（Maximum Correntropy Criterion, MCC） 的卡爾曼濾波器（Kalman Filter, KF）, 研究表明MCC-KF可以較好地應(yīng)對非高斯噪聲的影響, 濾波精度相比于傳統(tǒng)KF 有較大提升。 文獻［9］研究了一種魯棒的MCC-UKF 算法, 將MCC 與UKF 結(jié)合, 提高了算法的濾波精度和魯棒性。 文獻［10］研究了核寬對MCC 算法迭代性能的影響并得出了結(jié)論:在一定范圍內(nèi), 較大核寬能夠加快算法收斂速度, 但會導(dǎo)致穩(wěn)態(tài)誤差變大; 較小核寬收斂速度較慢, 但穩(wěn)態(tài)誤差較小。 文獻［11］ ～文獻［13］ 研究了幾種不同的核寬自適應(yīng)準(zhǔn)則, 結(jié)果表明: 相比于定核寬, 變核寬能在一定程度上提高算法的精度, 但是部分結(jié)果顯示應(yīng)對強脈沖噪聲時的穩(wěn)定性較差。 文獻［14］ 提出了一種變量核寬MCC,通過改寫代價函數(shù), 解決了強脈沖噪聲條件下的穩(wěn)定性問題, 然而這種方法提高了算法的復(fù)雜度。

本文針對傳統(tǒng)MCC 算法受核寬大小影響的問題, 對基于傳統(tǒng)MCC 的UKF 濾波算法進行了相關(guān)研究和改進。 利用改進的量測噪聲自適應(yīng)方法對量測噪聲進行自適應(yīng)調(diào)整, 并結(jié)合新的自適應(yīng)核寬算法, 提出了自適應(yīng)變核寬的ADMCC-UKF 算法。 通過SINS/GNSS 仿真實驗, 驗證了所提出的改進算法的有效性與優(yōu)越性。

1 傳統(tǒng)UKF 算法

考慮非線性離散系統(tǒng), 其狀態(tài)方程及量測方程如下

式（1）中,Xk∈Rn×1、Yk∈Rm×1分別為k時刻系統(tǒng)的狀態(tài)向量和量測向量;f（·）為非線性狀態(tài)轉(zhuǎn)移函數(shù); 系統(tǒng)噪聲qk、 量測噪聲rk為互不相關(guān)的零均值高斯白噪聲序列, 其統(tǒng)計特性如下

式（2）中,Qk為非負定矩陣,Rk為正定矩陣,δkj為Kronecker 函數(shù)。

UKF 算法的基本過程如下:

（1）初始化

（2）計算sigma 點

（3）時間更新

（4）量測更新

2 改進的ADMCC-UKF 算法

傳統(tǒng)MCC 算法利用核函數(shù)中核寬的設(shè)置, 在迭代中削弱了非高斯噪聲的影響。 然而傳統(tǒng)MCC算法中核寬常設(shè)置為定值, 不僅降低了算法的收斂速度, 而且往往無法獲得好的穩(wěn)態(tài)誤差結(jié)果。本文設(shè)計了一種自適應(yīng)核寬（Adaptive MCC, ADMCC）算法, 首先進行量測噪聲估計, 通過噪聲估計的變化程度確定核寬調(diào)整的上界和下界; 然后依據(jù)算法的迭代誤差對核寬進行自適應(yīng)調(diào)整, 大大提高了收斂速度。

2.1 核寬自適應(yīng)算法設(shè)計

（1）最大相關(guān)熵準(zhǔn)則

在信號處理領(lǐng)域, 常使用隨機過程的統(tǒng)計分布和時間結(jié)構(gòu)對信號進行描述和分析。 文獻［16］基于核方法（Kernel Method）和信息理論學(xué)習(xí)（Information Theoretic Learning, ITL） 技術(shù)提出了相關(guān)熵的概念, 文獻［17］ 對相關(guān)熵進行了進一步補充和分析。

相關(guān)熵表示兩個隨機變量X和Y之間的相似程度, 其定義如下

式（15）中,pXY（x,y）為聯(lián)合概率密度,κσ（x,y）為Mercer 核函數(shù)。 其中, 最常用的核函數(shù)為高斯核函數(shù)

式（16）中,e=x-y,σ為核寬（σ＞0）。 在實際情況中,X和Y的概率密度函數(shù)通常難以獲取,因此常使用樣本的平均估計量來對相關(guān)熵Cσ（X,Y）進行估計

由式（15）、 式（16）可知, 高斯核下的相關(guān)熵非負, 且在x=y時相關(guān)熵取得最大值。 相關(guān)熵的實質(zhì)是對兩個隨機變量之差高斯變換后的函數(shù)求取數(shù)學(xué)期望, 兩個分布的相關(guān)熵越大, 說明兩個分布越相似。 當(dāng)噪聲序列呈現(xiàn)非高斯分布時, 特別是出現(xiàn)脈沖分布時, 噪聲序列大概率會出現(xiàn)大值偏差。 相比于最小均方誤差準(zhǔn)則, 此時高斯核中負指數(shù)項和設(shè)置的核寬σ共同作用, 削弱大值偏差對相關(guān)熵的影響, 所以在非高斯噪聲環(huán)境下能取得更好的性能［18］。

（2）改進的ADMCC 算法

在MCC 算法迭代求解過程中, 初始迭代誤差大小與量測噪聲相關(guān), 因此可以利用量測噪聲的變化信息對核寬的變化范圍進行界定。 本文利用Sage-Husa 自適應(yīng)濾波的思想, 對當(dāng)前時刻的量測噪聲進行估計, 步驟如下:

傳統(tǒng)Sage-Husa 自適應(yīng)濾波認為量測噪聲的方差是緩慢變化的, 該方法并不適用于方差快速變化或量測出現(xiàn)粗差的情形, 文獻［19］對噪聲自適應(yīng)過程做了如下改進

式（20） 中,s為縮小因子, 一般取2 ～4。 由式（20）可知, 在高斯分布的范圍內(nèi)的量測噪聲都視為正常噪聲, 而不在此范圍的量測噪聲則視為粗差噪聲。 當(dāng)偏差嚴重時, 對應(yīng)的量測方差將對應(yīng)變大, 其對狀態(tài)估計的影響會減小, 一定程度上提升了抗干擾能力。

利用式（19）、 式（20）能夠確定改進的可變核寬σ（t）的變化區(qū)間為。 其中,σ0為初始核寬,k的取值如下

式（21）中,k為放大系數(shù), 1 ＜k＜σ0,mean（·）為平均值,Nw為窗口大小。 分析式（21） 可知, 當(dāng)k≤1 時, 即當(dāng)前時刻量測方差估計值小于歷史時刻量測方差均值時, 此時的可變核寬σ（t） =σ0,退化為定核寬, 與傳統(tǒng)MCC 一致, 可以認為此時不需要對核寬進行調(diào)整即可獲得較好的性能, 有效減少了計算量; 當(dāng)1 ＜k＜σ0時, 即當(dāng)前時刻量測方差估計值大于歷史時刻量測方差均值時, 令可變核寬σ（t） 在初始核寬σ0的鄰域和（σ0,kσ0］內(nèi)變化, 保證了算法的魯棒性。

當(dāng)1 ＜k＜σ0時, 本文利用高斯函數(shù)和迭代誤差e（t）來調(diào)整核寬

圖1 不同k 值下核寬隨迭代誤差變化曲線Fig.1 Curves of kernel width variation with iteration error under different k values

分析圖1 可知, 隨著k值增大, 可變核寬σ（t）的變化范圍越大, 核寬下界越窄, 有助于在迭代誤差較小時獲得更小的穩(wěn)態(tài)誤差; 隨著k值減小,σ（t）的變化范圍越小, 核寬下界越寬, 有助于提高系統(tǒng)收斂速度; 在同一迭代誤差下, 量測噪聲越大, 核寬越大, 量測對應(yīng)權(quán)值越小, 有助于對當(dāng)前的系統(tǒng)誤差進行控制。

2.2 基于變核寬MCC 的量測更新重構(gòu)

MCC 算法通過對卡爾曼濾波的量測更新部分進行重構(gòu), 獲得一種基于MCC 的魯棒濾波算法。本文將MCC 算法與提出的改進核寬自適應(yīng)算法相結(jié)合, 得到一種改進的自適應(yīng)MCC 魯棒濾波算法。

利用狀態(tài)預(yù)測誤差與量測誤差構(gòu)造誤差矩陣Ek

式（23）中,CP,k/k-1和CR,k為下三角矩陣, 分別由Pk/k-1和Rk通過Cholesky 分解得到［20］。

根據(jù)式（17） ～式（23）, 結(jié)合本文提出的自適應(yīng)核寬σ（t）, 可以構(gòu)造如下的代價函數(shù)

式（24）中,ei,k為Ek中的第i行元素,n為狀態(tài)量維數(shù),m為量測量維數(shù)。 通過式（24）構(gòu)造的代價函數(shù), 可以利用梯度下降法對狀態(tài)變量Xk進行迭代求解, 求解過程如下:

對代價函數(shù)J（xk）求取關(guān)于Xk的偏導(dǎo)數(shù), 可得

令式（25）為零并化簡, 可得

式（26）中,

可以令

則式（26）可化簡為

最后, 計算k時刻的狀態(tài)預(yù)測誤差

由預(yù)測誤差更新Pk

2.3 ADMCC-UKF 算法流程

基于2.1 節(jié)、 2.2 節(jié)內(nèi)容得到的自適應(yīng)ADMCC-UKF 算法流程如下:

（1）初始化

（2）時間更新

（3）量測噪聲自適應(yīng)

根據(jù)式（18） ～式（20）計算k時刻的Rk。

（4）量測更新

②根據(jù)式（23）計算ei,k;

③根據(jù)式（21）、 式（22）計算迭代核寬σ（t）;

④根據(jù)式（27） ～式（29）計算ΠP,k、ΠR,k、Πi,k;

⑦根據(jù)式（36）更新Pk。

3 實驗及分析

本文利用SINS/GNSS 組合導(dǎo)航模型對所提算法性能進行仿真驗證和實車驗證, 在此直接給出SINS/GNSS 組合導(dǎo)航的數(shù)學(xué)模型。

3.1 SINS/GNSS 組合導(dǎo)航系統(tǒng)模型

（1）系統(tǒng)狀態(tài)方程

選取地理坐標(biāo)系即“東、 北、 天” 坐標(biāo)系作為導(dǎo)航坐標(biāo)系（n系）, SINS 計算所得的導(dǎo)航坐標(biāo)系為p系。 SINS 的姿態(tài)、 速度、 位置誤差模型如下

設(shè)n系與p系的失準(zhǔn)角為φ=［φxφyφz］T,則坐標(biāo)變化矩陣為

（2）系統(tǒng)量測方程

利用GNSS 得到的三軸位置信息對SINS 進行定期濾波校正, 式中量測方程定義如下

式（40）中,O3×6為3 ×6 維零矩陣,I3×3為3 ×3 維單位矩陣。

3.2 仿真實驗設(shè)計

本實驗采用西北工業(yè)大學(xué)嚴恭敏團隊開發(fā)的高精度慣性導(dǎo)航工具箱［21］, 生成一組無人車軌跡數(shù)據(jù)以及對應(yīng)的SINS 數(shù)據(jù), 利用本文提出的ADMCC-UKF 算法, 與MCC-UKF 算法和傳統(tǒng)UKF 算法進行比較, 從組合導(dǎo)航系統(tǒng)精度、 算法運行時間兩方面驗證本文算法的有效性。 實驗參數(shù)設(shè)置如表1 所示。

表1 實驗參數(shù)設(shè)置Table 1 Settings of experiment parameters

設(shè)置小車初始位置為東經(jīng)108°、 北緯34°, 載體初始速度為0 m/s, 方向正北, 載體經(jīng)過加速、勻速、 左轉(zhuǎn)彎、 右轉(zhuǎn)彎、 繞圈、 上坡、 下坡、 減速等機動過程, 行駛時長為950 s, 行駛軌跡如圖2 所示, 紅色標(biāo)記處為小車的起始位置。

圖2 載體行駛軌跡Fig.2 Diagram of carrier driving trajectory

為了驗證改進算法對非高斯問題的有效性, 取載體行駛過程中的三個時段100 s ～200 s、 250 s ～350 s、 400 s ～500 s 添加受污染的高斯噪聲, 混合比α=0.3。 混合高斯噪聲分布如下

3.3 跑車實驗設(shè)計

為了進一步驗證算法的有效性, 使用實驗室無人車采集了一組軌跡數(shù)據(jù), 實驗場地位于南京理工大學(xué)噴泉廣場。 實驗設(shè)備性能參數(shù)如表2 所示。

表2 實驗設(shè)備性能參數(shù)Table 2 Performance parameters of experiment equipments

使用無人車的內(nèi)置高精度RTK 采集一組軌跡,如圖3 所示。 紅色標(biāo)記處為小車起始點, 小車逆時針行駛一圈后停止, 行駛時長約220 s。

圖3 無人車的行駛軌跡Fig.3 Diagram of unmanned vehicle driving trajectory

為了驗證改進算法對非高斯問題的有效性,在GNSS 原始數(shù)據(jù)中隨機添加10%如式（41）所示的混合高斯噪聲, 以模擬GNSS 定位質(zhì)量較差的場景。

3.4 實驗結(jié)果及分析

采用蒙特卡羅仿真方法, 設(shè)置仿真次數(shù)M=50, 計算位置均方誤差RMSEδp和平均位置均方誤差A(yù)RMSEδp, 并對仿真結(jié)果進行對比。 計算公式如下

利用慣導(dǎo)工具箱生成的載體行駛軌跡數(shù)據(jù),分別采用傳統(tǒng)UKF 算法、 MCC-UKF 算法和本文所提的ADMCC-UKF 算法對SINS/GNSS 組合導(dǎo)航系統(tǒng)進行仿真, 結(jié)果如圖4 ～圖6 所示。 采用無人車行駛時采集的IMU、 GNSS 數(shù)據(jù)進行仿真, 結(jié)果如圖7 ～圖10 所示。 根據(jù)式（43）計算仿真實驗和跑車實驗的平均位置均方誤差, 結(jié)果如表3、 表4 所示。 統(tǒng)計仿真實驗中三種算法的運行時間, 結(jié)果如表5 所示。

表3 仿真實驗的平均位置均方誤差Table 3 ARMSE of simulation experiment

表4 跑車實驗的平均位置均方誤差Table 4 ARMSE of real vehicle experiment

表5 算法單次運行時間Table 5 Single running time of the algorithm

圖4 東向位置仿真的RMSEFig.4 RMSE of simulation for east position

由圖4 ～圖6 可知, 傳統(tǒng)UKF 算法精度最差,本文提出的ADMCC-UKF 算法精度接近MCC-UKF算法, 能很好地減輕系統(tǒng)噪聲統(tǒng)計特性變化帶來的影響, 精度均優(yōu)于UKF 算法。 UKF 算法由于沒有對系統(tǒng)噪聲統(tǒng)計特性進行在線估計, 其精度受載體機動影響較大, 圖5 中天向位置誤差表現(xiàn)比較明顯。 對比圖4 可知, 在100 s ～200 s、 250 s ～350 s 和400 s ～500 s 間, 當(dāng)量測噪聲服從非高斯分布時, 傳統(tǒng)UKF 算法的濾波精度明顯低于其他量測噪聲服從高斯分布時的精度。 而本文提出的ADMCC-UKF 算法引入了對系統(tǒng)噪聲統(tǒng)計特性的在線估計, 并根據(jù)迭代誤差在線調(diào)整核寬大小, 能夠取得更優(yōu)的精度。 由圖7 ～圖9 可知, 在實際場景下的大部分時間中, ADMCC-UKF 算法可以取得比傳統(tǒng)UKF 算法、 MCC-UKF 算法更低的位置誤差。

圖5 北向位置仿真的RMSEFig.5 RMSE of simulation for north position

圖6 天向位置仿真的RMSEFig.6 RMSE of simulation for upward positon

圖7 東向位置實車的RMSEFig.7 RMSE of real vehicle for east position

圖8 北向位置實車的RMSEFig.8 RMSE of real vehicle for north position

圖9 天向位置實車的RMSEFig.9 RMSE of real vehicle for upward positon

圖10 所示為100 s 時ADMCC-UKF 算法參數(shù)6次迭代的變化過程, 可以看出: 在迭代過程中,隨著迭代誤差絕對值增大, 核寬迅速增大, 削弱了較大誤差的影響; 隨著迭代誤差絕對值減小,核寬迅速減小至閾值, 提高了解算精度。 由圖10和表4 可知, ADMCC-UKF 算法充分利用了迭代過程中的誤差信息, 可以較好地處理非高斯噪聲的影響。 相比UKF 算法、 MCC-UKF 算法, ADMCCUKF 算法有更高的精度和魯棒性。

圖10 算法參數(shù)迭代變化Fig.10 Iterative change of algorithm parameters

從表3、 表4 數(shù)據(jù)可以看出, 傳統(tǒng)UKF 算法精度最差, MCC-UKF 算法精度接近傳統(tǒng)UKF 算法,本文提出的ADMCC-UKF 算法精度優(yōu)于這兩種算法。 在仿真實驗中, ADMCC-UKF 算法相較于傳統(tǒng)UKF 算法平均提高了19.27%, 相較于MCC-UKF算法平均提高了11.13%; 在實際實驗中, ADMCC-UKF 算法相較于傳統(tǒng)UKF 算法平均提高了12.55%, 相較于 MCC-UKF 算法平均提高了11.23%。

表5 數(shù)據(jù)取自PC 電腦仿真, PC 電腦的配置為: 處理器AMD Ryzen 9 3.3 GHz、 內(nèi)存DDR4 3200MHz 32 G。 從表5 數(shù)據(jù)可以看出, 本文提出的ADMCC-UKF 算法運行時間略高于傳統(tǒng)UKF 算法,但優(yōu)于MCC-UKF 算法。

綜上所述, 本文提出的ADMCC-UKF 算法可以有效處理系統(tǒng)噪聲為非高斯噪聲時帶來的影響,能夠取得優(yōu)于MCC-UKF 算法和傳統(tǒng)UKF 算法的精度, 同時計算量小于MCC-UKF 算法。 相比于傳統(tǒng)UKF 算法提高的系統(tǒng)精度, ADMCC-UKF 算法提高的計算量可以接受。

4 結(jié)論

本文針對傳統(tǒng)無跡卡爾曼濾波器在非高斯噪聲情況下濾波精度下降的問題, 提出了改進的自適應(yīng)變核寬相關(guān)熵ADMCC-UKF 算法。 首先根據(jù)慣性測量單元的基本情況構(gòu)建系統(tǒng)噪聲的粗略統(tǒng)計模型, 對量測噪聲進行自適應(yīng)估計; 然后引入核寬自適應(yīng)算法, 在迭代過程中依據(jù)誤差變化自適應(yīng)調(diào)整核寬, 加快收斂速度; 最后通過基于最大相關(guān)熵準(zhǔn)則的濾波器得到狀態(tài)估計, 最終得到本文的ADMCC-UKF 算法。 相較于傳統(tǒng)UKF 算法,該算法有較為明顯的濾波精度提升; 相較于MCCUKF 算法, 該算法精度更高、 運行時間更短。 基于SINS/GNSS 組合導(dǎo)航系統(tǒng)對本文提出的算法進行仿真實驗和實際跑車實驗, 從濾波精度和解算時間兩方面驗證了本文所提算法的有效性, 對工程實踐有一定的參考意義。