[發明專利]一種實現陸用慣性導航系統運動對準的方法

無效

• 申請號: 201010227110.3
• 申請日: 2010-07-15
• 公開（公告）號: CN101900573A
• 公開（公告）日: 2010-12-01
• 發明（設計）人: 付夢印;王清哲;鄧志紅;肖烜;林杰
• 申請（專利權）人: 北京理工大學
• 主分類號: G01C25/00
• 分類號: G01C25/00;G01C21/16;G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種實現陸用慣性導航系統運動對準的方法，其特征在于：包括如下步驟：

步驟一、建立包括位置誤差、速度誤差、失準角和慣性器件誤差漂移的慣性導航系統動態誤差模型；

在東北天坐標系下，動態誤差模型表示如公式1所示；

x·(t)=F(t)x(t)+w(t)---(1)]]>

式中，t為時間值，是正實數；x(t)表示慣性導航系統動態誤差模型的狀態向量，由位置誤差δP、速度誤差δVⁿ、失準角陀螺儀零偏δ_g和加速度計零偏組成；w(t)表示慣性導航系統動態誤差模型的系統噪聲；F(t)為轉移矩陣；表示慣性導航系統動態誤差模型的狀態向量的變化量；

對該動態誤差模型進行離散化處理，可得：

x_k＝F_k-1x_k-1+w_k???????????????????????????????????????????(2)

式中，k表示時間值，為正整數；F_k-1表示離散化的慣性導航系統的一步轉移矩陣，x_k為離散化后的k時刻慣性導航系統的狀態向量，w_k表示k時刻的慣性導航系統動態誤差模型的系統噪聲，是均值為零，方差為Q的白噪聲序列，Q值根據實際應用環境人為設定，Q為正實數；

步驟二、建立步驟一所述的慣性導航系統動態誤差模型的觀測方程，如公式3所示；

Yk=VkINS-VkGPS=Hkxk+ηk---(3)]]>

式中，Y_k表示k時刻的觀測量；表示慣性導航系統的速度輸出；表示經桿臂補償后GPS的速度輸出；H_k為k時刻的觀測矩陣，H_k＝[O_3×3??I_3×3??O_3×9]；O_3×3表示3階O矩陣；I_3×3表示3階單位陣，O_3×9表示3行9列的O矩陣；η_k表示k時刻的慣性導航系統動態誤差模型的觀測方程噪聲；

步驟三、對GPS輸出進行實時野值檢測；

通過GPS測量得到的當前時刻載車東向速度和北向速度計算水平合速度Z_k，并以此作為觀測量，建立加加速度跟蹤模型，如公式4所示；

ddtv(t)a(t)a·(t)=010001000v(t)a(t)a·(t)+ζ(t)---(4)]]>

式中，t為時間值，為正實數；v(t)表示水平合速度，a(t)和分別表示水平加速度和水平加加速度；ζ(t)表示加加速度跟蹤模型的系統噪聲，是均值為零，方差為的白噪聲序列，值根據實際應用環境人為設定，為正實數；

對公式4進行離散化，離散化后的系統方程表示為：

vkaka·k=Gk-1vk-1ak-1a·k-1+ζk---(5)]]>

式中，v_k表示k時刻水平合速度，a_k和分別表示k時刻水平加速度和水平加加速度，G_k-1表示離散化的加加速度跟蹤模型的一步轉移矩陣，ζ_k表示離散后加加速度跟蹤模型的系統噪聲；

加加速度跟蹤模型的觀測方程表示為：

Zk=100vkaka·k+ξk---(6)]]>

式中，Z_k表示k時刻水平合速度的觀測量，其計算公式為：ξ_k表示加加速度跟蹤模型的觀測方程噪聲，是均值為零，方差為的白噪聲序列，值根據實際應用環境人為設定，為正實數；

根據公式5和公式6，采用卡爾曼濾波器即可估計出k時刻水平合速度v_k、相應的加速度a_k和加加速度和

根據載車的類型和對準動態情況，預先設定閾值J_max，如果成立，則判斷當前的觀測量為有效值，執行步驟四；否則，判斷其為野值，在該時刻做卡爾曼平滑，同時將k+1的值賦給k，然后重復步驟三；

