專利名稱：用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)方法及系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及導(dǎo)航系統(tǒng)領(lǐng)域，特別涉及一種用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)方法及系統(tǒng)。
背景技術(shù)：
MEMS/GPS (Micro-Electro-Mechanical Systems 微機(jī)電系統(tǒng) /GlobalPositioning System全球定位系統(tǒng))組合導(dǎo)航系統(tǒng)是指以基于MEMS技術(shù)的慣性測(cè)量單元(IMU)作為傳感器，由GPS衛(wèi)星導(dǎo)航系統(tǒng)進(jìn)行位置速度等參考信息輔助的導(dǎo)航系統(tǒng)。MEMS/GPS組合導(dǎo)航系統(tǒng)因其體積小、重量輕、功耗小、成本低在未來的車載、航空應(yīng)用中有著良好的前景。動(dòng)基座自對(duì)準(zhǔn)是指在載體機(jī)動(dòng)情況下，不依靠外部信息(如高精度慣導(dǎo)的導(dǎo)航信息)完成系統(tǒng)初始對(duì)準(zhǔn)的方法，是得到系統(tǒng)初始航向的重要過程。Scherzinger和Rogers提出對(duì)原有的線性化誤差模型進(jìn)行修改,利用兩個(gè)變量來表示方位角誤差即可以用線性模型解決非線性問題，利用僅有的GPS提供的速度+位置信息及卡爾曼濾波實(shí)現(xiàn)對(duì)準(zhǔn)。這種方法可以完成180°全角度范圍的對(duì)準(zhǔn)，但是修改后的誤差模型更加復(fù)雜， 計(jì)算量增大。Hong基于修正后的航向角誤差模型，利用基線方法解決初始對(duì)準(zhǔn)中大失準(zhǔn)角問題，但基線選擇會(huì)影響對(duì)準(zhǔn)的實(shí)用性，而且仍對(duì)初始誤差有要求。Crassidis提出基于大失準(zhǔn)角情況下的非線性誤差模型，利用無軌跡卡爾曼濾波完成對(duì)準(zhǔn)，但無軌跡濾波很難從理論上證明其穩(wěn)定性。

