一種改進的相關序列算法的重力匹配方法
【專利摘要】本發明公開了一種改進的相關序列算法的重力匹配方法。本發明引入最短路徑算法和慣導短期精度高的特性，有效地彌補傳統算法的缺陷，通過重力信息匹配過程提供更及時有效的位置信息修正慣導的積累誤差。
【專利說明】
一種改進的相關序列算法的重力匹配方法
技術領域
[0001] 本發明涉及相關分析匹配算法，具體涉及一種改進的相關序列算法的重力匹配方 法。
【背景技術】
[0002] 慣性導航系統是水下定位的主要設備。由于慣導的位置誤差會隨時間累積，長時 間航行時需要進行重調和校正才能保證載體的定位精度。利用重力信息校正慣導的累積誤 差能夠實現無源導航，因此重力匹配輔助導航是值得研究的方法。重力輔助慣導系統包括 慣導系統，重力匹配算法，濾波環節，重力測量和重力數據背景圖等部分。其中，匹配算法是 重力匹配輔助導航系統的核心。
[0003] 相關序列算法(TERC0M)是一種基于高程相關的地形輔助導航技術。其原理是通過 實測量序列與數據圖中所有可能的基準序列進行相關，計算實測數據和基準序列的相關 度，然后將最相關的基準序列對應的位置定位為匹配位置。TERC0M算法在重力數據變化劇 烈的區域具有較好的導航性能，但是也存在以下局限性：
[0004] 1)TERC0M算法實時性較差。由于TERC0M算法是序列匹配算法，一次匹配過程要完 成一系列點的采樣，匹配結果稀少而且利用率低。
[0005] 2)TEREC0M算法是局部最優匹配算法。由于TERC0M算法是相鄰序列點的分析匹配 算法，匹配結果還會受匹配序列長度影響，最終匹配結果是局部最優。
[0006] 3)TERC0M算法精度較差。由于相關分析函數是基于重力異常信息，匹配精度過度 依賴于匹配區域的重力異常變化情況，在重力異常變化平緩區域錯誤匹配率高。
【發明內容】
[0007] 有鑒于此，本發明提供了一種改進的相關序列算法的重力匹配方法，能夠提高傳 統匹配算法的實時性，同時解決算法局部最優的缺陷，提高匹配結果精度和穩定性。
[0008] 一種改進的相關序列算法的重力匹配方法；
[0009] 令：
[0010] 上一時刻載體在重力背景圖中所有可能的真實位置的網格點的集合作為已訪問 點集；
[0011] 當前時刻載體在重力背景圖中所有可能的真實位置的網格點的集合作為未訪問 點集；
[0012] 采用如下步驟實現：
[0013 ]步驟一、建立初始時刻k = 0時的已訪問點集;具體為：
[0014] 根據慣性導航系統提供的載體初步位置信息和載體慣導位置誤差特性，在重力數 據背景圖模塊中的重力背景圖上確定載體真實位置所在范圍，并將該范圍內的所有網格點 組成的點集標記為初始時刻的已訪問點集：
[0015] 已訪問點集中的每個網格點均包含當前網格點i的位置信息和權值信息；iG(l， 2，-_，n)，n為網格點的總數；
[0016]步驟二、令k = k+l;建立未訪問點集：
[0017]根據慣性導航系統提供的載體慣導位置誤差特性以及第k時刻將要達到的位置信 息，在重力背景圖上確定載體真實位置所在范圍，并將所述范圍內的所有網格點組成的點 集標記為第k時刻的未訪問點集；
[0018] 步驟三、選取未訪問點集中的任意一個網格點，定義為網格點j，與已訪問點集中 的任意一個網格點，形成可能路徑線段；
[0019] S31、利用相關序列分析函數，根據步驟一中獲得的位置信息，得到可能路徑線段 的權值；
[0020] S32、通過最短路徑算法，根據步驟一中獲得的權值信息，得到網格點j的權值；
[0021] S33、根據S31和S32的方法，遍歷未訪問點集中的所有網格點與已訪問點集中所有 網格點，并相應的形成可能路徑線段，并獲得未訪問點集中的所有網格點的權值；
[0022] 步驟四、比較未訪問點集中的所有網格點的權值，將權值最小的網格點作為載體 位置的最優估計值；
[0023]步驟五、將最優估計值反饋至慣性導航系統，慣性導航系統利用得到的最優估計 值對其采集獲得的慣導數據進行修正，并將修正后的結果輸出，作為載體的真實位置估計； [0024]步驟六、將未訪問點集標記為已訪問點集;返回步驟二。
[0025] 較佳地，所述步驟二中：如果當前載體真實位置所在范圍的網格點的重力異常值 超出重力異常可能范圍，則將所對應的網格點從未訪問點集中刪除;其中，重力異常可能范 圍是根據重力實時測量信息和重力測量誤差特性確定的。
[0026] 較佳地，將步驟四中獲得的最優估計值輸出后，采用標準卡爾曼濾波方法對最優 估計值進行濾波分析，并將濾波后得到的輸出值替代步驟五中的最優估計值，執行步驟五。 [0027] 有益效果：
[0028]本發明引入最短路徑算法和慣導短期精度高的特性，有效地彌補傳統算法的缺 陷，通過重力信息匹配過程提供更及時有效的位置信息修正慣導的積累誤差。
【附圖說明】
[0029]圖1為重力輔助慣性導航系統的模塊圖。
[0030] 圖2為改進的相關序列算法的流程圖。
【具體實施方式】
[0031] 下面結合附圖并舉實施例，對本發明進行詳細描述。
[0032] 本發明提供了一種改進的相關序列算法的重力匹配方法，本發明引入最短路徑算 法來提高匹配算法效率，根據慣導短時間高精度的特性，提出了一種新的相關分析函數。具 體為：
[0033] 1、最短路徑算法的原理是用于計算一個節點到其他所有節點的最短路徑。主要特 點是以起始點為中心向外層層擴展，直到擴展到終點為止。算法通過儲存所有可能結果和 權值，然后分析比較得到從起始點到此刻的全局最優結果。將最短路徑算法原理應用于匹 配算法中：從第一個時刻開始，將當前時刻點以前的所有可能位置點和對應權值儲存起來； 基于已存信息和當前時刻信息，更新已存信息;分析比較已存信息，得到從初始點到目前時 刻的最優匹配點。
[0034] 2、我們在改進算法中的相關分析函數中引入相鄰點組成線段的角度和長度信息 來提高分析結果精度。重力測量數據有其變化不規則特性，僅僅依靠重力數據進行匹配的 匹配結果變化劇烈，結果穩定性差。慣導短期精度的引入限制了大的匹配誤差，能有效提高 算法穩定性。同時，引入的參數閾值可以用來調節各個參數比重。例如，在重力數據變化劇 烈區域，重力參數在權值中比例變大;相反在重力數據變化緩慢區域，慣導短期數據參數在 權值中比例變小。這樣，在改進算法中引入的慣導的短時信息，能有效的限制由變化平緩重 力區域引起的較大匹配偏差。
[0035]本發明包括慣性導航系統、重力數據背景圖模塊、重力測量系統、匹配算法模塊以 及濾波器；
[0036]其中，所述慣性導航系統用于測量載體位置參考信息，并將該位置參考信息分別 發送到重力數據背景圖模塊、重力測量系統、匹配算法模塊以及濾波器模塊;其中，由于慣 性導航系統中的傳感器的測量結果是存在偏差的，所以所述的位置參考信息是具有一定誤 差的。
[0037]所述重力測量系統用于提供載體此刻的重力實時信息，并將檢測到的重力測量數 據發送至匹配算法模塊中；
[0038]所述重力數據背景圖模塊用于在位置參考信息的輔助下，獲得載體重力參考信 息，并發送到匹配算法模塊中；重力背景圖是一個區域的網格圖，其中每個網格點包括信息 為區域經度、煒度和每個位置點的重力異常信息。
[0039]所述匹配算法模塊根據獲得的位置參考信息、重力測量數據和重力參考信息進行 分析匹配，并輸出最佳匹配位置信息給濾波器；
[0040]所述濾波器通過匹配算法提供的最佳匹配位置信息對位置參考信息進行反饋矯 正。
[0041 ]具體實現方法：
[0042] 定義：
[0043] 已訪問點集:選取上一時刻載體在重力背景圖中所有可能的真實位置的網格點的 集合。
[0044] 未訪問點集:選取當前時刻載體在重力背景圖中所有可能的真實位置的網格點的 集合。
[0045]步驟一、建立初始時刻的已訪問點集；
[0046]由于初始時刻不存在上一時刻，所以需要在初始時刻下，建立已訪問點集;此時， 定義初始時刻下的時間戳k = 0具體建立方法為：
[0047]根據慣性導航系統提供的載體初步位置信息和載體慣導位置誤差特性，在重力數 據背景圖模塊中的重力背景圖上確定載體真實位置所在范圍，并將所述范圍內的所有網格 點組成的點集標記為初始時刻的已訪問點集：
[0048]其中，所述的載體真實位置所在范圍是以載體初步位置信息為中心，載體慣導位 置的圓概率誤差為半徑的圓形區域;根據圓概率誤差半徑定義可知:所述的載體真實位置 在該區域的可能性為99%。
[0049] 此外，已訪問點集中的每個網格點均包含以下信息：
[0050] 1 ' (1) Uk
[0051 ]其中，i為k時刻的網格點標簽，i G (1，2，…，n)，n為網格點的總數。（ <，<)是網格 點的位置信息，分別代表網格點的煒度和經度。ff是對應于當前網格點的權值信息。
[0052]步驟二、令k = k+l，建立未訪問點集；
[0053]根據慣性導航系統提供的載體慣導位置誤差特性以及第k時刻將要達到的位置信 息，在重力數據背景圖模塊中的重力背景圖上確定載體真實位置所在范圍，并將所述范圍 內的所有網格點組成的點集標記為第k時刻的未訪問點集:具體方法同步驟一。
[0054]在當前載體真實位置所在范圍的基礎上，根據重力測量系統獲得的載體實時重力 測量信息和重力測量誤差特性，縮小當前載體真實位置所在范圍：
[0055]如果當前載體真實位置所在范圍的網格點的重力異常值超出重力異常可能范圍， 這個點從未訪問點集中刪除。其中，重力異常可能范圍是根據重力實時測量信息和重力測 量誤差特性確定的。
[0056]步驟三、匹配算法模塊根據已訪問點集和未訪問點集，確定未訪問點集中的網格 點的權值：
[0057]本步驟采用了最短路徑算法，最短路徑算法是把到當前時刻為止的最優結果保存 起來，這些保存的信息實際包括了之前的一個序列點的匹配信息，所以之后每采樣一個點 進行匹配就相當于一個序列的匹配，這樣每采樣一個點就可以得到一個匹配結果。
[0058] S31，選取未訪問點集中的任意一個網格點，定義為網格點j，與已訪問點集中的任 意一個網格點，定義為網格點i，形成可能路徑線段;根據
(2)
[0060] 獲得可能路徑線段的權值Wl」；
[0061] 其中，0:」是當前可能路徑線段的方向，li」當前可能路徑線段的長度。0INS和1INS是 慣性導航系統提供慣導路徑線段的角度和長度，計算方法同公式(3)和公式(4)。A 是未 訪問點集中第j個點的重力異常值，Agm對應的載體的重力異常值.0thre5shcild是路徑線段角 度的閾值，lthreshold是路徑線段長度的閾值，A gthreshold是載體重力異常閾值，^threshold、 lthreshold和A gthreshold都是經驗所得。其中，123456i」和li」的計算公式為：
(3) 2
[0064]其中，h為已訪問點集中的網格點i的經度，\為未訪問點集中的網格點j的經度， 3 約為已訪問點集中的網格點i的煒度，外為未訪問點集中的網格點j的煒度。J (9 i」，91 ns )、J 4 (h」，lINS)和J( Agj，Agm)可看為關于J(X1，X2)的相關序列分析函數，其計算公式是： 5
[0065] J(xi,X2)= |xi-X2 (5) 6
[0066] x為自變量；
[0067] S32,根據可能路徑線段的權值，通過最短路徑算法，計算網格點j中所有權值，并取最 小的權值作為該網格點j的權值;權值越小，說明誤差越小;1 +<,? )， i
[0068] m為未訪問點集的個數；
[0069] S33，根據S31和S3 2的方法，遍歷未訪問點集中的所有網格點與已訪問點集中所有 網格點，并相應的形成可能路徑線段，并獲得未訪問點集中的所有網格點的權值。
[0070] 步驟四、
[0071]比較未訪問點集中的所有網格點的權值，將權值最小的網格點作為載體位置的最 優估計值，并輸出至濾波器模塊;最短路徑算法到每個時間戳都會得到所有可能路徑中的 最優結果，然后通過比較再選出最優結果，這樣保存下來的結果是從初始點到當前點的最 優結果，即為全局最優。
[0072] 步驟五、
[0073]濾波器模塊采用標準卡爾曼濾波方法，建立系統狀態模型：x=[SL SA SVE SVN <i>E <i>U ￡E ￡N eu Ae An]，5L和5入是慣導經煒度誤差，5ve和5vn慣導速度誤差，巾E、<i>N 和小LI是慣導姿態誤差，eE、和￡11是陀螺儀誤差，A E和A N是加速度計誤差。觀測模型為:H= [I2X2 02X1Q]T，觀測數據為步驟四輸出的載體位置的最優估計值。濾波后得到的輸出值反饋 至慣性導航系統，慣性導航系統利用得到的輸出值對其采集獲得的慣導數據進行修正，并 將修正后的結果輸出，作為載體的真實位置估計。
[0074] 步驟六、
[0075]將未訪問點集標記為已訪問點集;返回步驟二。
[0076]綜上所述，以上僅為本發明的較佳實施例而已，并非用于限定本發明的保護范圍。 凡在本發明的精神和原則之內，所作的任何修改、等同替換、改進等，均應包含在本發明的 保護范圍之內。
【主權項】
1. 一種改進的相關序列算法的重力匹配方法;其特征在于， 令： 上一時刻載體在重力背景圖中所有可能的真實位置的網格點的集合作為已訪問點集； 當前時刻載體在重力背景圖中所有可能的真實位置的網格點的集合作為未訪問點集； 采用如下步驟實現： 步驟一、建立初始時刻k=O時的已訪問點集;具體為： 根據慣性導航系統提供的載體初步位置信息和載體慣導位置誤差特性，在重力數據背 景圖模塊中的重力背景圖上確定載體真實位置所在范圍，并將該范圍內的所有網格點組成 的點集標記為初始時刻的已訪問點集： 已訪問點集中的每個網格點均包含當前網格點i的位置信息和權值信息；ie(l，2，···， η)，n為網格點的總數； 步驟二、令k = k+1;建立未訪問點集： 根據慣性導航系統提供的載體慣導位置誤差特性以及第k時刻將要達到的位置信息， 在重力背景圖上確定載體真實位置所在范圍，并將所述范圍內的所有網格點組成的點集標 記為第k時刻的未訪問點集； 步驟三、選取未訪問點集中的任意一個網格點，定義為網格點j，與已訪問點集中的任 意一個網格點，形成可能路徑線段； 531、 利用相關序列分析函數，根據步驟一中獲得的位置信息，得到可能路徑線段的權 值； 532、 通過最短路徑算法，根據步驟一中獲得的權值信息，得到網格點j的權值； 533、 根據S31和S3 2的方法，遍歷未訪問點集中的所有網格點與已訪問點集中所有網格 點，并相應的形成可能路徑線段，并獲得未訪問點集中的所有網格點的權值； 步驟四、比較未訪問點集中的所有網格點的權值，將權值最小的網格點作為載體位置 的最優估計值； 步驟五、將最優估計值反饋至慣性導航系統，慣性導航系統利用得到的最優估計值對 其采集獲得的慣導數據進行修正，并將修正后的結果輸出，作為載體的真實位置估計； 步驟六、將未訪問點集標記為已訪問點集;返回步驟二。2. 如權利要求1所述的重力匹配方法，其特征在于:所述步驟二中：如果當前載體真實 位置所在范圍的網格點的重力異常值超出重力異常可能范圍，則將所對應的網格點從未訪 問點集中刪除；其中，重力異常可能范圍是根據重力實時測量信息和重力測量誤差特性確 定的。3. 如權利要求1所述的重力匹配方法，其特征在于:將步驟四中獲得的最優估計值輸出 后，采用標準卡爾曼濾波方法對最優估計值進行濾波分析，并將濾波后得到的輸出值替代 步驟五中的最優估計值，執行步驟五。
【文檔編號】G01C25/00GK105928541SQ201610224078
【公開日】2016年9月7日
【申請日】2016年4月12日
【發明人】王博, 韓雨蓉, 鄧志紅, 肖烜
【申請人】北京理工大學