胡杰， 嚴(yán)勇杰， 王子卉

(1.中國電子科技集團公司 第二十八研究所 空中交通管理系統(tǒng)與技術(shù)國家重點實驗室， 江蘇 南京 210007；2.東南大學(xué) 微慣性儀表與先進導(dǎo)航技術(shù)教育部重點實驗室， 江蘇 南京 210096)

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)和全球定位系統(tǒng)(GPS)是兩種常用的導(dǎo)航定位系統(tǒng)，其中：GPS定位精度高，且位置誤差不隨時間積累，但是衛(wèi)星信號易受多路徑、建筑物遮擋等干擾影響，導(dǎo)航系統(tǒng)的魯棒性較差[1]；SINS具有全自主、全天候、隱蔽性強等優(yōu)點，但其慣性測量單元(IMU)漂移會引起位置隨時間積累性誤差[2]?？紤]到這兩種導(dǎo)航系統(tǒng)的互補性，SINS和GPS組合后的性能更優(yōu)，目前廣泛應(yīng)用于航空航天、測繪等軍民用領(lǐng)域，能夠連續(xù)輸出運載體速度、位置和姿態(tài)等信息，具有精度高、抗差性強等優(yōu)點[3]。

當(dāng)GPS信號不可用時，組合導(dǎo)航系統(tǒng)將切換至純慣性工作模式。針對GPS失效引起的導(dǎo)航系統(tǒng)精度降低問題，國內(nèi)外學(xué)者進行了相關(guān)研究。嚴(yán)恭敏等[4]、肖烜等[5]和Liu等[6]提出利用里程計輔助車載SINS實現(xiàn)長時間導(dǎo)航與定位；高鐘毓等[7]首次提出在車載組合導(dǎo)航系統(tǒng)中利用車輛停車時速度為零這一約束條件估計導(dǎo)航系統(tǒng)誤差，該方法需要每間隔一段時間停車一次，影響載車機動性能[8]。近年來，隨著人工智能(AI)算法的日漸成熟，許多學(xué)者提出利用AI模塊輔助增強組合導(dǎo)航系統(tǒng)衛(wèi)星失效時的性能。當(dāng)GPS信號有效時，AI模塊利用觀測數(shù)據(jù)進行訓(xùn)練；一旦GPS信號失效，利用訓(xùn)練得到的導(dǎo)航信息對SINS誤差進行校正。Zhang等[9]和Chen等[10]分別提出利用模糊神經(jīng)網(wǎng)絡(luò)和遺傳算法訓(xùn)練系統(tǒng)模型，并在衛(wèi)星失效時對SINS誤差進行校正，實現(xiàn)了車載無縫定位。其他一些學(xué)者也紛紛開展了基于AI算法的組合導(dǎo)航系統(tǒng)研究工作，比如集成學(xué)習(xí)算法[11]、小波神經(jīng)網(wǎng)絡(luò)[12]、多層感知機算法[13]等，但是AI算法計算量較大，實現(xiàn)過程比較復(fù)雜，在實際系統(tǒng)中使用時存在局限性。

Kalman濾波算法是一種常用的多源信息融合方法，目前在GPS定位[14]、組合導(dǎo)航[15]等方面得到廣泛應(yīng)用。Kalman濾波器狀態(tài)收斂速度和精度受限于系統(tǒng)噪聲與量測噪聲統(tǒng)計精度，為獲取濾波過程中噪聲特性，Sage等[16]提出了一種自適應(yīng)Kalman濾波(ADKF)算法，該方法在線計算系統(tǒng)噪聲與量測噪聲協(xié)方差矩陣，分析可知， ADKF無法同時估計系統(tǒng)噪聲與量測噪聲協(xié)方差矩陣。徐田來等[17]提出將簡化ADKF與交互式多模型(IMM)相結(jié)合，以ADKF估計得到的粗略噪聲協(xié)方差矩陣為中心進行IMM估計，該方法能夠較好地解決模型參數(shù)不確定性問題，但是該方法計算量大。王向華等[18]提出利用并行Kalman濾波器實現(xiàn)量測噪聲協(xié)方差矩陣的自適應(yīng)調(diào)節(jié)，需要在濾波估計時并行使用Kalman濾波器。

