[發(fā)明專(zhuān)利]基于李群非線(xiàn)性狀態(tài)誤差的慣性基組合導(dǎo)航濾波方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202010312638.4
• 申請(qǐng)日: 2020-04-20
• 公開(kāi)（公告）號(hào): CN111399023B
• 公開(kāi)（公告）日: 2022-02-08
• 發(fā)明（設(shè)計(jì)）人: 王茂松;吳文啟;何曉峰;崔加瑞
• 申請(qǐng)（專(zhuān)利權(quán)）人: 中國(guó)人民解放軍國(guó)防科技大學(xué)
• 主分類(lèi)號(hào): G01S19/47
• 分類(lèi)號(hào): G01S19/47
• 代理公司: 長(zhǎng)沙國(guó)科天河知識(shí)產(chǎn)權(quán)代理有限公司 43225
• 代理人: 董惠文

1.一種基于李群非線(xiàn)性狀態(tài)誤差的慣性基組合導(dǎo)航濾波方法，其特征在于，包括：

慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)中的衛(wèi)星接收機(jī)和捷聯(lián)慣導(dǎo)分別接收各自的數(shù)據(jù)信息；

根據(jù)衛(wèi)星接收機(jī)和捷聯(lián)慣導(dǎo)接收的數(shù)據(jù)信息，建立慣性/衛(wèi)星初始對(duì)準(zhǔn)模型，完成基于李群非線(xiàn)性狀態(tài)誤差的慣性基組合導(dǎo)航濾波方法的初始對(duì)準(zhǔn)；

建立慣性/衛(wèi)星組合導(dǎo)航模型，完成基于李群非線(xiàn)性狀態(tài)誤差的慣性基組合導(dǎo)航濾波方法的組合導(dǎo)航，將組合導(dǎo)航的姿態(tài)、速度、位置信息輸出；

其中慣性/衛(wèi)星組合導(dǎo)航模型與慣性/衛(wèi)星初始對(duì)準(zhǔn)模型相同，均包括基于李群非線(xiàn)性狀態(tài)誤差的慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)模型和慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)觀(guān)測(cè)模型，基于李群非線(xiàn)性狀態(tài)誤差的慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)模型的建立方法為：

(1)慣性導(dǎo)航狀態(tài)方程建立；

姿態(tài)表達(dá)采用即載體系到地心地固坐標(biāo)系ECEF的方向余弦矩陣，速度表達(dá)和位置的表達(dá)均采用載體相對(duì)于慣性系ECI的速度和位置在ECEF坐標(biāo)系的投影；

方向余弦矩陣對(duì)時(shí)間的導(dǎo)數(shù)為

其中，為載體相對(duì)于ECI系的角速度在ECI系下的投影，為地球自轉(zhuǎn)角速度在ECEF系下的投影；

載體相對(duì)ECI坐標(biāo)系的速度在ECI坐標(biāo)系的投影對(duì)時(shí)間的導(dǎo)數(shù)為：

其中，為在ECI系下投影的引力加速度，其為ECI坐標(biāo)系中笛卡爾位置的函數(shù)，f^b為比力值，為載體系到ECI坐標(biāo)系的方向余弦矩陣；

將公式(2)定義的速度在ECEF系下投影，得到：

其中，公式(3)的推導(dǎo)中用到了為ECEF坐標(biāo)系到ECI坐標(biāo)系的方向余弦矩陣；為引力加速度在ECEF坐標(biāo)系下的投影；為載體相對(duì)ECI坐標(biāo)系的速度在ECEF坐標(biāo)系下的投影；

ECEF系下定義的載體位置隨時(shí)間的導(dǎo)數(shù)為：

其中，為ECI坐標(biāo)系到ECEF坐標(biāo)系的方向余弦矩陣，為載體相對(duì)ECI坐標(biāo)系的位置在ECEF坐標(biāo)系的投影，為載體相對(duì)ECI坐標(biāo)系的位置在ECI坐標(biāo)系的投影；

(2)狀態(tài)誤差定義及其微分方程；

定義李群下的與姿態(tài)、速度、位置對(duì)應(yīng)的狀態(tài)誤差如下：

