輔助信息源短時(shí)失效下的捷聯(lián)慣性基組合導(dǎo)航算法

張夢(mèng)得, 李開龍, 胡柏青, 呂旭

(海軍工程大學(xué) 電氣工程學(xué)院, 湖北 武漢 430033)

0 引言

由于捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)不可避免的器件誤差積累，因此通常與輔助信息源(全球定位系統(tǒng)(GPS)，多普勒計(jì)程儀(DVL)等)實(shí)時(shí)修正，構(gòu)成捷聯(lián)慣性基組合導(dǎo)航系統(tǒng)[1-6]。捷聯(lián)慣性基組合導(dǎo)航系統(tǒng)的核心之一是組合導(dǎo)航算法，根據(jù)組合導(dǎo)航系統(tǒng)狀態(tài)模型的不同，可以分為基于狀態(tài)誤差量的線性模型和基于捷聯(lián)慣性導(dǎo)航(簡(jiǎn)稱捷聯(lián)慣導(dǎo))基本方程的非線性模型。對(duì)于線性模型，經(jīng)典的線性Kalman濾波即可解決；對(duì)于非線性模型，近年來有乘性擴(kuò)展Kalman濾波[7-10](MEKF)和四元數(shù)無味Kalman濾波[11-15](USQUE)等算法。相比于線性模型，非線性模型具有更高的估計(jì)精度，雖然計(jì)算量較大，但隨著硬件計(jì)算速度快速提高，非線性模型將具有更廣泛的應(yīng)用空間[15-18]。

但在捷聯(lián)慣性基組合導(dǎo)航實(shí)際應(yīng)用中，難以避免地會(huì)出現(xiàn)輔助信息源失效問題，雖然對(duì)于高精度慣導(dǎo)系統(tǒng)，輔助信息源短時(shí)失效并不會(huì)造成導(dǎo)航精度的急劇下降，但是對(duì)于使用低精度SINS的水下無人潛器或無人機(jī)等，即使是半分鐘的輔助信息源失效，都將造成導(dǎo)航精度急劇的降低[19-21]。因此，該問題也是捷聯(lián)慣性基組合導(dǎo)航研究的熱點(diǎn)之一。

文獻(xiàn)[22]通過神經(jīng)網(wǎng)絡(luò)算法對(duì)SINS導(dǎo)航輸出誤差進(jìn)行補(bǔ)償?shù)姆绞綄?duì)GPS失效時(shí)的SINS誤差進(jìn)行抑制；文獻(xiàn)[23]通過構(gòu)建偏最小二乘估計(jì)器與支持向量回歸估計(jì)器的聯(lián)合估計(jì)器，分別估計(jì)DVL短時(shí)失效時(shí)輸出速度的線性部分與其他部分，進(jìn)行速度補(bǔ)償取得較好效果；文獻(xiàn)[24]采用SINS/GPS與SINS/里程計(jì)(OD)兩套組合導(dǎo)航系統(tǒng)相結(jié)合的方式，若GPS出現(xiàn)短時(shí)失效，通過SINS/OD系統(tǒng)仍可以進(jìn)行組合導(dǎo)航；文獻(xiàn)[25]針對(duì)外部傳感器失效無法進(jìn)行動(dòng)基座初始對(duì)準(zhǔn)的問題，通過構(gòu)建車輛運(yùn)動(dòng)模型得到虛擬量測(cè)量，從而通過Kalman濾波進(jìn)行動(dòng)基座初始對(duì)準(zhǔn)。

針對(duì)該問題，本文提出一種高斯過程回歸(GPR)輔助捷聯(lián)慣性基組合導(dǎo)航算法。該方法利用組合導(dǎo)航閉環(huán)校正后的SINS速度作為GPR的訓(xùn)練輸入集，輔助信息源的速度作為訓(xùn)練輸出集，在未發(fā)生失效時(shí)對(duì)高斯過程回歸進(jìn)行訓(xùn)練；輔助信息源失效時(shí)利用高斯過程回歸預(yù)測(cè)輔助信息源輸出以實(shí)現(xiàn)組合濾波量測(cè)更新，從而實(shí)現(xiàn)失效時(shí)量測(cè)更新的不間斷。在低精度微機(jī)電系統(tǒng)(MEMS)/GPS組合車載實(shí)驗(yàn)中，利用實(shí)驗(yàn)數(shù)據(jù)比較本文算法與傳統(tǒng)算法在失效情況下姿態(tài)、位置和陀螺常值漂移估計(jì)效果。

