基于PMF和TERCOM組合算法的水下地形匹配技術

劉 洪, 高永琪, 諶 劍

?

基于PMF和TERCOM組合算法的水下地形匹配技術

劉 洪, 高永琪, 諶 劍

(海軍工程大學兵器工程系, 湖北武漢, 430033)

由于自主式水下航行器的導航受海水環境的影響, 其定位精度不高, 目前較好的解決方法之一是采用水下地形匹配輔助導航系統。本文針對該系統最核心部分的匹配算法, 提出了一種定位精度更高的組合算法。為了解決質點濾波(PMF)算法存在的計算量大、收斂速度慢和對初始誤差比較敏感的問題, 提出了一種將地形輪廓匹配(TERCOM)算法和PMF算法進行組合的方案。首先分析了搜索區域分辨率對PMF算法的影響, 然后分別對以TERCOM為粗匹配、PMF為精匹配和以PMF為粗匹配、TERCOM為精匹配的兩種組合算法進行了仿真比較。結果表明, 第1種組合算法無論在大起伏地形還是在平坦地形中都顯示出了更快的收斂性和更高的定位精度, 是一種具有較高工程應用價值的算法。

自主式水下航行器; 導航定位; 質點濾波算法; 地形輪廓匹配算法; 地形匹配

0 引言

自主式水下航行器(autonomous underwater vehicle, AUV)的導航問題一直是各國研究的重點, 由于受海水環境的影響, 其導航定位一直比較困難。目前比較熱門的解決方法之一就是采用水下地形匹配輔助導航系統。作為該系統最核心部分的匹配算法, Klass早在1974年就提出了地形輪廓匹配算法(terrain contour matching, TERCOM)算法。Hostetler 和Andreas在1983年提出桑迪亞(sandia inertia terrain-aided navigation, SITAN)算法。1986年Smith Reshelf M和Cheeseman P提出了引起許多學者關注的同時制圖定位(simultaneous localization and mapping, SLAM)技術。1993年Cordon等人提出了采樣重要性重采樣算法, 較好地解決了粒子濾波的粒子退化問題, 使粒子濾波成為近年來的熱門研究問題之一。

目前美國、英國、日本和挪威已經將卡爾曼濾波、TERCOM以及粒子濾波等算法在AUV上進行了相關的海上試驗。

質點濾波(point-mass filter, PMF)算法具有定位精度較高、地形適應性較好等優點, 同時該算法也存在計算量大、收斂速度慢、定位精度容易受初始誤差影響等缺點。Bergman就飛行器地形輔助導航問題, 研究并仿真證明了PMF算法的可行性。本文則以水下地形輔助導航為應用背景, 分析了搜索區域分辨率對該算法的影響, 并著重研究了不同地形特點對PMF組合算法性能的影響。

1 PMF算法

1.1 算法原理

PMF算法是一種基于遞推計算的貝葉斯濾波算法, 它將矩形搜索區域離散化為有限個小網格, 從而使遞推貝葉斯積分方程簡化為在搜索區域上的有限網格黎曼和。將狀態變量在每個網格的概率作為網格點的權值, 用這些帶權值的網格點集來逼近后驗概率密度, 從而不受非線性、非高斯問題的限制, 從理論上解決了由于非線性模型的線性化近似所帶來的各種問題, 并且達到減小解算時間和復雜度的目的。

給定2D非線性模型

(2)

(4)

測量更新

將權值進一步歸一化

(6)

1.2 搜索區域分辨率對PMF算法的影響

圖1 搜索區網格節點圖

一般情況下, 匹配算法的定位精度除了算法本身的影響外, 另外一個很重要的影響因素就是數字地圖的分辨率。在算法一定的情況下, 數字地圖分辨率越高, 其定位精度也越高。而由于PMF算法的搜索區域被離散為若干個網格節點, 若匹配點和真實位置點在同一個網格內的不同地方, PMF算法會認為此次匹配誤差為0, 但實際它是存在誤差的, 誤差大小與網格邊長值有關。所以該算法的定位精度不但受數字地圖分辨率的影響還受搜索區域分辨率的影響。數字地圖制作好以后, 其分辨率也就固定下來了, 如果要在此基礎上提高其分辨率必須通過插值方法, 不過其誤差也較大。