發(fā)明內(nèi)容
本發(fā)明的目的旨在至少解決上述的技術(shù)缺陷之一。為達(dá)到上述目的，本發(fā)明一方面的實(shí)施例提出一種用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)方法，包括以下步驟:s1:獲取導(dǎo)航信息和GPS觀測(cè)信息，其中，所述導(dǎo)航信息通過慣性測(cè)量單元獲取，并對(duì)所述導(dǎo)航信息進(jìn)行慣性導(dǎo)航解算；S2:建立動(dòng)基座對(duì)準(zhǔn)的誤差模型；S3:建立卡爾曼觀測(cè)控制器，其中，所述卡爾曼觀測(cè)控制器的工作被劃分為多個(gè)階段，且每個(gè)階段對(duì)應(yīng)一個(gè)狀態(tài)協(xié)方差矩陣；S4:卡爾曼觀測(cè)控制器根據(jù)所述導(dǎo)航信息、GPS觀測(cè)信息和誤差模型，計(jì)算出所述多個(gè)階段中每個(gè)階段的控制增益；S5:判斷所述控制增益是否滿足利亞普諾夫穩(wěn)定性條件，若不滿足，則返回步驟S3，若滿足，則進(jìn)入步驟S6 ；S6:根據(jù)所述控制增益計(jì)算補(bǔ)償向量，并將所述補(bǔ)償向量反饋給所述慣性測(cè)量單元，并由所述慣性測(cè)量單元進(jìn)行調(diào)整。根據(jù)本發(fā)明實(shí)施例的方法，通過采用卡爾曼觀測(cè)控制器，能夠迅速將航向角誤差壓低到很小的范圍，從而大大降低對(duì)準(zhǔn)時(shí)間，提高對(duì)準(zhǔn)精度。在本發(fā)明的一個(gè)實(shí)施例中，所述誤差模型和卡爾曼觀測(cè)控制器采用至少9維狀態(tài)量。在本發(fā)明的一個(gè)實(shí)施例中，所述誤差模型的狀態(tài)量表示為，
權(quán)利要求
1.一種用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)方法，其特征在于，包括以下步驟: 51:獲取導(dǎo)航信息和GPS觀測(cè)信息，其中，所述導(dǎo)航信息通過慣性測(cè)量單元獲取，并對(duì)所述導(dǎo)航信息進(jìn)行慣性導(dǎo)航解算； 52:建立動(dòng)基座對(duì)準(zhǔn)的誤差模型； 53:建立卡爾曼觀測(cè)控制器，其中，所述卡爾曼觀測(cè)控制器的工作被劃分為多個(gè)階段，且每個(gè)階段對(duì)應(yīng)一個(gè)狀態(tài)協(xié)方差矩陣； 54:卡爾曼觀測(cè)控制器根據(jù)所述導(dǎo)航信息、GPS觀測(cè)信息和誤差模型，計(jì)算出所述多個(gè)階段中每個(gè)階段的控制增益； 55:判斷所述控制增益是否滿足利亞普諾夫穩(wěn)定性條件，若不滿足，則返回步驟S3，若滿足，則進(jìn)入步驟S6 ； 56:根據(jù)所述控制增益計(jì)算補(bǔ)償向量，并將所述補(bǔ)償向量反饋給所述慣性測(cè)量單元，并由所述慣性測(cè)量單元進(jìn)行調(diào)整。
2.如權(quán)利要求1所述的用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)方法，其特征在于，所述誤差模型和卡爾曼觀測(cè)控制器采用至少9維狀態(tài)量。
3.如權(quán)利要求1所述的用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)方法，其特征在于，所述誤差模型的狀態(tài)量表示為，X = [(^jy δφΕ δφ Wn SVe Wjy SL δλ Sh ~f, 其中，N, E, D分別為當(dāng)?shù)貙?dǎo)航坐標(biāo)系的北向、東向和地向，Φ為姿態(tài)角，δ為誤差表示，δφΝ, δφΕ, 分別為北向、 東向和地向的速度誤差，L、λ和h分別為緯度、經(jīng)度和高度為，δ L, δ λ , Sh分別為緯度誤差、經(jīng)度誤差和高度誤差，T為矩陣轉(zhuǎn)置表示。
4.如權(quán)利要求1所述的用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)方法，其特征在于，所述誤差模型通過如下公式表示，其公式為，φ = —εη + δακ +φχ ω1} Ut ' litSVn = -δφη Xfn+C^fb+ SVn X (2 + )fy/ i1.-右+Vn Y.(2*-^ + <%)η ) + V"te enδΚτKt SL =——---5L—δΛ RM+h (RM+hr.5FV κ =---+---XmLBL 十 h) cos十 h)cosL —^l5/3 +hr cosL δ“-1D 其中，^為姿態(tài)微分矢量，ε n為陀螺零偏矢量，為當(dāng)?shù)貙?dǎo)航坐標(biāo)系相對(duì)地心慣性坐標(biāo)系旋轉(zhuǎn)角速度誤差，P為姿態(tài)角奶力當(dāng)?shù)貙?dǎo)航坐標(biāo)系相對(duì)地心慣性坐標(biāo)系旋轉(zhuǎn)角速度，JfV1為速度誤差微分矢量，#"為姿態(tài)角誤差，fb為加速度計(jì)輸出的比力，q為姿態(tài)矩陣，fn為加速度計(jì)輸出的比力在當(dāng)?shù)貙?dǎo)航坐標(biāo)系中的分量，SVn為速度誤差^為地球自轉(zhuǎn)角速度，《 Β為導(dǎo)航系相對(duì)地球系的角速度，Vn為載體相對(duì)地球的速度，V"為加速度計(jì)零偏矢量，為緯度誤差微分，δ Vn為北向速度誤差，Rm和Rn分別為當(dāng)?shù)氐厍蜃游缛兔先Φ那拾霃剑琱為高度，Vn為北向速度，Sh為高度誤差，為經(jīng)度誤差微分，3^為東向速度誤差，Ve為東向速度，δ L為緯度誤差，力高度誤差微分，δ Vd為地向速度誤差,上角標(biāo)η表示該物理量在導(dǎo)航坐標(biāo)系的投影，上角標(biāo)b表示該物理量在載體坐標(biāo)系的投影。
5.如權(quán)利要求1所述的用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)方法，其特征在于，所述卡爾曼觀測(cè)控制器的控制方程通過如下公式表示，其公式為，
6.一種用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)系統(tǒng)，其特征在于，包括: 獲取模塊，用于獲取導(dǎo)航信息和GPS觀測(cè)信息，其中，所述導(dǎo)航信息通過慣性測(cè)量單元獲取，并對(duì)所述導(dǎo)航信息進(jìn)行慣性導(dǎo)航解算； 第一建立模塊，用于建立動(dòng)基座對(duì)準(zhǔn)的誤差模型； 第二建立模塊，用于建立卡爾曼觀測(cè)控制器，其中，所述卡爾曼觀測(cè)控制器的工作被劃分為多個(gè)階段，且每個(gè)階段對(duì)應(yīng)一個(gè)狀態(tài)協(xié)方差矩陣； 計(jì)算模塊，用于卡爾曼觀測(cè)控制器根據(jù)所述導(dǎo)航信息、GPS觀測(cè)信息和誤差模型，計(jì)算出所述多個(gè)階段中每個(gè)階段的控制增益； 判斷模塊，用于判斷所述控制增益是否滿足利亞普諾夫穩(wěn)定性條件，若不滿足，則通過第二建立模塊和計(jì)算模塊重新進(jìn)行處理； 反饋調(diào)整模塊，用于根據(jù)所述控制增益計(jì)算補(bǔ)償向量，并將所述補(bǔ)償向量反饋給所述慣性測(cè)量單元，并由所述慣性測(cè)量單元進(jìn)行調(diào)整。
7.如權(quán)利要求6所述的用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)系統(tǒng)，其特征在于，所述誤差模型和卡爾曼觀測(cè)控制器采用至少9維狀態(tài)量。
8.如權(quán)利要求6所述的用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)系統(tǒng)，其特征在于，所述誤差模型的狀態(tài)量表示為，
9.如權(quán)利要求6所述的用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)系統(tǒng)，其特征在于，所述誤差模型通過如下公式表示，其公式為，
10.如權(quán)利要求6所述的用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)系統(tǒng)，其特征在于，所述卡爾曼觀測(cè)控制器的控制方程通過如下公式表示，其公式為，
全文摘要
本發(fā)明提出一種用于組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)方法及系統(tǒng)。其中，方法包括S1獲取導(dǎo)航信息和GPS觀測(cè)信息；S2建立動(dòng)基座對(duì)準(zhǔn)的誤差模型；S3建立卡爾曼觀測(cè)控制器，其中，卡爾曼觀測(cè)控制器的工作被劃分為多個(gè)階段，且每個(gè)階段對(duì)應(yīng)一個(gè)狀態(tài)協(xié)方差矩陣；S4卡爾曼觀測(cè)控制器根據(jù)導(dǎo)航信息、GPS觀測(cè)信息和誤差模型，計(jì)算每個(gè)階段的控制增益；S5判斷控制增益是否滿足利亞普諾夫穩(wěn)定性條件，若不滿足，返回步驟S3，若滿足，根據(jù)控制增益計(jì)算補(bǔ)償向量，并將補(bǔ)償向量反饋給慣性測(cè)量單元，并由慣性測(cè)量單元進(jìn)行調(diào)整。根據(jù)本發(fā)明實(shí)施例的方法，通過采用卡爾曼觀測(cè)控制器，能夠迅速將航向角誤差壓低到很小的范圍，從而大大降低對(duì)準(zhǔn)時(shí)間，提高對(duì)準(zhǔn)精度。
文檔編號(hào)G01C25/00GK103226022SQ20131010285
公開日2013年7月31日 申請(qǐng)日期2013年3月27日 優(yōu)先權(quán)日2013年3月27日
發(fā)明者郭美鳳, 包超, 張嶸, 劉剛 申請(qǐng)人:清華大學(xué)