差狀態(tài)，W速度誤差為觀測(cè)量，構(gòu)建卡爾曼濾波器，對(duì)加速 度計(jì)零偏和偏差禪合造成的北向等效巧螺漂移進(jìn)行在線標(biāo)定補(bǔ)償，對(duì)偏頻旋轉(zhuǎn)軸方向巧螺 漂移進(jìn)行預(yù)先標(biāo)定補(bǔ)償，改善了現(xiàn)有的卡爾曼濾波初始對(duì)準(zhǔn)方位角波動(dòng)幅度大的問題，進(jìn) 一步提高了恒速偏頻激光巧螺慣導(dǎo)系統(tǒng)的導(dǎo)航定位精度；
[0077] (3)IMU與轉(zhuǎn)臺(tái)安裝關(guān)系的精確標(biāo)校，改善水平姿態(tài)角波動(dòng)幅度。
【附圖說明】：
[0078] 1、圖1為本發(fā)明方案的流程圖；
[0079] 2、圖2為滾動(dòng)角收斂曲線對(duì)比圖；
[0080] 3、圖3為俯仰角收斂曲線對(duì)比圖；
[0081] 4、圖4為改進(jìn)卡爾曼濾波與標(biāo)準(zhǔn)卡爾曼濾波初始對(duì)準(zhǔn)方位角收斂曲線對(duì)比圖；
[0082] 5、圖5為4小時(shí)純慣導(dǎo)北向速度誤差曲線對(duì)比圖；
[0083] 6、圖6為4小時(shí)純慣導(dǎo)東向速度誤差曲線對(duì)比圖；
[0084] 7、圖7為4小時(shí)純慣導(dǎo)位置誤差曲線對(duì)比圖；
[0085] 8、圖8為第一組24小時(shí)純慣導(dǎo)速度誤差曲線圖；
[0086] 9、圖9為第一組24小時(shí)純慣導(dǎo)位置誤差曲線圖；
[0087] 10、圖10為第二組24小時(shí)純慣導(dǎo)速度誤差曲線圖；
[0088] 11、圖11為第二組24小時(shí)純慣導(dǎo)位置誤差曲線圖。
【具體實(shí)施方式】：
[0089] 下面結(jié)合附圖對(duì)本發(fā)明中的方法作進(jìn)一步詳細(xì)描述。
[0090] 步驟一:機(jī)械抖動(dòng)偏頻激光巧螺安裝:在Ξ個(gè)巧螺(gx、gy、gz )、Ξ個(gè)石英曉性加速 度計(jì)(ax、ay、az)構(gòu)成的高精度激光巧螺恒速偏頻尋北系統(tǒng)的基礎(chǔ)上，安裝一個(gè)不隨偏頻機(jī) 構(gòu)旋轉(zhuǎn)的固定在基座上的機(jī)械抖動(dòng)偏頻激光巧螺gdz，其敏感軸與旋轉(zhuǎn)軸一致，提供偏頻旋 轉(zhuǎn)軸方向的載體角運(yùn)動(dòng)信息；
[0091 ]步驟二:坐標(biāo)系及安裝關(guān)系定義：
[0092] 定義η系為當(dāng)?shù)厮降乩碜鴺?biāo)系，N-E-D(北-東-地)方向；
[OOW] bg系為斜裝的恒速偏頻IMU(慣性測(cè)量單元)坐標(biāo)系，其\軸與gx巧螺敏感軸重合， 軸在gj它螺與gy巧螺敏感軸所確定的平面內(nèi)且gy巧螺敏感軸方向偏離馬軸一個(gè)小的安 裝偏差角，\軸方向符合右手規(guī)則且gz巧螺敏感軸方向偏離\軸一個(gè)小的安裝偏差角； [0094] b系為變換后的IMU坐標(biāo)系，采用前-右-下方向，Zb軸與恒速偏頻旋轉(zhuǎn)軸重合，xb軸、 yb軸與偏頻旋轉(zhuǎn)軸正交，孔軸與·\軸及Zb軸在同一平面內(nèi)，與η系間的方向余弦矩陣為句; [00%] bg系、b系固聯(lián)于旋轉(zhuǎn)臺(tái)面中并隨轉(zhuǎn)臺(tái)連續(xù)轉(zhuǎn)動(dòng)，轉(zhuǎn)速Ω(轉(zhuǎn)臺(tái)旋轉(zhuǎn)軸指向下方J員 時(shí)針方向?yàn)檎芷赥，t時(shí)刻與轉(zhuǎn)臺(tái)零位夾角a(t);
[0096] bp系為轉(zhuǎn)臺(tái)臺(tái)體坐標(biāo)系，與b系在t = 0時(shí)刻重合，與η系間的方向余弦矩陣為;
[0097] 為b系與bp系之間的轉(zhuǎn)換矩陣，可由α (t)表示為：
[009引
（25)
[0099 ]步驟Ξ:偏頻旋轉(zhuǎn)軸方向等效巧螺采樣值計(jì)算：
[0100] b系下偏頻旋轉(zhuǎn)軸方向敏感角速率6^可表示為，
[0101]
(26)
[0102] 式中，(為是b系下載體角速度，bp系中機(jī)械抖動(dòng)激光巧螺敏感軸單位矢量為[kdx kdy kdz]T，其敏感角速率理論值為Qdz，上標(biāo)T表示向量或矩陣的轉(zhuǎn)置，At是采樣時(shí)間間隔；
[0103] 由式(2)積分可得相應(yīng)的巧螺角增量采樣值，根據(jù)b系中恒速偏頻激光巧螺IMU提 供的垂直于旋轉(zhuǎn)軸的角增量
固聯(lián)于bp系中的機(jī)抖巧螺的角增量
偏頻旋轉(zhuǎn)機(jī)構(gòu)的角位置a(t)和角增量Aa(t)，可計(jì)算出b系沿偏頻旋轉(zhuǎn)軸方向的 角增量
[0104]偏頻旋轉(zhuǎn)軸方向等效巧螺采樣值計(jì)算中《4求解方法為：
[01化]機(jī)抖巧螺敏感角速率理論值可表示為：
[0109] 把式(25)代入式(28)變換后可得為，的表達(dá)式；
[0110] 步驟四：精確標(biāo)校bg系與b系之間的安裝關(guān)系矩陣Ci ;設(shè)置bg系Ξ個(gè)恒速偏頻激光 巧螺工作在機(jī)械抖動(dòng)偏頻模式下，利用水平姿態(tài)角γ (滾動(dòng)角）和Θ(俯仰角）的計(jì)算結(jié)果對(duì) 皆進(jìn)行精確標(biāo)校；
[0111] 精確標(biāo)校具體方法為：
[0112] 設(shè)真實(shí)的IMU坐柄;系為t/系，則有：
[0113]
(29)
[0114] 式中，<5^是6。與η系間的方向余弦矩陣的真實(shí)值，可表示為：
[0115]
心〇)
[0116] 巧是b/系與η系間的方向余弦矩陣，可表示為：
[0117]
(31;
[0118] 將式(30)和代(31)入式(29)兩邊同時(shí)消去C：。得：
[0119]
(32)
[0120] 設(shè)ξχ和ξγ為b系和1/系之間的轉(zhuǎn)換的小角度偏差，則
[0121]
(芽3)
[0122] 將式(25)和式(33)代入式(32)解得：
[0123]
(34)
[0124] 靜基座下分別選取α (t) = 0和α (t) = π兩個(gè)時(shí)刻，從而解得：
[0125]
(35)
[0126] 則可根據(jù)式=:皆對(duì)€^進(jìn)行精確標(biāo)校；
[0127] 步驟五:精確標(biāo)校機(jī)械抖動(dòng)激光巧螺敏感軸的單位矢量比dx kdy kdz]:設(shè)置Ξ個(gè)恒 速偏頻激光巧螺工作在機(jī)械抖動(dòng)偏頻模式下，(?·保持不變時(shí)有：
[012引
餅）
[0129] 式中，ω|表示bp系的旋轉(zhuǎn)角速率；
[0130] 整個(gè)慣導(dǎo)系統(tǒng)按照不同的傾斜方式置于單軸轉(zhuǎn)臺(tái)上，單軸轉(zhuǎn)臺(tái)正反方向旋轉(zhuǎn)，消 除地球自轉(zhuǎn)角速度及巧螺常值漂移誤差影響，根I
即 可估計(jì)出參數(shù)[kdx kdy kdz];、
[0131 ]步驟六:初始對(duì)準(zhǔn)卡爾曼濾波器設(shè)計(jì)：
[0132] 選取Ξ個(gè)姿態(tài)誤差（Φν、Φε、Φο)和Ξ個(gè)速度誤差(SVn、SVe、SVd)作為誤差狀態(tài)，器 件誤差中，選取Ξ個(gè)加速度計(jì)零偏^<^乂6、<^/,6、（^大6)作為誤差狀態(tài)，選取偏頻旋轉(zhuǎn)軸方 向等效巧螺采樣值常值零偏&遍g、偏差禪合造成的北向等效巧螺漂移εν作為誤差狀態(tài)，即
[0133]
(37)
[0134] 構(gòu)建系統(tǒng)狀態(tài)方程如下：
[0135]
(38)
[0136] 式中，X表不系統(tǒng)誤差狀態(tài)，表不系統(tǒng)誤差狀態(tài)微分，F(xiàn)表不系統(tǒng)誤差矩陣，G表不 系統(tǒng)噪聲輸入矩陣，W表不系統(tǒng)噪聲：
[0137] w=[wgx Wgy Wgz Wax Way Waz]^ (39)
[013引式中，Wgx、Wgy和Wgz為1?螺測(cè)量噪聲，Wax、Way、Waz為加速度計(jì)測(cè)量噪聲；
[0139] 選取η系下速度誤差SVn、SVe和SVd作為觀測(cè)量，構(gòu)建觀測(cè)方程如下：
[0140] z = Hx+u (40)
[0141] 式中，Z為觀測(cè)量，Η為測(cè)量矩陣，U為測(cè)量噪聲；
[0142] 卡爾曼濾波系統(tǒng)狀態(tài)方程和觀測(cè)方程構(gòu)建方法為：
[0143] 系統(tǒng)狀態(tài)方程構(gòu)建如下：
[0144]
(41)
[0145] 式中，φη=[φΝ Φε Φο]τ代表失準(zhǔn)角，產(chǎn)表示η系下的比力值，<是η系的旋轉(zhuǎn)角 速率；
[0146] 根據(jù)系統(tǒng)誤差方程得到的系統(tǒng)狀態(tài)方程i = Fx + Gw中系統(tǒng)誤差矩陣F和系統(tǒng)噪聲 輸入矩陣G相應(yīng)為：
[0149] 卡爾曼濾波觀測(cè)方程構(gòu)建如下：
[0150] 觀測(cè)量z = [SVn 5Ve δν0]τ，則在觀測(cè)方程z = Hx+u中：
[0151]
(44)
[0152] υ = |>Ν 化化]T (45)
[0153] 步驟屯:純慣導(dǎo)誤差修正補(bǔ)償：
[0154] 利用初始對(duì)準(zhǔn)卡爾曼濾波對(duì)W&、《/,!'、《於、歲遂，和εν的估計(jì)值，在純慣導(dǎo)過 程中進(jìn)行修正補(bǔ)償，完成導(dǎo)航解算，修正補(bǔ)償方法如下：
[0158] 下面是利用實(shí)驗(yàn)室自主設(shè)計(jì)的高精度恒速偏頻激光巧螺尋北系統(tǒng)對(duì)本文提出的 參數(shù)標(biāo)校方法、初始對(duì)準(zhǔn)算法和純慣導(dǎo)精度進(jìn)行半實(shí)物仿真驗(yàn)證評(píng)估。方法如下：
[0159] 1)在實(shí)驗(yàn)室內(nèi)進(jìn)行恒速偏頻靜基座實(shí)驗(yàn)，轉(zhuǎn)臺(tái)轉(zhuǎn)速設(shè)定為40°/s，采樣頻率設(shè)定為 500Hz，采集5組實(shí)際的5小時(shí)數(shù)據(jù)，兩組實(shí)際的24小時(shí)數(shù)據(jù)。
[0160] 2)機(jī)抖巧螺角增量輸出從機(jī)械抖動(dòng)激光巧螺靜態(tài)數(shù)據(jù)中獲得，采用理論值加機(jī)抖 巧螺靜態(tài)噪聲和漂移實(shí)際采樣數(shù)據(jù)的方式仿真生成。
[