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

(1.中國(guó)科學(xué)院 微電子研究所， 北京 100029； 2.中國(guó)科學(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)中傳感器的隨機(jī)零偏會(huì)隨溫度變化、沖擊等環(huán)境因素影響而動(dòng)態(tài)發(fā)生變化，影響融合精度，因此需要對(duì)其進(jìn)行動(dòng)態(tài)估計(jì)。根據(jù)對(duì)組合導(dǎo)航系統(tǒng)的可觀測(cè)分析可知，只有當(dāng)載體運(yùn)動(dòng)包含角機(jī)動(dòng)、變加速等大機(jī)動(dòng)時(shí)，系統(tǒng)誤差向量才可全觀測(cè)[1-3]，當(dāng)載體長(zhǎng)時(shí)間沿直線運(yùn)動(dòng)或靜止時(shí)，狀態(tài)方程會(huì)退化[4]，將采用去除隨機(jī)零偏的降維系統(tǒng)模型進(jìn)行數(shù)據(jù)融合，而這也導(dǎo)致無(wú)法對(duì)傳感器隨機(jī)零偏進(jìn)行動(dòng)態(tài)估計(jì)的問(wèn)題。

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

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

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

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

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

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

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

Zk=HkXk+Vk

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

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

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

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

1)輸入交互。

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

2)濾波計(jì)算。

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

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

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

②Kalman濾波增益計(jì)算：

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

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

3)模型概率更新。

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

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

4)輸出融合。

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

總體協(xié)方差為:

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

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

3 仿真驗(yàn)證與分析

對(duì)算法進(jìn)行仿真驗(yàn)證，生成一組載體運(yùn)動(dòng)軌跡，運(yùn)動(dòng)狀態(tài)分別為：靜止30 s、曲線變加速運(yùn)動(dòng)30 s、勻速直線運(yùn)動(dòng)30 s、勻減速直線運(yùn)動(dòng)30 s、靜止30 s，仿真條件為陀螺噪聲0.01(°)/s，加速度計(jì)噪聲0.001 g、速度噪聲0.02 m/s、位置噪聲0.1 m、偏航角噪聲0.5°。同時(shí)為驗(yàn)證改進(jìn)算法對(duì)傳感器零偏的動(dòng)態(tài)估計(jì)性能，在20 s時(shí)將三軸陀螺零偏由0.1、0.2、0.3(°)/s分別增加0.1(°)/s；將三軸加速度計(jì)零偏由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)過(guò)骨髓造血組織中的血竇進(jìn)入血液循環(huán)成為血小板。血小板在凝血過(guò)程、血栓形成及機(jī)體免疫反應(yīng)中扮演重要的角色[6-8]。

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

圖2 模型概率Fig.2 Model probability

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

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

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

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

圖4 陀螺隨機(jī)零偏Fig.4 Gyro random zero bias

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

圖5 加速度計(jì)隨機(jī)零偏Fig.5 Accelerometer random zero bias

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

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

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

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

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

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

對(duì)比IMM-Kalman濾波算法與常規(guī)濾波方法，在衛(wèi)星失效情況下，INS獨(dú)立導(dǎo)航30 s的誤差統(tǒng)計(jì)如表1所示?？梢钥吹交贗MM-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)解算誤差對(duì)比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)模型，在載體機(jī)動(dòng)不足時(shí)，將發(fā)生模型退化，導(dǎo)致系統(tǒng)數(shù)據(jù)融合精度降低。

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

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

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

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