基于DKF的IMU誤差預(yù)測算法

秦閃閃，陳夏蘭，徐 穎，馬滿帥，王 瑩，梁任騰，楊子佳

(1.中國科學(xué)院空天信息創(chuàng)新研究院導(dǎo)航系統(tǒng)部，北京 100094；2.中國科學(xué)院大學(xué)電子電氣與通訊工程學(xué)院，北京 100049；3.北京信息科技大學(xué)信息與通信工程學(xué)院，北京 100192)

0 引言

全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)/慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)組合導(dǎo)航的核心是如何有效地將GNSS觀測量與慣性測量單元(Inertial Measurement Unit，IMU)測量結(jié)果結(jié)合得到載體導(dǎo)航結(jié)果的最優(yōu)估計，在目前的組合導(dǎo)航技術(shù)中，包括松耦合、緊耦合和超緊耦合[1]。其中松耦合在軟件層面組合，將GNSS接收機(jī)導(dǎo)航定位結(jié)果和INS測量結(jié)果進(jìn)行濾波融合[2-3]，當(dāng)前常用的融合算法是卡爾曼濾波(Kalman Filter，KF)[4-5]。KF利用GNSS觀測量和IMU測量結(jié)果解算得到IMU誤差的估計值，再將估計值反饋給IMU，IMU測量結(jié)果經(jīng)過估計值校正后作為導(dǎo)航解輸出。GNSS/INS組合導(dǎo)航系統(tǒng)長時間運(yùn)行的精度更多地依賴于GNSS，然而實際中存在各種隨機(jī)的或復(fù)雜的誤差源，如可見衛(wèi)星數(shù)目、多徑效應(yīng)和儀器內(nèi)部的量測噪聲等，都會使得衛(wèi)星導(dǎo)航系統(tǒng)的量測噪聲隨時發(fā)生變化，標(biāo)準(zhǔn)KF難以對上述變化進(jìn)行檢測和調(diào)整，導(dǎo)致其使用的噪聲統(tǒng)計特征與事實發(fā)生偏差，無法得到最優(yōu)濾波結(jié)果，甚至?xí)霈F(xiàn)濾波發(fā)散現(xiàn)象[6]。為了克服標(biāo)準(zhǔn)KF在組合導(dǎo)航中應(yīng)用的缺點，研究者們開始針對其設(shè)計與應(yīng)用場景相適應(yīng)的濾波算法。

針對全球定位系統(tǒng)(Global Positioning System，GPS)測量值發(fā)生較大變化時如何提高KF的濾波精度和魯棒性的問題，卞鴻巍利用對實際新息的測量計算直接修正卡爾曼濾波器增益，但需要采用GPS測量系統(tǒng)為INS提供外部修正信息[7]。付夢印研究了應(yīng)用于慣性技術(shù)領(lǐng)域的相關(guān)法自適應(yīng)濾波和Sage-Husa 自適應(yīng)KF，然而帶有衰減因子或記憶因子的卡爾曼濾波器系統(tǒng)狀態(tài)維度增加，計算量加大[8]。高為廣和楊元喜使用反向傳播(Back Propagation, BP)神經(jīng)網(wǎng)絡(luò)在線修正自適應(yīng)濾波器的預(yù)測值，該算法提高了濾波精度和可靠性，但算法中提到的“經(jīng)過復(fù)雜函數(shù)映射到當(dāng)前函數(shù)的網(wǎng)絡(luò)預(yù)測值”難以獲取[9]。M.Narasimhappa等優(yōu)化了Sage-Husa自適應(yīng)魯棒KF算法，更新了自適應(yīng)比例因子的計算方法，從而有效降低了估計誤差的平均值和標(biāo)準(zhǔn)差，但增大了系統(tǒng)計算的復(fù)雜度[10]。S.Hosseinyalamdary提出了一種使用遞歸神經(jīng)網(wǎng)絡(luò)訓(xùn)練組合導(dǎo)航觀測模型的方法，從而克服了因INS誤差模型的復(fù)雜性導(dǎo)致的計算誤差，但大大增加了算法的計算量[11]。

