基于EKF 的多模型自適應(yīng)組合導(dǎo)航算法

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

（南京理工大學(xué), 南京 210094）

0 引言

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

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

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

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

本文引入交互式多模型的思想, 結(jié)合EKF 根據(jù)實(shí)際環(huán)境設(shè)計多個不同的模型, 每個模型對應(yīng)一個EKF 子濾波器, 各個子濾波器同時進(jìn)行濾波,并通過似然函數(shù)實(shí)現(xiàn)模型之間的交互, 然后對各個子濾波器的濾波結(jié)果進(jìn)行概率加權(quán)融合, 以得到最優(yōu)的狀態(tài)估計值。 將交互式多模型與擴(kuò)展Kalman 濾波結(jié)合在一起, 充分發(fā)揮非線性濾波適用于非線性系統(tǒng)的特點(diǎn), 更好地匹配實(shí)際環(huán)境中的非線性輸入輸出關(guān)系, 提高系統(tǒng)的定位精度, 增強(qiáng)系統(tǒng)的魯棒性。

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

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

1.1 SINS 非線性誤差模型

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

假設(shè)Q=［q0q1q2q3］T為SINS 真實(shí)的姿態(tài)四元數(shù),為實(shí)際解算的四元數(shù), 則Q和Q～分別滿足如下的微分方程

式（1）中, ?表示四元數(shù)乘積,b表示載體坐標(biāo)系,n表示導(dǎo)航坐標(biāo)系,i表示慣性坐標(biāo)系。式（2）中,

根據(jù)式（1）和式（2）, 可得如下的SINS 姿態(tài)誤差方程

同樣地, 根據(jù)SINS 真實(shí)速度、 實(shí)際測量速度以及慣導(dǎo)的比力方程可以得到SINS 的速度誤差方程為

式（4）中,Cn b為姿態(tài)矩陣, 其與四元數(shù)Q的關(guān)系為

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

參數(shù)的具體含義可參考文獻(xiàn)［11］, 在此不做過多介紹。 由于姿態(tài)轉(zhuǎn)移矩陣與四元數(shù)之間存在的非線性關(guān)系, 使得SINS 誤差方程在姿態(tài)誤差角較大時含有較強(qiáng)的非線性。

1.2 組合導(dǎo)航系統(tǒng)模型

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

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

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

采用松組合時, 系統(tǒng)量測方程為線性模型。將GNSS 接收機(jī)輸出的速度和位置信息與捷聯(lián)慣性導(dǎo)航系統(tǒng)相應(yīng)的輸出信息做差, 得到量測方程如下

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

2 擴(kuò)展Kalman 濾波算法

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

將GNSS/SINS 組合導(dǎo)航系統(tǒng)的狀態(tài)方程和量測方程離散化, 得到如下方程組

其中,

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

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

經(jīng)過Taylor 展開并一階截斷后, 就將系統(tǒng)變?yōu)榱司€性系統(tǒng), 可以使用基本Kalman 濾波方程完成迭代優(yōu)化的過程。 至此, EKF 方程如下:

時間預(yù)測方程為

量測更新方程為

3 基于IMM-EKF 的GNSS/SINS 組合導(dǎo)航算法

3.1 交互式多模型算法

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

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

（1）輸入交互

對給定的n個模型的初始狀態(tài)進(jìn)行輸入交互運(yùn)算, 定位目標(biāo)的初始狀態(tài)估計值為

狀態(tài)估計協(xié)方差如下

其中,

（2）狀態(tài)濾波

將上一步得到的初始狀態(tài)和協(xié)方差作為濾波器的輸入, 對每一個模型進(jìn)行EKF 濾波, 多個濾波器并行工作, 得到每個模型當(dāng)前時刻的狀態(tài)估計及其均方誤差陣, 時間更新過程和量測更新過程同標(biāo)準(zhǔn)EKF 算法。

（3）模型概率更新

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

新息協(xié)方差為

計算各模型的似然函數(shù)值為

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

采用Bayes 假設(shè)檢驗(yàn)方法進(jìn)行模型概率更新

（4）融合輸出

融合輸出即按照各濾波器估計值的概率加權(quán)融合, 計算各模型的聯(lián)合狀態(tài)估計和狀態(tài)估計協(xié)方差矩陣, 得到IMM 的最終輸出。 最終得到的狀態(tài)估計值表達(dá)式為