為了從工程應用的角度探討搜索區域分辨率對PMF算法的影響, 本文在數字地圖分辨率一定的情況下, 將搜索區域分辨率分別設置為數字地圖的0.5倍、1倍、2倍和4倍, 然后再進行仿真, 仿真所用的數字地圖為peaks函數所產生的, 其分辨率為4 m, , 仿真結果如圖2。整個過程的仿真時間為150 s, 航行過程中每秒鐘匹配一次。收斂后4種方法分別得出了80個最佳匹配位置點, 對所有最佳匹配點的誤差進行統計分析, 結果如表1所示。

表1 誤差統計表

圖2 不同分辨率匹配誤差比較

從圖2和表1可知, 4種不同分辨率的收斂速度基本一致。不過在定位精度上, 搜索區域的分辨率越高其定位精度越高。搜索區域分辨率為0.5倍時, 匹配精度最低, 且前期波動幅度較大, 收斂后的誤差約為11.5ｍ, 比較穩定。分辨率為1倍時, 定位精度約為5.７m, 仿真時間和0.5倍時相差不大。而分辨率為2倍和4倍時, 定位精度分別為3.2 m和1.9 m。不過計算量后者是前者的10倍以上, 進行150次匹配仿真所用時間分別為1.709 s和20.414 s, 并且精度提高不是十分明顯。這主要是因為在計算時間上, 由于分辨率翻倍, 其數據量增加為原來的4倍, 而PMF算法中存在卷積處理, 所以耗時將遠遠超過4倍。在定位精度上, 分辨率由0.5倍到1倍時精度提高得最快, 1倍到2倍時次之, 2倍到4倍時提高得不是十分明顯。這說明了當搜索區域的分辨率比數字地圖高時, 雖然能提高定位精度, 但是會因為數字地圖分辨率的限制而提高得越來越不明顯, 其中匹配所耗時間卻將越來越多。所以綜合考慮定位精度和匹配的實時性, 搜索區域的分辨率為數字地圖的2倍時比較合適。

2 組合PMF算法及實現流程

2.1 組合PMF算法

由于PMF算法的計算量大、收斂速度較慢、對初始誤差比較敏感, 但地形適應性較強。而TERCOM算法在無旋轉偏差時精度較高, 且算法簡單、計算量小、收斂速度快, 但地形適應能力較低。針對上述2種算法各自的優缺點, 提出了2種組合算法, 來解決收斂速度、定位精度和計算量的問題, 其基本方法如下。

1) TERCOM+PMF算法

用TERCOM算法作預匹配進行初始搜索, 然后將其結果作為PMF算法的初始輸入值, 既大大減小PMF算法的搜索區范圍, 降低了計算量, 提高了算法的收斂速度, 又降低了初始誤差對PMF算法的影響。將判斷準則引入算法中作為該算法的防誤匹配機制, 當PMF算法的匹配結果不滿足判斷準則時就認為此次匹配無效。這樣就減小了匹配誤差, 提高了算法精確性。這種組合適用于初始誤差中平移分量大的情況。

2) PMF+TERCOM算法

將PMF算法作為初始匹配, 并利用判斷準則對其進行防誤匹配處理, 然后將其匹配值作為TERCOM算法的初始值。由于TERCOM算法的計算量較小, 可以大大減少匹配計算時間, 并且在無旋轉偏差或偏差很小時能獲得一個比較高的匹配精度。這種組合適用于運動速度快、實時性強的情況。

TERCOM算法是在存儲地形圖中找出一列路徑, 這些路徑平行于導航系統指示的路徑。接著根據一種性能指標, 檢驗從地圖中提取的地形剖面與實測地形剖面的相關程度, 從中選相關程度最高的一條作為最佳匹配剖面。性能指標采用均方差(mean square difference, MSD)算法, 定義

2.2 組合PMF算法的實現流程

1) 由AUV的INS系統獲得當前的實時位置、速度、姿態、航向和加速度等相關信息;

2) 由測潛測深儀得到AUV當前位置區域的海底深度數據, 即為實時圖;

3) 以INS系統指示位置為基準, 劃定搜索區的大小, 讀取數字地圖中相應數據作基準圖;

