基于EKF 的多模型自適應組合導航算法

袁思思, 陳 帥, 牛仁杰, 周 興, 程 玉

（南京理工大學, 南京 210094）

0 引言

慣性導航系統（Strapdown Inertial Navigation System, SINS）具有自主性強、 動態性能好、 導航信息全面且輸出頻率高等優點, 但其誤差隨時間不斷積累, 長期精度不高［1］。 相比而言, 全球衛星定位系統（Global Navigation Satellite System, GNSS）的優點是定位精度高并且誤差不隨時間積累, 但是導航信息不夠全面且容易受到干擾, 在室內和遮擋等環境下無法使用。 慣性導航和衛星導航之間有很強的互補性, 慣性/衛星組合導航提高了系統整體的精度和可靠性, 被公認為是最佳的組合導航方案［2］。

然而, 由于組合導航系統本質上具有的非線性特征, 當采用非線性系統模型來描述導航誤差的傳播特性時, 常規的Kalman 濾波（KF）就不再適用, 此時需要采用非線性濾波算法來解決系統中的狀態估計問題。 王維等［3］采用自適應無跡Kalman 濾波來解決系統的非線性問題, 對導航精度有一定的提升效果, 但是采用無跡變換存在著計算量隨著狀態量維度的增加而增大的問題, 不適用于實際的工 程 實現。 張 園等［4］在GNSS/SINS 組合導航系統中分別應用擴展Kalman 濾波（EKF）和粒子濾波（Particle Filter, PF）兩種非線性濾波算法進行對比, 結果表明擴展Kalman 濾波更適用于弱非線性情況下的組合導航系統, 定位精度更高。 劉江等［5］將 魯 棒 容 積Kalman 濾 波（Cubature Kalman Filter, CKF）應用于非線性組合導航系統中, 可以有效抑制異常觀測值的影響, 但是當模型噪聲不確定時濾波質量和效率降低。

另外, 當載體處于復雜環境中時, 傳感器量測噪聲的統計特性難以準確獲取且具有時變性［6］。此時若使用單一固定的濾波器模型對系統進行估計, 則在長時間運行過程中必定導致濾波精度的下降［7］。 由此, 很容易想到采用多個模型并行估計的方法。 1965 年, Magill 提出了第一代多模型自適應估計算法, 其主要特點是使用固定個數的濾波器, 每個濾波器對應一個特定的模型, 模型之間不能交互, 對每個濾波器的估計狀態值賦予固定的不同的權值, 每個狀態估計值乘以相應的權重相加得到最終的狀態估計值。 該方法雖能在一定程度上提高估計的準確性, 但由于模型之間缺少交互且權值固定, 因此適用范圍有限［8］。 在多模型自適應估計的基礎上, Kirubarajan 等［8］提出了交互式多模型（IMM）算法, 這一方法在環境未知多變的情況下顯示了更好的魯棒性和更高的濾波精度。

IMM 是通過模型轉換概率實現系統模式的Markov 切換過程, 相比于廣義偽Bayes 算法, 兼具了計算復雜度低和估計精度高的優點, 被認為是一種有效的混合估計方案［8］。 交互式多模型濾波算法對于處理模型參數不確定情況下的估計問題有著其他方法所不具備的效果, 但估計精度比較依賴于模型集［9-10］。

本文引入交互式多模型的思想, 結合EKF 根據實際環境設計多個不同的模型, 每個模型對應一個EKF 子濾波器, 各個子濾波器同時進行濾波,并通過似然函數實現模型之間的交互, 然后對各個子濾波器的濾波結果進行概率加權融合, 以得到最優的狀態估計值。 將交互式多模型與擴展Kalman 濾波結合在一起, 充分發揮非線性濾波適用于非線性系統的特點, 更好地匹配實際環境中的非線性輸入輸出關系, 提高系統的定位精度, 增強系統的魯棒性。

1 GNSS/SINS 組合導航系統

GNSS/SINS 組合導航是目前常見的組合導航系統, 兩者之間相互取長補短, 克服了各自的缺點, 使得組合后的導航系統精度明顯高于兩個系統單獨工作時的精度。 一般來說, 根據系統狀態的不同選擇, 可以搭建兩種不同的組合導航系統模型: 第一種是將導航子系統的導航輸出參數直接作為狀態向量, 稱為直接法濾波; 第二種是將導航子系統的誤差量作為狀態向量, 稱為間接法濾波。 由于直接法中系統狀態向量的數量級相差太大, 比如姿態與位置信息, 在數學計算上會產生較大誤差, 實際應用中會存在較多問題, 因此本文使用間接法, 采用松組合, 將GNSS 接收機和SINS 輸出的位置和速度信息的差值作為觀測量,經濾波器濾波估計捷聯式慣性導航系統的誤差,從而對捷聯式慣性系統進行矯正, 最后得到高精度、 穩定、 持續的載體姿態速度位置信息。 本文采用地理系（東北天坐標系）作為導航坐標系, 對系統模型做如下介紹。

