本發(fā)明涉及一種適用于慣性/天文組合導(dǎo)航系統(tǒng)的信息融合方法，主要以天文導(dǎo)航量測(cè)信息來(lái)校正慣性導(dǎo)航系統(tǒng)的姿態(tài)誤差，并通過(guò)濾波器估計(jì)陀螺的常值漂移。同時(shí)利用上一時(shí)刻的天文導(dǎo)航量測(cè)信息和當(dāng)前時(shí)刻陀螺輸出值對(duì)當(dāng)前時(shí)刻的天文導(dǎo)航量測(cè)信息進(jìn)行野值檢測(cè)。利用已經(jīng)估計(jì)出的陀螺常值漂移來(lái)校準(zhǔn)慣性傳感器的陀螺輸出，使得即使在天文導(dǎo)航系統(tǒng)失效后，慣性導(dǎo)航系統(tǒng)的姿態(tài)依舊可以保持在很高的精度。本發(fā)明大幅提升了組合導(dǎo)航系統(tǒng)濾波收斂速度，同時(shí)可以精確估計(jì)陀螺誤差的常值漂移，提高組合導(dǎo)航系統(tǒng)的精度，可用于任何包含天文和慣性的組合導(dǎo)航系統(tǒng)中。

背景技術(shù)：
天文導(dǎo)航是通過(guò)測(cè)量自然天體相對(duì)運(yùn)動(dòng)載體的矢量方向來(lái)實(shí)現(xiàn)定位導(dǎo)航。天文導(dǎo)航主要有以下幾個(gè)優(yōu)勢(shì)：1)完全自主式導(dǎo)航；2)導(dǎo)航精度高；3)抗干擾能力強(qiáng)，可靠性高；4)導(dǎo)航誤差不隨時(shí)間積累。慣性/天文組合導(dǎo)航系統(tǒng)主要以天文導(dǎo)航系統(tǒng)的信息來(lái)修正由陀螺漂移和初始失準(zhǔn)角引起的誤差，獲得高精度的位置、速度和姿態(tài)導(dǎo)航信息。卡爾曼濾波是目前組合導(dǎo)航系統(tǒng)中使用最廣泛的一種信息融合方法，其主要采用信號(hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和新時(shí)刻的觀測(cè)值來(lái)對(duì)狀態(tài)變量進(jìn)行估計(jì)，求出新時(shí)刻的估計(jì)值。標(biāo)準(zhǔn)離散卡爾曼濾波算法主要由時(shí)間更新方程和量測(cè)更新方程組成：1)時(shí)間更新方程X^k/k-1=AkX^k-1]]>Pk/k-1＝Ak-1Pk-1ATk-1+Qk-12)量測(cè)更新方程Kk=Pk/k-1HkT(HkPk/k-1HkT+Rk)-1]]>Pk=(I-KkHk)Pk/k-1(I-KkHk)T+KkRkKkT]]>X^k=X^k/k-1+Kk(Zk-HkX^k/k-1)]]>其中代表k時(shí)刻系統(tǒng)的狀態(tài)估計(jì)量,Ak為一步轉(zhuǎn)移陣；Pk/k-1為一步預(yù)測(cè)均方誤差陣；Hk為量測(cè)陣；Qk-1為系統(tǒng)噪聲序列方差陣；Rk為量測(cè)噪聲序列方差陣；Kk為濾波增益陣；Pk為估計(jì)均方誤差陣。本發(fā)明涉及一種適用于慣性/天文組合導(dǎo)航系統(tǒng)的信息融合方法，主要以天文導(dǎo)航量測(cè)信息來(lái)校正慣性導(dǎo)航系統(tǒng)姿態(tài)誤差，并通過(guò)濾波器估計(jì)陀螺常值漂移。當(dāng)天文導(dǎo)航失效后，利用已經(jīng)估計(jì)出的陀螺常值漂移來(lái)校準(zhǔn)慣性傳感器的輸出，使得慣性導(dǎo)航系統(tǒng)的姿態(tài)依舊可以保持在很高的精度。本發(fā)明相對(duì)于其他慣性/天文組合導(dǎo)航方法，狀態(tài)變量的選取僅為七維，大大降低了濾波器的計(jì)算量，狀態(tài)變量中載體姿態(tài)誤差主要以四元數(shù)乘性誤差表示，更加符合實(shí)際。同時(shí)卡爾曼濾波器的一步預(yù)測(cè)值與一步預(yù)測(cè)均方誤差隨慣性導(dǎo)航系統(tǒng)頻率更新，使得一步預(yù)測(cè)更加準(zhǔn)確。此外組合導(dǎo)航系統(tǒng)可以精確估計(jì)陀螺誤差的常值漂移，可用于任何包含天文和慣性的組合導(dǎo)航系統(tǒng)中。