狀態(tài)估計值對應(yīng)的協(xié)方差為

3.2 基于IMM-EKF 的組合導(dǎo)航算法

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

圖2 基于IMM-EKF 的組合導(dǎo)航系統(tǒng)原理框圖Fig.2 Block diagram of integrated navigation system based on IMM-EKF

4 仿真校驗(yàn)及結(jié)果分析

為了驗(yàn)證提出的IMM-EKF 算法的準(zhǔn)確性和自適應(yīng)性, 分別進(jìn)行了仿真校驗(yàn)和實(shí)際道路測試。

4.1 IMM-EKF 算法仿真校驗(yàn)

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

表1 MEMS 誤差參數(shù)Table 1 Error parameters of MEMS

為了更好地驗(yàn)證模型發(fā)生變化時IMM-EKF 算法的準(zhǔn)確性和魯棒性, 本次實(shí)驗(yàn)選擇在同一組數(shù)據(jù)中改變GNSS 接收機(jī)的量測噪聲協(xié)方差來模擬該情況。 將傳統(tǒng)EKF 的量測噪聲協(xié)方差設(shè)置如下

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

按照實(shí)際動態(tài)環(huán)境中GNSS 輸出誤差分布特點(diǎn), 將900s 的數(shù)據(jù)分成三段: 0s ～300s、 300s ～600s 和600s ～900s。 將GNSS 的輸出誤差模型在第一段時長中設(shè)置為量測噪聲協(xié)方差陣為3R的零均值Gauss 白噪聲, 同樣第二段設(shè)置為6R, 第三段設(shè)置為9R。 三段中設(shè)置的量測噪聲協(xié)方差陣均大于傳統(tǒng)EKF 中設(shè)置的R, 但是仍處于IMM-EKF 量測噪聲協(xié)方差陣的覆蓋范圍之內(nèi), 以保證所設(shè)模型能夠通過加權(quán)融合準(zhǔn)確匹配當(dāng)前時刻的噪聲情況, 并且盡可能多地模擬不同噪聲下的估計結(jié)果。按照設(shè)置好的仿真條件, 分別使用EKF 和IMMEKF 對GNSS/SINS 進(jìn)行濾波融合, 并將輸出的結(jié)果與實(shí)際真實(shí)軌跡做差得到位置誤差與速度誤差曲線, 如圖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 經(jīng)度誤差對比Fig.7 Comparison of longitude errors

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

從實(shí)驗(yàn)仿真結(jié)果可知, 由于EKF 設(shè)定的量測噪聲協(xié)方差固定為R, 先驗(yàn)噪聲協(xié)方差與實(shí)際濾波運(yùn)算過程中的協(xié)方差不一致, 導(dǎo)致相比于IMMEKF, EKF 的定位精度略低, 而IMM-EKF 由于能夠自適應(yīng)調(diào)整量測噪聲協(xié)方差矩陣, 因此能夠在一定程度上抑制量測噪聲變化對于濾波的影響,增強(qiáng)系統(tǒng)的魯棒性。

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

表2 均方誤差結(jié)果Table 2 Result of mean square error

4.2 實(shí)地道路測試

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

圖9 道路測試實(shí)驗(yàn)設(shè)備Fig.9 Diagram of road test equipments

圖10 跑車實(shí)驗(yàn)路線Fig.10 Roadmap of vehicle experiment

表3 設(shè)備主要性能參數(shù)Table 3 Main performance parameters of the devices

跑車時長300s, 數(shù)據(jù)通過上位機(jī)監(jiān)控存入計算機(jī), 然后分別進(jìn)行IMM-EKF 和EKF 算法數(shù)據(jù)融合離線仿真, 通過與高精度基準(zhǔn)對比得出誤差。圖11、 圖12 給出了在實(shí)際跑車測試中GNSS/SINS組合在使用EKF 和IMM-EKF 時的速度誤差對比曲線和位置誤差對比曲線, 圖13 給出了兩種算法估計出的二維位置坐標(biāo)與基準(zhǔn)所給的二維位置坐標(biāo)對比, 表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 兩種算法二維位置與實(shí)際二維位置對比Fig.13 Comparison of the two algorithms two-dimensional positions and the actual two-dimensional positions

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

5 結(jié)論

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