很多高動態(tài)導(dǎo)航載體，如快速運(yùn)動的無人機(jī)，也使用GNSS/INS組合系統(tǒng)來導(dǎo)航。由于GNSS在高動態(tài)下跟蹤環(huán)路可能失鎖，在信號被掩蓋及多徑效應(yīng)等場景下測量值不準(zhǔn)確甚至沒有，導(dǎo)致此時的載體只能依賴IMU進(jìn)行導(dǎo)航，導(dǎo)航結(jié)果會在時間積累下迅速失去有效性。本文擬解決的問題就是在GNSS接收機(jī)輸出中斷或測量結(jié)果不可信時，如何在IMU獨(dú)立導(dǎo)航條件下盡可能保持較高的導(dǎo)航精度。關(guān)于這個問題，也有學(xué)者進(jìn)行研究，如德國卡爾斯魯厄大學(xué)的研究者通過在無人機(jī)上添加磁強(qiáng)計和高程計以解決組合導(dǎo)航的這個問題[12]；荷蘭特溫特大學(xué)的研究者則將深度學(xué)習(xí)的思想引入KF，利用循環(huán)神經(jīng)網(wǎng)絡(luò)和長短期神經(jīng)網(wǎng)絡(luò)開展了初步的研究[11]。

1 KF算法

KF估計的未知量為狀態(tài)向量，表示為x∈Rn，其是一個時間變量，即xt表示時刻t的狀態(tài)向量。其與過去直到現(xiàn)在的觀測矢量z1∶t相關(guān)，z∈Rm。設(shè)初始狀態(tài)為x0，則xt的概率為Pr(xt|z1∶t,x0)。根據(jù)極大似然估計(Maximum Likelihood Estimation，MLE)

(1)

系統(tǒng)當(dāng)前的狀態(tài)與過去的狀態(tài)有關(guān)，即

(2)

去掉常數(shù)

(3)

應(yīng)用前一個狀態(tài)向量的邊緣化得到基于前一個狀態(tài)向量的狀態(tài)向量估計，即

Pr(x1∶t-1|z1∶t-1,x0)dx1∶t-1

(4)

基于馬爾可夫假設(shè)，即

Pr(xt-1|z1∶t-1,x0)dxt-1

(5)

(6)

在KF中，系統(tǒng)狀態(tài)與上一時刻的狀態(tài)和系統(tǒng)模型有關(guān)，即

xt=f(xt-1)+εt

(7)

式中，εt為系統(tǒng)噪聲模型。同時系統(tǒng)狀態(tài)通過觀測方程g映射到觀測量zt∈Rm，即

zt=g(xt)+ωt

(8)

式中，ωt為觀測噪聲模型。在KF中，狀態(tài)模型和觀測模型都是線性的，分別用F和G矩陣表示。系統(tǒng)模型為

xt=Fxt-1+εt

(9)

同理，觀測模型為

zt=Gxt+ωt

(10)

假設(shè)KF中系統(tǒng)噪聲和觀測噪聲都服從正態(tài)分布

εt～N(0,Qt)

(11)

ωt～N(0,Rt)

(12)

式(11)和式(12)中，Qt和Rt分別為系統(tǒng)噪聲和觀測噪聲的協(xié)方差矩陣。

KF過程分為預(yù)測過程和更新過程。系統(tǒng)當(dāng)前狀態(tài)由前一時刻狀態(tài)預(yù)測，即

(13)

狀態(tài)向量的上標(biāo)“-”表示預(yù)測量，上標(biāo)“+”表示更新量。采用誤差傳播法，根據(jù)前一時刻狀態(tài)向量的協(xié)方差矩陣估計當(dāng)前狀態(tài)向量的協(xié)方差矩陣

(14)

在更新過程中，當(dāng)前狀態(tài)的更新值由當(dāng)前觀測值和預(yù)測值得出

(15)

則狀態(tài)變量更新值的協(xié)方差矩陣為

(16)

式中，Kt為卡爾曼增益

(17)

2 深度卡爾曼濾波(DKF)

為了解決KF更新過程中觀測量獲取不到的問題，在KF中添加建模步驟，即在KF中加入隱變量。隱變量在狀態(tài)向量中不可見，但能決定狀態(tài)向量，用ht表示，是時間相關(guān)變量。當(dāng)前系統(tǒng)狀態(tài)取決于過去T個時刻隱變量的值ht-1∶t-T。

假設(shè)當(dāng)前系統(tǒng)狀態(tài)只與當(dāng)前隱變量有關(guān)，即不直接依賴于過去系統(tǒng)狀態(tài)xt-1∶t-T，馬爾可夫假設(shè)不成立。新的假設(shè)不僅符合很多系統(tǒng)，而且簡化了計算。

