基于粒子濾波的變尺度單方向重力采樣矢量匹配定位方法技術

本發明專利技術公開了基于粒子濾波的變尺度單方向重力采樣矢量匹配定位方法，包括以下步驟：一、建立重力輔助慣導單點匹配模型，二、利用重力輔助慣導單點匹配模型得到基于粒子濾波的匹配點，三、判斷重力輔助慣導單點匹配點是否可信，四、計算當前匹配點的最終結果；本發明專利技術考慮了載體的機動航行，克服了原有的重力矢量匹配算法在載體的非直線或者非勻速航行情況下失效的缺點，減小了匹配誤差。

【技術實現步驟摘要】
基于粒子濾波的變尺度單方向重力采樣矢量匹配定位方法
本專利技術屬于重力輔助慣導系統匹配方法領域，具體涉及一種基于粒子濾波的變尺度單方向重力采樣矢量匹配定位方法。
技術介紹
水下運載體長時間隱蔽航行最常用的無源導航方式是慣導系統，但慣導誤差會隨著時間累積。為保證導航的隱蔽性和自主性，利用地球物理特征的無源導航來校正慣導誤差，因此開展了地形匹配導航技術、重力輔助導航技術、地磁輔助導航技術等研究。對于水下導航，地形數據的測量比較困難，地球的磁場不是很穩定，重力場穩定而且重力數據可以利用衛星數據反演，因此應用重力導航具有很大的優勢。重力匹配算法是重力輔助慣導技術的核心技術之一，它是利用重力儀實時測量的重力信息與預存重力圖信息以一定的算法進行比較，從而估計慣導的位置。現階段借鑒較為成熟的地形匹配，重力匹配算法按照采樣方式主要分為兩大類，序列匹配和單點匹配。序列匹配是一種驗后估計或批處理方法，每一次匹配都要在采樣到足夠的點數后進行，所以實時性比較差，主要以ICCP算法和相關分析法為代表。ICCP算法最初由圖像配準算法發展而來的，是一種尋找全局意義下的最優對準方法，精度叫較高。相關分析方法是由地形匹配中的地形輪廓匹配TERCOM算法發展而來的，它的匹配精度不受慣導位置誤差的影響，但實時性差并且難以從理論上進行提高。單點匹配算法主要為由美國桑迪亞實驗室提出的桑迪亞(SITAN)輔助導航方法，利用擴展卡爾曼濾波技術實現，該算法精度高實時性好，但由于重力特征的非線性，在重力特征明顯的區域，擴展卡爾曼濾波線性化帶來的誤差較大，嚴重時導致濾波發散，匹配失去意義。這些匹配算法各個采樣點之間相互獨立，基于單點濾波的重力采樣匹配算法考慮了各個采樣點之間的位置相關性，匹配結果相比傳統的匹配算法精度和可信度都有所提高。但是，定尺度定方向的重力矢量采樣方法對運載體的機動適應性差，并不適用于實際航行。
技術實現思路
有鑒于此，本專利技術提供了一種基于粒子濾波的變尺度單方向重力采樣矢量匹配定位方法，能夠解決傳統匹配算法匹配結果可信度不高和定尺度定方向重力矢量匹配算法機動適應性不強的問題。實現本專利技術的技術方案如下：一種基于粒子濾波的變尺度單方向重力采樣矢量匹配定位方法，具體步驟包括：步驟一、將水下運載體的經緯度差值作為狀態變量，將重力儀實時測量的水下載體當前位置的重力異常值作為觀測量，建立重力輔助慣導單點匹配模型：Δxk,k+1＝Δxk-1,k+ΔUk+ek(1)yk+1＝hk+1(xk+1)+vk+1(2)式中，Δxk,k+1表示k時刻到k+1時刻水下運載體的經緯度差值，k≥1，當k＝0時，Δx0,1＝ΔU0+e0，ΔUk為慣導系統給出的k時刻的位置偏移增量，ek為慣導系統k時刻的高斯白噪聲誤差，yk+1表示k+1時刻的重力儀測量的重力異常值，xk+1為慣導系統輸出的k+1時刻水下運載體的經緯度信息，hk+1(xk+1)表示在xk+1處在重力基準圖上讀取的重力異常值，vk+1表示k+1時刻的測量誤差，設定為高斯白噪聲；步驟二、利用重力輔助慣導單點匹配模型得到基于粒子濾波的匹配點；具體包括：S201、由慣導初始位置的先驗概率生成j個初始采樣粒子其中，l＝1,2,3……j，采樣粒子的運動規律與水下運載體的運動規律一致，則采樣粒子的狀態方程為Δqk,k+1＝Δqk-1,k+ΔUk+ek(3)式中，Δqk,k+1表示k時刻到k+1時刻采樣粒子的經緯度差值，k≥1，當k＝0時，Δq0,1＝ΔU0+e0；S202、利用得到第一個時刻采樣粒子的經緯度信息，利用重力儀測量的y1與各個粒子在重力基準圖上讀取的重力異常值的差值計算各個粒子的權值各個粒子的經緯度信息乘以相應的權值得到當前時刻水下運載體的匹配點x1′，按照對第一個時刻匹配點的計算方法計算第2,3……個時刻的匹配點；步驟三、當k＜3時，認定匹配點x′k可信；當k≥3時，對x′k-n中的所有匹配點計算其重力異常變化率Gk-n，n＝1,2,......k-1；從x′k-n中選出h個匹配點分別與當前匹配點x′k組成h個匹配點對，其中2≤h≤k-1，確保所選的匹配點中至少有一個匹配點的重力異常變化率至少為前后相鄰兩個匹配點的兩倍；若所有匹配點x′k-n的重力異常變化率均小于前后相鄰匹配點的兩倍，則選擇所有匹配點x′k-n分別與當前匹配點x′k組成k-1個匹配點對；所述匹配點對的歐氏距離應滿足慣導匹配點之間的距離相關性，即滿足式其中，(m′k,n′k)表示水下運載體k時刻的匹配點的經緯度，表示理想慣導系統兩匹配點pk和pk-n之間的歐氏距離，ξ表示設定誤差；步驟四、若步驟三中所述匹配點對均滿足式(5)，則將當前匹配點xk′視為可信點；若步驟三中所述匹配點對不全滿足式(5)，則根據理想慣導系統兩匹配點pk和pk-n之間的歐氏距離更新Δqk-1,k并根據S202中的計算方法得到新的匹配點x″k,k-n；新的匹配點x″k,k-n與不滿足式(5)匹配點組成新的匹配點對，新的匹配點對與原先滿足式(5)的匹配點對按照最小二乘法計算每個匹配點對的權值，將每個匹配點對的權值與每個匹配點對中的x″k,k-n或x′k的經緯度相乘并將所有乘積相加得到當前匹配點的最終結果；若步驟三中所述匹配點對全不滿足式(5)，則根據理想慣導系統兩匹配點pk和pk-n之間的歐氏距離更新Δqk-1,k并根據S202中的計算方法得到新的匹配點x″k,k-n；新的匹配點x″k,k-n與所選的x′k-n組成新的匹配點對，將新的匹配點對按照最小二乘法計算每個匹配點對的權值，將每個匹配點對的權值與每個匹配點對中的x″k,k-n的經緯度相乘并將所有乘積相加得到當前匹配點的最終結果。進一步地，步驟四中所述按照最小二乘法計算每個匹配點對的權值為：當存在i個匹配點對，每個匹配點對的長度為lm,m＝1,2…i,Δl＝l1+l2+...+lm，則每個匹配點對的權值wm為有益效果：與已有的定尺度定方向的重力矢量匹配算法相比，本專利技術考慮了載體的機動航行，克服了原有的重力矢量匹配算法在載體的非直線或者非勻速航行情況下失效的缺點，減小了匹配誤差。本專利技術利用基于貝葉斯估計的粒子濾波方法實現，在解決重力特征非線性問題時，避免了擴展卡爾曼濾波線性化帶來的誤差，克服了傳統點匹配算法桑迪亞算法在重力特征變化明顯的區域內易發散的缺點，適應各種重力特征的變化，拓寬了傳統算法的適用范圍。附圖說明圖1為本專利技術的流程示意圖；圖2為本專利技術的具體實例中匹配軌跡示意圖；圖3為本專利技術的具體實例緯度誤差比較示意圖；圖4為本專利技術的具體實例經度誤差比較示意圖。具體實施方式下面結合附圖并舉實施例，對本專利技術進行詳細描述。如圖1所示，本專利技術提供了一種基于粒子濾波的變尺度單方向重力采樣矢量匹配定位方法，具體步驟包括：步驟一、選取合適的匹配區域，所用重力異常數據由衛星測高獲得的分辨率為30′×30′的數據插值而來，插值后重力圖分辨率為1′×1′。重力儀實測數據由真實值加隨機噪聲構成。仿真條件：水下潛器航行速度5節；慣導三個方向陀螺零偏為0.02°/h，隨機漂移均為0.01°/h；三個方向加速度零偏均為1×10-4g,隨機漂移均為5×10-5g；初始姿態角誤差均為5′；初始位置誤差均為1′；初始速度誤差均為0.1m/s；重本文檔來自技高網...