1 捷聯(lián)慣導(dǎo)基本模型

SINS基本模型是非線性模型，以姿態(tài)、速度、位置、陀螺輸出、加速度計(jì)輸出為狀態(tài)量。由于本文研究的是USQUE算法，因此給出四元數(shù)姿態(tài)表達(dá)下的SINS基本模型[26]。

模型的姿態(tài)部分為

(1)

(2)

SINS其他部分模型為

(3)

(4)

(5)

(6)

(7)

(8)

陀螺的量測(cè)模型為

(9)

(10)

加速度計(jì)的量測(cè)模型為

(11)

(12)

2 GPR算法基本原理

GPR算法是高斯過程在回歸領(lǐng)域的應(yīng)用，該方法利用核函數(shù)來擬合數(shù)據(jù)，并且能夠給出預(yù)測(cè)結(jié)果的不確定度。最大的特點(diǎn)是可以提供預(yù)測(cè)結(jié)果的后驗(yàn)，且具有解析形式，非常適用于低維數(shù)且數(shù)量不大樣本的回歸問題。同大部分人工智能算法相同，GPR同樣分為訓(xùn)練與預(yù)測(cè)兩個(gè)部分[27]。

在訓(xùn)練部分中，訓(xùn)練數(shù)據(jù)集合D=〈x,y〉，其中x=[x1,x2,…,xn]表示訓(xùn)練數(shù)據(jù)的輸入，y=[y1,y2,…,yn]表示訓(xùn)練數(shù)據(jù)的輸出，n為訓(xùn)練集的數(shù)據(jù)個(gè)數(shù)。

輸入與輸出的關(guān)系為

yi=f(xi)+δ，

(13)

式中：f(xi)是函數(shù)真實(shí)值，i=1,2,…,n；噪聲δ服從正態(tài)分布，δ～N(0,σn)，σn為方差。

高斯過程是隨機(jī)變量的集合，要求集合中任意數(shù)量的隨機(jī)變量都必須滿足聯(lián)合高斯分布的隨機(jī)過程[28]。因此，均值函數(shù)與協(xié)方差函數(shù)就成了高斯過程訓(xùn)練的關(guān)鍵。

y～GP(μ(x),σ(xi,xj))，

(14)

式中：GP(·)代表高斯過程函數(shù)；μ(x)是均值函數(shù)，通常取0；σ(xi,xj)是協(xié)方差函數(shù)，xi、xj是訓(xùn)練集x中的任意變量，j=1，2，…，n.

(15)

式中：σf、w、σn為超參數(shù)，記為θ={σf,w,σn}，這3個(gè)超參數(shù)的取值是整個(gè)GPR的關(guān)鍵；In為n階單位矩陣。因此整個(gè)問題就轉(zhuǎn)化成了在(15)式條件下求取目標(biāo)函數(shù)的最小值問題。

目標(biāo)函數(shù)為

(16)

式中：p(·)為概率函數(shù)。

由于目標(biāo)函數(shù)(16)式是非凸的，不可能求出最優(yōu)解，可以采用粒子群、共軛梯度等算法進(jìn)行搜索優(yōu)化，得到目標(biāo)函數(shù)的次優(yōu)解，通過多次迭代，逐步逼近最優(yōu)解的值，并得到與之相對(duì)應(yīng)的超參數(shù)。訓(xùn)練結(jié)束后，代入新的數(shù)據(jù)x*，可以得到預(yù)測(cè)值y*：

y*～GP(μ*,σ*)，

(17)

3 GPR-USQUE算法

圖1 GPR-USQUE訓(xùn)練與估計(jì)流程圖Fig.1 Training and estimation of GPR-USQUE algorithm

通過研究發(fā)現(xiàn)GPR對(duì)于插值預(yù)測(cè)效果非常好，但是對(duì)于出現(xiàn)一定數(shù)量的不位于訓(xùn)練數(shù)據(jù)集范圍內(nèi)數(shù)據(jù)的情況預(yù)測(cè)效果不佳，這是由于GPR能夠進(jìn)行回歸預(yù)測(cè)的核心是數(shù)據(jù)的相關(guān)性，一定數(shù)量不位于訓(xùn)練數(shù)據(jù)集的數(shù)據(jù)會(huì)造成該部分?jǐn)?shù)據(jù)與GPR訓(xùn)練的黑箱相關(guān)性過低，從而導(dǎo)致預(yù)測(cè)的穩(wěn)定性急劇下降。這也是大部分人工智能算法的通病。因此，對(duì)于GPR的輸入量與訓(xùn)練集的選擇非常重要。對(duì)于輸入量的數(shù)值區(qū)間不要過大，訓(xùn)練集中數(shù)據(jù)量可以不大，但是數(shù)值區(qū)間要盡量大些。