設(shè)當(dāng)前隱變量與過去隱變量和系統(tǒng)狀態(tài)之間的映射關(guān)系為φ，即

(18)

當(dāng)前系統(tǒng)狀態(tài)直接與當(dāng)前隱變量相關(guān)

(19)

(20)

(21)

式中，Wh=[Wxh,Whh]。建模任務(wù)轉(zhuǎn)化為估計Wh和Wxx。如圖1所示，深度卡爾曼濾波(Deep Kalman Filter，DKF)的上層網(wǎng)絡(luò)與傳統(tǒng)KF基本一致，建模步驟位于下層網(wǎng)絡(luò)。根據(jù)IMU隨時間積累的特性，在時間維度上使用深度學(xué)習(xí)算法進(jìn)行建模。

圖1 DKF的概率圖模型

2.1 模型參數(shù)估計

(22)

基于式(22)的結(jié)果估計隱變量

(23)

根據(jù)式(21)，計算系統(tǒng)狀態(tài)可以轉(zhuǎn)化為計算Wxx，同理，計算隱變量轉(zhuǎn)化為計算Wh。將式(20)和式(21)代入式(22)，則

(24)

(25)

隱變量和狀態(tài)向量的迭代估計一直持續(xù)到算法收斂到其解，最后確定系統(tǒng)模型并得出狀態(tài)向量的估計。P.Mirowski和Y.Lecun的研究表明,EM算法可以使用遞歸神經(jīng)網(wǎng)絡(luò)訓(xùn)練[13]。

EM算法最終收斂到凸函數(shù)的全局最大值，然而對于一些建模為非凸函數(shù)的系統(tǒng)，EM很有可能收斂到局部極大值，所以如何找到式(24)和式(25)中的最大值是一個挑戰(zhàn)。

DKF的建模過程分為兩步：通過過去的系統(tǒng)狀態(tài)和隱變量估計當(dāng)前隱變量；通過當(dāng)前隱變量估計當(dāng)前系統(tǒng)狀態(tài)。

用隱變量估計重新構(gòu)造式(6)，則

(26)

(27)

將式(21)代入式(26)，得到

(28)

則Wxx的梯度為

(29)

Wh的梯度為

(30)

利用梯度下降方法最小化能量函數(shù)。建模系統(tǒng)的參數(shù)矩陣Wh和Wxx為

(31)

(32)

順應(yīng)時勢的轉(zhuǎn)型要求電視新聞記者在努力學(xué)習(xí)最新科學(xué)技術(shù)、傳媒技術(shù)的同時，更新理念，深化對不同業(yè)務(wù)的理解，成為一個“新聞全才”。更重要的是，媒體融合時代對主流媒體新聞記者職業(yè)操守的要求更加嚴(yán)格，電視新聞記者務(wù)須確保采編的所有新聞內(nèi)容真實、客觀，并且能夠引導(dǎo)正確社會價值觀。在遵循媒體融合時期新聞傳播規(guī)律的基礎(chǔ)上，電視媒體可嘗試打造具有“標(biāo)簽”效應(yīng)的品牌記者，如知名調(diào)查報道記者、知名環(huán)保欄目記者、知名時政新聞記者等。

2.2 長短期記憶網(wǎng)絡(luò)(LSTM)

遞歸神經(jīng)網(wǎng)絡(luò)的缺陷是梯度爆炸和梯度消失[14]。當(dāng)T比較大且建模時間較長時，梯度會在幾層網(wǎng)絡(luò)中做乘法運(yùn)算，則較大的梯度乘積更大，最終導(dǎo)致梯度爆炸。反之，如果梯度較小，乘積則趨近于零，從而導(dǎo)致梯度消失。為了抑制遞歸神經(jīng)網(wǎng)絡(luò)中的這種效應(yīng)，長短期記憶網(wǎng)絡(luò)(Long Short-Term Memory，LSTM)中采用了門記憶[15]，計算單元具體結(jié)構(gòu)如圖 2所示。

圖2 LSTM(Peephple connection)結(jié)構(gòu)圖

LSTM在遞歸神經(jīng)網(wǎng)絡(luò)中加入了判斷信息是否有用的處理器——cell，一個cell當(dāng)中放置了三扇門，分別叫作輸入門、遺忘門和輸出門。用it表示輸入門，ot表示輸出門，ft表示遺忘門，這三種門都可以用線性函數(shù)和非線性函數(shù)的組合來表示

