一種基于粒子濾波的重力采樣矢量匹配定位方法
【技術領域】
[0001] 本發(fā)明涉及一種重力采樣矢量匹配定位方法，屬于重力輔助慣導系統(tǒng)匹配方法領 域。
【背景技術】
[0002] 水下運載體長時間隱蔽航行最常用的無源導航方式是慣導系統(tǒng)，但慣導誤差會隨 著時間累積。為保證導航的隱蔽性和自主性，利用地球物理特征的無源導航來校正慣導誤 差，因此開展了地形匹配導航技術、重力輔助導航技術、地磁輔助導航技術等研究。對于水 下導航，地形數(shù)據(jù)的測量比較困難，地球的磁場不是很穩(wěn)定，重力場穩(wěn)定而且重力數(shù)據(jù)可以 利用衛(wèi)星數(shù)據(jù)反演，因此應用重力導航具有很大的優(yōu)勢。重力匹配算法是重力輔助慣導技 術的核心技術之一，它是利用重力儀實時測量的重力信息與預存重力圖信息以一定的算法 進行比較，從而估計慣導的位置。
[0003] 現(xiàn)階段借鑒較為成熟的地形匹配，重力匹配算法按照采樣方式主要分為兩大類， 序列匹配和單點匹配。序列匹配是一種驗后估計或批處理方法，每一次匹配都要在采樣到 足夠的點數(shù)后進行，所以實時性比較差，主要以ICCP算法和相關分析法為代表。ICCP算法 最初由圖像配準算法發(fā)展而來的，是一種尋找全局意義下的最優(yōu)對準方法，精度叫較高。相 關分析方法是由地形匹配中的地形輪廓匹配TERCOM算法發(fā)展而來的，它的匹配精度不受 慣導位置誤差的影響，但實時性差并且難以從理論上進行提高。單點匹配算法主要為由美 國桑迪亞實驗室提出的桑迪亞（SITAN)輔助導航方法，利用擴展卡爾曼濾波技術實現(xiàn)，該 算法精度高實時性好，但由于重力特征的非線性，在重力特征明顯的區(qū)域，擴展卡爾曼濾波 線性化帶來的誤差較大，嚴重時導致濾波發(fā)散，匹配失去意義。這些匹配算法各個采樣點之 間相互獨立，沒有考慮慣導相鄰點之間的位置相關性，匹配結(jié)果可信度低。

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