1.1 SINS 非線性誤差模型

在SINS 姿態解算過程中, 四元數方法由于計算簡單、 精度高而被廣泛應用［11］。 本文通過四元數來表示姿態, 構建SINS 誤差模型, 現對誤差模型的構建做簡要介紹, SINS 非線性誤差模型的具體介紹可以參考文獻［11］。

假設Q=［q0q1q2q3］T為SINS 真實的姿態四元數,為實際解算的四元數, 則Q和Q～分別滿足如下的微分方程

式（1）中, ?表示四元數乘積,b表示載體坐標系,n表示導航坐標系,i表示慣性坐標系。式（2）中,

根據式（1）和式（2）, 可得如下的SINS 姿態誤差方程

同樣地, 根據SINS 真實速度、 實際測量速度以及慣導的比力方程可以得到SINS 的速度誤差方程為

式（4）中,Cn b為姿態矩陣, 其與四元數Q的關系為

根據SINS 位置P=［L λ h］T與SINS 速度Vn之間的關系可得SINS 位置誤差方程

參數的具體含義可參考文獻［11］, 在此不做過多介紹。 由于姿態轉移矩陣與四元數之間存在的非線性關系, 使得SINS 誤差方程在姿態誤差角較大時含有較強的非線性。

1.2 組合導航系統模型

（1）系統狀態方程

式（9）中,X（t）為由SINS 誤差變量構成的19 維狀態向量。 其中,δq0、δq1、δq2、δq3分別為捷聯式慣性導航系統姿態四元數誤差,δVE、δVN、δVU分別為東向、 北向、 天向上的速度誤差,δL、δλ、δh分別為緯度、 經度、 高度的位置誤差,εbx、εby、εbz分別為陀螺儀在x、y、z三個軸向上的隨機常值誤差,εrx、εry、εrz分別為陀螺儀在x、y、z三個軸向上的一階Markov 偏移誤差,Δx、Δy、Δz分別為加速度計在x、y、z三個軸向上的一階Markov偏移誤差。 式（10）中,G（t）為19 ×9 階系統噪聲驅動矩陣,W（t）為9 維系統噪聲隨機誤差向量。

（2）系統量測方程

采用松組合時, 系統量測方程為線性模型。將GNSS 接收機輸出的速度和位置信息與捷聯慣性導航系統相應的輸出信息做差, 得到量測方程如下

式（11） 中,Z（t） 為6 維的量測向量,H（t） 為6 ×19 階量測矩陣,V（t）為6 維量測噪聲并且假設E［Vk］ =0 和, 具體矩陣設置可參考文獻［12］。

2 擴展Kalman 濾波算法

對于非線性系統, 一種常見的解決思路是先進行Taylor 級數展開, 之后進行線性化截斷略去高階項后近似為線性系統, 再做線性Kalman 濾波估計, 這種處理非線性系統的Kalman 濾波方法即稱為擴展Kalman 濾波（EKF）［13］。 而根據截斷的位置, 擴展Kalman 濾波可以分為一階擴展Kalman 濾波和二階擴展Kalman 濾波等, 二階的濾波精度并不明顯比一階高, 反而對于高維系統而言, 二階濾波的計算量增加了很多［1］, 因此在這里主要討論一階擴展Kalman 濾波。 同樣地, 根據實際工程應用, 主要對離散狀態下的系統進行討論。

將GNSS/SINS 組合導航系統的狀態方程和量測方程離散化, 得到如下方程組

其中,

式（12）、 式（13） 中,Xk為n維 狀 態 向 量,f（Xk） =［f1（Xk）f2（Xk） …fn（Xk）］T為n維非 線性向量函數,Zk為m維量測向量,Γk-1為k-1 時刻的n×p階系統噪聲驅動矩陣,Hk為k時刻的m×n階量測矩陣,Wk-1為k-1 時刻滿足均值為0 的Gauss 白噪聲條件的p維過程噪聲向量,Vk為k時刻滿足均值為0 的Gauss 白噪聲條件的m維量測噪聲向量,Qk為過程噪聲向量的協方差矩陣且為非負定矩陣,Rk為量測噪聲向量的協方差矩陣且為正定矩陣,δkj為Kronecker-delta 函數。δkj滿足

EKF 的一個核心思想是對式（12）中的非線性方程做Taylor 展開數學變換并將結果再做一階截斷,得到一組線性化方程

