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

【發(fā)明內容】

[0004] 本發(fā)明的目的是為了克服已有技術存在的不足，提出一種基于貝葉斯估計的粒子 濾波重力輔助慣導匹配方法，在解決非線性問題時，粒子濾波避免了線性化帶來的誤差，解 決了傳統(tǒng)點匹配算法桑迪亞算法在重力異常變化大的匹配區(qū)域易發(fā)散的缺點。
[0005] 本發(fā)明的目的是通過下述技術方案實現(xiàn)的。
[0006] -種基于貝葉斯估計的粒子濾波重力輔助慣導匹配方法，包括如下步驟：
[0007] 步驟一、根據水下載體的運動規(guī)律，將運載體的經煒度信息作為狀態(tài)變量，重力儀 實時測量重力異常值為觀測量，建立重力輔助慣導單點匹配模型：
[0008]
[0009]
[0010] 式中，AXk, k+1表示k時刻到k+Ι時刻輸出的潛艇的經煒度差值，AUk為慣導系統(tǒng) 給出k時刻的偏移增量，e k慣導系統(tǒng)的誤差，y k表示k時刻的重力儀測量的重力異常信息， hk(Xk)表示在Xk處在重力基準圖上讀取的重力異常值，V k表示重力異常測量誤差和重力基 準圖誤差；
[0011] 步驟二、序貫重要性采樣：假設從后驗概率密度P(X1JYk)中抽出N個獨立同分布的 隨機樣本= i，…，N，則p(Xk|Yk)被表示成這些隨機樣本求和形式，引入一個已知、容 易采樣的重要性概率密度函數q (xk I Yk)，從中生成采樣粒子，實現(xiàn)求和逼近后驗概率密度函 數P (xk I Yk)，則后驗概率密度表示為從重要性密度函數采樣的隨機樣本點與每個樣本所對 應的權值乘積和的形式，粒子權值定義為重要性密度函數逼近后驗概率函數的程度，從重 要性概率密度采樣中生成采樣粒子，并隨著測量值的依次到來遞推求得相應的權值，進而 得到狀態(tài)估計；
[0012] 步驟三：重采樣：根據步驟二得到的粒子權值，計算有效粒子數來衡量粒子權值 的退化程度；
[0013] 步驟四、對慣性導航系統(tǒng)的狀態(tài)Xk進行估計：根據現(xiàn)有的觀測其中量Yk，其中\(zhòng)表 示yp y2，…，yk，估計出重力輔助慣導定位系統(tǒng)的位置信息xk。
[0014] 步驟五：按照步驟一中的λ Uk更新P (X k I xk i)，再根據步驟三更新粒子權值，進行 k+Ι時刻的慣導位置估計。
[0015] 進一步地，貝葉斯估計包含預測和更新兩個階段，預測過程利用建立的系統(tǒng)模型 預測狀態(tài)的先驗概率密度，更新過程則利用最新的測量值對先驗概率密度進行修正，得到 后驗概率密度。
[0016] 本發(fā)明的有益效果：
[0017] 與已有的單點匹配方法相比，本發(fā)明利用基于貝葉斯估計的粒子濾波方法實現(xiàn)， 在解決重力特征非線性問題時，避免了擴展卡爾曼濾波線性化帶來的誤差，克服了傳統(tǒng)點 匹配算法桑迪亞算法在重力特征變化明顯的區(qū)域內易發(fā)散的缺點，適應各種重力特征的變 化，拓寬了傳統(tǒng)算法的適用范圍，并且匹配精度明顯提高，實時性好。
【具體實施方式】
[0018] 選取經煒度跨度各一度的匹配區(qū)域，在此區(qū)域內相鄰采樣點之間重力異常變化 率平均值為57. 413mgal，重力特征明顯。所用重力異常數據由衛(wèi)星測高獲得的分辨率為 30' X30'的數據插值而來，插值后重力圖分辨率為I' Xl'。重力儀實測數據由真實值 加隨機噪聲構成。仿真條件：水下潛器航行速度5節(jié)；慣導三個方向陀螺零偏為0. 02° /h， 隨機漂移均為0. 0Γ /h ;三個方向加速度零偏均為I X 10 4g，隨機漂移均為5 X 10 5g ;初始 姿態(tài)角誤差均為5初始位置誤差均為1初始速度誤差均為0. lm/s ;重力儀測量噪聲 為方差為2mgal的高斯白噪聲，仿真時間共10小時。
[0019]
[0020] 步驟一：根據水下載體的運動規(guī)律，將運載體的經煒度信息作為狀態(tài)變量，重力儀 實時測量重力異常值為觀測量，建立重力輔助慣導單點匹配模型：
[0021]
[0022]
[0023] 式中，Λ Xk, k+1表示k時刻到k+Ι時刻輸出的潛艇的經煒度差值，Δ U k為慣導系統(tǒng) 給出k時刻的偏移增量，ek慣導系統(tǒng)的誤差，Y k表示k時刻的重力儀測量的重力異常信息， hk(Xk)表示在Xk處在重力基準圖上讀取的重力異常值，V k表示重力異常測量誤差和重力基 準圖誤差。
[0024] 步驟二：序貫重要性采樣
[0025] 貝葉斯估計將狀態(tài)估計視為一個概率推理過程，即將目標狀態(tài)估計問題轉換為利 用貝葉斯公式求解后驗概率密度p (xk I Yk)，進而積分獲得慣導系統(tǒng)的最優(yōu)估計。貝葉斯估 計需要積分運算，對于重力特征的非線性系統(tǒng)，很難得到后驗概率的封閉解析式，則利用 粒子濾波將積分問題轉化為有限樣本點求和問題。粒子濾波就是尋找一組在狀態(tài)空間中傳 播的隨機樣本對后驗概率密度函數P (Xk IYk)進行近似，將服從P (Xk IYk)分布的隨機樣本稱 為粒子。
[0026] 在實際計算中，通常無法直接從后驗分布中采樣，則引入一個已知、容易采樣 的重要性概率密度函數q(x k| Yk)，從中生成采樣粒子，實現(xiàn)求和逼近后驗概率密度函數 P (xk I Yk)，本發(fā)明選取狀態(tài)變量的轉移概率密度函數P (xk I Xk i)作為重要性概率密度函數， 則后驗概率密度可以表示為
[0027]
(m)
[0028] 其中δ (·)為狄拉克函數，#為每個粒子相應的權值
[0029]
[0030] 但是在基于重要性采樣的粒子濾波中，估計后驗概率概率密度需要利用所有的觀 測數據，每次更新觀測數據來到都需要重新計算整個狀態(tài)的重要性權值，計算量大，影響算 法的實時性。
[0031] 為解決這一問題，將統(tǒng)計學中的序貫分析方法應用到蒙特卡洛方法中，稱為序貫 重要性采樣。即從重要性概率密度采樣中生成采樣粒子，并隨著測量值的依次到來遞推求 得相應的權值，進而得到狀態(tài)估計。重要性概率密度函數可以分解為
[0032]
[0033] 粒子權值 < 的遞歸形式可以表示為
[0034] (2i>
[0035] 代入選取的重要性密度函數p (xk I xk J得粒子的權值為
[0036]
[0037] 對粒子的權值進行歸一化處理，即