基于速度約束與模糊自適應濾波的車載組合導航

胡杰， 嚴勇杰， 王子卉

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

0 引言

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

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

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

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

1 SINS/GPS組合模型

1.1 系統狀態方程

(1)

進一步根據SINS誤差方程[19]，并結合系統狀態向量可以得到組合導航濾波狀態方程為

(2)

式中：A為SINS誤差方程對應的狀態轉移矩陣；W為系統噪聲矢量。

1.2 系統量測方程

1.2.1 衛星有效

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

Z1=H1X+V1，

(3)

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

(4)

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

1.2.2 衛星失效

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

圖1 車輛運動學約束Fig.1 Vehicle kinematics constraints

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

(5)

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

(6)

式中：δvb表示b系中速度誤差分量；φn表示數學平臺失準角向量；δvn表示n系中速度誤差分量；“×”表示取對應3維向量的反對稱矩陣。

將(6)式改寫成

δvb=M1δvn+M2φn，

(7)

式中：

(8)

(9)

利用車輛運動學約束條件構成濾波器量測值為

(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為系統量測噪聲矢量。

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

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

(13)

式中：Xk表示k時刻系統狀態矢量；Φk,k-1表示離散方程k-1時刻至k時刻轉移矩陣；Xk-1表示k-1時刻系統狀態矢量；Wk-1表示系統噪聲矢量；Zk表示k時刻量測矢量；Hk表示k時刻量測矩陣；Vk表示k時刻量測噪聲矢量。

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

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

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

(14)

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

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

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

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

2 基于FIS的ADKF算法

Kalman濾波器被廣泛應用于組合導航系統中，根據系統狀態方程、量測方程以及噪聲先驗統計特性可以估計得到最優狀態值。然而由于導航系統具有時變性，其量測噪聲特性受外界環境影響較大，因此需要在線調節量測噪聲協方差矩陣大小，使其能夠適應外界環境變化。新息序列能夠實時反應量測噪聲統計特性的變化，因此可以根據新息序列的變化在線調節量測協方差矩陣。

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

ek=Zk-Hkk/k-1，

(15)

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

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

(16)

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

新息序列實際協方差矩陣可計算為

(17)

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

新息序列理論協方差矩陣可計算為

(18)

式中：Pk/k-1表示k時刻狀態一步預測協方差矩陣；Rk表示k時刻量測噪聲協方差矩陣。

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

AveDoMk=‖DoMk‖1/m，

(19)

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

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

時間更新：

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根據輸入的AveDOMk值確定在線調整系數c，其實現過程如下：

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

2)確定FIS隸屬度函數與模糊規則。

3)去模糊化以獲得對應的量測噪聲協方差矩陣在線調整系數，本文調整系數c規則為：

IfAveDOMkis decrease, thencis decrease；

IfAveDOMkis keep, thencis keep；

IfAveDOMkis increase, thencis increase.

其采用的隸屬度函數如圖3所示。

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

綜上所述，圖4為基于速度約束和FIS-ADKF的SINS/GPS組合導航系統結構圖。

圖4 SINS/GPS組合導航系統框圖Fig.4 Block diagram of SINS/GPS integrated navigation system

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

3 算法仿真與性能分析

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

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

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

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

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

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

表1 位置誤差標準差對比統計Tab.1 Statistical comparison of standard deviations of position errors

4 驗證試驗

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

表2 光纖SINS參數Tab.2 Parameters of optical fiber SINS

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

圖7 試驗設備及環境Fig.7 Experimental facilities and environment

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

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

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

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

由圖8可以看出，當衛星信號失效時，SINS工作于純慣性導航模式，其位置誤差隨時間積累，當衛星信號恢復后，組合導航系統位置誤差能夠較快收斂，試驗結果與理論分析一致。

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

5 結論

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

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

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