(33)

式中，σ表示非線性函數(shù)，線性函數(shù)由參數(shù)矩陣表示。cell的狀態(tài)ct和隱藏層ht按如下計算

ct=ft°ct-1+it°tanh(Wxxt+Whht-1)

(34)

(35)

式中，°表示按位相乘。對于具有長期相關(guān)性的輸入數(shù)據(jù)，輸入門可以保持以前狀態(tài)向量的信息，并且可以訪問以前狀態(tài)向量的梯度。因此，梯度在反向傳播過程中不會爆炸或消失。遺忘門控制了模型的復(fù)雜性，并刪除了不相關(guān)的過去狀態(tài)向量。

基于DKF的導(dǎo)航系統(tǒng)分為訓(xùn)練部分和應(yīng)用部分。已經(jīng)知道的是：如果GNSS測量值能夠得到，將其作為觀測值輸入到KF中，應(yīng)用系統(tǒng)模型和觀測模型來預(yù)測和更新系統(tǒng)狀態(tài)，同時使用DKF對IMU模型進(jìn)行訓(xùn)練，如圖3(a)所示；如果GNSS失鎖或其他無法獲得GNSS數(shù)據(jù)的情況發(fā)生時，KF只能完成預(yù)測過程，此時應(yīng)用訓(xùn)練好的模型進(jìn)行導(dǎo)航解算，如圖3(b)所示。

(a)DKF訓(xùn)練模型

(a)DKF訓(xùn)練網(wǎng)絡(luò)結(jié)構(gòu)圖

3 基于DKF的仿真分析

以仿真數(shù)據(jù)為例對算法進(jìn)行有效性驗證，仿真數(shù)據(jù)長400s，采樣頻率1Hz，包含有兩種數(shù)據(jù)：第一部分是設(shè)定的真實導(dǎo)航數(shù)據(jù)；第二部分是仿真輸出的IMU數(shù)據(jù)。每一秒都包含有變加速運(yùn)動導(dǎo)航載體的實時真實速度和IMU速度信息。

在GNSS失鎖時，作為自主式導(dǎo)航系統(tǒng)，導(dǎo)航載體上的INS仍然正常運(yùn)行，其初始導(dǎo)航狀態(tài)是GNSS失鎖前最后時刻輸出的導(dǎo)航狀態(tài)。實驗所使用的數(shù)據(jù)是長400s的含有標(biāo)識的IMU數(shù)據(jù)以及在真實導(dǎo)航數(shù)據(jù)上仿真的GNSS數(shù)據(jù)。在GNSS失鎖時，INS單獨(dú)運(yùn)行，其誤差迅速增大，失鎖200s后，東北天坐標(biāo)系下3個方向的速度誤差已經(jīng)分別達(dá)到1.1537m/s、1.2146m/s和0.3332m/s。

為了評估算法的結(jié)果，將400s的數(shù)據(jù)劃分為兩段：前200s數(shù)據(jù)作為訓(xùn)練集，用來測試和評估；后200s數(shù)據(jù)作為測試集，用來驗證模型的有效性。前200s訓(xùn)練部分，在真實導(dǎo)航數(shù)據(jù)上添加噪聲作為GNSS數(shù)據(jù)，將其與IMU數(shù)據(jù)組合通過KF得到IMU誤差，該IMU誤差作為DKF的訓(xùn)練數(shù)據(jù)使用。后200s使用訓(xùn)練得到的IMU誤差模型預(yù)測IMU誤差，用其校正IMU數(shù)據(jù)，并和設(shè)定的真實導(dǎo)航數(shù)據(jù)進(jìn)行比較，以評估DKF的有效性。

DKF網(wǎng)絡(luò)設(shè)置輸入序列長度為5，中間節(jié)點數(shù)為50，迭代55次，DKF模型預(yù)測的IMU誤差結(jié)果如圖5(a)和(b)所示。

