田易， 閻躍鵬， 鐘燕清， 李繼秀， 孟真

(1.中國科學(xué)院 微電子研究所， 北京 100029； 2.中國科學(xué)院大學(xué)， 北京 100049)

數(shù)據(jù)融合技術(shù)是慣性(inertial navigation system, INS)/衛(wèi)星(global navigation satellite system, GNSS)組合導(dǎo)航系統(tǒng)中的核心技術(shù)之一，是決定組合導(dǎo)航系統(tǒng)導(dǎo)航精度的關(guān)鍵。而系統(tǒng)中傳感器的隨機零偏會隨溫度變化、沖擊等環(huán)境因素影響而動態(tài)發(fā)生變化，影響融合精度，因此需要對其進(jìn)行動態(tài)估計。根據(jù)對組合導(dǎo)航系統(tǒng)的可觀測分析可知，只有當(dāng)載體運動包含角機動、變加速等大機動時，系統(tǒng)誤差向量才可全觀測[1-3]，當(dāng)載體長時間沿直線運動或靜止時，狀態(tài)方程會退化[4]，將采用去除隨機零偏的降維系統(tǒng)模型進(jìn)行數(shù)據(jù)融合，而這也導(dǎo)致無法對傳感器隨機零偏進(jìn)行動態(tài)估計的問題。

交互多模型(interacting multiple model, IMM)數(shù)據(jù)融合算法是一種常用的目標(biāo)跟蹤的多模型估計方法[5-6]，通過轉(zhuǎn)移概率矩陣可實現(xiàn)系統(tǒng)在不同模型之間的自適應(yīng)切換[7]。其在組合導(dǎo)航系統(tǒng)中已有廣泛的應(yīng)用，一方面用來描述系統(tǒng)模型的不確定性和觀測噪聲[8-13]，另一方面則是對多種融合方式進(jìn)行交互[14-19]，以提高系統(tǒng)數(shù)據(jù)融合精度。

基于IMM數(shù)據(jù)融合算法在組合導(dǎo)航系統(tǒng)的成功應(yīng)用，本文提出了一種基于IMM-Kalman的數(shù)據(jù)融合算法，分別建立了用于大機動時的常規(guī)15維狀態(tài)方程(模型1)和用于機動不足將隨機零偏作為控制量的降維狀態(tài)方程(模型2)2種系統(tǒng)模型，在IMM框架下，根據(jù)載體運動狀態(tài)，自適應(yīng)選擇系統(tǒng)模型，根據(jù)模型概率對不同模型的濾波結(jié)果加權(quán)，實現(xiàn)對傳感器隨機零偏的動態(tài)最優(yōu)估計，提高組合導(dǎo)航系統(tǒng)數(shù)據(jù)融合精度。

1 慣性/衛(wèi)星組合導(dǎo)航基本原理

1.1 慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)狀態(tài)方程

根據(jù)組合導(dǎo)航系統(tǒng)誤差方程[20]，選擇15維狀態(tài)向量為：X(t)=[φxφyφzδvxδvyδvzδλδLδh

則常規(guī)15維狀態(tài)方程(模型1)，滿足：

式中:Q(t)為系統(tǒng)的狀態(tài)噪聲；F1(t)為15×15維的狀態(tài)系數(shù)矩陣。F1(t)可以寫成：

(1)

將隨機零偏作為控制量的降維狀態(tài)方程(模型2)滿足：

式中:B(t)為控制向量，是傳感器的隨機零偏向量，滿足:

F2(t)為15×15維的狀態(tài)系數(shù)矩陣，與式(1)相似，其中F11(t)部分與式(1)中F11(t)完全相同，而F12(t)處變成了全0矩陣，使傳感器隨機零偏與姿態(tài)、速度及位置誤差狀態(tài)量完全獨立，保證其與降維系統(tǒng)模型等價。模型2不具備對傳感器隨機零偏動態(tài)估計的能力，因此控制向量B(t)需要通過模型1估計得到。

1.2 慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)觀測方程

以雙天線衛(wèi)星系統(tǒng)的航向、速度、位置為參考，與慣導(dǎo)系統(tǒng)輸出的航向、速度、位置的差值作為觀測量，滿足的觀測方程為[20]:

