[發(fā)明專利]基于交互式濾波的無人機(jī)航向估計(jì)方法、系統(tǒng)及電子設(shè)備

在審

• 申請(qǐng)?zhí)枺?/td>: 202110721430.2
• 申請(qǐng)日: 2021-06-28
• 公開（公告）號(hào): CN113566821A
• 公開（公告）日: 2021-10-29
• 發(fā)明（設(shè)計(jì)）人: 樓根銓;張文俊;郭劍輝;張國華;高治;闞正佳;汪彥斌
• 申請(qǐng)（專利權(quán)）人: 江南造船（集團(tuán)）有限責(zé)任公司;南京理工大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20
• 代理公司: 上海光華專利事務(wù)所(普通合伙) 31219
• 代理人: 徐秋平

1.一種基于交互式濾波的無人機(jī)航向估計(jì)方法，其特征在于，包括：

獲取所述無人機(jī)預(yù)設(shè)運(yùn)動(dòng)模型的參數(shù)，并得到所述預(yù)設(shè)運(yùn)動(dòng)模型之間的轉(zhuǎn)換概率；

依據(jù)所述轉(zhuǎn)換概率得到各所述預(yù)設(shè)運(yùn)動(dòng)模型交互后的狀態(tài)估計(jì)與估計(jì)協(xié)方差，并通過預(yù)設(shè)函數(shù)得到同一時(shí)刻各所述預(yù)設(shè)運(yùn)動(dòng)模型的占比；

基于所述狀態(tài)估計(jì)與所述估計(jì)協(xié)方差以及所述占比得到目標(biāo)交互式濾波模型，進(jìn)而得到所述無人機(jī)的航向估計(jì)。

2.根據(jù)權(quán)利要求1所述的基于交互式濾波的無人機(jī)航向估計(jì)方法，其特征在于，還包括對(duì)所述預(yù)設(shè)運(yùn)動(dòng)模型進(jìn)行條件濾波。

3.根據(jù)權(quán)利要求1所述的基于交互式濾波的無人機(jī)航向估計(jì)方法，其特征在于，所述預(yù)設(shè)運(yùn)動(dòng)模型具體包括無人機(jī)當(dāng)前統(tǒng)計(jì)運(yùn)動(dòng)模型以及勻速運(yùn)動(dòng)模型。

4.根據(jù)權(quán)利要求3所述的基于交互式濾波的無人機(jī)航向估計(jì)方法，其特征在于，所述勻速運(yùn)動(dòng)模型的參數(shù)包括所述勻速運(yùn)動(dòng)模型的狀態(tài)向量以及其轉(zhuǎn)移矩陣，其中，所述勻速運(yùn)動(dòng)模型的狀態(tài)向量計(jì)算式如下：

所述勻速運(yùn)動(dòng)模型的轉(zhuǎn)移矩陣計(jì)算式如下：

5.根據(jù)權(quán)利要求3所述的基于交互式濾波的無人機(jī)航向估計(jì)方法，其特征在于，所述無人機(jī)當(dāng)前統(tǒng)計(jì)運(yùn)動(dòng)模型參數(shù)包括所述無人機(jī)當(dāng)前統(tǒng)計(jì)運(yùn)動(dòng)模型的狀態(tài)向量以及其轉(zhuǎn)移矩陣，其中，所述無人機(jī)當(dāng)前統(tǒng)計(jì)運(yùn)動(dòng)模型的狀態(tài)向量計(jì)算式如下：

所述無人機(jī)當(dāng)前統(tǒng)計(jì)運(yùn)動(dòng)模型的轉(zhuǎn)移矩陣計(jì)算式如下：

其中，α為所述無人機(jī)的機(jī)動(dòng)頻率。

6.根據(jù)權(quán)利要求1所述的基于交互式濾波的無人機(jī)航向估計(jì)方法，其特征在于，所述轉(zhuǎn)換概率的計(jì)算式如下：

其中，M＝{m_i,i＝1,2,…r}為所述預(yù)設(shè)運(yùn)動(dòng)模型集，m_i(k)表示k時(shí)刻所述預(yù)設(shè)運(yùn)動(dòng)模型為m_i模型；m_j(k+1)來表示k時(shí)刻所述預(yù)設(shè)運(yùn)動(dòng)模型為m_j模型。

7.根據(jù)權(quán)利要求1所述的基于交互式濾波的無人機(jī)航向估計(jì)方法，其特征在于，所述預(yù)設(shè)函數(shù)采用似然函數(shù)。

8.一種基于交互式濾波的無人機(jī)航向估計(jì)系統(tǒng)，其特征在于，包括：

獲取模塊，用于獲取所述無人機(jī)預(yù)設(shè)運(yùn)動(dòng)模型的參數(shù)，并得到所述預(yù)設(shè)運(yùn)動(dòng)模型之間的轉(zhuǎn)換概率；

計(jì)算模塊，用于依據(jù)所述轉(zhuǎn)換概率得到各所述預(yù)設(shè)運(yùn)動(dòng)模型交互后的狀態(tài)估計(jì)與估計(jì)協(xié)方差，并通過預(yù)設(shè)函數(shù)得到同一時(shí)刻各所述預(yù)設(shè)運(yùn)動(dòng)模型的占比；

估計(jì)模塊，用于基于所述狀態(tài)估計(jì)與所述估計(jì)協(xié)方差以及所述占比得到目標(biāo)交互式濾波模型，進(jìn)而得到所述無人機(jī)的航向估計(jì)。

9.一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，其上存儲(chǔ)有計(jì)算機(jī)程序，其特征在于，該程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)權(quán)利要求1至7任一項(xiàng)所述基于交互式濾波的無人機(jī)航向估計(jì)方法。

10.一種電子設(shè)備，其特征在于，所述電子設(shè)備包括：所述存儲(chǔ)器用于存儲(chǔ)計(jì)算機(jī)程序，所述處理器用于執(zhí)行所述存儲(chǔ)器存儲(chǔ)的計(jì)算機(jī)程序，以使所述電子設(shè)備執(zhí)行如權(quán)力要求1至7中任一項(xiàng)所述基于交互式濾波的無人機(jī)航向估計(jì)方法。