[發明專利]一種基于粒子濾波算法的地磁_INS組合導航方法

有效

• 申請號: 202010635866.5
• 申請日: 2020-07-03
• 公開（公告）號: CN111964667B
• 公開（公告）日: 2022-05-20
• 發明（設計）人: 張東麗;尚俊娜;孫建強;劉新華
• 申請（專利權）人: 杭州電子科技大學
• 主分類號: G01C21/08
• 分類號: G01C21/08;G01C21/16
• 代理公司: 杭州杭誠專利事務所有限公司 33109
• 代理人: 尉偉敏

1.一種基于粒子濾波算法的地磁_INS組合導航方法，其特征在于，包括以下步驟：

步驟S1：通過一個三軸地磁傳感器模塊和一個慣性器件模塊采集地磁信息；

步驟S2：根據采集的地磁信息建立離散地磁數據庫并在地磁圖上獲取一運動軌跡；

步驟S3：利用導數動態時間規整算法進行趨勢匹配，得到一定位結果；

步驟S4：通過慣性導航(inertial navigation system，INS)解算的運動軌跡進行輔助地磁匹配定位解算，得到另一定位結果；

步驟S5：采用粒子濾波算法對步驟S3的定位結果和步驟S4的定位結果進行融合，得到地磁導航最終的定位估計；

所述步驟S4中，通過INS解算的運動軌跡進行輔助地磁匹配定位解算的具體方法為：采用滑動窗口的形式對地磁序列進行整合，滑動窗口的長度主要由經驗值得到，通過輪數計解算的里程信息縮小離散地磁數據庫中待匹配的地磁場強序列范圍，采用滑動匹配的方式進行定位解算，當匹配條件成立的時候，即輸出匹配的定位位置，否則地磁數據庫場強序列的窗口向左或向右滑動。

2.根據權利要求1所述的一種基于粒子濾波算法的地磁_INS組合導航方法，其特征在于，所述步驟S3中，利用導數動態時間規整算法進行趨勢匹配的具體方法為：在地磁匹配中，通過考慮序列的一階導數來獲取形狀的相關信息，將序列的形態特征考慮進來，導數動態時間規整算法(derivative dynamic time warping，DDTW)的實現是通過構造一個n×m的矩陣，其中矩陣中的(i^th,j^th)參數包含兩條時間序列上的點p_i和q_j的對于的D_x[p]和D_x[q]導數之差的平方，這一點與DTW不同，用以下計算式進行計算：

計算基于導數估計的局部距離為：

d(i,j)＝(D(p_i)-D(q_j))²

由上式得出，該導數估計值D_x[p]通過該點p_i與該點左鄰點的直線斜率以及通過該點左鄰點和右鄰點的直線斜率的平均值，DDTW對時間序列的第二個節點和倒數第二個節點進行估計，地磁匹配之前將待匹配的地磁序列進行平滑處理，采用廣義延拓擬合的辦法，DDTW的時間復雜度為O(mn)。

3.根據權利要求1或2所述的一種基于粒子濾波算法的地磁_INS組合導航方法，其特征在于，所述步驟S5中，采用粒子濾波算法進行地磁導航最終定位估計時，包括以下步驟：

步驟S51：初始化，根據先驗條件概率抽取隨機樣本其中N為樣本個數；

步驟S52：預測過程，由系統的狀態方程和隨機樣本得到進一步預測樣本

步驟S53：更新過程，由k時刻的測量值z_k和測量值的預測值z'_k計算每個粒子的權值，并將其歸一化得w，其中i＝1,2…N；

步驟S54：初步狀態估計，由粒子和其權重來計算狀態的估計值并利用該時刻的粒子和他們的權重來進行重采樣，從而得到下一時刻的粒子樣本，返回步驟S52；

步驟S55：依據重采樣得到的索引去挑選對應的粒子，重構的集合便是濾波的狀態集合，對這個狀態集合求均值，得到最終的目標狀態。

4.根據權利要求1所述的一種基于粒子濾波算法的地磁_INS組合導航方法，其特征在于，所述導數動態時間規整算法是一種趨勢匹配算法，DDTW沒有對時間序列的第一個數據節點和最后一個數據節點進行估計，而采用第二個節點和倒數第二個節點進行估計。

5.根據權利要求1所述的一種基于粒子濾波算法的地磁_INS組合導航方法，其特征在于，所述慣性導航解算的運動軌跡進行輔助地磁匹配定位解算，需要采用滑動窗口的形式對地磁序列整合，滑動窗口的長度主要由經驗值得到，通過輪數計解算的里程信息縮小離散地磁數據庫中待匹配的地磁場強序列范圍，采用滑動匹配的方式進行定位解算，該方法可以降低匹配過程中的計算復雜度。