一種多傳感器組合導(dǎo)航實時仿真系統(tǒng)的研究

一種多傳感器組合導(dǎo)航實時仿真系統(tǒng)的研究

為了解決多傳感器信息的融合問題，speyer和kerr在1979年至1987年間提出了并行分散濾波。在1988年的情況下，kalason提出了聯(lián)邦濾波。與第一種相比，后者信息傳輸量小，濾波精度高，容錯性強，成為美國的新一代通用濾波。盡管聯(lián)邦濾波的實際應(yīng)用考慮了不同的通用軟件，但該方法對更新頻率有特殊要求。也就是說，在融合期間，傳輸?shù)淖觽鞲衅鞯妮敵鍪峭降模@在實際系統(tǒng)中通常是不可能的。在這種情況下，許多中國科學(xué)家對聯(lián)邦濾波進行了改進，并取得了良好的結(jié)果。但在實踐工程中,聯(lián)邦濾波由于多個濾波器的同時存在,大大降低了實時性,這在高動態(tài)環(huán)境下顯得尤為不利.基于這一點,本文對實時性好的集中濾波加以改進,解決了多傳感器異步組合的問題,并在此基礎(chǔ)上提出量測滯后的方案.實時仿真系統(tǒng)的驗證表明該算法融合了2種濾波的優(yōu)點,具有較高的濾波精度.1gps-cns-sins組合導(dǎo)航數(shù)學(xué)模型本文以輸出速度、位置信息的GPS、輸出三維姿態(tài)信息的捷聯(lián)天文導(dǎo)航系統(tǒng)CNS和捷聯(lián)慣導(dǎo)系統(tǒng)SINS為組合對象進行多傳感器組合研究.1.1系統(tǒng)狀態(tài)變量的定義采用SINS的誤差方程作為系統(tǒng)的狀態(tài)方程,導(dǎo)航坐標系選用北東地坐標系,通過對SINS系統(tǒng)的性能及誤差源的分析,可以獲得濾波器的誤差方程為˙X(t)=F(t)X(t)+G(t)W(t)(1)式中,系統(tǒng)狀態(tài)變量定義為18階,即X={?Ν,?E,?D,δvΝ,δvE,δvD,δL,δλ,δh,εbx,εby,εbz,εrx,εry,εrz,?x,?y,?z}Τ式中,?N,?E,?D為數(shù)學(xué)平臺誤差角;δvN,δvE,δvD為速度誤差;δL,δλ,δh為緯度、經(jīng)度和高度誤差;εbx,εby,εbz,εrx,εry,εrz分別為陀螺常值漂移誤差和一階馬爾可夫漂移誤差;?x,?y,?z為加速度計零偏,方程其他參數(shù)詳見文獻.1.2u3000主客觀姿態(tài)誤差組合導(dǎo)航系統(tǒng)共有2組觀測值:①GPS給出的速度、位置與捷聯(lián)慣導(dǎo)系統(tǒng)給出的速度、位置差值,如下式:Ζ1(t)=[vΙΝ-vGΝvΙE-vGEvΙD-vGD(LΙ-LG)RΝ(λΙ-λG)REcosLhΙ-hG]=[δvΝδvEδvDRΝδLREcosLδλδh]+[vΝvEvDΝΝΝEΝD]=ΗGX(t)+VG(t)(2)ΗG=[03×3diag03×303×903×303×3diag[RΝREcosL1]03×9]式中,vN,vE,vD為GPS接收機沿北、東、地方向的速度誤差;NN,NE,ND為GPS沿北、東、地方向的位置誤差;RN,RE分別為地球子午圈和卯酉圈的曲率半徑.②由CNS提供的載體姿態(tài)與捷聯(lián)慣導(dǎo)的姿態(tài)計算出的平臺誤差值,如下式:Ζ2(t)=[?Ν+ΟΝ?E+ΟE?D+ΟD]=[?Ν?E?D]+[ΟΝΟEΟD]=ΗCX(t)+ΟC(3)式中,ΗC=[Ι3×303×15];ON,OE,OD分別為CNS的姿態(tài)誤差;OC為該誤差矩陣.2異步多傳感器濾波算法對于上述GPS/CNS/SINS異步多傳感器通常采用信息同步處理的聯(lián)邦濾波.2.1邦濾波器的特點為了確保組合系統(tǒng)具有滿意的容錯性,可以采用無復(fù)位聯(lián)邦濾波.圖1給出其計算流程,虛框內(nèi)為子濾波器的迭代過程.從圖1流程可以看出該模式的聯(lián)邦濾波具有2個特點:①各子濾波器量測值不同,但狀態(tài)變量相同;②子濾波器實際上就是集中濾波,包含了2個信息更新過程:時間更新?Xi(k-1)→?Xi(k,k-1)Pi(k-1,k-1)→Pi(k,k-1)量測更新?Xi(k-1)→?Xi(k?k)Pi(k-1,k-1)→Pi(k,k)在沒有量測值的情況下,時間更新可以保證濾波的連續(xù)性.利用這一點,可以使量測信息同步化,從而解決了聯(lián)邦濾波中異步組合的問題.2.2集中濾波算法借鑒如上特點,集中濾波也可以實現(xiàn)多傳感器異步組合.考慮如下的狀態(tài)方程和量測方程:Xk=Φk,k-1Xk-1+Γk-1Wk-1Ζk=ΗkXk+Vk}(4)可以獲得常規(guī)的卡爾曼濾波器方程為?Xk?k-1=Φk,k-1?Xk-1(5)?Xk=?Xk?k-1+Κk(Ζk-Ηk?Xk?k-1)(6)Pk,k-1=Φk,k-1Pk-1ΦTk,k-1+Γk-1Qk-1ΓTk-1(7)Kk=Pk,k-1HTk(HkPk,k-1HTk+Rk)-1(8)Pk=(I-KkHk)Pk,k-1(9)由式(9)可以看出,集中濾波也存在時間更新和量測更新的過程,因此也具備解決異步組合條件.同時借鑒聯(lián)邦濾波子濾波器的思想,即量測值不同但狀態(tài)值相同,相應(yīng)的在集中濾波中設(shè)置多個量測矩陣和量測噪聲矩陣,根據(jù)量測值的不同,用相應(yīng)的矩陣進行切換,對相同的狀態(tài)變量進行估計,獲得X^k和Pk.圖2給出該濾波算法的流程圖.在濾波器設(shè)計時,選擇輔助傳感器輸出周期的最小值為濾波的固有組合周期,在固有組合時刻沒有輔助傳感器量測信息時,則進行時間更新;當在某個時刻獲得傳感器信息時,根據(jù)獲得的量測值選擇H陣、R陣進行濾波,即“誰來誰組合”,從而解決了異步組合的問題.在本文GPS/CNS/SINS組合中,根據(jù)傳感器周期的不同,一共有3組量測矩陣和量測噪聲陣供選擇:Η1=ΗGR1=VG}(10a)Η2=ΗCR2=ΟC}(10b)Η3=[ΗC000ΗG0]R3=[ΟC00VG]}(10c)采用這種濾波方式,在任一組合時刻都只有一個濾波器工作,與聯(lián)邦濾波多個子濾波器同時工作相比,大大提高了系統(tǒng)的實時性;同時通過對X^k的故障檢測,可以確認當前傳感器是否異常,一旦異常則拋棄當前的濾波值X^k,僅對X^k進行時間更新;從而有效地隔離了故障傳感器,并保證了濾波的連續(xù)性,具有了聯(lián)邦濾波容錯性強的特點.3基于補償?shù)牧繙y滯后估計通過上述方法可以很好地解決多傳感器異步組合問題.但沒有考慮傳感器輸出滯后的情況.傳感器輸出滯后在高動態(tài)的環(huán)境下給量測值Z帶來很大的誤差,由式(6)可以看出,會導(dǎo)致導(dǎo)航結(jié)果惡化,而這種情況單純依靠濾波本身往往不容易檢測.解決量測滯后,文獻提出在與滯后量測值對應(yīng)的過去時刻組合,再將濾波結(jié)果遞推到當前時刻的方法.這種方法導(dǎo)致在一次濾波中需要進行2次時間更新:組合濾波一次,將濾波結(jié)果遞推到當前時刻再進行一次,因此算法比較復(fù)雜.本文采用補償?shù)姆椒?用其他傳感器的值補償滯后量,從而在當前組合時刻進行濾波.如圖3所示,在某個組合時刻tk獲得帶有延時量Δt的量測值Zk,與其對應(yīng)的時刻是tk-Δt,捷聯(lián)的解算周期是TINS,用一個數(shù)組A以TINS為間隔記錄Δt時間內(nèi)m個主傳感器量測值,計算出Zk增量ΔZ,從而將量測值遞推到當前時刻,并利用下式:X^k=X^k,k-1+Κk[(Ζk+ΔΖ)-ΗkX^k?k-1](11)取代圖2中對X^k的估計,獲得量測滯后修正的最優(yōu)狀態(tài)估計.4網(wǎng)絡(luò)仿真實驗假設(shè)該組合導(dǎo)航系統(tǒng)的載體是彈道導(dǎo)彈,GPS/CNS/SINS實時仿真系統(tǒng)盡量與實際工程相接近:組合導(dǎo)航程序包括捷聯(lián)慣導(dǎo)程序、組合濾波程序都用C語言實現(xiàn),在DOS操作系統(tǒng)下實時運行,采用嵌入式PC104為導(dǎo)航計算機.仿真彈道軌跡如圖4(a)所示,包括:垂直上升段OA,拐彎段AB,直線瞄準段BC,被動斜拋段CDE.仿真系統(tǒng)生成的彈道,用Matlab作圖如圖4(b)所示,發(fā)射點120°(E)20°(N)500m;目標點160°(E)60°(N)500m,總射程6092.814km.GPS,CNS和SINS的原始IMU數(shù)據(jù)都依據(jù)彈道仿真生成,整個仿真時間根據(jù)彈道計算為656.56s.設(shè)GPS的輸出周期是1s,帶有400ms延時;CNS輸出周期是1.5s,不帶延時.捷聯(lián)慣導(dǎo)解算周期是20ms.慣導(dǎo)系統(tǒng)的等效陀螺漂移為0.01(°)/h,等效加速度零偏為10-4g;陀螺一階馬爾可夫過程相關(guān)時間為3600s,加速度零偏一階馬爾可夫過程相關(guān)時間為1800s.GPS位置誤差為10m,速度誤差為0.1m/s;CNS姿態(tài)誤差為20″,設(shè)置濾波器工作初始值如表1所示.系統(tǒng)仿真時,根據(jù)仿真彈道同步生成的GPS,CNS和SINS原始數(shù)據(jù)在組合導(dǎo)航計算機內(nèi)進行實時的組合運算,并輸出導(dǎo)航結(jié)果.圖5給出解決量測滯后的異步傳感器集中濾波估計精度和終點誤差.從圖5可以看出:①雖然濾波器的量測矩陣和量測噪聲矩陣是切換的,但濾波曲線是收斂、穩(wěn)定的,沒有因為矩陣的切換出現(xiàn)跳變;②濾波協(xié)方差統(tǒng)計表明濾波精度很高,平臺誤差角維持在10.3″,速度誤差為0.25m/s,位置誤差為1.5m.由于有無解決量測滯后僅是量測值Z不同,從圖2可以看出Z值對P陣的遞推沒有影響,因此本文采用導(dǎo)航系統(tǒng)終點姿態(tài)、速度和位置誤差(即組合結(jié)果與仿真彈道的差值)表示有無解決量測滯后的區(qū)別.表2為某次的仿真結(jié)果,其中δR,δP和δY分別表示橫滾、俯仰和航向誤差.表2組合導(dǎo)航終點誤差從表2看出,由于彈道導(dǎo)彈屬于高速載體,0.4s的速度延時將造成很大位置誤差,通過累計最終導(dǎo)致落點偏差達到公里級.采用量測滯后補償算法,則落點誤差大大減小,優(yōu)于20m(CEP),同時其他指標精度也獲得提高.5基于gps/cns/sins的濾波算法仿真本文對如何在集中濾波下實現(xiàn)異步