基于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-), 男, 在读硕士, 主要研究方向为兵器制导与控制技术.