本文針對車載GPS失效時SINS精度降低問題，提出一種采用車輛速度約束輔助的組合導(dǎo)航方法，在不增加外部傳感器的前提下，提高SINS導(dǎo)航精度。同時針對SINS/GPS組合導(dǎo)航中量測噪聲的不確定性以及由此引起的導(dǎo)航精度降低問題，提出了一種基于模糊推理系統(tǒng)(FIS)的ADKF算法，對算法性能進行了仿真驗證，并進行了實際系統(tǒng)驗證試驗。

1 SINS/GPS組合模型

1.1 系統(tǒng)狀態(tài)方程

(1)

進一步根據(jù)SINS誤差方程[19]，并結(jié)合系統(tǒng)狀態(tài)向量可以得到組合導(dǎo)航濾波狀態(tài)方程為

(2)

式中：A為SINS誤差方程對應(yīng)的狀態(tài)轉(zhuǎn)移矩陣；W為系統(tǒng)噪聲矢量。

1.2 系統(tǒng)量測方程

1.2.1 衛(wèi)星有效

衛(wèi)星信號有效時利用GPS接收機輸出速度和位置作為濾波器觀測值，此時量測方程為

Z1=H1X+V1，

(3)

式中：Z1為衛(wèi)星有效時觀測向量，Z1=[(δPn)T(δVn)T]T，其中δPn、δVn分別為導(dǎo)航坐標(biāo)系n系中的3個位置和速度誤差；H1為量測矩陣，H1表達式為

(4)

I3×3表示3×3維單位矩陣，03×3、03×9分別表示3×3和3×9維零矩陣；V1為系統(tǒng)量測噪聲矢量。

1.2.2 衛(wèi)星失效

衛(wèi)星信號失效時利用車輛行駛特點抑制SINS誤差[20]，如果車輛沒有發(fā)生側(cè)滑和跳躍，則車體坐標(biāo)系b系中側(cè)向速度與天向速度可近似為0 m/s，如圖1所示，由于僅側(cè)向和天向兩個方向存在約束，因此該方法也被稱為非完整性約束。

圖1 車輛運動學(xué)約束Fig.1 Vehicle kinematics constraints

IMU安裝于車輛后首先進行安裝誤差標(biāo)校，因此假設(shè)IMU的測量軸與b系重合，則可得b系下的速度分量為

(5)

對(5)式進行全微分，則有

(6)

式中：δvb表示b系中速度誤差分量；φn表示數(shù)學(xué)平臺失準(zhǔn)角向量；δvn表示n系中速度誤差分量；“×”表示取對應(yīng)3維向量的反對稱矩陣。

將(6)式改寫成

δvb=M1δvn+M2φn，

(7)

式中：

(8)

(9)

利用車輛運動學(xué)約束條件構(gòu)成濾波器量測值為

(10)

由(7)式和(10)式可得量測方程為

Z2=H2X+V2，

(11)

式中：H2為量測矩陣，H2表達式為

(12)

M1(1,)、M2(1,)分別表示取矩陣M1、M2的第1行，M1(3,)、M2(3,)分別表示取矩陣M1、M2的第3行，01×3、01×6分別表示1×3維和1×6維零矩陣；V2為系統(tǒng)量測噪聲矢量。

對(2)式、 (3)式和(11)式進行離散化，可以得到組合導(dǎo)航狀態(tài)方程和量測方程的離散形式為

Xk=Φk,k-1Xk-1+Wk-1，Zk=HkXk+Vk，

(13)

式中：Xk表示k時刻系統(tǒng)狀態(tài)矢量；Φk,k-1表示離散方程k-1時刻至k時刻轉(zhuǎn)移矩陣；Xk-1表示k-1時刻系統(tǒng)狀態(tài)矢量；Wk-1表示系統(tǒng)噪聲矢量；Zk表示k時刻量測矢量；Hk表示k時刻量測矩陣；Vk表示k時刻量測噪聲矢量。

根據(jù)上述分析可知，當(dāng)車輛急速拐彎時，其向心加速度使得車體側(cè)向速度不為零，此時不能滿足上述約束條件。一般情況下，急速拐彎時間較短，因此短時間內(nèi)可只進行純慣性解算，待車輛運行狀態(tài)平穩(wěn)時，切換至非完整性約束模式，可以根據(jù)IMU輸出信號判斷車輛是否處于急拐彎狀態(tài)。

由牛頓第二運動定律可得車輛向心加速度為

a(t)=v(t)×ωz(t)，

(14)

式中：a(t)表示t時刻車輛向心加速度；v(t)表示t時刻車輛行駛速度；ωz(t)表示陀螺輸出z軸向車輛角速度。