Zk=HkXk+Vk

式中：Zk為觀測向量;Hk為系數(shù)矩陣。Hk滿足:

2 IMM-Kalman數(shù)據(jù)融合

基于IMM-Kalman的數(shù)據(jù)融合算法原理如圖1所示，主要包括輸入交互、濾波計算、模型概率更新以及輸出融合4個步驟。

圖1 IMM-Kalman數(shù)據(jù)融合原理Fig.1 Block diagram of IMM-Kalman data fusion

1)輸入交互。

對于模型j(j=1,2)在k-1時刻的狀態(tài)最優(yōu)估計值和估計協(xié)方差矩陣為Xj(k-1k-1)和Pj(k-1k-1)。則交互后各模型的初始濾波條件為：

2)濾波計算。

對模型j(j=1,2)并行進(jìn)行Kalman濾波估計，濾波過程包括：

①一次預(yù)測方程和預(yù)測協(xié)方差矩陣計算：

Xj(kk-1)=FjXj(k-1k-1)+μk

②Kalman濾波增益計算：

Kj(k)=Pj(kk-1)HT[HPj(kk-1)HT+Rj]-1

③k時刻對應(yīng)單個濾波器輸出的系統(tǒng)狀態(tài)估計和協(xié)方差估計分別為：

3)模型概率更新。

式中：Sj(k)=HPj(kk-1)HT+Rj，dj(k)=Z(k)-HXj(kk-1)，則可以得到模型j的更新概率為：

式中c為歸一化常數(shù)，即：

4)輸出融合。

輸出融合通過更新的模型概率對每個模型的濾波結(jié)果加權(quán)，得到k時刻最優(yōu)估計結(jié)果為：

總體協(xié)方差為:

[Xj(kk)-X(kk)][Xj(kk)-X(kk)]T}

基于IMM-Kalman數(shù)據(jù)融合算法，當(dāng)載體大機動運動時，模型1可全觀測，模型概率增加，輸出融合時權(quán)重增加，實現(xiàn)對傳感器隨機零偏的動態(tài)估計，并用于更新模型2中的控制向量；當(dāng)載體機動不足，準(zhǔn)確的控制向量使模型2的模型概率增加，輸出融合權(quán)重增加，有效避免了機動不足引起的方程退化問題。改進(jìn)方法在實現(xiàn)對傳感器隨機零偏動態(tài)估計的同時，有效地提高了組合導(dǎo)航系統(tǒng)數(shù)據(jù)融合的精度。

3 仿真驗證與分析

對算法進(jìn)行仿真驗證，生成一組載體運動軌跡，運動狀態(tài)分別為：靜止30 s、曲線變加速運動30 s、勻速直線運動30 s、勻減速直線運動30 s、靜止30 s，仿真條件為陀螺噪聲0.01(°)/s，加速度計噪聲0.001 g、速度噪聲0.02 m/s、位置噪聲0.1 m、偏航角噪聲0.5°。同時為驗證改進(jìn)算法對傳感器零偏的動態(tài)估計性能，在20 s時將三軸陀螺零偏由0.1、0.2、0.3(°)/s分別增加0.1(°)/s；將三軸加速度計零偏由2、3、4 mg分別增加2 mg。數(shù)據(jù)融合算法中INS更新頻率為100 Hz，數(shù)據(jù)融合頻率為1 Hz。

血小板由骨髓造血組織中的巨核細(xì)胞產(chǎn)生，定向分化成原始的巨核細(xì)胞，又進(jìn)一步成為成熟的巨核細(xì)胞[5]。成熟的巨核細(xì)胞膜表面形成許多凹陷，伸入胞質(zhì)之中，相鄰的凹陷細(xì)胞膜在凹陷深部相互融合，使巨核細(xì)胞部分胞質(zhì)與母體分開。最后這些被細(xì)胞膜包圍的與巨核細(xì)胞胞質(zhì)分離開的成分脫離巨核細(xì)胞，經(jīng)過骨髓造血組織中的血竇進(jìn)入血液循環(huán)成為血小板。血小板在凝血過程、血栓形成及機體免疫反應(yīng)中扮演重要的角色[6-8]。