在組合導(dǎo)航系統(tǒng)中，通常作為Kalman濾波中量測(cè)量的導(dǎo)航信息通常為速度，位置或者速度與位置。對(duì)于位置信息，可以認(rèn)為是無界的，這對(duì)于GPR算法并不適用。因此對(duì)速度信息進(jìn)行回歸預(yù)測(cè)，無論從訓(xùn)練集的規(guī)模還是數(shù)據(jù)的相關(guān)性上都非常恰當(dāng)。

以姿態(tài)部分為例，短時(shí)失效時(shí)GPR-USQUE算法見圖2，圖中δRk和δRk-1分別為第k和k-1次次濾波的羅德里格斯參數(shù)姿態(tài)。

圖2 短時(shí)失效時(shí)GPR-USQUE算法流程圖Fig.2 Flow chart of GPR-USQUE algorithm during short-term failure

4 實(shí)驗(yàn)與分析

為了驗(yàn)證GPR-USQUE算法在輔助信息源短時(shí)失效情況下的有效性，本文通過車載實(shí)驗(yàn)數(shù)據(jù)比較GPR-USQUE與USQUE算法在輔助信息源短時(shí)失效情況下的性能。車上載有型號(hào)為XW-IMU5220的MEMS，兩臺(tái)XW-AD U3601型GPS接收機(jī)以及XW-ADU7612型姿態(tài)航向高精度參考系統(tǒng)(用以提供實(shí)驗(yàn)高精度姿態(tài)參考)。在MEMS/GPS車載實(shí)驗(yàn)中，選取姿態(tài)、速度、位置與陀螺漂移作為狀態(tài)量進(jìn)行研究。由于將GPS的速度作為輔助信息源，因此，重點(diǎn)關(guān)注姿態(tài)、位置和陀螺常值漂移的估計(jì)結(jié)果。MEMS的更新頻率為100 Hz，GPS的水平速度誤差小于0.1 m/s，位置誤差小于2 m，更新率為10 Hz. MEMS/GPS組合導(dǎo)航系統(tǒng)觀測(cè)采用速度松組合方式。MEMS的IMU相關(guān)性能參數(shù)見表1.

表1 XW-IMU5220型MEMS相關(guān)性能Tab.1 Related performance of XW-IMU5220 MEMS

濾波初始協(xié)方差矩陣設(shè)置為

P0=diag([Pq0;Pv0;Pp0;Pε0])，

圖3 測(cè)試軌跡Fig.3 Test track

實(shí)驗(yàn)軌跡為駕車?yán)@操場(chǎng)一周，實(shí)驗(yàn)時(shí)間為250 s. 在實(shí)驗(yàn)過程中，170～230 s區(qū)間段內(nèi)將一臺(tái)GPS關(guān)機(jī)模擬GPS短時(shí)失效，另一臺(tái)GPS接收機(jī)正常工作為實(shí)驗(yàn)提供速度與位置基準(zhǔn)。為了確保GPR訓(xùn)練集小而有效，在100多秒左右的訓(xùn)練時(shí)間內(nèi)要求車速、航向應(yīng)有一定的變化，以使訓(xùn)練集包含多種運(yùn)動(dòng)情況。圖3為實(shí)驗(yàn)軌跡，由于是對(duì)實(shí)驗(yàn)數(shù)據(jù)的離線處理，選取正常組合模式下0～170 s的SINS閉環(huán)校正輸出速度與GPS輸出速度，共計(jì)1 710組數(shù)據(jù)作為GPR訓(xùn)練集的輸入與輸出。為了說明算法的有效性與普適性，在170～230 s的失效時(shí)間段內(nèi)，實(shí)驗(yàn)車輛進(jìn)行了轉(zhuǎn)彎、變速、勻速等多種機(jī)動(dòng)，通過對(duì)170～230 s的GPS速度輸出做出預(yù)測(cè)，并作為USQUE算法的偽量測(cè)量進(jìn)行組合導(dǎo)航。對(duì)于USQUE算法，170～230 s則不進(jìn)行量測(cè)更新，只進(jìn)行純慣性導(dǎo)航解算。圖4～圖7給出了兩種方法在姿態(tài)與陀螺漂移估計(jì)誤差的對(duì)比結(jié)果；圖8給出了信息源短時(shí)失效期間東向、北向速度GPR的估

