專利名稱：一種慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法
技術(shù)領(lǐng)域：
本發(fā)明屬于導(dǎo)航技術(shù)領(lǐng)域，具體涉及一種慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法。
背景技術(shù)：
在陸用定位定向應(yīng)用領(lǐng)域，國(guó)內(nèi)外對(duì)高精度車載定位定向系統(tǒng)都有著迫切的需求。其中，將慣導(dǎo)系統(tǒng)與GNSS、里程計(jì)進(jìn)行組合導(dǎo)航是目前陸用定位定向系統(tǒng)所普遍采用的方法。然而，由于GNSS信息易受干擾，目前國(guó)內(nèi)外典型的陸用定位定向系統(tǒng)產(chǎn)品所采用的主要工作模式為慣導(dǎo)/里程計(jì)組合導(dǎo)航，兼容零速修正技術(shù)，獲得較高的定位定向精度，并具有完全自主性。目前，慣導(dǎo)/里程計(jì)組合導(dǎo)航普遍采用里程計(jì)速度信息與慣導(dǎo)進(jìn)行組合導(dǎo)航，但由于里程計(jì)輸出位移增量脈沖信號(hào)，且存在刻度系數(shù)誤差，由位移增量信息進(jìn)行微分得到的速度信息后，存在較大的速度噪聲，且不能保證速度信息的實(shí)時(shí)性和準(zhǔn)確性，從而影響自主導(dǎo)航精度。

發(fā)明內(nèi)容
本發(fā)明要解決的技術(shù)問(wèn)題為:現(xiàn)有慣導(dǎo)/里程計(jì)組合導(dǎo)航方法中里程計(jì)量化噪聲過(guò)大，降低了組合導(dǎo)航精度。本發(fā)明的技術(shù)方案如下所述:
一種慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法，包括以下步驟:步驟(I)進(jìn)行慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)，得到慣導(dǎo)系統(tǒng)初始姿態(tài)；慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)結(jié)束后轉(zhuǎn)入慣導(dǎo)/里程計(jì)組合導(dǎo)航狀態(tài)，對(duì)卡爾曼濾波器參數(shù)進(jìn)行初始化；步驟(2)進(jìn)行慣性導(dǎo)航計(jì)算，同時(shí)采集里程計(jì)脈沖信號(hào)，解算慣導(dǎo)速度、位置、姿態(tài)信息，里程計(jì)姿態(tài)陣，并將里程計(jì)脈沖信息轉(zhuǎn)換為位移信息，計(jì)算里程計(jì)航位推算位移增量；同時(shí)計(jì)算離散狀態(tài)轉(zhuǎn)移矩陣和量測(cè)矩陣；步驟(3)導(dǎo)航計(jì)算每到卡爾曼濾波周期時(shí)刻，采用基于位移誤差的求和測(cè)量，計(jì)算濾波觀測(cè)量；步驟(4)進(jìn)行里程計(jì)信息故障檢測(cè)；步驟(5)進(jìn)行卡爾曼濾波；步驟(6)對(duì)慣導(dǎo)姿態(tài)陣、速度、位置誤差進(jìn)行修正，對(duì)里程計(jì)姿態(tài)陣進(jìn)行修正；對(duì)里程計(jì)刻度系數(shù)進(jìn)行修正。本方法的坐標(biāo)系定義如下:η:導(dǎo)航坐標(biāo)系oxyz,北天東地理坐標(biāo)系，χ軸指北，y軸指天，z軸指東；n':計(jì)算導(dǎo)航坐標(biāo)系o' χ' γ' Z' ,χ'軸指北，y'軸指天，Z'軸指東；b:慣導(dǎo)載體系ο" χ" y" z"，與陀螺坐標(biāo)系重合，前上右坐標(biāo)系，χ"軸指向前，y"軸朝上，z"軸指右；m:里程計(jì)坐標(biāo)系ο"，χ"，y"，z"，，前上右坐標(biāo)系，χ"，軸指向前，y"，軸朝上，z",軸指右。