[0004] 本發(fā)明的目的是為了克服已有技術存在的不足，提出一種基于粒子濾波的重力采 樣矢量匹配定位方法，矢量匹配算法考慮匹配點之間的位置相關性，解決了傳統(tǒng)匹配算法 匹配結(jié)果可信度不高的問題。
[0005] 本發(fā)明的原理：利用重力采樣點所組成的矢量進行匹配，對單點匹配結(jié)果的二次 估計，利用基于貝葉斯估計的粒子濾波實現(xiàn)。在單點匹配之后，計算匹配結(jié)果相鄰點之間的 歐氏距離，判斷其是否滿足無誤差慣導系統(tǒng)兩相鄰采樣點之間的歐氏距離所確定的范圍。 若滿足，則該點匹配結(jié)果可信；若不滿足，則根據(jù)限定條件重新進行單點匹配。
[0006] 本發(fā)明的目的是通過下述技術方案實現(xiàn)的。
[0007] -種基于粒子濾波的重力采樣矢量匹配定位方法，包括如下步驟：
[0008] 步驟一、根據(jù)水下載體的運動規(guī)律，將運載體的經(jīng)煒度信息作為狀態(tài)變量，重力儀 實時測量重力異常值為觀測量，建立重力輔助慣導單點匹配模型：
[0009] Δ Xkik+1 = A xk lik+ Δ Uk+ek (I)
[0010] yk= h k(xk)+vk (2)
[0011] 式中，Λ xkik+1表示k時刻到k+1時刻輸出的潛艇的經(jīng)煒度差值，Λ Uk為慣導系統(tǒng) 給出k時刻的偏移增量，ek慣導系統(tǒng)的誤差，y k表示k時刻的重力儀測量的重力異常信息， hk(Xk)表示在Xk處在重力基準圖上讀取的重力異常值，V k表示重力異常測量誤差和重力基 準圖誤差；
[0012] 步驟二、計算矢量匹配基準條件：設水下運載體為勻速航行，Pk表示運載體航行過 程中慣導系統(tǒng)輸出的點k的位置信息，則等間隔采樣點集{Pk 4, Pk 3, Pk 2, Pk i，PJ的矢量信 息
應滿足，其中k^4
度信息表示無誤差慣導系統(tǒng)兩相鄰采樣點之間的歐氏距離，σ表示測量誤差的 上限；
[0015] 步驟三、序貫重要性采樣：假設從后驗概率密度p(Xk|Yk)中抽出N個獨立同分布的 隨機樣本C/ = 1,…，見則ρ (xk I Yk)被表示成這些隨機樣本求和形式；引入一個已知、容 易采樣的重要性概率密度函數(shù)q (xk I Yk)，從中生成采樣粒子，實現(xiàn)求和逼近后驗概率密度 函數(shù)p (xk I Yk)，則后驗概率密度表示為從重要性密度函數(shù)采樣的隨機樣本點與每個樣本所 對應的權值乘積和的形式，粒子權值定義為重要性密度函數(shù)逼近后驗概率函數(shù)的程度；從 重要性概率密度采樣中生成采樣粒子，并隨著測量值的依次到來遞推求得相應的權值，進 而得到狀態(tài)估計；
[0016] 步驟四：重采樣：根據(jù)步驟三得到的粒子權值，計算有效粒子數(shù)次y來衡量粒子權 值的退化程度；
[0017] 步驟五、對慣性導航系統(tǒng)的狀態(tài)進行估計：根據(jù)現(xiàn)有的觀測其中量估計出重力輔 助慣導定位系統(tǒng)的位置信息；
[0018] 步驟六、判斷單點匹配結(jié)果是否可信：單點匹配結(jié)果應滿足慣導相鄰點之間位相 關性，則單點匹配結(jié)果也應滿足公式（3);若滿足，則將匹配結(jié)果視為可信點，若不滿足，則 在點匹配過程中加入泣的限定的條件得到新的重要性函數(shù)，根據(jù)由式（3)所 確定的可信范圍重新計算狀態(tài)變量的轉(zhuǎn)移概率密度函數(shù)，得到;進行步驟五，得 到新的匹配結(jié)果|? ;
[0019] 步驟七、計算矢量匹配結(jié)果權值：若匹配結(jié)果均不滿足式（3)，則得到四個局部最 優(yōu)解，利用加權最小二乘估計得到最終估計值為. .，
[0020] 步驟八：按照步驟一中的Λ Uk更新p (X k I Xk i)，再根據(jù)步驟四更新的粒子權值，進 行k+Ι時刻的慣導位置估計，再進行步驟六、七，得到矢量匹配結(jié)果。
[0021] 進一步地，所述的估計包含預測和更新兩個階段，預測過程利用建立的系統(tǒng)模型 預測狀態(tài)的先驗概率密度，更新過程則利用最新的測量值對先驗概率密度進行修正，得到 后驗概率密度。
[0022] 本發(fā)明的有益效果：
[0023] 與已有的單點匹配算法相比，矢量匹配定位方法，考慮了慣導相鄰點的位置相關 性，對區(qū)域的重力特征變化具有較強的魯棒性，并且減小了匹配誤差，匹配結(jié)果更可信，實 時性好評。本發(fā)明利用基于貝葉斯估計的粒子濾波方法實現(xiàn)，在解決重力特征非線性問題 時，避免了擴展卡爾曼濾波線性化帶來的誤差，克服了傳統(tǒng)點匹配算法桑迪亞算法在重力 特征變化明顯的區(qū)域內(nèi)易發(fā)散的缺點，適應各種重力特征的變化，拓寬了傳統(tǒng)算法的適用 范圍。
【具體實施方式】
[0024] 下面對本發(fā)明進行詳細說明。
[0025] 選取經(jīng)煒度跨度各一度的匹配區(qū)域，等值線越密集，顏色變化程度越大，該區(qū)域重 力特征越明顯。所用重力異常數(shù)據(jù)由衛(wèi)星測高獲得的分辨率為30' X30'的數(shù)據(jù)插值而 來，插值后重力圖分辨率為P XP。重力儀實測數(shù)據(jù)由真實值加隨機噪聲構(gòu)成。仿真條 件：水下潛器航行速度5節(jié)；慣導三個方向陀螺零偏為0.02° /h，隨機漂移均為0.0Γ /h; 三個方向加速度零偏均為1X10 4g，隨機漂移均為5X10 5g;初始姿態(tài)角誤差均為Y ;初 始位置誤差均為P ;初始速度誤差均為〇. lm/s ;重力儀測量噪聲為方差為2mgal的高斯白 噪聲，仿真時間共10小時。
[0026] 步驟一：根據(jù)水下載體的運動規(guī)律，將運載體的經(jīng)煒度信息作為狀態(tài)變量，重力儀 實時測量重力異常值為觀測量，建立重力輔助慣導單點匹配模型：
[0027] Δ Xkik+1 = A xk lik+ Δ Uk+ek (19)
[0028] yk=hk (xk)+vk (20)
[0029] 式中，Λ xkik+1表示k時刻到k+1時刻輸出的潛艇的經(jīng)煒度差值，Λ Uk為慣導系統(tǒng) 給出k時刻的偏移增量，ek慣導系統(tǒng)的誤差，Y k表示k時刻的重力儀測量的重力異常信息， hk(Xk)表示在Xk處在重力基準圖上讀取的重力異常值，V k表示重力異常測量誤差和重力基 準圖誤差。
[0030] 步驟二：計算矢量匹配基準條件
[0031] 假設水下運載體為勻速航行，Pk表示運載體航行過程中慣導系統(tǒng)輸出的 點k的位置信息，則等間隔采樣點集{Pk 4, Pk 3, Pk 2, Pk i，Pk} (k彡4)中的矢量信息
度信息，表示無誤差慣導系統(tǒng)兩相鄰采樣點之間的歐氏距離，σ表示設定誤差。
[0034] 步驟三：序貫重要性采樣
[0035] 貝葉斯估計將狀態(tài)估計視為一個概率推理過程，即將目標狀態(tài)估計問題轉(zhuǎn)換為利 用貝葉斯公式求解后驗概率密度P (xk I Yk)，進而積分獲得慣導系統(tǒng)的最優(yōu)估計。貝葉斯估 計需要積分運算，對于重力特征的非線性系統(tǒng)，很難得到后驗概率的封閉解析式，則利用粒 子濾波將積分問題轉(zhuǎn)化為有限樣本點求和問題。粒子濾波就是尋找一組在狀態(tài)空間中傳 播的隨機樣本對后驗概率密度函數(shù)P (Xk IYk)進行近似，將服從P (Xk IYk)分布的隨機樣本稱 為粒子。
[0036] 在實際計算中，通常無法直接從后驗分布中采樣，則引入一個已知、容易采樣 的重要性概率密度函數(shù)q(xk|Yk)，從中生成采樣粒子，實現(xiàn)求和逼近后驗概率密度函數(shù) P (xk I Yk)，本發(fā)明選取狀態(tài)變量的轉(zhuǎn)移概率密度函數(shù)P (xk I Xkl)作為重要性概率密度函數(shù)， 則后驗概率密度可以表示為
[0038] 其中δ (i)為狄拉克函數(shù)，^為每個粒子相應的權值
[0040] 但是在基于重要性采樣的粒子濾波中，估計后驗概率概率密度需要利用所有的觀 測數(shù)據(jù)，每次更新觀測數(shù)據(jù)來到都需要重新計算整個狀態(tài)的重要性權值，計算量大，影響算 法的實時性。
[0041] 為解決這一問題，將統(tǒng)計學中的序貫分析方法應用到蒙特卡洛方法中，稱為序貫 重要性采樣。即從重要性概率密度采樣中生成采樣粒子，并隨著測量值的依次到來遞推求 得相應的權值，進而得到狀態(tài)估計。重要性概率密度函數(shù)可以分解為
[0042] p(x〇:k|y1:k) = q(x〇：k ilyi：k i)q(xk|x〇：k iyi：k) (24)
[0043] 粒子權值》f的遞歸形式可以表示為
[0045] 代入選取的重要性密度函數(shù)p (xk k J得粒子的權值為
[0047] 對粒子的權值進行歸一化處理，即
[0049] 步驟四：重采樣
[0050] 為了得到正確的狀態(tài)估計，希望粒子權值的方差盡可能趨近于零。但在實際計算 中，經(jīng)過數(shù)次迭代，只有少數(shù)粒子的權值較大，其余粒子的權值可忽略不計。權值的方差隨 著時間增大，狀態(tài)空間的有效粒子數(shù)較少。隨著無效采樣粒子數(shù)目的增多，使得大量的計算 浪費在對估計后驗概率密度概率分布幾乎不起作用的粒子更新上，使得估計性能下降。采 樣有效粒子數(shù)來衡量粒子權值的退化程度，即
[0052]