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

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

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

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

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

1 KF算法

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

(1)

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

(2)

去掉常數(shù)

(3)

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

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)與上一時(shí)刻的狀態(tài)和系統(tǒng)模型有關(guān)，即

xt=f(xt-1)+εt

(7)

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

zt=g(xt)+ωt

(8)

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

xt=Fxt-1+εt

(9)

同理，觀測(cè)模型為

zt=Gxt+ωt

(10)

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

εt～N(0,Qt)

(11)

ωt～N(0,Rt)

(12)

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

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

(13)

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

(14)

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

(15)

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

(16)

式中，Kt為卡爾曼增益

(17)

2 深度卡爾曼濾波(DKF)

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

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

設(shè)當(dāng)前隱變量與過(guò)去隱變量和系統(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)化為估計(jì)Wh和Wxx。如圖1所示，深度卡爾曼濾波(Deep Kalman Filter，DKF)的上層網(wǎng)絡(luò)與傳統(tǒng)KF基本一致，建模步驟位于下層網(wǎng)絡(luò)。根據(jù)IMU隨時(shí)間積累的特性，在時(shí)間維度上使用深度學(xué)習(xí)算法進(jìn)行建模。

圖1 DKF的概率圖模型

2.1 模型參數(shù)估計(jì)

(22)

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

(23)

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

(24)

(25)

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

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

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

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

(26)

(27)

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

(28)

則Wxx的梯度為

(29)

Wh的梯度為

(30)

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

(31)

(32)

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

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

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

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

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

(33)

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

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

(34)

(35)

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

基于DKF的導(dǎo)航系統(tǒng)分為訓(xùn)練部分和應(yīng)用部分。已經(jīng)知道的是：如果GNSS測(cè)量值能夠得到，將其作為觀測(cè)值輸入到KF中，應(yīng)用系統(tǒng)模型和觀測(cè)模型來(lái)預(yù)測(cè)和更新系統(tǒng)狀態(tài)，同時(shí)使用DKF對(duì)IMU模型進(jìn)行訓(xùn)練，如圖3(a)所示；如果GNSS失鎖或其他無(wú)法獲得GNSS數(shù)據(jù)的情況發(fā)生時(shí)，KF只能完成預(yù)測(cè)過(guò)程，此時(shí)應(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ù)為例對(duì)算法進(jìn)行有效性驗(yàn)證，仿真數(shù)據(jù)長(zhǎng)400s，采樣頻率1Hz，包含有兩種數(shù)據(jù)：第一部分是設(shè)定的真實(shí)導(dǎo)航數(shù)據(jù)；第二部分是仿真輸出的IMU數(shù)據(jù)。每一秒都包含有變加速運(yùn)動(dòng)導(dǎo)航載體的實(shí)時(shí)真實(shí)速度和IMU速度信息。

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

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

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

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

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

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

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

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

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

圖6 四種方法的估計(jì)誤差對(duì)比圖

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

4 結(jié)論

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

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

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