根據(jù)實際路況以及多次實驗數(shù)據(jù)統(tǒng)計結(jié)果可以確定向心加速度檢測閾值λa，如果|a(t)|≥λa，說明檢測得車輛急速拐彎，此時切換至“僅時間更新的非完整性約束”，即不進行濾波器量測更新，僅進行時間更新[21]，否則進行非完整性約束。

綜上所示，可以得到基于速度約束的組合導(dǎo)航系統(tǒng)量測更新流程示意圖，如圖2所示。

圖2 基于速度約束的組合導(dǎo)航系統(tǒng)量測更新流程Fig.2 Measurement updating process of integrated navigation system based on velocity constraint

2 基于FIS的ADKF算法

Kalman濾波器被廣泛應(yīng)用于組合導(dǎo)航系統(tǒng)中，根據(jù)系統(tǒng)狀態(tài)方程、量測方程以及噪聲先驗統(tǒng)計特性可以估計得到最優(yōu)狀態(tài)值。然而由于導(dǎo)航系統(tǒng)具有時變性，其量測噪聲特性受外界環(huán)境影響較大，因此需要在線調(diào)節(jié)量測噪聲協(xié)方差矩陣大小，使其能夠適應(yīng)外界環(huán)境變化。新息序列能夠?qū)崟r反應(yīng)量測噪聲統(tǒng)計特性的變化，因此可以根據(jù)新息序列的變化在線調(diào)節(jié)量測協(xié)方差矩陣。

Kalman濾波器量測更新中新息序列可表示為

ek=Zk-Hkk/k-1，

(15)

式中：ek表示k時刻濾波器新息序列；k/k-1表示k時刻濾波器狀態(tài)一步預(yù)測值。

定義新息序列實際協(xié)方差與理論協(xié)方差之間的不匹配度為

(16)

式中：DoMk[i]表示k時刻第i個量測值的不匹配度；Sk、k分別為k時刻新息序列的理論與實際協(xié)方差矩陣；Sk[i,i]、k[i,i]表示對應(yīng)矩陣的對角元素；1≤i≤m，其中m表示觀測向量的個數(shù)。

新息序列實際協(xié)方差矩陣可計算為

(17)

式中：N為平滑窗口大小，該值需要根據(jù)經(jīng)驗確定；N值越大，平滑后噪聲越小；N值越小，其跟蹤能力就越強；本文根據(jù)試驗取N值為15.

新息序列理論協(xié)方差矩陣可計算為

(18)

式中：Pk/k-1表示k時刻狀態(tài)一步預(yù)測協(xié)方差矩陣；Rk表示k時刻量測噪聲協(xié)方差矩陣。

由 (16) 式知，DoMk是一個維數(shù)為m的向量，取其平均值作為FIS的輸入，即定義一個新的變量AveDOMk，其表達式為

AveDoMk=‖DoMk‖1/m，

(19)

式中：‖·‖1表示計算向量的1范數(shù)，根據(jù)AveDOMk值的大小可以判別濾波器的收斂情況，AveDOMk=1表示此時濾波器嚴(yán)格收斂，否則表示濾波器異常。

由(19)式可知，當(dāng)AveDOMk1時，說明系統(tǒng)量測噪聲增大，此時需要調(diào)大量測噪聲矩陣，而當(dāng)AveDOMk1時，說明系統(tǒng)量測噪聲減小，此時需要調(diào)小量測噪聲矩陣，最終保持AveDOMk值在1附近，由此可以得到自適應(yīng)濾波遞推形式為

時間更新：

k/k-1=Φk,k-1k-1，

(20)

(21)

量測更新：

(22)

k=k/k-1+Kk[Zk-Hkk/k-1],

(23)

Pk=Pk/k-1-KkHkPk/k-1,

(24)

Rk=cRk-1,

(25)

本文采用FIS根據(jù)輸入的AveDOMk值確定在線調(diào)整系數(shù)c，其實現(xiàn)過程如下：

1)定義模糊集合，設(shè)decrease表示小于1的模糊子集，keep表示等于1的模糊子集，increase表示大于1的模糊子集。

2)確定FIS隸屬度函數(shù)與模糊規(guī)則。

3)去模糊化以獲得對應(yīng)的量測噪聲協(xié)方差矩陣在線調(diào)整系數(shù)，本文調(diào)整系數(shù)c規(guī)則為：