其中，為ECEF坐標(biāo)系到載體系的方向余弦矩陣的估計(jì)值，exp(·)代表矩陣的指數(shù)，φ^e為姿態(tài)誤差；為載體相對(duì)ECI坐標(biāo)系的速度在載體系下投影的估計(jì)值，為載體相對(duì)ECI坐標(biāo)系的速度在ECEF坐標(biāo)系下投影的估計(jì)值；為載體相對(duì)ECI坐標(biāo)系的位置在載體系下投影的估計(jì)值，為載體相對(duì)ECI坐標(biāo)系的位置在ECEF坐標(biāo)系下投影的估計(jì)值；I為3乘3的單位矩陣；

設(shè)帶有測(cè)量噪聲的三軸陀螺儀測(cè)量的角速率和三軸加速度計(jì)測(cè)量的比力為：

其中，為三軸陀螺儀測(cè)量誤差，δf^b為三軸加速度計(jì)測(cè)量誤差，ε^b為三軸陀螺儀零偏，為三軸加速度計(jì)零偏，w_g＝[w_gx w_gy w_gz]^T和w_a＝[w_ax w_ay w_az]^T均為白噪聲過(guò)程；

利用一階近似得到李群下的非線(xiàn)性姿態(tài)誤差φ^e的微分方程為：

李群下的非線(xiàn)性速度誤差的微分方程為:

其中，μ為引力常數(shù)；

李群下的非線(xiàn)性位置誤差的微分方程為：

(3)構(gòu)建基于李群非線(xiàn)性狀態(tài)誤差的慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)模型；

三軸陀螺儀測(cè)量誤差和三軸加速度計(jì)測(cè)量誤差可以建模為隨機(jī)游走過(guò)程:

根據(jù)公式(7)-(10)，得到基于李群非線(xiàn)性狀態(tài)誤差的慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)模型為：

其各變量的定義如下：

w＝[w_gx w_gy w_gz w_ax w_ay w_az]^T (13)

構(gòu)建基于李群非線(xiàn)性狀態(tài)誤差的慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)觀(guān)測(cè)模型方法為：

將慣導(dǎo)與衛(wèi)星的速度、位置求差作為觀(guān)測(cè)，需要用到如下的ECEF和ECI坐標(biāo)系之間狀態(tài)轉(zhuǎn)換公式：

其中，為載體相對(duì)ECEF坐標(biāo)系的位置在ECEF坐標(biāo)系下投影的估計(jì)值，為載體相對(duì)ECI坐標(biāo)系的位置在ECI坐標(biāo)系下投影的估計(jì)值，為載體相對(duì)ECEF坐標(biāo)系的位置在ECI坐標(biāo)系下投影的估計(jì)值；和分別為載體相對(duì)ECI系的線(xiàn)性位置誤差估計(jì)值和載體相對(duì)ECEF系的線(xiàn)性位置誤差估計(jì)值在ECEF系的投影，二者相同，因?yàn)镋CI和ECEF的坐標(biāo)原點(diǎn)相同；為載體相對(duì)ECI坐標(biāo)系的速度在ECI坐標(biāo)系下投影的估計(jì)值，為載體相對(duì)ECEF坐標(biāo)系的速度在ECI坐標(biāo)系下投影的估計(jì)值；為地球自轉(zhuǎn)角速度在ECI坐標(biāo)系下的投影；為載體相對(duì)ECEF坐標(biāo)系的速度在ECEF坐標(biāo)系下投影的估計(jì)值；和分別為載體相對(duì)ECI系的線(xiàn)性速度誤差估計(jì)值和載體相對(duì)ECEF系的線(xiàn)性速度誤差估計(jì)值在ECEF系的投影；

基于李群非線(xiàn)性狀態(tài)誤差的慣性/衛(wèi)星松組合觀(guān)測(cè)模型為：

δz＝Hx_{LGEKF-INS-ECEF}+υ (17)

其中：

δz_v和δz_r分別為速度觀(guān)測(cè)誤差向量和位置觀(guān)測(cè)誤差向量，υ_v,3×1和υ_r,3×1分別為速度和位置觀(guān)測(cè)白噪聲；

公式(18)中的H矩陣是根據(jù)公式(19)得到：