【技術保護點】
基于粒子濾波的變尺度單方向重力采樣矢量匹配定位方法，其特征在于，包括以下步驟：步驟一、將水下運載體的經緯度差值作為狀態變量，將重力儀實時測量的水下載體當前位置的重力異常值作為觀測量，建立重力輔助慣導單點匹配模型：Δxk,k+1＝Δxk?1,k+ΔUk+ek?????(1)yk+1＝hk+1(xk+1)+vk+1??????????(2)式中，Δxk,k+1表示k時刻到k+1時刻水下運載體的經緯度差值，k≥1，當k＝0時，Δx0,1＝ΔU0+e0ΔUk為慣導系統給出的k時刻的位置偏移增量，ek為慣導系統k時刻的高斯白噪聲誤差，yk+1表示k+1時刻的重力儀測量的重力異常值，xk+1為慣導系統輸出的k+1時刻水下運載體的經緯度信息，hk+1(xk+1)表示在xk+1處在重力基準圖上讀取的重力異常值，vk+1表示k+1時刻的測量誤差，設定為高斯白噪聲；步驟二、利用重力輔助慣導單點匹配模型得到基于粒子濾波的匹配點；具體包括：S201、由慣導初始位置的先驗概率生成j個初始采樣粒子其中，采樣粒子的運動規律與水下運載體的運動規律一致，則采樣粒子的狀態方程為Δqk,k+1＝Δqk?1,k+ΔUk+ek?????(3)式中，Δqk,k+1表示k時刻到k+1時刻采樣粒子的經緯度差值，k≥1，當k＝0時，Δq0,1＝ΔU0+e0；S202、利用得到第一個時刻采樣粒子的經緯度信息，利用重力儀測量的y1與各個粒子在重力基準圖上讀取的重力異常值的差值計算各個粒子的權值各個粒子的經緯度信息乘以相應的權值得到當前時刻水下運載體的匹配點x′1，按照對第一個時刻匹配點的計算方法計算第2,3……個時刻的匹配點；步驟三、當k＜3時，認定匹配點x′k可信；當k≥3時，對x′k?n中的所有匹配點計算其重力異常變化率Gk?n，n＝1,2,......k?1；從x′k?n中選出h個匹配點分別與當前匹配點x′k組成h個匹配點對，其中2≤h≤k?1，確保所選的匹配點中至少有一個匹配點的重力異常變化率至少為前后相鄰兩個匹配點的兩倍；若所有匹配點x′k?n的重力異常變化率均小于前后相鄰匹配點的兩倍，則選擇所有匹配點x′k?n分別與當前匹配點x′k組成k?1個匹配點對；所述匹配點對的歐氏距離應滿足慣導匹配點之間的距離相關性，即滿足式其中，(m′k,n′k)表示水下運載體k時刻的匹配點的經緯度信息，表示理想慣導系統兩匹配點pk和pk?n之間的歐氏距離，ξ表示設定誤差；步驟四、若步驟三中所述匹配點對均滿足式(5)，則將當前匹配點x′k視為可信點；若步驟三中所述匹配點對不全滿足式(5)，則根據預設的理想慣導系統兩匹配點pk和pk?n之間的歐氏距離更新Δqk?1,k并根據S202中的計算方法得到新的匹配點x″k,k?n；新的匹配點x″k,k?n與不滿足式(5)匹配點組成新的匹配點對，新的匹配點對與原先滿足式(5)的匹配點對按照最小二乘法計算每個匹配點對的權值，將每個匹配點對的權值與每個匹配點對中的x″k,k?n或x′k的經緯度相乘并將所有乘積相加得到當前匹配點的最終結果；若步驟三中所述匹配點對全不滿足式(5)，則根據預設的理想慣導系統兩匹配點pk和pk?n之間的歐氏距離更新Δqk?1,k并根據S202中的計算方法得到新的匹配點x″k,k?n；新的匹配點x″k,k?n與所選的x′k?n組成新的匹配點對，將新的匹配點對按照最小二乘法計算每個匹配點對的權值，將每個匹配點對的權值與每個匹配點對中的x″k,k?n的經緯度相乘并將所有乘積相加得到當前匹配點的最終結果。...

