張夢得, 李開龍, 胡柏青, 呂旭

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

0 引言

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

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

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

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

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

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

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

(1)

(2)

SINS其他部分模型為

(3)

(4)

(5)

(6)

(7)

(8)

陀螺的量測模型為

(9)

(10)

加速度計的量測模型為

(11)

(12)

2 GPR算法基本原理

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

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

yi=f(xi)+δ，

(13)

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

高斯過程是隨機變量的集合，要求集合中任意數(shù)量的隨機變量都必須滿足聯(lián)合高斯分布的隨機過程[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個超參數(shù)的取值是整個GPR的關(guān)鍵；In為n階單位矩陣。因此整個問題就轉(zhuǎn)化成了在(15)式條件下求取目標(biāo)函數(shù)的最小值問題。

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

(16)

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

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

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

(17)

3 GPR-USQUE算法

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

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

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

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

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

4 實驗與分析

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

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

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

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

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

圖7 陀螺常值漂移估計Fig.7 Gyro drift estimate

圖8 GPR速度估計Fig.8 GPR velocity estimate

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

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

圖10 位置估計誤差EpFig.10 Estimated position errors Ep

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

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

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

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

5 結(jié)論

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