步驟四、對慣性導航系統動態誤差模型的觀測方程噪聲η_k的均值和方差進行估計；

根據步驟二中的慣性導航系統動態誤差模型的觀測方程得到的k時刻的觀測量Y_k以及觀測矩陣H_k，采用主、從兩個卡爾曼濾波器分別對k時刻慣性導航系統的狀態x_k、k時刻的觀測噪聲η_k的均值和方差進行估計；具體為：和的狀態方程為：

xk+1R=I100I2xkR+wkR---(7)]]>

式中，表示主對角線元素；該狀態方程的系統噪聲是均值為零，方差為Q_R的白噪聲序列，Q_R值根據實際應用環境人為設定，Q_R為正實數；為m₁行1列的向量；I₁為m₁行m₁列的單位陣；為m₂行1列的向量；I₂為m₂行m₂列的單位陣；

公式7對應的觀測方程為：

zkR=Yk-Hkx^kdiag(Ck-HkPk|k-1HkT)---(8)]]>

式中，C_k表示殘差序列的方差，滿足和P_k|k-1的初始值分別為和P₀，均為人為設定值；此后，和P_k|k-1由主卡爾曼濾波器提供；

公式8可進一步整理為：

zkR=I00IxkR+ηkR---(9)]]>

式中，量測噪聲是均值為零，方差為R_R的白噪聲序列，R_R值根據實際應用環境人為設定，R_R為正實數；

根據公式7和公式9，采用從卡爾曼濾波器即可估計出k時刻的觀測噪聲η_k的均值和方差從卡爾曼濾波器為經典卡爾曼濾波器；

步驟五、對慣性導航系統的狀態x_k進行估計；

根據步驟一建立的慣性導航系統動態誤差模型及步驟二中的觀測方程，結合步驟四給出的觀測噪聲η_k的均值和方差的估計值，采用主卡爾曼濾波器對慣性導航系統動態誤差模型的狀態向量x_k進行估計；具體為：

主卡爾曼濾波器為經典卡爾曼濾波器的改進，計算過程如公式10～14所示：

x^k|k-1=Fk-1x^k-1---(10)]]>

Pk|k-1=Fk-1Pk-1Fk-1T+Qk---(11)]]>

Kk=Pk|k-1HkT[HkPk|k-1HkT+x^kR(1)]-1---(12)]]>

x^k=x^k|k-1+Kk[Yk-Fk-1x^k|k-1-x^kR(2)]---(13)]]>

P_k＝P_k|k-1-K_kH_kP_k|k-1????????????????????????????????(14)

其中，表示慣性導航系統動態誤差模型的狀態向量x_k的一步預測；P_k|k-1表示一步預測方差；表示慣性導航系統動態誤差模型的狀態向量x_k的估計值；P_k表示估計方差；K_k表示濾波增益；表示步驟四中的估計值；表示的估計值，其主對角線元素等于步驟四中的估計值，非主對角線的元素為0；

經過上述步驟即可得到k時刻慣性導航系統動態誤差模型狀態向量x_k的估計值包含位置誤差(δP)_k，速度誤差(δVⁿ)_k和失準角利用這些誤差估計結果對慣性導航系統的位置輸出速度輸出和姿態矩陣輸出進行校正，通過公式15～17即可得到修正后的載車的位置速度姿態矩陣

P~k=PkINS-(δP)k---(15)]]>

V~k=VkINS-(δVn)k---(16)]]>

其中，和分別表示k時刻慣性導航系統輸出的位置、速度和姿態矩陣，是已知量；I_3×3表示3階單位陣；表示由構成的斜負對稱陣；

步驟六、對步驟一中的慣性導航系統動態誤差模型一步轉移矩陣F_k-1進行更新，同時將k+1的值賦給k，然后返回到步驟二。

2.如權利要求1所述的一種實現陸用慣性導航系統運動對準的方法，其特征在于：步驟一中所述的動態誤差模型為φ角誤差方程或ψ角誤差方程。