技術(shù)實(shí)現(xiàn)要素：
本發(fā)明的技術(shù)解決問(wèn)題是：克服現(xiàn)有技術(shù)的不足，提供一種適用于慣性/天文組合導(dǎo)航的方法。本發(fā)明的技術(shù)解決方案為：一種慣性/天文組合導(dǎo)航方法，其特征在于包括以下步驟：(1)在天文導(dǎo)航量測(cè)信息不可用時(shí)，利用慣性測(cè)量單元的量測(cè)信息進(jìn)行慣性導(dǎo)航系統(tǒng)解算，獲得載體的位置、速度和姿態(tài)參數(shù)。在天文導(dǎo)航量測(cè)信息可用時(shí)，建立慣性/天文組合導(dǎo)航系統(tǒng)的狀態(tài)方程和量測(cè)方程，采用卡爾曼濾波信息融合方法，利用組合導(dǎo)航量測(cè)信息去修正慣性導(dǎo)航系統(tǒng)的誤差，實(shí)現(xiàn)高精度的導(dǎo)航。(2)利用上一時(shí)刻的天文導(dǎo)航量測(cè)信息和當(dāng)前時(shí)刻陀螺輸出值對(duì)當(dāng)前時(shí)刻的天文導(dǎo)航量測(cè)信息進(jìn)行估計(jì)，并將估計(jì)值與當(dāng)前時(shí)刻的天文導(dǎo)航實(shí)際量測(cè)信息進(jìn)行比較，若偏離超過(guò)一定的閾值，則判定天文導(dǎo)航量測(cè)信息為野值，將其剔除。(3)利用卡爾曼濾波器的振蕩程度來(lái)判斷陀螺常值漂移的估計(jì)是否收斂。若卡爾曼濾波器狀態(tài)量估計(jì)值振蕩衰減，則表明卡爾曼濾波器的估計(jì)值逐漸趨向于收斂。若判斷陀螺常值漂移估計(jì)值收斂后，則利用估計(jì)出的陀螺常值漂移校正慣性測(cè)量單元的陀螺輸出。本發(fā)明的原理是：在慣性空間里恒星的方位基本保持不變，因此天文導(dǎo)航系統(tǒng)相當(dāng)于沒有漂移的陀螺，可以實(shí)現(xiàn)高精度的定姿，所以可以用天文導(dǎo)航量測(cè)信息修正慣性器件誤差。本發(fā)明選取載體姿態(tài)乘性四元數(shù)誤差和三軸陀螺的常值漂移作為狀態(tài)變量，建立系統(tǒng)狀態(tài)方程和量測(cè)方程進(jìn)行組合導(dǎo)航系統(tǒng)信息融合。在組合導(dǎo)航過(guò)程中，濾波器狀態(tài)矩陣只與陀螺測(cè)量值相關(guān)，因而濾波器一步預(yù)測(cè)值與一步預(yù)測(cè)均方誤差隨慣性導(dǎo)航系統(tǒng)頻率更新。以當(dāng)前時(shí)刻的陀螺輸出值作為天文導(dǎo)航周期內(nèi)陀螺輸出均值，以上一時(shí)刻的天文導(dǎo)航量測(cè)信息為起點(diǎn)，采用畢卡逼近法估計(jì)當(dāng)前時(shí)刻的天文導(dǎo)航量測(cè)信息。將估計(jì)的天文導(dǎo)航量測(cè)信息與當(dāng)前時(shí)刻實(shí)際的天文導(dǎo)航量測(cè)信息進(jìn)行比較，若兩者偏離超過(guò)一定的闕值，則判定當(dāng)前時(shí)刻的天文導(dǎo)航量測(cè)信息為野值，將其剔除。以最近30次的卡爾曼濾波器陀螺常值漂移的估計(jì)值作為樣本進(jìn)行統(tǒng)計(jì)分析，當(dāng)標(biāo)準(zhǔn)差小于均值的一百倍時(shí)，即可認(rèn)為陀螺常值漂移估計(jì)值趨于收斂，并以此均值作為陀螺的常值漂移。最后在每次系統(tǒng)狀態(tài)變量估計(jì)值去修正慣性導(dǎo)航系統(tǒng)后，將系統(tǒng)狀態(tài)變量中的載體姿態(tài)乘性四元數(shù)誤差重置成姿態(tài)無(wú)誤差狀態(tài)，作為下一次濾波迭代的初值。本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于：狀態(tài)變量的選取僅為七維，大大降低了濾波器的計(jì)算量，狀態(tài)變量中載體姿態(tài)誤差主要以四元數(shù)乘性誤差表示，更加符合實(shí)際。同時(shí)卡爾曼濾波器的一步預(yù)測(cè)值與一步預(yù)測(cè)均方誤差隨慣性導(dǎo)航系統(tǒng)頻率更新，使得一步預(yù)測(cè)更加準(zhǔn)確。此外組合導(dǎo)航系統(tǒng)可以精確估計(jì)陀螺誤差的常值漂移。附圖說(shuō)明圖1為本發(fā)明的慣性/天文組合導(dǎo)航方法流程圖。具體實(shí)施方式如圖1所示，本發(fā)明的具體方法如下：(1)在地心慣性坐標(biāo)系下，建立慣性/天文組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型。僅考慮陀螺的常值漂移誤差和白噪聲誤差，所以陀螺的量測(cè)方程為：ωm＝ω+ε+h其中，ω為載體真實(shí)角速度，ε為陀螺的常值漂移誤差，h為陀螺的高斯白噪聲，ωm為載體測(cè)得的實(shí)際角速度。定義乘性誤差四元數(shù)dq為:qm＝qtοdq其中，qt為載體真實(shí)的四元數(shù)姿態(tài)，qm為載體經(jīng)解算獲得的含誤差的四元數(shù)姿態(tài)。狀態(tài)變量X(t)選取載體姿態(tài)誤差四元數(shù)dq0、dq1、dq2、dq3以及陀螺三軸常值漂移εx、εy、εz，即卡爾曼濾波器的狀態(tài)變量X(t)為：X(t)＝[dq0dq1dq2dq3εxεyεz]T則卡爾曼濾波器的狀態(tài)方程為：X·(t)=F(t)X(t)+G(t)W(t)]]>其中：F(t)=000000000ω^ibzb-ω^ibyb0.5000-ω^ibzb0ω^ibxb00.500ω^ibyb-ω^ibxb0000.5000000000000000000000]]>G(t)=01×312I3×303×3,W(t)=ηxηyηz]]>星敏感器測(cè)得的載體姿態(tài)四元數(shù)QCNS相對(duì)于慣性導(dǎo)航系統(tǒng)計(jì)算出載體姿態(tài)四元數(shù)QINS精度更高，且不隨時(shí)間發(fā)散。所以可以認(rèn)為QCNS為組合導(dǎo)航系統(tǒng)的姿態(tài)真值，QINS為組合導(dǎo)航系統(tǒng)的姿態(tài)估計(jì)值。則由上面誤差四元數(shù)的定義可知：則卡爾曼濾波器的量測(cè)方程為：Z(t)＝H(t)X(t)+V(t)其中：(2)每一步慣性導(dǎo)航解算后，利用陀螺輸出的數(shù)據(jù)可求得當(dāng)前時(shí)刻卡爾曼濾波的狀態(tài)矩陣F(t)，利用卡爾曼濾波離散化方程，將F(t)進(jìn)行離散化可以求得卡爾曼濾波一步轉(zhuǎn)移陣Ak，繼而求得卡爾曼濾波的狀態(tài)量一步預(yù)測(cè)和一步預(yù)測(cè)均方誤差陣若當(dāng)前天文導(dǎo)航數(shù)據(jù)不可用，則令狀態(tài)量一步預(yù)測(cè)等于狀態(tài)估計(jì)量一步預(yù)測(cè)均方誤差陣等于估計(jì)均方誤差Pk；若當(dāng)前天文導(dǎo)航數(shù)據(jù)可用，則使用標(biāo)準(zhǔn)離散卡爾曼濾波方法進(jìn)行系統(tǒng)狀態(tài)估計(jì)量和一步均方誤差陣Pk的計(jì)算。(3)將當(dāng)前時(shí)刻的陀螺輸出值視為兩次天文導(dǎo)航量測(cè)之間的陀螺輸出均值，采用畢卡逼近法，可以從上一時(shí)刻的天文導(dǎo)航量測(cè)信息Q1估計(jì)當(dāng)前時(shí)刻的天文導(dǎo)航量測(cè)信息記當(dāng)前時(shí)刻天文導(dǎo)航實(shí)際量測(cè)信息為Q2，則和Q2的誤差四元數(shù)δQ2為：若誤差四元數(shù)δQ2大于一定的闕值，則判定當(dāng)前時(shí)刻天文導(dǎo)航量測(cè)信息為野值，應(yīng)將其剔除。(4)實(shí)時(shí)在線估計(jì)卡爾曼濾波系統(tǒng)狀態(tài)變量中陀螺常值漂移的收斂情況。陀螺常值漂移估計(jì)值是否收斂的判斷依據(jù)是卡爾曼濾波器的振蕩程度是否趨于穩(wěn)定。以最近30次的卡爾曼濾波器陀螺常值漂移的估計(jì)值作為樣本，當(dāng)其標(biāo)準(zhǔn)差小于均值的一百倍時(shí)，即可認(rèn)為陀螺常值漂移估計(jì)值趨于收斂。(5)若某軸陀螺常值漂移估計(jì)值判斷為收斂后，利用最近30次的陀螺常值漂移估計(jì)值的平均值作為陀螺常值漂移，然后對(duì)該軸慣性測(cè)量單元的陀螺的輸出值進(jìn)行誤差的修正。(6)利用卡爾曼濾波得到的系統(tǒng)狀態(tài)變量的最優(yōu)估計(jì)值去修正慣性導(dǎo)航系統(tǒng)后，需將狀態(tài)變量中姿態(tài)的乘性四元數(shù)誤差重置成無(wú)姿態(tài)誤差狀態(tài)，即：X^k=δq0δq1δq2δq3ϵxϵyϵzT=1000ϵxϵyϵzT]]>。