4) 采用TERCOM算法(或PMF算法)進行全局搜索, 找到一個離實際點較為接近的匹配位置。

5) 將TERCOM算法(或PMF算法)輸出的匹配位置作為初始位置, 然后采用PMF算法(或TERCOM算法)進行匹配計算, 通過判斷準則剔除匹配野值, 直到達到要求的匹配時間為止。

2.3 判斷準則的設定

(9)

如果式(8)滿足, 則認為此估計值反映了真實INS的定位誤差。令

在接下來的地形高度匹配過程中, 搜索窗口的中心移動到經過用估計值修正過的位置。

(11)

3 組合PMF算法實現及仿真分析

使用2塊真實水下數字地圖來進行仿真, 圖3所示的數字地圖地形起伏較大, 圖4所示的數字地圖地形較為平坦, 其分辨率都為20 m, 大小為10 km×10 km。

圖3 大起伏地形

圖4 平坦地形

搜索區域的大小為420 m×420 m, PMF算法搜索區域的分辨率為地圖的2倍。航行器的實際航跡為平行方向航行, 速度為9 m/s。PMF算法匹配速度為每3 s匹配一次。由于測量誤差與深度有關, 在大起伏地形和平坦地形中航行時的測量噪聲分別是協方差為20和2的白噪聲, 過程噪聲是協方差在和方向均為20的白噪聲。

在大起伏地形中航行時, 初始位置偏差在和方向均為200 m, 真實的位置起點為(1 200 m, 4 200 m), 仿真結果如圖5和圖6。在平坦區航行時初始位置偏差在和方向均為100 m, 真實位置起點為(1 000 m, 4 500 m), 仿真結果如圖7和圖8。

圖5 大起伏地形下匹配航跡圖

圖6 大起伏地形下X方向誤差比較

圖7 平坦地形下匹配航跡圖

圖8 在平坦地形下X方向誤差比較

通過對匹配航跡和方向誤差曲線圖的分析可知, 在大起伏地形中, AUV進入匹配區后, 2種改進算法都可以迅速進入收斂狀態, 收斂后的定位精度TERCOM+PMF算法略比PMF+ TERCOM算法高, 這說明了TERCOM算法在大起伏地形中也具有較高的定位精度; 在平坦地形中, TERCOM+PMF算法的收斂性明顯比PMF+ TERCOM算法好, 并且定位精度和魯棒性也明顯好于后者, PMF+TERCOM算法在進入2500m~ 4 000 m和5 000 m~6 000 m更平坦區域時波動突然變大, 說明了TERCOM算法雖然收斂速度很快, 但是地形適應性比PMF算法差很多。這主要是因為PMF算法是基于遞推貝葉斯的匹配算法, 必須知道上一時刻的相關先驗信息, 才能進行當前時刻的匹配位置估計。所以當前的狀態估計與以前的狀態信息有關, 它不會因為當前時刻地形的變化或其他誤差因素而使定位精度突然降低, 故其地形適應能力較好; 而TERCOM算法是對當前數據序列直接進行相關處理, 與以前的位估計信息無關, 所以當前地形變化直接影響該算法的定位精度, 從而使其地形適應性不高, 不過地形信息較好時會馬上進入收斂狀態。

整個仿真共進行了250次匹配計算。其中, TERCOM+PMF算法平均耗時0.482841 s, 而PMF+TERCOM算法平均耗時0.690 571 s, 說明當PMF算法作為精匹配時, 由于搜索范圍減半, 大大減小了匹配時間。

綜上可知, TERCOM+PMF算法無論在大起伏地形還是在較平坦地形中都能獲得良好的收斂性和定位精度, 具有較好的工程應用價值。

4 結束語

本文通過對PMF算法的分析, 提出了2種組合匹配算法。在真實水下數字地圖的基礎之上, 對2種算法進行了仿真計算, 結果顯示ERCOM+PMF算法效果比較理想, 證明了該算法具有更廣的應用價值。下一步工作主要是對2種改進算法進行抗干擾性驗證和分析。

[1] 劉承香. 水下潛器的地形匹配輔助定位技術研究[D]. 哈爾濱: 哈爾濱工程大學, 2003.