經過Taylor 展開并一階截斷后, 就將系統變為了線性系統, 可以使用基本Kalman 濾波方程完成迭代優化的過程。 至此, EKF 方程如下:

時間預測方程為

量測更新方程為

3 基于IMM-EKF 的GNSS/SINS 組合導航算法

3.1 交互式多模型算法

交互式多模型算法的核心思想是將系統的不同模型映射為模型集, 其中的每個模型對應一個濾波器, 各濾波器經過輸入交互之后并行工作,利用每個濾波器輸出的殘差信息、 殘差協方差信息以及各模型的先驗信息, 依據某種假設檢驗規則, 得到每個濾波器所對應的模型與真實系統模型匹配的概率（稱為模型概率）, 最后根據模型概率加權融合各濾波器估計, 得到系統的最終估計［14］。 在交互式多模型算法的基礎上, 本文采用EKF 濾波構建了IMM-EKF 濾波算法, 該算法采用三個濾波器來構建多模型, 原理框圖如圖1所示。

圖1 交互式多模型算法原理框圖Fig.1 Block diagram of an interactive multiple models algorithm

（1）輸入交互

對給定的n個模型的初始狀態進行輸入交互運算, 定位目標的初始狀態估計值為

狀態估計協方差如下

其中,

（2）狀態濾波

將上一步得到的初始狀態和協方差作為濾波器的輸入, 對每一個模型進行EKF 濾波, 多個濾波器并行工作, 得到每個模型當前時刻的狀態估計及其均方誤差陣, 時間更新過程和量測更新過程同標準EKF 算法。

（3）模型概率更新

模型概率的計算是假設檢驗過程, 一般采用Bayes 假設檢驗法, 原理是同時檢驗各個濾波器的殘差, 狀態濾波后的新息為

新息協方差為

計算各模型的似然函數值為

式（24）中,為k時刻模型j的似然函數值,m為量測向量的維數。

采用Bayes 假設檢驗方法進行模型概率更新

（4）融合輸出

融合輸出即按照各濾波器估計值的概率加權融合, 計算各模型的聯合狀態估計和狀態估計協方差矩陣, 得到IMM 的最終輸出。 最終得到的狀態估計值表達式為

狀態估計值對應的協方差為

3.2 基于IMM-EKF 的組合導航算法

本文將多模型的思想引入GNSS/SINS 融合算法中, 提出了基于IMM-EKF 的GNSS/SINS 組合導航多模型結構, 中心思想是將IMM 算法中的模型集設置為對應不同量測噪聲協方差的EKF 濾波器,構造出3 個測量噪聲協方差的濾波模型, 這樣可以避免傳統Kalman 濾波算法中需要事先知道量測噪聲統計特性的不足, 可以更好地匹配實際環境中的非線性輸入輸出關系。 本文提出的基于IMMEKF 的GNSS/SINS 組合導航算法原理框圖如圖2所示。

圖2 基于IMM-EKF 的組合導航系統原理框圖Fig.2 Block diagram of integrated navigation system based on IMM-EKF

4 仿真校驗及結果分析

為了驗證提出的IMM-EKF 算法的準確性和自適應性, 分別進行了仿真校驗和實際道路測試。

4.1 IMM-EKF 算法仿真校驗

為了驗證本文算法的量測噪聲自適應跟蹤性能, 以GNSS/SINS 組合導航系統為例, 采用傳統的EKF 和本文算法進行Matlab 仿真對比, 利用軌跡仿真器生成載體運動軌跡對應的導航數據, 包括加速、 減速、 勻速直行、 轉彎等運動狀態, 時長為900s。 MEMS 參數如表1 所示。

表1 MEMS 誤差參數Table 1 Error parameters of MEMS

為了更好地驗證模型發生變化時IMM-EKF 算法的準確性和魯棒性, 本次實驗選擇在同一組數據中改變GNSS 接收機的量測噪聲協方差來模擬該情況。 將傳統EKF 的量測噪聲協方差設置如下

IMM-EKF 算法中設置了3 個模型, 3 個子濾波器對應的量測噪聲協方差分別為R、 6R和12R。初始模型概率矩陣μk=［1/3 1/3 1/3］, 初始模型轉移概率矩陣為

