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

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

0 引言

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

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

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

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

1 SINS/GPS組合模型

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

(1)

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

(2)

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

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

1.2.1 衛(wèi)星有效

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

Z1=H1X+V1，

(3)

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

(4)

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

1.2.2 衛(wèi)星失效

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

圖1 車輛運(yùn)動(dòng)學(xué)約束Fig.1 Vehicle kinematics constraints

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

(5)

對(duì)(5)式進(jìn)行全微分，則有

(6)

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

將(6)式改寫成

δvb=M1δvn+M2φn，

(7)

式中：

(8)

(9)

利用車輛運(yùn)動(dòng)學(xué)約束條件構(gòu)成濾波器量測(cè)值為

(10)

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

Z2=H2X+V2，

(11)

式中：H2為量測(cè)矩陣，H2表達(dá)式為

(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)量測(cè)噪聲矢量。

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

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

(13)

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

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

由牛頓第二運(yùn)動(dòng)定律可得車輛向心加速度為

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

(14)

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

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

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

圖2 基于速度約束的組合導(dǎo)航系統(tǒng)量測(cè)更新流程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)方程、量測(cè)方程以及噪聲先驗(yàn)統(tǒng)計(jì)特性可以估計(jì)得到最優(yōu)狀態(tài)值。然而由于導(dǎo)航系統(tǒng)具有時(shí)變性，其量測(cè)噪聲特性受外界環(huán)境影響較大，因此需要在線調(diào)節(jié)量測(cè)噪聲協(xié)方差矩陣大小，使其能夠適應(yīng)外界環(huán)境變化。新息序列能夠?qū)崟r(shí)反應(yīng)量測(cè)噪聲統(tǒng)計(jì)特性的變化，因此可以根據(jù)新息序列的變化在線調(diào)節(jié)量測(cè)協(xié)方差矩陣。

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

ek=Zk-Hkk/k-1，

(15)

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

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

(16)

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

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

(17)

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

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

(18)

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

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

AveDoMk=‖DoMk‖1/m，

(19)

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

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

時(shí)間更新：

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

(20)

(21)

量測(cè)更新：

(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，其實(shí)現(xiàn)過程如下：

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

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

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

3 算法仿真與性能分析

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

車輛運(yùn)行速度為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，仿真時(shí)間為2 400 s.

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

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

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

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

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

4 驗(yàn)證試驗(yàn)

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

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

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

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

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

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

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

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

由圖8可以看出，當(dāng)衛(wèi)星信號(hào)失效時(shí)，SINS工作于純慣性導(dǎo)航模式，其位置誤差隨時(shí)間積累，當(dāng)衛(wèi)星信號(hào)恢復(fù)后，組合導(dǎo)航系統(tǒng)位置誤差能夠較快收斂，試驗(yàn)結(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. 同時(shí)本文所提出的基于FIS-ADKF的組合導(dǎo)航精度要優(yōu)于標(biāo)準(zhǔn)Kalman組合精度，尤其在GPS信號(hào)遮擋期間，由于濾波器量測(cè)信息不確定性較大，采用基于FIS-ADKF算法的經(jīng)度誤差由2.56 m減小為0.72 m，緯度誤差由8.61 m減小為3.42 m，高度誤差由2.52 m減小為0.86 m，驗(yàn)證了本文所提出自適應(yīng)濾波算法的有效性。

5 結(jié)論

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

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

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