IfAveDOMkis decrease, thencis decrease；

IfAveDOMkis keep, thencis keep；

IfAveDOMkis increase, thencis increase.

其采用的隸屬度函數(shù)如圖3所示。

圖3 FIS輸入輸出變量隸屬函數(shù)Fig.3 Membership functions of input and output variables of FIS

綜上所述，圖4為基于速度約束和FIS-ADKF的SINS/GPS組合導(dǎo)航系統(tǒng)結(jié)構(gòu)圖。

圖4 SINS/GPS組合導(dǎo)航系統(tǒng)框圖Fig.4 Block diagram of SINS/GPS integrated navigation system

由圖4可以看出，當(dāng)衛(wèi)星信號有效時，利用GPS速度和位置信息與SINS輸出速度和位置信息進行組合，一旦衛(wèi)星信號失效，此時需要檢測車輛急速轉(zhuǎn)彎，如果車輛行駛較為平穩(wěn)，則利用虛擬側(cè)向和天向速度輔助SINS進行導(dǎo)航與定位。同時新息序列實際協(xié)方差矩陣與理論協(xié)方差矩陣之比作為模糊推理系統(tǒng)輸入，利用FIS可以得到量測噪聲協(xié)方差矩陣調(diào)整系數(shù)，從而實現(xiàn)Kalman濾波器最優(yōu)估計。

3 算法仿真與性能分析

通過改變量測噪聲值對本文所提出算法進行驗證，假設(shè)陀螺常值漂移為0.01°/h，角度隨機游走系數(shù)為0.005°/h1/2，標(biāo)度因數(shù)誤差為50×10-6；加速度計常值偏置為100×10-6g，隨機游走系數(shù)為80×10-6g·s1/2，SINS“數(shù)學(xué)平臺”初始失準(zhǔn)角分別為0.5′、0.5′以及3′.

車輛運行速度為5 m/s，基準(zhǔn)速度和位置根據(jù)GPS速度和位置誤差模型仿真得到，并分別假設(shè)：速度誤差均值為0.1 m/s，標(biāo)準(zhǔn)差為0.1 m/s；位置誤差均值為0.5 m，標(biāo)準(zhǔn)差為0.1 m；初始航向角、俯仰角以及橫搖角分別為30°、1°以及1°，Kalman濾波周期設(shè)置為1 s，仿真時間為2 400 s.

仿真過程中，在0～900 s之間保持模型準(zhǔn)確，在900～1 500 s之間設(shè)置量測噪聲標(biāo)準(zhǔn)差為初始時刻的5倍，即速度和位置量測噪聲分別為0.5 m/s和0.5 m，在1 500～2 400 s之間量測噪聲標(biāo)準(zhǔn)差恢復(fù)為初始值，分別利用標(biāo)準(zhǔn)Kalman濾波和本文所提出的FIS-ADKF對上述過程進行仿真，圖5為設(shè)定的車輛行駛軌跡二維圖，圖中藍色實線和紅色點劃線分別為標(biāo)準(zhǔn)Kalman濾波和FIS-ADKF解算得車輛行駛軌跡。表1為位置誤差標(biāo)準(zhǔn)差對比統(tǒng)計，圖6為位置誤差對比曲線。

圖5 車輛行駛軌跡二維圖Fig.5 2-D map of vehicle trajectory

圖6 位置誤差對比曲線Fig.6 Contrast curves of position errors

由圖6以及表1可以看出，在時間1～900 s之間，由于模型較為準(zhǔn)確，標(biāo)準(zhǔn)Kalman濾波與本文所提出的FIS-ADKF精度相當(dāng)。在900～1 500 s之間，由于量測噪聲發(fā)生變化，標(biāo)準(zhǔn)Kalman濾波器的量測噪聲協(xié)方差矩陣無相應(yīng)變化，所以Kalman濾波器的估計性能受到較大影響，輸出速度與位置等參數(shù)均出現(xiàn)較大偏差，同樣條件下，F(xiàn)IS-ADKF由于可以通過實時估計新息序列協(xié)方差使其逼近外部實際噪聲，然后進一步通過減小量測增益矩陣以削弱量測噪聲對濾波器估計精度影響，因此濾波器具有較高的估計精度和抗干擾性。

表1 位置誤差標(biāo)準(zhǔn)差對比統(tǒng)計Tab.1 Statistical comparison of standard deviations of position errors

