專利名稱：一種完全自主的相對慣性導(dǎo)航方法
技術(shù)領(lǐng)域：
本發(fā)明屬于慣性導(dǎo)航技術(shù)領(lǐng)域，具體涉及一種完全自主的相對慣性導(dǎo)航方法，用于在衛(wèi)星導(dǎo)航系統(tǒng)不可用或水下航行器導(dǎo)航系統(tǒng)故障重啟等特殊情況下快速建立應(yīng)急導(dǎo)航能力。
背景技術(shù)：
在正常情況下，慣導(dǎo)系統(tǒng)工作流程為裝訂初始位置、進(jìn)行初始對準(zhǔn)、在此基礎(chǔ)上進(jìn)行航位推算；初始位置是慣導(dǎo)系統(tǒng)進(jìn)行對準(zhǔn)、導(dǎo)航的基本信息。但是，若由于某種原因無法得到準(zhǔn)確的初始位置，如野外作戰(zhàn)GPS被干擾或水下航行器慣導(dǎo)系統(tǒng)故障后重啟。如何在這種情況下進(jìn)行導(dǎo)航成為亟需要解決的問題。

發(fā)明內(nèi)容
本發(fā)明的目的是提出一種完全自主的相對慣性導(dǎo)航方法，可在由于某種原因無法獲得準(zhǔn)確初始位置的特殊情況下，自主確定方位角和緯度。本發(fā)明所采用的技術(shù)方案是一種完全自主的相對慣性導(dǎo)航方法，包括如下步驟設(shè)置初始的航向角與姿態(tài)角均為0°，初始經(jīng)緯度均為0° ;采用陀螺和加速度計輸出的角速度和比力，進(jìn)行導(dǎo)航解算的步驟；估計、修正初始誤差步驟以公式(I)為誤差模型，以速度和位置相對誤差為觀測量，來估計并修正初始航向角、姿態(tài)角及緯度的誤差；
W
權(quán)利要求
1.一種完全自主的相對慣性導(dǎo)航方法，包括如下步驟 設(shè)置初始的航向角與姿態(tài)角均為0°，初始經(jīng)緯度均為0° ;采用陀螺和加速度計輸出的角速度和比力，進(jìn)行導(dǎo)航解算的步驟；估計、修正初始誤差步驟以公式(I)為誤差模型，以速度和位置相對誤差為觀測量，來估計并修正初始航向角、姿態(tài)角及緯度的誤差；
2.根據(jù)權(quán)利要求I所述的一種完全自主的相對慣性導(dǎo)航方法，其特征在于在估計、修正初始誤差步驟之后，獲得準(zhǔn)確的外界位置信息之前，采用陀螺、加速度計輸出進(jìn)行導(dǎo)航解算；當(dāng)獲得準(zhǔn)確外界位置信息后，將經(jīng)度和緯度信息重置為該準(zhǔn)確值。
3.根據(jù)權(quán)利要求I所述的一種完全自主的相對慣性導(dǎo)航方法，其特征在于所述估計、修正初始誤差步驟采用閉環(huán)卡爾曼濾波器，系統(tǒng)狀態(tài)變量為X = 'ySL^ &1、SVn^ SVu^ SVe、φΝ、Φυ、φΕ、Vx> V,、Vζ> εχ、Sy^ Sz ] δ L、δ λ、δ VN、δ Vp δ Ve> ΦΝ> ΦΕ>νχ>、y、Vz、ε χ、ε y、ε ζ 依次表示諱度誤差、經(jīng)度誤差、北速誤差、天速誤差、東速誤差、北向誤差角、方位誤差角、東西誤差角、二個加速度計零偏、三個陀螺漂移； 濾波計算過程如下，符號為載體坐標(biāo)系下的表示 狀態(tài)一步預(yù)測=XkH = fI3kjk-IXk-I 狀態(tài)方差陣一步預(yù)測Pklk-I = Φ^-Ι Pk-I ΦΙλ-ι + Π-1 Qk_x rL 濾波增益 = Ρ·—\Η ( υ_—\Η + ΙΟ—1 狀態(tài)估計xk = Xk/H+KjZk-KkXkn) 狀態(tài)方差陣更新pk = (I-KkHk)PkA-I 上述式中，k為卡爾曼濾波拍數(shù)，Xlri為上一拍的估計值，XkI1為一步預(yù)測值，Φ,,Η，為狀態(tài)轉(zhuǎn)移矩陣，Hk為量測矩陣，Kk為濾波增益矩陣，Rk為觀測噪聲陣，PkI1為一步預(yù)測誤差方差陣，ΡρΡη為估計誤差方差陣，Γη為系統(tǒng)噪聲驅(qū)動陣，Qlri為系統(tǒng)噪聲陣，I為單位矩陣，Xk為狀態(tài)估計量，Zk為觀測量，λ表示經(jīng)度，上標(biāo)T表示轉(zhuǎn)置； 每完成一次濾波計算，隨后進(jìn)行一次閉環(huán)修正，修正量如下 位置修正L = L-X⑴λ = λ -x(2)，L，λ分別表示緯度和經(jīng)度； 速度修正VN = Vn-X (3)，Vu = Vu-X (4)，Ve = Ve-X (5)，Vn, Vu, Ve 分別表示北向速度、天向速度、東向速度； 姿態(tài)修正Κ=(/ + Μχ]) φ = [Χ(6)Χ(7)Χ(8)Γ ,Cf分別表示姿態(tài)矩陣，Φ表示北向誤差角、方位誤差角、東西誤差角組成的矢量；
4.根據(jù)權(quán)利要求I所述的一種完全自主的相對慣性導(dǎo)航方法，其特征在于采用旋轉(zhuǎn)調(diào)制方法來增強導(dǎo)航精度，用旋轉(zhuǎn)機構(gòu)編碼器輸出的旋轉(zhuǎn)角度對航向角和姿態(tài)角進(jìn)行旋轉(zhuǎn)解調(diào)，消除旋轉(zhuǎn)調(diào)制的運動。
全文摘要
本發(fā)明屬于慣性導(dǎo)航技術(shù)領(lǐng)域，具體涉及一種完全自主的相對慣性導(dǎo)航方法。目的是在無法獲得準(zhǔn)確初始位置的特殊情況下，自主確定方位角和緯度。該方法包括下述步驟設(shè)置初始的航向角與姿態(tài)角均為0°，始經(jīng)緯度均為0°；采用陀螺和加速度計輸出的角速度和比力，利用現(xiàn)有導(dǎo)航解算方法計算導(dǎo)航參數(shù)；閉環(huán)估計、修正初始誤差；相對初始位置進(jìn)行導(dǎo)航解算；當(dāng)獲得準(zhǔn)確外界位置信息后，將經(jīng)度信息重置為該準(zhǔn)確值，航向角、姿態(tài)角、緯度保持不變。利用該方法可進(jìn)行高精度的相對導(dǎo)航，且能提供高精度的真實緯度信息，而初始經(jīng)度誤差不影響相對導(dǎo)航精度；相對導(dǎo)航精度優(yōu)于2nmile/9h。
文檔編號G01C21/20GK102628691SQ20121010258
公開日2012年8月8日 申請日期2012年4月9日 優(yōu)先權(quán)日2012年4月9日
發(fā)明者周章華, 徐海剛, 練濤, 邱宏波, 陳明剛, 黃妍妍 申請人:北京自動化控制設(shè)備研究所