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

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

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