[發(fā)明專(zhuān)利]一種GNSS和INS組合導(dǎo)航觀(guān)測(cè)量引入方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201811496616.7
• 申請(qǐng)日: 2018-12-07
• 公開(kāi)（公告）號(hào): CN109357674A
• 公開(kāi)（公告）日: 2019-02-19
• 發(fā)明（設(shè)計(jì)）人: 吳鎮(zhèn);宋振華;張迪;陳意芬;李晨;雷明兵;秦峰;袁杰波;劉偉鵬;王暉輝;韓琳
• 申請(qǐng)（專(zhuān)利權(quán)）人: 上海機(jī)電工程研究所
• 主分類(lèi)號(hào): G01C21/16
• 分類(lèi)號(hào): G01C21/16;G01C21/20;G01S19/47
• 代理公司: 上海漢聲知識(shí)產(chǎn)權(quán)代理有限公司 31236
• 代理人: 莊文莉

本發(fā)明公開(kāi)了一種GNSS/INS組合導(dǎo)航觀(guān)測(cè)量引入方法。本發(fā)明通過(guò)調(diào)節(jié)Kalman濾波器觀(guān)測(cè)誤差方差陣R，從而實(shí)現(xiàn)觀(guān)測(cè)量引入，能夠進(jìn)行不同GNSS精度下的GNSS/INS組合導(dǎo)航解算。

技術(shù)領(lǐng)域

本發(fā)明涉及導(dǎo)彈組合導(dǎo)航領(lǐng)域，具體地涉及一種GNSS和INS組合導(dǎo)航觀(guān)測(cè)量引入方法，基于調(diào)節(jié)Kalman濾波器觀(guān)測(cè)誤差方差陣R進(jìn)行觀(guān)測(cè)量引入方法，能夠進(jìn)行不同GNSS精度下的組合導(dǎo)航解算。

背景技術(shù)

提高精度和可靠性是導(dǎo)航系統(tǒng)要解決的基本問(wèn)題。目前，尚沒(méi)有一種導(dǎo)航方式能夠同時(shí)滿(mǎn)足高精度與高可靠性的要求。如廣泛使用的衛(wèi)星導(dǎo)航技術(shù)，它雖然具有精度高、定位精度不隨時(shí)間增長(zhǎng)而積累從而能保持長(zhǎng)期穩(wěn)定、可以全天候全時(shí)段工作、覆蓋區(qū)域廣、近實(shí)時(shí)輸出導(dǎo)航數(shù)據(jù)、終端成本低廉等特點(diǎn)，但也存在復(fù)雜地形(如城市峽谷、密林、室內(nèi)和地下建筑等)條件下的信號(hào)可用性差、易受惡意或無(wú)意干擾以及信號(hào)欺騙影響等弱點(diǎn)，且由于受觀(guān)測(cè)噪聲的影響，衛(wèi)星定位精度的短期穩(wěn)定性也不是特別高。與此相對(duì)應(yīng)，慣性導(dǎo)航雖然長(zhǎng)期穩(wěn)定度較差，但其不受外界影響、短期穩(wěn)定度高的特點(diǎn)則恰恰可以彌補(bǔ)衛(wèi)星導(dǎo)航技術(shù)的不足。衛(wèi)星導(dǎo)航信息作為觀(guān)測(cè)量引入Kalman濾波器，可以對(duì)慣性導(dǎo)航誤差進(jìn)行補(bǔ)償，因此組合導(dǎo)航系統(tǒng)可以滿(mǎn)足高精度與高可靠性的要求。

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

針對(duì)現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的是提供一種GNSS和INS組合導(dǎo)航觀(guān)測(cè)量引入方法，基于調(diào)節(jié)觀(guān)測(cè)誤差方差陣R的觀(guān)測(cè)量引入方法，可以有效實(shí)現(xiàn)觀(guān)測(cè)量的合理引入，實(shí)現(xiàn)Kalman濾波器的最優(yōu)解。

本發(fā)明是根據(jù)以下技術(shù)方案實(shí)現(xiàn)的：

一種GNSS和INS組合導(dǎo)航觀(guān)測(cè)量引入方法，其特征在于，包括如下步驟：

步驟S1：建立組合導(dǎo)航方程：

式中，X為狀態(tài)變量，各元素依次為東向、北向、天向3個(gè)失準(zhǔn)角誤差，東向、北向、天向3個(gè)速度誤差，緯度、經(jīng)度、高度3個(gè)位置誤差，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣，W為系統(tǒng)噪聲，Z為系統(tǒng)觀(guān)測(cè)量，H為觀(guān)測(cè)矩陣，V為觀(guān)測(cè)噪聲，表示X的微分；

步驟S2：將組合方程離散化：

式中：X_k-1、X_k分別表示k-1時(shí)刻、k時(shí)刻的狀態(tài)變量數(shù)值，Z_k為k時(shí)刻的系統(tǒng)觀(guān)測(cè)量數(shù)值，Φ為狀態(tài)轉(zhuǎn)移矩陣、Q為系統(tǒng)噪聲矩陣。狀態(tài)轉(zhuǎn)移矩陣Φ、系統(tǒng)噪聲矩陣Q分別由F、W離散化得到；

觀(guān)測(cè)噪聲方差陣R取值方法為：衛(wèi)星導(dǎo)航信息有效時(shí)觀(guān)測(cè)噪聲方差陣R取R1，無(wú)效時(shí)R取R2；

R1＝diag[Δp²Δp²Δp²Δv²Δv²Δv²]，

R2＝diag[∞∞∞∞∞∞]，

式中：Δp、Δv分別表示衛(wèi)星導(dǎo)航單向位置、速度均方差；∞表示正無(wú)窮；

步驟S3：建立Kalman濾波方程：

P(-)＝ΦP_k-1Φ^T+Q，

K＝P(-)H^T[HP_k-1H^T+R]^-1，

X_k＝ΦX_k-1+K[Z_k-HΦX_k-1]，

P_k＝[I-KH]P(-)[I-KH]^T+KRK^T，