4 驗證試驗

利用實驗室研制的光纖SINS進行車載組合導(dǎo)航驗證試驗，其IMU由3個光纖陀螺和3個石英撓性加速度計組成，同時試驗車上有一臺GPS接收機，其位置精度優(yōu)于5 m，IMU主要性能指標(biāo)如表2所示。

表2 光纖SINS參數(shù)Tab.2 Parameters of optical fiber SINS

為了獲取車輛行駛過程中真實位置，根據(jù)GPS接收機保存的原始觀測數(shù)據(jù)并利用后處理的動態(tài)定位方法(PPK)計算車輛位置信息。車載試驗實物圖如圖7所示，車載試驗地點為貴陽市某條道路，車載試驗行駛過程中控制車輛速度為10 km/h，試驗時長為3 600 s.

圖7 試驗設(shè)備及環(huán)境Fig.7 Experimental facilities and environment

試驗路段衛(wèi)星無遮擋，因此在時間點1 500 s設(shè)定衛(wèi)星信號不可用，持續(xù)時間600 s，該時間段內(nèi)車輛無拐彎現(xiàn)象。圖8為衛(wèi)星信號失效時無速度約束位置誤差曲線，圖9為采用速度約束輔助標(biāo)準(zhǔn)Kalman濾波與本文所提出的FIS-ADKF算法所得位置誤差對比曲線，表3為衛(wèi)星信號失效時無速度約束輔助、速度約束輔助標(biāo)準(zhǔn)Kalman濾波算法以及速度約束輔助FIS-ADKF算法位置誤差對比統(tǒng)計。

圖8 無速度約束位置誤差曲線Fig.8 Position errors without velocity constraint

圖9 不同濾波方法位置誤差對比曲線Fig.9 Contrast curves of position errors of different filtering methods

表3 最大位置誤差對比統(tǒng)計Tab.3 Statistical comparison of maximum position errors

由圖8可以看出，當(dāng)衛(wèi)星信號失效時，SINS工作于純慣性導(dǎo)航模式，其位置誤差隨時間積累，當(dāng)衛(wèi)星信號恢復(fù)后，組合導(dǎo)航系統(tǒng)位置誤差能夠較快收斂，試驗結(jié)果與理論分析一致。

由于SINS純慣性模式下高度通道發(fā)散，因此表3中未給出純慣性模式下的高度誤差結(jié)果。由圖9和表3可以看出，當(dāng)利用側(cè)向與天向速度組合后，SINS導(dǎo)航精度得到提高，緯度最大誤差由41.33 m減小至8.61 m，經(jīng)度最大誤差由17.65 m減小至2.56 m. 同時本文所提出的基于FIS-ADKF的組合導(dǎo)航精度要優(yōu)于標(biāo)準(zhǔn)Kalman組合精度，尤其在GPS信號遮擋期間，由于濾波器量測信息不確定性較大，采用基于FIS-ADKF算法的經(jīng)度誤差由2.56 m減小為0.72 m，緯度誤差由8.61 m減小為3.42 m，高度誤差由2.52 m減小為0.86 m，驗證了本文所提出自適應(yīng)濾波算法的有效性。

5 結(jié)論

為進一步提高車載SINS/GPS組合導(dǎo)航精度，本文針對衛(wèi)星信號失效時SINS位置誤差隨時間積累以及SINS/GPS組合導(dǎo)航中量測噪聲協(xié)方差矩陣不確定性問題進行了分析與研究。得到以下主要結(jié)論：

1)對車輛運動學(xué)約束條件進行了研究，研究結(jié)果表明衛(wèi)星信號失效時利用車輛側(cè)向與天向速度為零這一約束條件可以在不增加外部傳感器的前提下提高SINS長時間導(dǎo)航精度。

2)為實時獲取較為準(zhǔn)確的量測噪聲協(xié)方差矩陣，提出了一種新的FIS-ADKF組合導(dǎo)航融合算法。進行了車載導(dǎo)航驗證試驗，結(jié)果表明速度約束能夠抑制SINS位置誤差隨時間的快速積累，同時本文所提出的FIS-ADKF組合導(dǎo)航融合算法能夠提高濾波器狀態(tài)估計精度。后續(xù)研究中將會考慮引入里程計，利用里程計提供的前向速度進一步提高組合導(dǎo)航系統(tǒng)精度，進而實現(xiàn)車載SINS/GPS的高精度組合導(dǎo)航。