如圖5(a)所示，在GNSS失鎖后，使用DKF算法訓(xùn)練的模型估計出IMU的誤差與實際IMU誤差接近，將其作為校正量代入IMU數(shù)據(jù)中，能夠保持一定時間的高精度導(dǎo)航。如圖5(b)所示，如果在GNSS失鎖后使用DKF進(jìn)行IMU誤差校正，200s后速度誤差只增長到0.3746m/s，且200s內(nèi)的平均誤差只有0.1350m/s。與GNSS失鎖200s后的系統(tǒng)誤差1.1537m/s相比，DKF對短期內(nèi)導(dǎo)航解的改善是顯而易見的。

DKF通過前5個時刻預(yù)測下一個時刻的IMU誤差，平均估計使用前5個時刻的平均值作為下一個時刻的IMU誤差，兩者的對比實驗結(jié)果如圖5(c)和(d)所示。如果在GNSS失鎖后使用平均估計進(jìn)行IMU誤差校正，由于平均估計預(yù)測的IMU誤差維持在GNSS失鎖時刻的數(shù)據(jù)，在200s內(nèi)誤差達(dá)到1.1203m/s，200s內(nèi)平均誤差為0.5890m/s。平均誤差對于GNSS/INS組合系統(tǒng)的持續(xù)導(dǎo)航改善不如DKF。

GNSS失鎖后，GNSS/INS組合系統(tǒng)由于丟失觀測量而無法使用KF，但是基于持續(xù)輸出的IMU數(shù)據(jù)，可以使用卡爾曼的預(yù)測部分對IMU誤差進(jìn)行估計，卡爾曼預(yù)測和DKF的對比實驗結(jié)果如圖5(e)和(f)所示。如果在GNSS失鎖后使用卡爾曼預(yù)測進(jìn)行IMU誤差校正，其估計的IMU誤差在200s內(nèi)達(dá)到0.7479m/s，200s內(nèi)平均估計誤差為0.4421m/s。卡爾曼預(yù)測對于GNSS/INS組合系統(tǒng)的持續(xù)導(dǎo)航改善不如DKF。

最小二乘估計作為常用的估計方法，在曲線擬合方面效果顯著，可以使用最小二乘估計擬合GNSS未失鎖前的IMU誤差模型，從而預(yù)測GNSS失鎖后的IMU誤差。最小二乘估計和DKF的對比實驗結(jié)果如圖5(g)和(h)所示。如果在GNSS失鎖后使用最小二乘估計進(jìn)行IMU誤差校正，其估計的IMU誤差在200s內(nèi)的估計誤差達(dá)到0.9725m/s，200s內(nèi)平均估計誤差為0.3352m/s。最小二乘估計對于GNSS/INS組合系統(tǒng)的持續(xù)導(dǎo)航改善不如DKF。

(a)DKF預(yù)測結(jié)果圖

為了進(jìn)一步說明DKF算法的有效性和性能，將DKF的估計誤差與平均估計、卡爾曼預(yù)測和最小二乘估計進(jìn)行對比，結(jié)果如圖6所示。

圖6 四種方法的估計誤差對比圖

如圖6所示，在GNSS失鎖200s內(nèi)，對于GNSS/INS組合系統(tǒng)的持續(xù)導(dǎo)航改善最好的是DKF，其次是最小二乘估計。

4 結(jié)論

針對GNSS/INS組合系統(tǒng)中GNSS失鎖后導(dǎo)航精度的維持問題，本文提出了一種基于LSTM的DKF算法。首先使用GNSS數(shù)據(jù)和IMU數(shù)據(jù)通過KF獲取失鎖前的高精度導(dǎo)航數(shù)據(jù)；其次，基于高精度導(dǎo)航數(shù)據(jù)訓(xùn)練IMU誤差模型；最后，在GNSS失鎖后使用IMU誤差模型進(jìn)行誤差校正。算法分析與實驗結(jié)果表明：

1)DKF算法在時間維度上分為2個階段執(zhí)行，實現(xiàn)了基于歷史信息對當(dāng)前IMU誤差的校正。因此具有比傳統(tǒng)KF更高的信息利用率，可以在GNSS失鎖時維持GNSS/INS組合系統(tǒng)的高精度運(yùn)行。

2)本文提出的DKF算法依靠LSTM對序列信息的敏感度對IMU誤差進(jìn)行建模，而機(jī)器學(xué)習(xí)的本質(zhì)要求含有大量信息的大數(shù)據(jù)集進(jìn)行實驗。在實際的復(fù)雜環(huán)境中如何獲取這些數(shù)據(jù)并且保證算法的實時性，值得做進(jìn)一步研究。