圖4 俯仰角估計(jì)誤差EθFig.4 Estimated pitch error Eθ

圖5 橫滾角估計(jì)誤差EΦFig.5 Estimated roll error EΦ

圖6 航向角估計(jì)誤差EΨFig.6 Estimated yaw error EΨ

圖7 陀螺常值漂移估計(jì)Fig.7 Gyro drift estimate

圖8 GPR速度估計(jì)Fig.8 GPR velocity estimate

計(jì)值與GPS參考系統(tǒng)的對(duì)比結(jié)果；圖9給出了USQUE與GPR-USQUE算法估計(jì)的實(shí)驗(yàn)軌跡；圖10給出了USQUE與GPR-USQUE算法對(duì)經(jīng)度與緯度的估計(jì)誤差。

圖9 GPR-USQUE與USQUE算法位置估計(jì)效果比較Fig.9 Comparison of position estimates of GPR-USQUE and USQUE

圖10 位置估計(jì)誤差EpFig.10 Estimated position errors Ep

整個(gè)實(shí)驗(yàn)過程可以分為3個(gè)階段：0～170 s是正常組合導(dǎo)航階段；170～230 s是GPS失效階段；230～250 s是失效后恢復(fù)正常組合導(dǎo)航階段。GPR-USQUE算法與USQUE算法在0～170 s工作階段估計(jì)效果是一樣的，這是因?yàn)镚PR處于訓(xùn)練階段，并沒有進(jìn)行工作，只是單純地進(jìn)行USQUE算法組合導(dǎo)航。230～250 s是失效后恢復(fù)正常組合導(dǎo)航階段,在該階段GPR不再發(fā)揮作用，而是又一次進(jìn)入訓(xùn)練階段為下一次可能發(fā)生的GPS短時(shí)失效做準(zhǔn)備，因此該階段不是本文研究的重點(diǎn)。在170～230 s階段中GPS失效，GPR開始工作。通過分析姿態(tài)估計(jì)結(jié)果圖4～圖6，使用GPR構(gòu)造偽速度信息進(jìn)行量測(cè)更新的GPR-USQUE算法估計(jì)精度相比于純慣性導(dǎo)航解算的USQUE算法要高出很多。對(duì)于陀螺漂移的估計(jì)見圖7，以估計(jì)誤差的均方根(RMS)作為評(píng)價(jià)估計(jì)效果的標(biāo)準(zhǔn)，計(jì)算結(jié)果見表2. 圖8體現(xiàn)的是GPS失效階段GPR對(duì)速度的估計(jì)精度，圖9和圖10主要用于分析比較GPS短時(shí)失效時(shí)，GPR-USQUE算法與單純慣性導(dǎo)航解算的USQUE算法對(duì)位置估計(jì)的精度。

表2 GPS失效段內(nèi)導(dǎo)航誤差RMS統(tǒng)計(jì)值Tab.2 Navigation error RMS statistics in GPS failure phase

