[發(fā)明專利]基于卡爾曼濾波的無(wú)人機(jī)定位方法及裝置

有效

• 申請(qǐng)?zhí)枺?/td>: 202110943418.6
• 申請(qǐng)日: 2021-08-17
• 公開（公告）號(hào): CN113390421B
• 公開（公告）日: 2021-12-10
• 發(fā)明（設(shè)計(jì)）人: 王安睿;董佳霖;崔迅;石兵
• 申請(qǐng)（專利權(quán)）人: 武漢理工大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G06F17/16
• 代理公司: 武漢智嘉聯(lián)合知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 42231
• 代理人: 黃君軍

本發(fā)明提供了一種基于卡爾曼濾波的無(wú)人機(jī)定位方法及裝置，其方法包括：基于預(yù)設(shè)采樣方法對(duì)無(wú)人機(jī)的當(dāng)前位置值進(jìn)行采樣，獲得多個(gè)采樣點(diǎn)；基于所述多個(gè)采樣點(diǎn)，確定所述無(wú)人機(jī)下一時(shí)刻的先驗(yàn)位置值和先驗(yàn)協(xié)方差矩陣；根據(jù)所述當(dāng)前時(shí)刻的當(dāng)前滑動(dòng)窗口長(zhǎng)度確定所述下一時(shí)刻的下一滑動(dòng)窗口長(zhǎng)度；根據(jù)所述下一滑動(dòng)窗口長(zhǎng)度確定所述下一時(shí)刻的估計(jì)系統(tǒng)噪聲協(xié)方差矩陣和估計(jì)量測(cè)噪聲協(xié)方差矩陣；獲取下一時(shí)刻的觀測(cè)位置值，并根據(jù)所述觀測(cè)位置值、所述估計(jì)系統(tǒng)噪聲協(xié)方差矩陣、所述估計(jì)量測(cè)噪聲協(xié)方差矩陣、所述先驗(yàn)位置值以及所述先驗(yàn)協(xié)方差矩陣確定所述下一時(shí)刻的后驗(yàn)位置值和后驗(yàn)協(xié)方差矩陣，以對(duì)無(wú)人機(jī)定位。本發(fā)明提高了無(wú)人機(jī)定位的準(zhǔn)確性。

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

本發(fā)明涉及無(wú)人機(jī)定位技術(shù)領(lǐng)域，具體涉及一種基于卡爾曼濾波的無(wú)人機(jī)定位方法及裝置。

背景技術(shù)

隨著無(wú)人機(jī)技術(shù)的迅猛發(fā)展，無(wú)人機(jī)的使用在各個(gè)領(lǐng)域都嶄露頭角，各行業(yè)都對(duì)無(wú)人機(jī)的需求量劇增。無(wú)人機(jī)不僅應(yīng)用于農(nóng)藥噴灑、施肥，還是進(jìn)行海上搜救的重要“引航人”。而無(wú)人機(jī)的定位精度好壞決定了海上搜救效率的高低，較低定位精度的無(wú)人機(jī)會(huì)浪費(fèi)大量人力、物力資源，也會(huì)浪費(fèi)寶貴的搜救時(shí)間。

現(xiàn)有技術(shù)中，對(duì)無(wú)人機(jī)進(jìn)行定位使用的算法是傳統(tǒng)卡爾曼濾波算法，傳統(tǒng)卡爾曼濾波假設(shè)系統(tǒng)噪聲的方差為常數(shù)；而在實(shí)際應(yīng)用中，系統(tǒng)噪聲的統(tǒng)計(jì)特性很難準(zhǔn)確獲取且是動(dòng)態(tài)變化的。所以，假定系統(tǒng)噪聲協(xié)方差矩陣為常數(shù)，會(huì)造成設(shè)定的系統(tǒng)噪聲協(xié)方差矩陣值與真實(shí)值不匹配，從而嚴(yán)重影響無(wú)人機(jī)定位精度，造成無(wú)人機(jī)定位不準(zhǔn)確的技術(shù)問(wèn)題。

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