模型1為常規(guī)的15維濾波模型，模型2為將傳感器隨機零偏作為控制向量的濾波模型。IMM-Kalman數(shù)據(jù)融合過程中獲得的動態(tài)模型概率由圖2所示，由于運動狀態(tài)的改變，系統(tǒng)在不同階段選擇模型的概率不同。在0～20 s，載體靜止，模型1可觀測性不足，模型2由于準(zhǔn)確的初始零偏值作為控制向量，因此模型概率更高；20 s時，由于傳感器隨機零偏改變，導(dǎo)致模型2誤差增加，模型概率下降；在30 s處，載體機動性增強，模型1的可觀測大大提高，其模型概率增大；當(dāng)60 s后，載體機動不足導(dǎo)致模型1退化，而模型2的控制向量得到了更新，因此模型2的模型概率再次增加。由模型概率圖可見，基于IMM-Kalman的數(shù)據(jù)融合算法對系統(tǒng)模型概率的估計，準(zhǔn)確反映了載體不同的運動狀態(tài)下模型的準(zhǔn)確度。

圖2 模型概率Fig.2 Model probability

采用IMM-Kalman數(shù)據(jù)融合法對仿真軌跡進(jìn)行系統(tǒng)誤差估計，并分別與采用模型1和模型2為狀態(tài)方程的傳統(tǒng)濾波方法結(jié)果進(jìn)行對比。由于偏航角誤差、速度誤差、位置誤差為直接觀測向量，因此僅將俯仰角誤差、滾轉(zhuǎn)角誤差及傳感器隨機零偏值對比估計結(jié)果繪于圖3～5中。

圖3中為俯仰角誤差和滾轉(zhuǎn)角誤差估計對比結(jié)果，采用模型1濾波，僅在30～60 s載體大機動運動時，系統(tǒng)可全觀測，姿態(tài)估計準(zhǔn)確，其他時刻，由于方程退化，導(dǎo)致姿態(tài)估計誤差增大。模型2由于不能對傳感隨機零偏動態(tài)更新，因此在20 s后，傳感器零偏發(fā)生變化，直接影響了姿態(tài)估計精度。而改進(jìn)算法，通過IMM對模型自適應(yīng)選擇，并將模型1估計的隨機零偏用于動態(tài)更新模型2中的控制向量，使改進(jìn)算法的姿態(tài)精度不受載體運動狀態(tài)變化的影響。

圖3 姿態(tài)角解算誤差Fig.3 Attitude angle errors

基于模型2的濾波方法不具備對傳感器零偏動態(tài)估計的能力。因此圖4為模型1濾波與改進(jìn)算法對陀螺隨機零偏的估計結(jié)果。陀螺隨機零偏可觀測性較好，不受載體運動狀態(tài)影響，因此2種方法的濾波結(jié)果相當(dāng)。

圖4 陀螺隨機零偏Fig.4 Gyro random zero bias

圖5中為模型1與改進(jìn)算法對加速度計隨機零偏的估計結(jié)果，與姿態(tài)估計結(jié)果相似，基于模型1的常規(guī)方法，由于模型退化的原因，導(dǎo)致X、Y軸加速度計零偏估計僅在30～60 s估計準(zhǔn)確；而改進(jìn)算法實現(xiàn)了自適應(yīng)地對加速度計隨機零偏的動態(tài)估計。

圖5 加速度計隨機零偏Fig.5 Accelerometer random zero bias

INS/GNSS組合導(dǎo)航系統(tǒng)存在衛(wèi)星失效INS獨立導(dǎo)航的情況，而數(shù)據(jù)融合精度是影響INS獨立導(dǎo)航精度的關(guān)鍵。因此仿真分別采用傳統(tǒng)方法和IMM-Kalman數(shù)據(jù)融合方法，在遇到衛(wèi)星中斷30 s時，對INS的獨立導(dǎo)航精度進(jìn)行對比分析。在120 s時，開始INS獨立導(dǎo)航。采用模型1的Kalman濾波初始俯仰角誤差為0.26°、滾轉(zhuǎn)角誤差為-0.23°、X軸加計零偏為0.007 5 g、Y軸加計零偏為0.008 6 g；采用模型2的Kalman濾波初始俯仰角誤差為-0.18°、滾轉(zhuǎn)角誤差為0.04°，X軸加計零偏為0.002 g、Y軸加計零偏為0.003 g；采用改進(jìn)算法初始俯仰角誤差僅為-0.11°、滾轉(zhuǎn)角誤差僅為-0.04°、X軸加計零偏估計為0.004 4 g、Y軸加計零偏估計為0.005 4 g；而3種方法的其他可觀測變量的初始誤差基本相當(dāng)。

