輔助信息源短時失效下的捷聯慣性基組合導航算法

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

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

0 引言

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

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

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

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

1 捷聯慣導基本模型

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

模型的姿態部分為

(1)

(2)

SINS其他部分模型為

(3)

(4)

(5)

(6)

(7)

(8)

陀螺的量測模型為

(9)

(10)

加速度計的量測模型為

(11)

(12)

2 GPR算法基本原理

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

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

輸入與輸出的關系為

yi=f(xi)+δ，

(13)

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

高斯過程是隨機變量的集合，要求集合中任意數量的隨機變量都必須滿足聯合高斯分布的隨機過程[28]。因此，均值函數與協方差函數就成了高斯過程訓練的關鍵。

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

(14)

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

(15)

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

目標函數為

(16)

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

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

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

(17)

3 GPR-USQUE算法

圖1 GPR-USQUE訓練與估計流程圖Fig.1 Training and estimation of GPR-USQUE algorithm

通過研究發現GPR對于插值預測效果非常好，但是對于出現一定數量的不位于訓練數據集范圍內數據的情況預測效果不佳，這是由于GPR能夠進行回歸預測的核心是數據的相關性，一定數量不位于訓練數據集的數據會造成該部分數據與GPR訓練的黑箱相關性過低，從而導致預測的穩定性急劇下降。這也是大部分人工智能算法的通病。因此，對于GPR的輸入量與訓練集的選擇非常重要。對于輸入量的數值區間不要過大，訓練集中數據量可以不大，但是數值區間要盡量大些。

在組合導航系統中，通常作為Kalman濾波中量測量的導航信息通常為速度，位置或者速度與位置。對于位置信息，可以認為是無界的，這對于GPR算法并不適用。因此對速度信息進行回歸預測，無論從訓練集的規模還是數據的相關性上都非常恰當。

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

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

4 實驗與分析

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

表1 XW-IMU5220型MEMS相關性能Tab.1 Related performance of XW-IMU5220 MEMS

濾波初始協方差矩陣設置為

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

圖3 測試軌跡Fig.3 Test track

實驗軌跡為駕車繞操場一周，實驗時間為250 s. 在實驗過程中，170～230 s區間段內將一臺GPS關機模擬GPS短時失效，另一臺GPS接收機正常工作為實驗提供速度與位置基準。為了確保GPR訓練集小而有效，在100多秒左右的訓練時間內要求車速、航向應有一定的變化，以使訓練集包含多種運動情況。圖3為實驗軌跡，由于是對實驗數據的離線處理，選取正常組合模式下0～170 s的SINS閉環校正輸出速度與GPS輸出速度，共計1 710組數據作為GPR訓練集的輸入與輸出。為了說明算法的有效性與普適性，在170～230 s的失效時間段內，實驗車輛進行了轉彎、變速、勻速等多種機動，通過對170～230 s的GPS速度輸出做出預測，并作為USQUE算法的偽量測量進行組合導航。對于USQUE算法，170～230 s則不進行量測更新，只進行純慣性導航解算。圖4～圖7給出了兩種方法在姿態與陀螺漂移估計誤差的對比結果；圖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參考系統的對比結果；圖9給出了USQUE與GPR-USQUE算法估計的實驗軌跡；圖10給出了USQUE與GPR-USQUE算法對經度與緯度的估計誤差。

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

表2 GPS失效段內導航誤差RMS統計值Tab.2 Navigation error RMS statistics in GPS failure phase

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

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

5 結論

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