r>[0111] h--摜性高度，單位：m;
[0112] ωιβ--地球自轉(zhuǎn)角速率，常數(shù)，取7.292115 XlO 5rad/s ;
[0113] Ψ-緯度，單位：rad ;
[0114] fN、fu、fE-濾波計(jì)算用η系下三軸比力分量,單位：m/s2 ;
[0115] Cn、C21、C31、C12、C22、C 32、C13、C23、C33--姿態(tài)矩陣(t 中的元素；
[0116] h)位置誤差方程
[0118] i)姿態(tài)角誤差方程
[0119]
[0120] j)陀螺和加速度計(jì)誤差方程
[0122] k)航位推算位置誤差方程
[0124] 1)里程計(jì)刻度系數(shù)誤差方程
[0126] 綜合式（21)~（26)，可得系統(tǒng)狀態(tài)方程如下：
[0128] 式中：
[0129] X(t)--系統(tǒng)狀態(tài)變量，即上述的卡爾曼濾波狀態(tài)方程19個(gè)狀態(tài)變量；
[0130] F--根據(jù)公式（21)~（26)確定的系統(tǒng)矩陣；
[0131] ff(t)--系統(tǒng)噪聲；
[0132] (3)確定系統(tǒng)觀測(cè)方程
[0133] 系統(tǒng)觀測(cè)量取導(dǎo)航解算位置與航位推算位置之差，則系統(tǒng)觀測(cè)方程如下：
[0135] 式中：
[0136] Z(t)--觀測(cè)變量；
[0137] H--觀測(cè)矩陣；
[0138] V(t)--觀測(cè)噪聲；
[0139] 爐、λ、h--摜性緯度、經(jīng)度、高度,單位：rad，rad，m ;
[0140] Ψα：^· AD>hD-航位推算纟韋度、經(jīng)度、高度，單位：rad，rad, m ;
[0141] 將公式（27)和（28)離散化，可得：
[0143] 式中：
[0144] Xk--狀態(tài)變量；
[0145] --上一濾波周期至當(dāng)前濾波周期的一步轉(zhuǎn)移矩陣；
[0146] Zk--觀測(cè)量；
[0147] Hk--觀測(cè)矩陣；
[0148] Wk--系統(tǒng)激勵(lì)噪聲序列；
[0149] Vk--觀測(cè)噪聲序列；
[0150] -步轉(zhuǎn)移矩陣〇k，k i可用下式計(jì)算：
[0152] 式中：
[0153] I--單位矩陣；
[0154] Tn--導(dǎo)航周期；
[0155] η--相鄰兩次濾波時(shí)刻之間的導(dǎo)航周期個(gè)數(shù)；
[0156] (4)確定卡爾曼濾波遞推方程組
[0157] 卡爾曼濾波遞推方程組如下：
[0158] f)狀態(tài)一步預(yù)測(cè)
[0160] g)狀態(tài)估計(jì)
[0162] h)濾波增益
[0164] i) 一步預(yù)測(cè)均方誤差
[0166] j)估計(jì)均方誤差
[0169] Kt--當(dāng)前濾波周期的實(shí)時(shí)狀態(tài)估計(jì)值；
[0170] --上一濾波周期對(duì)當(dāng)前濾波周期的狀態(tài)預(yù)測(cè)值；
[0171] Kk--當(dāng)前濾波周期的濾波增益陣；
[0172] Pk/kl--上一濾波周期對(duì)當(dāng)前濾波周期的預(yù)測(cè)誤差估計(jì)的協(xié)方差陣；
[0173] Pk--當(dāng)前濾波周期的實(shí)時(shí)誤差估計(jì)協(xié)方差陣；
[0174] Qk !--系統(tǒng)噪聲方差陣；
[0175] Rk--觀測(cè)噪聲方差陣；
[0176] (5)卡爾曼濾波遞推計(jì)算和反饋修正
[0177] 經(jīng)過(guò)卡爾曼濾波遞推計(jì)算后，估計(jì)出北向失準(zhǔn)角ΦΝ、東向失準(zhǔn)角ΦΕ、X陀螺漂移 ε x、Z陀螺漂移ε ζ ;然后對(duì)其進(jìn)行反饋修正以提高姿態(tài)精；修正方法如下：
[0178] c)姿態(tài)修正，用Cf表示修正后的姿態(tài)矩陣
[0179] 失準(zhǔn)角計(jì)算公式如下：
[0181] Xk(i)，i = 1~19,表示上述的卡爾曼濾波狀態(tài)方程19個(gè)狀態(tài)變量中的第i個(gè)狀 態(tài)變量；
[0182] 姿態(tài)修正公式如下：
[0184] 式中：c代表cos，s代表sin ;
[0186] d)陀螺漂移修正
[0187] 陀螺漂移修正公式如下：
[0189] 將陀螺漂移補(bǔ)償?shù)捷d體系下的角速率中，實(shí)時(shí)參與導(dǎo)航解算：
[0191]式中：
表示導(dǎo)航周期內(nèi)載體坐標(biāo)系下陀螺角速率輸出，單位： rad/so
【主權(quán)項(xiàng)】
1. 一種慣性里程計(jì)組合高精度姿態(tài)測(cè)量方法，其特征在于，包括w下步驟： (1) 確定卡爾曼濾波狀態(tài)方程狀態(tài)變量 卡爾曼濾波狀態(tài)方程取19個(gè)狀態(tài)變量，分別為；北向速度誤差δVw、天向速度誤差δV東向速度誤差δVe、締度誤差卻、高度誤差δK經(jīng)度誤差δλ、北向失準(zhǔn)角Φ,、天向 失準(zhǔn)角Φ。、東向失準(zhǔn)角Φε、Χ巧螺漂移ε、、γ巧螺漂移Ey、Z巧螺漂移ε,、χ加表零偏VyV Y加表零偏^、、Z加表零偏Vf、航位推算締度誤差瑪>。、航位推算高度誤差δh。、航位推算 經(jīng)度誤差δλ。、里程計(jì)刻度系數(shù)誤差δKd; (2) 確定系統(tǒng)各誤差方程 a) 速度誤差方程式中： VVu、Ve--北向、天向、東向速度，單位；m/s; RM-子午圈半徑，單位；m; RN--卯酉圈半徑，單位；m;h--'胺性高度，單位;m; "1。--地球自轉(zhuǎn)角速率，常數(shù)，取7. 292115X10 5rad/s; Ψ--締度，單位；rad; fw、fu、fE-濾波計(jì)算用η系下Η軸比力分量，單位；m/s2 ; 〔11、〔21、〔31、〔12、〔22、〔32、〔13、〔23、C33 姿態(tài)矩陣中的兀素； b) 位置誤差方程C)姿態(tài)角誤差方程f)里程計(jì)刻度系數(shù)誤差方程 δκ,, =0 （6) 綜合式（1)~化），可得系統(tǒng)狀態(tài)方程如下： 乂(〇=_ΡΑ'(/)十林'.（,') ···················· （7) 式中： X(t)-系統(tǒng)狀態(tài)變量，即上述的卡爾曼濾波狀態(tài)方程19個(gè)狀態(tài)變量； F-根據(jù)公式（1)~（6)確定的系統(tǒng)矩陣； W(t)--系統(tǒng)噪聲； (3)確定系統(tǒng)觀測(cè)方程 系統(tǒng)觀測(cè)量取導(dǎo)航解算位置與航位推算位置之差，則系統(tǒng)觀測(cè)方程如下：式中： Z(t)--觀測(cè)變量； Η--觀測(cè)矩陣； V(t)--觀測(cè)噪聲； 界、λ、h--胺性締度、經(jīng)度、高度，單位；rad,rad,m; 從、λD、h。 航似推算締度、經(jīng)度、局度,單似；Γ曰d,rad,m; 將公式（7)和做離散化，可得：式中： Xk--狀態(tài)變量； -上一濾波周期至當(dāng)前濾波周期的一步轉(zhuǎn)移矩陣； \--觀測(cè)量； Hk-觀測(cè)矩陣； Wk-系統(tǒng)激勵(lì)噪聲序列； Vk--觀測(cè)噪聲序列； 一步轉(zhuǎn)移矩陣〇k,k1可用下式計(jì)算：式中： I-單位矩陣； T。一導(dǎo)航周期； η-相鄰兩次濾波時(shí)刻之間的導(dǎo)航周期個(gè)數(shù)； (4)確定卡爾曼濾波遞推方程組 卡爾曼濾波遞推方程組如下： a)狀態(tài)一步預(yù)測(cè)式中： 與一當(dāng)前濾波周期的實(shí)時(shí)狀態(tài)估計(jì)值； .1郵_、-上一濾波周期對(duì)當(dāng)前濾波周期的狀態(tài)預(yù)測(cè)值； Kk-當(dāng)前濾波周期的濾波增益陣； Pk/k1-上一濾波周期對(duì)當(dāng)前濾波周期的預(yù)測(cè)誤差估計(jì)的協(xié)方差陣； Pk-當(dāng)前濾波周期的實(shí)時(shí)誤差估計(jì)協(xié)方差陣； Qki--系統(tǒng)噪聲方差陣； Rk-觀測(cè)噪聲方差陣； (5)卡爾曼濾波遞推計(jì)算和反饋修正 經(jīng)過(guò)卡爾曼濾波遞推計(jì)算后，估計(jì)出北向失準(zhǔn)角Φκ、東向失準(zhǔn)角Φε、Χ巧螺漂移ε、、Ζ巧螺漂移ε,;然后對(duì)其進(jìn)行反饋修正W提高姿態(tài)精度。2.如權(quán)利要求1所述的一種慣性里程計(jì)組合高精度姿態(tài)測(cè)量方法，其特征在于：步驟 (5)中，對(duì)經(jīng)過(guò)卡爾曼濾波遞推計(jì)算后估計(jì)的出北向失準(zhǔn)角Φ,、東向失準(zhǔn)角Φε、Χ巧螺漂移 ε、、Ζ巧螺漂移ε,進(jìn)行反饋修正，修正方法如下： a)姿態(tài)修正，用<1表示修正后的姿態(tài)矩陣 失準(zhǔn)角計(jì)算公式如下：Xk(i)，i= 1~19,表示上述的卡爾曼濾波狀態(tài)方程19個(gè)狀態(tài)變量中的第i個(gè)狀態(tài)變 量； 姿態(tài)修正公式如下：式中： 4。=[巧L端.。份ij表示導(dǎo)航周期內(nèi)載體坐標(biāo)系下巧螺角速率輸出，單位：rad/ So
【專利摘要】本發(fā)明屬于導(dǎo)航方法技術(shù)領(lǐng)域，具體涉及一種慣性/里程計(jì)組合高精度姿態(tài)測(cè)量方法，可應(yīng)用于車(chē)載長(zhǎng)航時(shí)組合導(dǎo)航，復(fù)雜地形測(cè)繪等領(lǐng)域。本發(fā)明技術(shù)方案以航位推算結(jié)果為基準(zhǔn)，進(jìn)行組合卡爾曼濾波計(jì)算，對(duì)水平失準(zhǔn)角和水平陀螺漂移進(jìn)行估計(jì)并修正。其中卡爾曼濾波狀態(tài)方程取19個(gè)狀態(tài)變量，建立了系統(tǒng)誤差模型，利用航位推算結(jié)果，對(duì)水平失準(zhǔn)角和水平陀螺漂移進(jìn)行實(shí)時(shí)在線估計(jì)，并進(jìn)行反饋修正，提高姿態(tài)精度，從而有效的提高了系統(tǒng)組合導(dǎo)航精度。
【IPC分類(lèi)】G01C21/26
【公開(kāi)號(hào)】CN105318876
【申請(qǐng)?zhí)枴緾N201410324640
【發(fā)明人】艾瀛濤, 郭玉勝, 莊廣琛, 馬小艷, 宋雅蘭, 莫明崗
【申請(qǐng)人】北京自動(dòng)化控制設(shè)備研究所
【公開(kāi)日】2016年2月10日
【申請(qǐng)日】2014年7月9日