按照實際動態環境中GNSS 輸出誤差分布特點, 將900s 的數據分成三段: 0s ～300s、 300s ～600s 和600s ～900s。 將GNSS 的輸出誤差模型在第一段時長中設置為量測噪聲協方差陣為3R的零均值Gauss 白噪聲, 同樣第二段設置為6R, 第三段設置為9R。 三段中設置的量測噪聲協方差陣均大于傳統EKF 中設置的R, 但是仍處于IMM-EKF 量測噪聲協方差陣的覆蓋范圍之內, 以保證所設模型能夠通過加權融合準確匹配當前時刻的噪聲情況, 并且盡可能多地模擬不同噪聲下的估計結果。按照設置好的仿真條件, 分別使用EKF 和IMMEKF 對GNSS/SINS 進行濾波融合, 并將輸出的結果與實際真實軌跡做差得到位置誤差與速度誤差曲線, 如圖3 ～圖8 所示。

圖3 東向速度誤差對比Fig.3 Comparison of east-facing speed errors

圖4 北向速度誤差對比Fig.4 Comparison of north-facing speed errors

圖5 天向速度誤差對比Fig.5 Comparison of celestial speed errors

圖6 緯度誤差對比Fig.6 Comparison of latitude errors

圖7 經度誤差對比Fig.7 Comparison of longitude errors

圖8 高度誤差對比Fig.8 Comparison of height errors

從實驗仿真結果可知, 由于EKF 設定的量測噪聲協方差固定為R, 先驗噪聲協方差與實際濾波運算過程中的協方差不一致, 導致相比于IMMEKF, EKF 的定位精度略低, 而IMM-EKF 由于能夠自適應調整量測噪聲協方差矩陣, 因此能夠在一定程度上抑制量測噪聲變化對于濾波的影響,增強系統的魯棒性。

由表2 可知, 當系統模型發生變化時, 若噪聲協方差變化在IMM-EKF 算法模型集的覆蓋范圍內,則IMM-EKF 算法定位精度較EKF 更高, 誤差變化要更平緩。 由此可推廣至模型其他參數發生變化的情況, 只需設計相應的模型集, 即可在一定程度上提高系統的魯棒性和定位精度。

表2 均方誤差結果Table 2 Result of mean square error

4.2 實地道路測試

為了進一步驗證本文算法的有效性和優越性,設計了半物理車載實驗。 采集真實車載環境下的傳感器數據（真實車載環境包括: 載體加速、 減速、變道、 停車等真實動態環境）, 并通過高精度基準導航系統作為參考進行對比。 其中, SINS 數據由MEMS 慣性導航系統提供, GNSS 數據由自研制北斗導航接收機提供, 高精度基準導航系統為Novatel 公司推出的SPAN-KVH1750 高精度組合導航系統。 上述設備的主要性能指標如表3 所示, 跑車實驗設備如圖9 所示, 跑車實驗路線如圖10 所示。

圖9 道路測試實驗設備Fig.9 Diagram of road test equipments

圖10 跑車實驗路線Fig.10 Roadmap of vehicle experiment

表3 設備主要性能參數Table 3 Main performance parameters of the devices

跑車時長300s, 數據通過上位機監控存入計算機, 然后分別進行IMM-EKF 和EKF 算法數據融合離線仿真, 通過與高精度基準對比得出誤差。圖11、 圖12 給出了在實際跑車測試中GNSS/SINS組合在使用EKF 和IMM-EKF 時的速度誤差對比曲線和位置誤差對比曲線, 圖13 給出了兩種算法估計出的二維位置坐標與基準所給的二維位置坐標對比, 表4 給出了兩種算法的均方誤差對比。

表4 兩種算法均方誤差對比Table 4 Comparison of mean square error between two algorithms

圖11 速度誤差對比Fig.11 Comparison of velocity errors

圖12 位置誤差對比Fig.12 Comparison of position errors

圖13 兩種算法二維位置與實際二維位置對比Fig.13 Comparison of the two algorithms two-dimensional positions and the actual two-dimensional positions

由此可以得出, 傳統的EKF 由于先驗信息的不準確, 使得輸出的參數誤差波動較大, 而IMMEKF 算法可以實現不同量測噪聲協方差之間的相互交互, 自適應地調整模型集, 能夠在一定程度上降低量測粗差對濾波精度的影響, 改善了復雜環境下因量測噪聲統計特性未知多變引起的濾波 精度下降的問題, 提高了組合導航定位精度。

5 結論

本文針對復雜環境下組合導航系統模型非線性和量測噪聲統計特性未知多變而引起的定位精度下降的問題, 提出了一種基于EKF 的交互式多模型算法。 該算法突破了傳統算法定結構的限制,通過模型集自適應調整策略, 使得模型能夠快速匹配當前的量測噪聲統計特性。 以GNSS/SINS 組合導航系統為例, 通過仿真實驗和實地道路測試對本文算法進行了驗證, 結果表明: 本文算法能夠在一定程度上降低量測噪聲統計特性未知多變對組合導航濾波精度的影響, 增強了系統的魯棒性, 提高了系統的定位精度。