[發(fā)明專利]一種基于粒子濾波的SINS/DR組合導(dǎo)航系統(tǒng)位置跟蹤確定方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201810920933.0
• 申請(qǐng)日: 2018-08-14
• 公開(kāi)（公告）號(hào): CN110823212B
• 公開(kāi)（公告）日: 2023-04-07
• 發(fā)明（設(shè)計(jì)）人: 王萬(wàn)征;鄧亮;裴興凱;陳靜;莊廣琛
• 申請(qǐng)（專利權(quán)）人: 北京自動(dòng)化控制設(shè)備研究所
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01C21/20
• 代理公司: 核工業(yè)專利中心 11007
• 代理人: 高安娜

本發(fā)明屬于捷聯(lián)導(dǎo)航控制技術(shù)，具體公開(kāi)了一種基于粒子濾波的SINS/DR組合導(dǎo)航系統(tǒng)位置跟蹤確定方法，首先根據(jù)目標(biāo)的初始狀態(tài)量及可能出現(xiàn)的誤差半徑對(duì)粒子進(jìn)行隨機(jī)分布，然后進(jìn)行粒子狀態(tài)預(yù)測(cè)，計(jì)算粒子權(quán)值，確定各個(gè)粒子與真實(shí)位置的近似程度和所有粒子的狀態(tài)融合成的估計(jì)值，當(dāng)某粒子的權(quán)值小于某一閾值時(shí)，進(jìn)行重采樣。本方法以粒子濾波理論為指導(dǎo)，以目標(biāo)位置為特征實(shí)現(xiàn)了SINS/DR組合導(dǎo)航系統(tǒng)中的位置跟蹤算法，可以避免里程計(jì)輸出異常時(shí)引起的位置跳變，且誤差不隨時(shí)間發(fā)散，提高了位置和速度的實(shí)時(shí)輸出精度。

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

本發(fā)明屬于捷聯(lián)導(dǎo)航控制技術(shù)，具體涉及一種導(dǎo)航系統(tǒng)位置跟蹤確定方法。

背景技術(shù)

在SINS/DR組合導(dǎo)航系統(tǒng)中，通常以里程計(jì)進(jìn)行航位推算所得的位置、速度作為量測(cè)量與捷聯(lián)慣導(dǎo)系統(tǒng)進(jìn)行組合導(dǎo)航，通過(guò)卡爾曼濾波對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)的姿態(tài)誤差進(jìn)行估計(jì)，從而保持慣導(dǎo)姿態(tài)精度。在載體行進(jìn)過(guò)程中，如果里程計(jì)測(cè)量發(fā)生異常情況，如打滑、側(cè)移、離地等，會(huì)導(dǎo)致航位推算結(jié)果發(fā)生異常。在實(shí)時(shí)處理過(guò)程中，無(wú)法通過(guò)濾波器相關(guān)量對(duì)航位推算結(jié)果進(jìn)行修正，使得里程計(jì)在發(fā)生異常情況時(shí)，航位推算結(jié)果會(huì)發(fā)生跳變，影響精度。

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

本發(fā)明的目的在于提供一種基于粒子濾波的SINS/DR組合導(dǎo)航系統(tǒng)位置跟蹤確定方法，能夠避免里程計(jì)輸出異常時(shí)引起的位置跳變，且誤差不隨時(shí)間發(fā)散。

本發(fā)明的技術(shù)方案如下：

一種基于粒子濾波的SINS/DR組合導(dǎo)航系統(tǒng)位置跟蹤確定方法，該方法包括如下步驟：

1)粒子初始化，確定目標(biāo)的初始狀態(tài)量及可能出現(xiàn)的誤差半徑，然后根據(jù)式(1)對(duì)粒子進(jìn)行隨機(jī)分布

其中，S(i)_t為第i個(gè)粒子的航程，ΔS為根據(jù)運(yùn)動(dòng)速度預(yù)估的誤差，ΔV為目標(biāo)預(yù)估的速度，random為(-1,1)內(nèi)的隨機(jī)數(shù)，V(i)_t為第i個(gè)粒子的速度，weight(i)為第i個(gè)粒子的權(quán)重，N為粒子數(shù)；

2)粒子狀態(tài)預(yù)測(cè)

通過(guò)狀態(tài)轉(zhuǎn)移方程來(lái)計(jì)算每個(gè)粒子在該周期的狀態(tài)預(yù)測(cè)值，用來(lái)描述目標(biāo)運(yùn)動(dòng)的狀態(tài)轉(zhuǎn)移方程如式(2)所示：

其中，V_odo為里程計(jì)速度；

3)粒子權(quán)值計(jì)算，確定各個(gè)粒子與真實(shí)位置的近似程度

其中，weight(i)為每個(gè)粒子的權(quán)重，S為參考位置；

利用下式確定每個(gè)粒子權(quán)值占整個(gè)累計(jì)權(quán)值的比重

其中，sum_weight為權(quán)值累加值；

4)跟蹤估計(jì)，確定所有粒子的狀態(tài)融合成的估計(jì)值

5)設(shè)置一個(gè)權(quán)值閾值weight_min，當(dāng)某粒子的權(quán)值小于該閾值時(shí)，進(jìn)行重采樣

所述的步驟3)中的參考位置S是根據(jù)SINS解算結(jié)果和航位推算結(jié)果綜合計(jì)算的，其計(jì)算方法如式(4)所示：

S＝0.995×(S+V_SINS×t)+0.005×S_DR????????(4)

其中，V_SINS為組合導(dǎo)航系統(tǒng)解算的速度，t為計(jì)算周期，S_DR為航位推算位置。

所述的權(quán)值閾值weight_min為

本發(fā)明的顯著效果在于：