[2] 劉準, 侶文芳, 陳哲. 海底地形匹配技術研究[J]. 系統仿真學, 2004, 16(4): 700-702.Liu Zhun, Si Wen-fang, Chen Zhe. Research on Technology in Sea Bottom Terrain Match[J]. Journal of System Simulation, 2004, 16(4): 700-702.

[3] 陳紹順, 李彥斌, 李云. 地形匹配制導技術研究[J]. 制導與引信, 2003, 24(3): 17-21.Chen Shao-shun, Li Yan-bin, Li Yun. Study on the Technology of Terrain Contour Matching Guidance[J]. Guidance & Fuze, 2003, 24(3): 17-21.

[4] Hicks S. Advanced Cruise Missile Guidance System Description [C]//Proceedings of the IEEE 1993 National Aerospace and Electronics Conference. Naecon, 1993: 355-361.

[5] Larry D H, Ronald D A. Nonlinear Kalman Filtering Techniques for Terrain-aided Navigation[J]. IEEE Transactions on Automatic Control, 1983, 28(3): 315-323.

[6] Smith R, Self M, Cheeseman P. Estimating Uncertain Spatial Relationships in Robotics[J]. Autonomous Robot Vehicles Springer Verlag, 1990, 21(5): 167-193.

[7] Doucet A, Godsill S, Andrieu C. On Sequential Monte Carlo Sampling Methods for Bayesian Filtering[J]. Statistics and Computing, 2000, 10(3): 197-208.

[8] Niclas B. Bayesian Estimation Navigation and Tracking Application[D]. Linkoping Studies in Science and Technology. Dissertations. 1999: 1-46.

[9] Hagen O, ?nonsen K, Mandt M. The HUGIN Real-time Ter-rain Navigation System[C]//MTS/IEEE Seattle, Oceans 2010 Seattle, 2010.

[10] Morice C, Veres S, McPhail S.Terrain Referencing for Autonomous Navigation of Underwater Vehicles[C]//Oceans 2009 Europe, 2009.

[11] Taylor M, Wilby A. Design Considerations and Operational Advantages of a Modular AUV with Synthetic Aperture Sonar[C]// OCEANS`11 MTS/IEEE Kona, 2011.

[12] Kondo H, Maki T, Sakamak U T. AUV Navigation Based on Multi-sensor Fusion For Breakwater Observation[C]//Interna- tional Symposium on Automation and Robotics in Construction 2006, 2006.

[13] Bergman N. Recursive Bayesian Estimation Navigation and Tracking Applications[D]. Dissertation, Department of Electrical Engineering, Linkoping University, Sweden, 1999.

[14] Anonsen K, Hallingstad O. Terrain Aided Underwater Navigation Using Point Mass and Particle Filters[C]//Position, Location, and Navigation Symposium, 2006 IEEE/ION.

[15] 馮慶堂. 地形匹配新方法及其環境適應性研究[D]. 長沙: 國防科技大學, 2004.

(責任編輯: 楊力軍)

Underwater Terrain Matching Techniques Based on Combination of PMF and TERCOM Algorithms

LIU Hong, GAO Yong-qi, SHEN Jian

(Department of Weaponry Engineering, Naval University of Engineering, Wuhan 430033, China)

Aiming at the problems that point-mass filter(PMF) algorithm is of great computation, slow convergence and initial error sensitivity, a combination scheme of terrain contour matching(TERCOM) algorithm and PMF algorithm is proposed. Firstly, the influence of search area resolution on the PMF algorithm is analyzed. Then a combination scheme taking TERCOM for coarse matching and PMF for fine matching is simulated and compared with the other scheme taking PMF for coarse matching and TERCOM for fine matching. The result shows the former scheme behaves better in convergence and positioning accuracy for both rough and flat terrains with a good engineering application.

autonomous underwater vehicle(AUV); navigation positioning; point-mass filter(PMF) algorithm; terrain contour matching(TERCOM) algorithm; terrain matching

TJ630.33; U666.12

A

1673-1948(2012)06-0437-06

2012-05-17;

2012-06-19.

劉 洪(1988-), 男, 在讀碩士, 主要研究方向為兵器制導與控制技術.