根據(jù)對隨機零偏的估計結(jié)果分別對傳感器零偏進(jìn)行補償，從120 s開始純慣性導(dǎo)航，姿態(tài)誤差累積結(jié)果如圖6所示。明顯地,基于IMM-Kalman數(shù)據(jù)融合后，由于初始姿態(tài)誤差最小、傳感器零偏估計最準(zhǔn)確，因此INS獨立導(dǎo)航過程中的姿態(tài)漂移最小。

速度誤差累積結(jié)果及位置誤差累積結(jié)果如圖7和圖8所示，準(zhǔn)確的姿態(tài)角和傳感器隨機零偏估計，使得改進(jìn)算法的東、北、天速度累積誤差分別為1.2、0.2和0.2 m/s，位置累積誤差分別為21.0、2.3和2.7 m，改進(jìn)算法的速度誤差和位置誤差累積最小。

圖6 無衛(wèi)星姿態(tài)誤差Fig.6 Attitude error without GPS

圖7 無衛(wèi)星速度誤差Fig.7 Velocity error without GPS

圖8 無衛(wèi)星位置誤差Fig.8 Position error without GPS

對比IMM-Kalman濾波算法與常規(guī)濾波方法，在衛(wèi)星失效情況下，INS獨立導(dǎo)航30 s的誤差統(tǒng)計如表1所示。可以看到基于IMM-Kalman的改進(jìn)算法得到的俯仰角誤差為0.02°，滾轉(zhuǎn)角誤差為0.16°、偏航角誤差為0.03°、速度誤差為1.25 m/s及位置誤差為21.8 m，均優(yōu)于基于模型1和模型2的常規(guī)方法，其中位置誤差減少了近50%，進(jìn)一步證明了改進(jìn)算法的有效。

表1 IMM-Kalman算法與傳統(tǒng)算法純慣導(dǎo)解算誤差對比Table 1 Comparison between IMM-Kalman algorithm and traditional algorithm of pure INS navigation

4 結(jié)論

1)常規(guī)的15維狀態(tài)方程的組合導(dǎo)航系統(tǒng)模型，在載體機動不足時，將發(fā)生模型退化，導(dǎo)致系統(tǒng)數(shù)據(jù)融合精度降低。

2)常規(guī)的去除傳感器隨機零偏的降維組合導(dǎo)航系統(tǒng)模型，無法實現(xiàn)對傳感器隨機零偏的動態(tài)估計，當(dāng)零偏隨溫度等環(huán)境因素改變后，也將導(dǎo)致系統(tǒng)數(shù)據(jù)融合精度降低。

3)基于IMM-Kalman的改進(jìn)數(shù)據(jù)融合方法，基于IMM框架，實現(xiàn)不同模型之間的自適應(yīng)切換，可以實現(xiàn)對系統(tǒng)誤差及傳感器零偏的動態(tài)最優(yōu)估計。

通過仿真驗證，INS/GNSS組合導(dǎo)航系統(tǒng)采用改進(jìn)算法的數(shù)據(jù)融合精度優(yōu)于傳統(tǒng)算法；同時，在衛(wèi)星失鎖后，與傳統(tǒng)算法相比，改進(jìn)算法在數(shù)據(jù)融合過程中對姿態(tài)及隨機零偏的準(zhǔn)確估計，使得系統(tǒng)獨立導(dǎo)航時姿態(tài)、速度和位置的累積誤差最小，其中位置誤差僅為21.8 m，與常規(guī)方法的40.4 m和42.2 m相比提高了近50%，進(jìn)一步證明了改進(jìn)算法的有效性。

接下來將進(jìn)一步開展對基于IMM-Kalman的改進(jìn)數(shù)據(jù)融合算法進(jìn)行實測數(shù)據(jù)驗證的工作，完成改進(jìn)算法的嵌入式實現(xiàn)。