由于MEMS本身精度不高，60 s不進(jìn)行量測(cè)更新使得MEMS發(fā)散程度很高，導(dǎo)致導(dǎo)航信息輸出精度急劇下降，使用GPR-USQUE算法可以較好地抑制這種情況。在使用GPR-USQUE算法后，俯仰角估計(jì)誤差減小了84.3%，橫滾角估計(jì)誤差減小了94.3%，航向角估計(jì)誤差減小了70.7%. 東向陀螺漂移估計(jì)精度提高了10.3%，北向陀螺漂移估計(jì)精度提高了10.8%，天向陀螺漂移估計(jì)精度提高了5.2%. 圖8是GPR速度估計(jì)圖與局部放大圖，在短時(shí)失效時(shí)間段內(nèi)，速度進(jìn)行了一定程度的變化。圖8表明了GPR在GPS失效時(shí)對(duì)速度信息的估計(jì)與參考系統(tǒng)提供的速度信息的關(guān)系，可以計(jì)算出GPR估計(jì)結(jié)果：北向速度最大誤差為0.150 9 m/s，RMS為0.054 9 m/s；東向速度最大估計(jì)誤差為0.201 2 m/s，RMS為0.075 2 m/s. 這也是GPR-USQUE算法在短時(shí)失效情況下有較高估計(jì)精度的原因。由圖8可以看到東向、北向速度估計(jì)中放大部分，由于GPR估計(jì)不準(zhǔn)確，最大誤差達(dá)到了0.15 m/s，與之對(duì)應(yīng)的3倍標(biāo)準(zhǔn)差范圍也相應(yīng)增大，表明此處GPR估計(jì)值的取值置信度比較低，這也是GPR與其他人工智能方法相比的優(yōu)勢(shì)。對(duì)于圖9和圖10中體現(xiàn)的位置估計(jì)效果，GPR-USQUE算法表現(xiàn)出了較高的估計(jì)水平：經(jīng)度最大估計(jì)誤差為5.72 m，RMS為3.574 m；緯度最大估計(jì)誤差為3.56 m，RMS為2.323 m. USQUE算法在GPS失效段由于只能單純進(jìn)行慣導(dǎo)解算，從圖9可以看出，解算軌跡已經(jīng)發(fā)散。通過分析圖10的位置估計(jì)誤差可知：USQUE算法在正常組合導(dǎo)航階段，位置估計(jì)誤差正常；在GPS失效段，經(jīng)度和緯度估計(jì)誤差均值超過了100 m.

在計(jì)算時(shí)間方面，對(duì)250 s的實(shí)驗(yàn)數(shù)據(jù)處理，USQUE算法的計(jì)算時(shí)間為61.36 s，GPR-USQUE算法的計(jì)算時(shí)間為242.19 s. USQUE算法的計(jì)算復(fù)雜度為狀態(tài)向量維數(shù)的2次冪，GPR在學(xué)習(xí)和預(yù)測(cè)過程中的計(jì)算復(fù)雜度分別是訓(xùn)練數(shù)據(jù)的3次冪和2次冪，計(jì)算復(fù)雜度的增加是導(dǎo)致GPR-USQUE算法計(jì)算時(shí)間較長(zhǎng)的原因。由于GPR訓(xùn)練集所需的數(shù)據(jù)量并不大，使得GPR-USQUE算法并不需要增加較多的時(shí)間就可以將估計(jì)精度提高到較高的水平。

5 結(jié)論

本文針對(duì)捷聯(lián)慣性基組合系統(tǒng)中發(fā)生的短時(shí)失效情況，提出了GPR-USQUE算法。在GPS或DVL短時(shí)失效期間，利用GPR提供的量測(cè)信息輔助SINS進(jìn)行組合導(dǎo)航以獲取準(zhǔn)確的導(dǎo)航信息。為了克服短時(shí)失效帶來的不利影響，采用失效前時(shí)刻的組合導(dǎo)航閉環(huán)校正的SINS速度信息作為訓(xùn)練集的輸入，以GPS或DVL提供的速度信息作為輸出，訓(xùn)練基于高斯過程的偽觀測(cè)模型。失效時(shí)，利用訓(xùn)練的偽觀測(cè)模型替代觀測(cè)模型實(shí)現(xiàn)組合濾波量測(cè)更新。通過基于車載MEMS/GPS組合導(dǎo)航系統(tǒng)的實(shí)測(cè)實(shí)驗(yàn)對(duì)算法有效性進(jìn)行了檢驗(yàn)，結(jié)果表明：在含有60 s GPS短時(shí)失效的實(shí)驗(yàn)過程中，俯仰角估計(jì)誤差減小了84.3%，橫滾角估計(jì)誤差減小了94.3%，航向角估計(jì)誤差減小了70.7%. 東向陀螺漂移估計(jì)精度提高了10.3%，北向陀螺漂移估計(jì)精度提高了10.8%，天向陀螺漂移估計(jì)精度提高了5.2%. 經(jīng)度最大估計(jì)誤差為5.72 m，RMS為3.574 m,提高了90.1%；緯度最大估計(jì)誤差為3.56 m,RMS為2.323 m，提高了90.4%. 證明算法具有較高的估計(jì)精度，可以有效地解決組合導(dǎo)航中輔助信息源短時(shí)失效問題。