【技術特征摘要】
1.基于粒子濾波的變尺度單方向重力采樣矢量匹配定位方法，其特征在于，包括以下步驟：步驟一、將水下運載體的經緯度差值作為狀態變量，將重力儀實時測量的水下載體當前位置的重力異常值作為觀測量，建立重力輔助慣導單點匹配模型：Δxk,k+1＝Δxk-1,k+ΔUk+ek(1)yk+1＝hk+1(xk+1)+vk+1(2)式中，Δxk,k+1表示k時刻到k+1時刻水下運載體的經緯度差值，k≥1，當k＝0時，Δx0,1＝ΔU0+e0，ΔUk為慣導系統給出的k時刻的位置偏移增量，ek為慣導系統k時刻的高斯白噪聲誤差，yk+1表示k+1時刻的重力儀測量的重力異常值，xk+1為慣導系統輸出的k+1時刻水下運載體的經緯度信息，hk+1(xk+1)表示在xk+1處在重力基準圖上讀取的重力異常值，vk+1表示k+1時刻的測量誤差，設定為高斯白噪聲；步驟二、利用重力輔助慣導單點匹配模型得到基于粒子濾波的匹配點；具體包括：S201、由慣導初始位置的先驗概率生成j個初始采樣粒子其中，l＝1,2,3……j，采樣粒子的運動規律與水下運載體的運動規律一致，則采樣粒子的狀態方程為Δqk,k+1＝Δqk-1,k+ΔUk+ek(3)式中，Δqk,k+1表示k時刻到k+1時刻采樣粒子的經緯度差值，k≥1，當k＝0時，Δq0,1＝ΔU0+e0；S202、利用得到第一個時刻采樣粒子的經緯度信息，利用重力儀測量的y1與各個粒子在重力基準圖上讀取的重力異常值的差值計算各個粒子的權值各個粒子的經緯度信息乘以相應的權值得到當前時刻水下運載體的匹配點x′1，按照對第一個時刻匹配點的計算方法計算第2,3……個時刻的匹配點；步驟三、當k<3時，認定匹配點x′k可信；當k≥3時，對x′k-n中的所有匹配點計算其重力異常變化率Gk-n，n＝1,2,......k-1；從x′k-n中選出h個匹配點分別與當前匹配點x′k組成h個匹配...

【專利技術屬性】
技術研發人員：王博，于力，肖烜，鄧志紅，
申請(專利權)人：北京理工大學，
類型：發明
國別省市：北京;11