一種慣性里程計組合高精度姿態(tài)測量方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于導(dǎo)航方法技術(shù)領(lǐng)域，具體涉及一種慣性/里程計組合高精度姿態(tài)測量 方法，可應(yīng)用于車載長航時組合導(dǎo)航，復(fù)雜地形測繪等領(lǐng)域。
【背景技術(shù)】
[0002] 對自主性、可靠性、精度等要求高的車輛導(dǎo)航應(yīng)用場合，常使用多傳感器組合導(dǎo) 航系統(tǒng)，尤其在軍用場合，組合導(dǎo)航更是首選方案。慣性導(dǎo)航系統(tǒng)可以完全自主地提供全 面的導(dǎo)航信息，但其誤差會隨著時間不斷積累。里程計作為車載導(dǎo)航系統(tǒng)中十分重要的組 成部分，其特點是成本較低，且誤差不隨時間積累，可以測得較精確的車速或里程，所以可 以將慣導(dǎo)系統(tǒng)與里程計進(jìn)行組合，以取得較高精度的測量結(jié)果。
[0003] 慣性/里程計組合的常用方法為航位推算，即利用慣導(dǎo)系統(tǒng)進(jìn)行姿態(tài)測量，利用 里程計進(jìn)行速度或路程測量，并經(jīng)過刻度系數(shù)修正后，借助慣導(dǎo)系統(tǒng)的姿態(tài)信息，從載體坐 標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系，從而得到載體不隨時間發(fā)散的、較精確的位置信息。該方法的精 度主要受里程計刻度系數(shù)和姿態(tài)精度的影響，因此如何提高姿態(tài)測量精度，成為影響慣性/ 里程計組合導(dǎo)航精度的主要因素。
[0004] 綜上，亟需提出一種基于慣性/里程計的高精度姿態(tài)測量方法，將航位推算結(jié)果 和慣性導(dǎo)航解算結(jié)果進(jìn)行組合，利用卡爾曼濾波算法對慣導(dǎo)系統(tǒng)姿態(tài)誤差和陀螺漂移誤差 進(jìn)行估計并修正，從而提高系統(tǒng)姿態(tài)測量精度。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明要解決的技術(shù)問題是提出一種慣性/里程計組合高精度姿態(tài)測量方法，對 慣導(dǎo)系統(tǒng)姿態(tài)誤差和陀螺漂移誤差進(jìn)行估計并修正，提高系統(tǒng)姿態(tài)測量精度，進(jìn)而提高系 統(tǒng)組合導(dǎo)航精度。
[0006] 為了實現(xiàn)這一目的，本發(fā)明采取的技術(shù)方案是：
[0007] -種慣性里程計組合高精度姿態(tài)測量方法，包括以下步驟：
[0008] (1)確定卡爾曼濾波狀態(tài)方程狀態(tài)變量
[0009] 卡爾曼濾波狀態(tài)方程取19個狀態(tài)變量，分別為：北向速度誤差δ Vn、天向速度誤 差δ Vu、東向速度誤差δ VE、諱度誤差知、商度誤差δ h、經(jīng)度誤差δ λ、北向失準(zhǔn)角φ Ν、天 向失準(zhǔn)角、東向失準(zhǔn)角ΦΕ、Χ陀螺漂移εχ、γ陀螺漂移e y、Z陀螺漂移εζ、χ加表零偏 Vi.、Y加表零偏％、Z加表零偏1、航位推算緯度誤差灸^、航位推算高度誤差δ hD、航位 推算經(jīng)度誤差S λ D、里程計刻度系數(shù)誤差SKD;
[0010] (2)確定系統(tǒng)各誤差方程
[0011] a)速度誤差方程
[0012]
[0013] 式中：
[0014] VN、Vu、VE-北向、天向、東向速度，單位：m/s ;
[0015] RM-子午圈半徑，單位：m ;
[0016] RN-卯酉圈半徑，單位：m ;
[0017] h--摜性高度，單位：m;
[0018] ωιβ--地球自轉(zhuǎn)角速率，常數(shù)，取7.292115 XlO 5rad/s ;
[0019] φ-緯度，單位：rad ;
[0020] fN、fu、fE-濾波計算用n系下三軸比力分量，單位：m/s2 ;
[0021] Cn、C21、C31、C12、C22、C 32、C13、C23、C33--姿態(tài)矩陣< 中的元素；
[0022] b)位置誤差方程
[0024] c)姿態(tài)角誤差方程
[0026] d)陀螺和加速度計誤差方程
[0028] e)航位推算位置誤差方程
[0030] f)里程計刻度系數(shù)誤差方程
[0032] 綜合式（1)~（6)，可得系統(tǒng)狀態(tài)方程如下：
[0034] 式中：
[0035] X(t)--系統(tǒng)狀態(tài)變量，即上述的卡爾曼濾波狀態(tài)方程19個狀態(tài)變量；
[0036] F--根據(jù)公式（1)~（6)確定的系統(tǒng)矩陣；
[0037] ff(t)--系統(tǒng)噪聲；
[0038] (3)確定系統(tǒng)觀測方程
[0039] 系統(tǒng)觀測量取導(dǎo)航解算位置與航位推算位置之差，則系統(tǒng)觀測方程如下：
[0041] 式中：
[0042] Z(t)--觀測變量；
[0043] H--觀測矩陣；
[0044] V(t)--觀測噪聲；
[0045] Ψ、\、h--摜性緯度、經(jīng)度、高度,單位：rad，rad，m ;
[0046] Ψο '' AD>hD-航位推算諱度、經(jīng)度、高度，單位：rad，rad, m ;
[0047] 將公式（7)和⑶離散化，可得：
[0049] 式中：
[0050] Xk--狀態(tài)變量；
[0051] --上一濾波周期至當(dāng)前濾波周期的一步轉(zhuǎn)移矩陣；
[0052] Zk--觀測量；
[0053] Hk--觀測矩陣；
[0054] Wk--系統(tǒng)激勵噪聲序列；
[0055] Vk--觀測噪聲序列；
[0056] 一步轉(zhuǎn)移矩陣〇k,k i可用下式計算：
[0058] 式中：
[0059] I--單位矩陣；
[0060] Tn--導(dǎo)航周期；
[0061] η--相鄰兩次濾波時刻之間的導(dǎo)航周期個數(shù)；
[0062] (4)確定卡爾曼濾波遞推方程組
[0063] 卡爾曼濾波遞推方程組如下：
[0064] a)狀態(tài)一步預(yù)測
[0066] b)狀態(tài)估計
[0068] c)濾波增益
[0070] d) -步預(yù)測均方誤差
[0072] e)估計均方誤差
[0074] 式中：
[0075] Xh--當(dāng)前濾波周期的實時狀態(tài)估計值；
[0076] --上一濾波周期對當(dāng)前濾波周期的狀態(tài)預(yù)測值；
[0077] Kk--當(dāng)前濾波周期的濾波增益陣；
[0078] Pk/kl--上一濾波周期對當(dāng)前濾波周期的預(yù)測誤差估計的協(xié)方差陣；
[0079] Pk--當(dāng)前濾波周期的實時誤差估計協(xié)方差陣；
[0080] Qk!--系統(tǒng)噪聲方差陣；
[0081] Rk--觀測噪聲方差陣；
[0082] (5)卡爾曼濾波遞推計算和反饋修正
[0083] 經(jīng)過卡爾曼濾波遞推計算后，估計出北向失準(zhǔn)角ΦΝ、東向失準(zhǔn)角ΦΕ、X陀螺漂移 ε x、Z陀螺漂移ε ζ ;然后對其進(jìn)行反饋修正以提高姿態(tài)精；修正方法如下：
[0084] a)姿態(tài)修正，用Cf表示修正后的姿態(tài)矩陣
[0085] 失準(zhǔn)角計算公式如下：
[0087] Xk (i)，i = 1~19,表示上述的卡爾曼濾波狀態(tài)方程19個狀態(tài)變量中的第i個狀 態(tài)變量；
[0088] 姿態(tài)修正公式如下：
[0090] 式中：c代表cos, s代表sin ;
[0092] b)陀螺漂移修正
[0093] 陀螺漂移修正公式如下：
[
[0095] 將陀螺漂移補(bǔ)償?shù)捷d體系下的角速率中，實時參與導(dǎo)航解算：
[
[0097]式中：
表示導(dǎo)航周期內(nèi)載體坐標(biāo)系下陀螺角速率輸出，單位： rad/s〇
[0099] 本發(fā)明提出了一種慣性/里程計組合高精度姿態(tài)測量方法，主要測量車輛三維姿 態(tài)、三維速度、三維位置等導(dǎo)航信息。本發(fā)明技術(shù)方案以航位推算結(jié)果為基準(zhǔn)，進(jìn)行組合卡 爾曼濾波計算，對水平失準(zhǔn)角和水平陀螺漂移進(jìn)行估計并修正。其中卡爾曼濾波狀態(tài)方程 取19個狀態(tài)變量，建立了系統(tǒng)誤差模型，利用航位推算結(jié)果，對水平失準(zhǔn)角和水平陀螺漂 移進(jìn)行實時在線估計，并進(jìn)行反饋修正，提高姿態(tài)精度，從而有效的提高了系統(tǒng)組合導(dǎo)航精 度。
【具體實施方式】
[0100] 下面結(jié)合【具體實施方式】對本發(fā)明技術(shù)方案進(jìn)行進(jìn)一步詳細(xì)說明。
[0101] -種慣性里程計組合高精度姿態(tài)測量方法，包括以下步驟：
[0102] (1)確定卡爾曼濾波狀態(tài)方程狀態(tài)變量
[0103] 卡爾曼濾波狀態(tài)方程取19個狀態(tài)變量，分別為：北向速度誤差δ Vn、天向速度誤 差δ Vu、東向速度誤差δ VE、諱度誤差:辱、商度誤差δ h、經(jīng)度誤差δ λ、北向失準(zhǔn)角φΝ、天 向失準(zhǔn)角、東向失準(zhǔn)角ΦΕ、Χ陀螺漂移εχ、γ陀螺漂移e y、Z陀螺漂移εζ、χ加表零偏 ▽ Xi、Y加表零偏\、Z加表零偏兄2、航位推算緯度誤差和>〇、航位推算高度誤差δ hD、航位 推算經(jīng)度誤差S λ D、里程計刻度系數(shù)誤差SKD;
[0104] (2)確定系統(tǒng)各誤差方程
[0105] g)速度誤差方程
[0107] 式中：
[0108] VN、Vu、VE-北向、天向、東向速度，單位：m/s ;
[0109] RM-子午圈半徑，單位：m ;
[0110] RN-卯酉圈半徑，單位：m;