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

有效

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

1.一種基于粒子濾波的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è)

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

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

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

其中，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)行重采樣

2.如權(quán)利要求1所述的一種基于粒子濾波的SINS/DR組合導(dǎo)航系統(tǒng)位置跟蹤確定方法，其特征在于：

所述的步驟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為航位推算位置。

3.如權(quán)利要求1所述的一種基于粒子濾波的SINS/DR組合導(dǎo)航系統(tǒng)位置跟蹤確定方法，其特征在于：所述的權(quán)值閾值weight_min為