有鑒于此，有必要提供一種基于卡爾曼濾波的無(wú)人機(jī)定位方法及裝置，用以解決現(xiàn)有技術(shù)中存在的無(wú)人機(jī)定位不準(zhǔn)確的技術(shù)問(wèn)題。

為了解決上述技術(shù)問(wèn)題，本發(fā)明提供了一種基于卡爾曼濾波的無(wú)人機(jī)定位方法，包括：

基于預(yù)設(shè)采樣方法對(duì)無(wú)人機(jī)的當(dāng)前位置值進(jìn)行采樣，獲得多個(gè)采樣點(diǎn)；

基于所述多個(gè)采樣點(diǎn)，確定所述無(wú)人機(jī)下一時(shí)刻的先驗(yàn)位置值和先驗(yàn)協(xié)方差矩陣；

根據(jù)所述當(dāng)前時(shí)刻的當(dāng)前滑動(dòng)窗口長(zhǎng)度確定所述下一時(shí)刻的下一滑動(dòng)窗口長(zhǎng)度；

根據(jù)所述下一滑動(dòng)窗口長(zhǎng)度確定所述下一時(shí)刻的估計(jì)系統(tǒng)噪聲協(xié)方差矩陣和估計(jì)量測(cè)噪聲協(xié)方差矩陣；

獲取下一時(shí)刻的觀測(cè)位置值，并根據(jù)所述觀測(cè)位置值、所述估計(jì)系統(tǒng)噪聲協(xié)方差矩陣、所述估計(jì)量測(cè)噪聲協(xié)方差矩陣、所述先驗(yàn)位置值以及所述先驗(yàn)協(xié)方差矩陣確定所述下一時(shí)刻的后驗(yàn)位置值和后驗(yàn)協(xié)方差矩陣，以對(duì)無(wú)人機(jī)定位。

在一種可能的實(shí)現(xiàn)方式中，所述基于所述多個(gè)采樣點(diǎn)，確定所述無(wú)人機(jī)下一時(shí)刻的先驗(yàn)位置值和先驗(yàn)協(xié)方差矩陣包括：

確定所述多個(gè)采樣點(diǎn)的一階統(tǒng)計(jì)權(quán)系數(shù)和二階統(tǒng)計(jì)權(quán)系數(shù)；

確定所述多個(gè)采樣點(diǎn)的多個(gè)一步預(yù)測(cè)采樣點(diǎn)；

根據(jù)所述一階統(tǒng)計(jì)權(quán)系數(shù)、所述二階統(tǒng)計(jì)權(quán)系數(shù)以及所述多個(gè)一步預(yù)測(cè)采樣點(diǎn)確定所述先驗(yàn)位置值和所述先驗(yàn)協(xié)方差矩陣。

在一種可能的實(shí)現(xiàn)方式中，所述根據(jù)所述當(dāng)前時(shí)刻的當(dāng)前滑動(dòng)窗口長(zhǎng)度確定所述下一時(shí)刻的下一滑動(dòng)窗口長(zhǎng)度包括：

獲取當(dāng)前時(shí)刻的當(dāng)前觀測(cè)值；

根據(jù)所述當(dāng)前觀測(cè)值、所述當(dāng)前位置值以及所述量測(cè)方程確定所述當(dāng)前時(shí)刻的當(dāng)前殘差序列；

確定所有歷史時(shí)刻的歷史平均殘差序列；

根據(jù)所述當(dāng)前滑動(dòng)窗口長(zhǎng)度、所述當(dāng)前殘差序列以及所述歷史平均殘差序列確定所述下一滑動(dòng)窗口長(zhǎng)度。

在一種可能的實(shí)現(xiàn)方式中，所述根據(jù)所述觀測(cè)位置值、所述估計(jì)系統(tǒng)噪聲協(xié)方差矩陣、所述估計(jì)量測(cè)噪聲協(xié)方差矩陣、所述先驗(yàn)位置值以及所述先驗(yàn)協(xié)方差矩陣確定所述下一時(shí)刻的后驗(yàn)位置值和后驗(yàn)協(xié)方差矩陣包括：