改進的自適應最大相關熵UKF 算法在SINS/GNSS 組合導航中的應用

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

（南京理工大學自動化學院, 南京 210094）

0 引言

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

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

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

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

1 傳統UKF 算法

考慮非線性離散系統, 其狀態方程及量測方程如下

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

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

UKF 算法的基本過程如下:

（1）初始化

（2）計算sigma 點

（3）時間更新

（4）量測更新

2 改進的ADMCC-UKF 算法

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

2.1 核寬自適應算法設計

（1）最大相關熵準則

在信號處理領域, 常使用隨機過程的統計分布和時間結構對信號進行描述和分析。 文獻［16］基于核方法（Kernel Method）和信息理論學習（Information Theoretic Learning, ITL） 技術提出了相關熵的概念, 文獻［17］ 對相關熵進行了進一步補充和分析。

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

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

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

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

（2）改進的ADMCC 算法

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

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

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

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

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

當1 ＜k＜σ0時, 本文利用高斯函數和迭代誤差e（t）來調整核寬

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

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

2.2 基于變核寬MCC 的量測更新重構

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

利用狀態預測誤差與量測誤差構造誤差矩陣Ek

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

根據式（17） ～式（23）, 結合本文提出的自適應核寬σ（t）, 可以構造如下的代價函數

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

對代價函數J（xk）求取關于Xk的偏導數, 可得

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

式（26）中,

可以令

則式（26）可化簡為

最后, 計算k時刻的狀態預測誤差

由預測誤差更新Pk

2.3 ADMCC-UKF 算法流程

基于2.1 節、 2.2 節內容得到的自適應ADMCC-UKF 算法流程如下:

（1）初始化

（2）時間更新

（3）量測噪聲自適應

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

（4）量測更新

②根據式（23）計算ei,k;

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

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

⑦根據式（36）更新Pk。

3 實驗及分析

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

3.1 SINS/GNSS 組合導航系統模型

（1）系統狀態方程

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

設n系與p系的失準角為φ=［φxφyφz］T,則坐標變化矩陣為

（2）系統量測方程

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

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

3.2 仿真實驗設計

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

表1 實驗參數設置Table 1 Settings of experiment parameters

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

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

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

3.3 跑車實驗設計

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

表2 實驗設備性能參數Table 2 Performance parameters of experiment equipments

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

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

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

3.4 實驗結果及分析

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

利用慣導工具箱生成的載體行駛軌跡數據,分別采用傳統UKF 算法、 MCC-UKF 算法和本文所提的ADMCC-UKF 算法對SINS/GNSS 組合導航系統進行仿真, 結果如圖4 ～圖6 所示。 采用無人車行駛時采集的IMU、 GNSS 數據進行仿真, 結果如圖7 ～圖10 所示。 根據式（43）計算仿真實驗和跑車實驗的平均位置均方誤差, 結果如表3、 表4 所示。 統計仿真實驗中三種算法的運行時間, 結果如表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 可知, 傳統UKF 算法精度最差,本文提出的ADMCC-UKF 算法精度接近MCC-UKF算法, 能很好地減輕系統噪聲統計特性變化帶來的影響, 精度均優于UKF 算法。 UKF 算法由于沒有對系統噪聲統計特性進行在線估計, 其精度受載體機動影響較大, 圖5 中天向位置誤差表現比較明顯。 對比圖4 可知, 在100 s ～200 s、 250 s ～350 s 和400 s ～500 s 間, 當量測噪聲服從非高斯分布時, 傳統UKF 算法的濾波精度明顯低于其他量測噪聲服從高斯分布時的精度。 而本文提出的ADMCC-UKF 算法引入了對系統噪聲統計特性的在線估計, 并根據迭代誤差在線調整核寬大小, 能夠取得更優的精度。 由圖7 ～圖9 可知, 在實際場景下的大部分時間中, ADMCC-UKF 算法可以取得比傳統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 算法參數6次迭代的變化過程, 可以看出: 在迭代過程中,隨著迭代誤差絕對值增大, 核寬迅速增大, 削弱了較大誤差的影響; 隨著迭代誤差絕對值減小,核寬迅速減小至閾值, 提高了解算精度。 由圖10和表4 可知, ADMCC-UKF 算法充分利用了迭代過程中的誤差信息, 可以較好地處理非高斯噪聲的影響。 相比UKF 算法、 MCC-UKF 算法, ADMCCUKF 算法有更高的精度和魯棒性。

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

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

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

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

4 結論

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