步驟(1)中，慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)方法采用靜基座對(duì)準(zhǔn)或動(dòng)基座對(duì)準(zhǔn)方法。步驟(2)中，設(shè)里程計(jì)位移輸出在里程計(jì)坐標(biāo)系m中表示為:ASm = [ASx O 0]τ (I)
則里程計(jì)位移在導(dǎo)航坐標(biāo)系η中表示為:Mn = CnmASm(2)里程計(jì)導(dǎo)航坐標(biāo)系η位移誤差方程:
權(quán)利要求
1.一種慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法，其特征在于:包括以下步驟: 步驟(I) 進(jìn)行慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)，得到慣導(dǎo)系統(tǒng)初始姿態(tài)；慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)結(jié)束后轉(zhuǎn)入慣導(dǎo)/里程計(jì)組合導(dǎo)航狀態(tài)，對(duì)卡爾曼濾波器參數(shù)進(jìn)行初始化； 步驟(2) 進(jìn)行慣性導(dǎo)航計(jì)算，同時(shí)采集里程計(jì)脈沖信號(hào)，解算慣導(dǎo)速度、位置、姿態(tài)信息，里程計(jì)姿態(tài)陣，并將里程計(jì)脈沖信息轉(zhuǎn)換為位移信息，計(jì)算里程計(jì)航位推算位移增量；同時(shí)計(jì)算離散狀態(tài)轉(zhuǎn)移矩陣和量測(cè)矩陣； 步驟(3) 導(dǎo)航計(jì)算 每到卡爾曼濾波周期時(shí)刻，采用基于位移誤差的求和測(cè)量，計(jì)算濾波觀測(cè)量; 步驟(4) 進(jìn)行里程計(jì)信息故障檢測(cè)； 步驟(5) 進(jìn)行卡爾曼濾波； 步驟(6) 對(duì)慣導(dǎo)姿態(tài)陣、速度、位置誤差進(jìn)行修正，對(duì)里程計(jì)姿態(tài)陣進(jìn)行修正；對(duì)里程計(jì)刻度系數(shù)進(jìn)行修正。
2.根據(jù)權(quán)利要求1所述的慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法，其特征在于:本方法的坐標(biāo)系定義如下: η:導(dǎo)航坐標(biāo)系oxyz,北天東地理坐標(biāo)系，X軸指北，y軸指天，z軸指東；n/:計(jì)算導(dǎo)航坐標(biāo)系o' X' γ' z' ,X'軸指北，y'軸指天，z'軸指東；b:慣導(dǎo)載體系ο" X" y" z"，與陀螺坐標(biāo)系重合，前上右坐標(biāo)系，X"軸指向前，y"軸朝上，z"軸指右； m:里程計(jì)坐標(biāo)系ο"，X"，y"，z"，，前上右坐標(biāo)系，X"，軸指向前，y"，軸朝上，z",軸指右。
3.根據(jù)權(quán)利要求2所述的慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法，其特征在于: 步驟(I)中，慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)方法采用靜基座對(duì)準(zhǔn)或動(dòng)基座對(duì)準(zhǔn)方法。
4.根據(jù)權(quán)利要求2或3所述的慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法，其特征在于:步驟(2)中， 設(shè)里程計(jì)位移輸出在里程計(jì)坐標(biāo)系m中表示為: ASm = [ASx O 0]τ (I) 則里程計(jì)位移在導(dǎo)航坐標(biāo)系η中表示為:Mn = CnmASm(2) 里程計(jì)導(dǎo)航坐標(biāo)系η位移誤差方程:Δ分=CnmASm = Q'ASm = Cnn CnmC^(I+ SKD)ASm (3)^Α5η-φχ ASn -Q[ASmx^a + SK0ASn可得: = MX" [AT X桃 + SK0ASn(4)式中:ASm:理想里程計(jì)坐標(biāo)系m輸出位移；ASx:理想里程計(jì)輸出載體前向位移；ASn:理想里程計(jì)坐標(biāo)系m輸出位移在導(dǎo)航坐標(biāo)系η中的表示；ASn=[AS^DO ΑΞΓ Δ和1^ ]、實(shí)際里程計(jì)坐標(biāo)系m輸出位移在導(dǎo)航坐標(biāo)系η投影；:慣導(dǎo)載體系b到計(jì)算導(dǎo)航坐標(biāo)系η,的計(jì)算姿態(tài)轉(zhuǎn)移矩陣；.:實(shí)際里程計(jì)坐標(biāo)系m輸出位移；Cf:理想導(dǎo)航坐標(biāo)系η到計(jì)算導(dǎo)航坐標(biāo)系η'的姿態(tài)轉(zhuǎn)移矩陣；C:::里程計(jì)坐標(biāo)系m到理想導(dǎo)航坐標(biāo)系η的姿態(tài)轉(zhuǎn)移矩陣；0:::計(jì)算里程計(jì)坐標(biāo)系m到計(jì)算導(dǎo)航坐標(biāo)系n'的姿態(tài)轉(zhuǎn)移矩陣；C::慣導(dǎo)載體系b到里程計(jì)坐標(biāo)系m的姿態(tài)轉(zhuǎn)移矩陣；δ Kd:里程計(jì)刻度系數(shù)誤差；Φ = [ΦΝ Φυ ΦΕ]Τ:慣導(dǎo)失準(zhǔn)角SAS^里程計(jì)坐標(biāo)系m位移誤差在導(dǎo)航坐標(biāo)系η中的投影分量；(K= [ΦΜ Ctay (KJt:里程計(jì)坐標(biāo)系m與慣導(dǎo)載體系b之間X軸安裝誤差角，y軸安裝誤差角，z軸安裝偏差角； 設(shè)計(jì)如下慣導(dǎo)/里程計(jì)組合導(dǎo)航系統(tǒng)誤差模型:
5.根據(jù)權(quán)利要求4所述的慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法，其特征在于:步驟(3)中，所述濾波觀測(cè)量通過(guò)下式計(jì)算:
6.根據(jù)權(quán)利要求5所述的慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法，其特征在于:步驟(3)中，所述濾波周期為Is。
7.根據(jù)權(quán)利要求5所述的慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法，其特征在于:步驟(4)中，采用量測(cè)序列檢測(cè)器隔離里程計(jì)故障信息； 定義殘差: -HkXklk^，其中Zk為觀測(cè)量，Hk為量測(cè)矩陣，名丨為一步預(yù)測(cè)狀態(tài)陣； 當(dāng)無(wú)故障發(fā)生時(shí)，卡爾曼濾波器的殘差rk是零均值白噪聲，其方差為:Ak=HkPklkJIl+R(10) 其中，Pk/Η為卡爾曼濾波一步預(yù)測(cè)協(xié)方差矩陣；R為量測(cè)噪聲協(xié)方差陣。
而當(dāng)量測(cè)發(fā)生故障時(shí)，殘差rk的均值不再為零，通過(guò)對(duì)殘差rk的檢驗(yàn)確定系統(tǒng)是否發(fā)生故障； 故障監(jiān)測(cè)函數(shù)為:
8.根據(jù)權(quán)利要求7所述的慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法，其特征在于:步驟(4)中，Td = 0.2 ο
9.根據(jù)權(quán)利要求7所述的慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法，其特征在于:步驟(5)中，卡爾曼濾波方程如下所示:
10.根據(jù)權(quán)利要求9所述的慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法，其特征在于:步驟(6)中， 對(duì)里程計(jì)姿態(tài)陣進(jìn)行修正，修正方法如下式所示:
全文摘要
本發(fā)明屬于導(dǎo)航技術(shù)領(lǐng)域，具體涉及一種慣導(dǎo)/里程計(jì)自主組合導(dǎo)航方法。本方法包括以下步驟(1)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)，對(duì)卡爾曼濾波器參數(shù)進(jìn)行初始化；(2)進(jìn)行慣性導(dǎo)航計(jì)算，得到離散狀態(tài)轉(zhuǎn)移矩陣和量測(cè)矩陣；(3)基于位移誤差的求和測(cè)量，計(jì)算濾波觀測(cè)量；(4)里程計(jì)信息故障檢測(cè)；(5)卡爾曼濾波；(6)修正。本發(fā)明的方法解決了現(xiàn)有慣導(dǎo)/里程計(jì)組合導(dǎo)航方法中里程計(jì)量化噪聲過(guò)大，降低組合導(dǎo)航精度的技術(shù)問(wèn)題；采用基于位移積分匹配慣性/里程計(jì)組合導(dǎo)航策略，摒棄了傳統(tǒng)的利用位移計(jì)算速度作為觀測(cè)量時(shí)引入的量化噪聲，顯著提高了組合導(dǎo)航系統(tǒng)的可靠性。
文檔編號(hào)G01C22/00GK103217157SQ20121001641
公開(kāi)日2013年7月24日 申請(qǐng)日期2012年1月18日 優(yōu)先權(quán)日2012年1月18日
發(fā)明者吳亮華, 鄭辛, 李群, 蔡善軍, 石志興, 馬二杰, 朱紅 申請(qǐng)人:北京自動(dòng)化控制設(shè)備研究所