[發明專利]一種基于粒子濾波嵌套粒子濾波算法的重力匹配定位方法

有效

• 申請號: 201910054237.0
• 申請日: 2019-01-21
• 公開（公告）號: CN109631906B
• 公開（公告）日: 2021-06-01
• 發明（設計）人: 王博;朱經緯;鄧志紅;肖烜
• 申請（專利權）人: 北京理工大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/00;G01C21/08;G01C21/16
• 代理公司: 北京理工大學專利中心 11120
• 代理人: 高會允;仇蕾安

本發明公開了一種基于粒子濾波嵌套粒子濾波算法的重力匹配定位方法，能夠抑制矢量匹配算法中的誤差。包括：通過慣性導航系統獲取載體的曲線航跡，根據所述航跡的判斷第i時刻的真實位置與第i?1時刻的真實位置的相對位置，記為向量其中O_i?1、O_i分別表示慣性導航系統在第i?1時刻和第i時刻指示的所述載體所在的位置；i為時刻；第i?1時刻的粒子群為其中表示第i?1時刻的第j個粒子，j為整數；對于每個加上相對距離得到即在附近進行撒點得到粒子群求解每一個粒子的歸一化權值，經過濾波得到求出與粒子群中每一個對應的組成粒子群對粒子群進行粒子濾波得到第i時刻的匹配點。

技術領域

本發明涉及導航、制導與控制技術領域，具體涉及一種基于粒子濾波嵌套粒子濾波算法的重力匹配定位方法。

背景技術

重力匹配算法直接決定重力輔助慣導系統的精度，是水下重力無源輔助導航的核心技術。其本質是利用各種合適的信息處理融合方法，將實際測量的水下重力數據和存儲好的重力基準圖中的相應的重力數據進行相關性分析，根據一定的準則判斷這兩類數據的相似程度，最終得到對慣導位置的最佳估計結果。

傳統的重力匹配算法按照采樣方式可以分為序列匹配算法和單點匹配算法。序列匹配算法主要有ICCP和TERCOM算法。但是序列匹配算法的實時性比較差。單點匹配算法實時性較好，但是需要較高精度的初始位置，線性化方法選用不當或者線性化的誤差較大的話，會出現濾波發散的情況。最近出現了矢量匹配算法。矢量匹配算法是一種單點粒子濾波匹配算法基礎上的二次匹配算法，是在單點粒子濾波匹配結果的基礎上加入慣導相鄰點之間的位置相關性，對匹配結果進行二次估計，并借鑒貪心算法對多個校正結果利用加權最小二乘法得到最終匹配結果。矢量匹配算法相比較傳統的重力匹配算法來說具有較好的精度和魯棒性。但是該方法的精度經過理論的分析是可以進一步提高的。也就是說，該方法的誤差是可以繼續縮小的。

而目前國內外尚未有能夠抑制矢量匹配誤差的方案，因此如何進一步提高矢量匹配算法的精度，是亟待解決的問題。

發明內容

有鑒于此，本發明提供了一種基于粒子濾波嵌套粒子濾波算法的重力匹配定位方法，能夠抑制矢量匹配算法中的誤差，使得匹配的軌跡更加逼近于真實的軌跡。

為達到上述目的，本發明所提供的基于粒子濾波嵌套粒子濾波算法的重力匹配定位方法，包括：

步驟一、通過慣性導航系統獲取載體的曲線航跡，根據所述航跡的判斷第i時刻的真實位置與第i-1時刻的真實位置的相對位置，記為向量其中O_i-1、O_i分別表示慣性導航系統在第i-1時刻和第i時刻指示的所述載體所在的位置；i為時刻；

步驟二、第i-1時刻的粒子群為其中表示第i-1時刻的第j個粒子，j為整數；對于每個加上相對距離得到即在附近進行撒點得到粒子群求解每一個粒子的歸一化權值，經過濾波得到

求出與粒子群中每一個對應的組成粒子群

步驟三、對粒子群進行粒子濾波得到第i時刻的匹配點。

有益效果：

1、本發明所提供的基于粒子濾波嵌套粒子濾波算法的重力匹配定位方法，利用概率論的知識，采用粒子濾波嵌套粒子濾波對重力匹配的方法進行誤差抑制，大大減小了經度誤差和緯度誤差，使得最終匹配的軌跡更加逼近于真實的軌跡。

2、本發明采用大量的粒子進行濾波得到匹配點，同時每個粒子的位置都是通過粒子濾波的得到，所以本發明的魯棒性很好，在復雜環境中也能有很好的效果。

附圖說明

圖1為本發明所提供的一種基于粒子濾波嵌套粒子濾波算法的重力匹配定位方法的流程圖；

圖2為粒子濾波嵌套粒子濾波算法誤差分析圖。

具體實施方式