海底地形匹配高效质点滤波导航方法

马 腾，丁硕硕，范佳佳，李 晔

（1.哈尔滨工程大学 水下机器人技术国家级重点实验室，黑龙江 哈尔滨 150001；2.哈尔滨理工大学 自动化学院，黑龙江 哈尔滨 150001）

0 引言

自主水下机器人（Autonomous Underwater Vehicle，AUV）是执行海底矿物勘探、水下管道检测、海洋环境调查等任务的重要载体平台[1]。AUV能否高效、精确地执行海底勘探等任务取决于其导航精度[2]。在传统导航方法中，由于无线电讯号在水下衰减严重，全球定位系统（Global Positioning System，GPS）无法在AUV水下航行时为其提供精确定位信息。惯性导航系统（Inertial Navigation System，INS）不仅需要价格昂贵的多普勒计程仪（Doppler Velocity Log，DVL）和航姿参考系统（Attitude and Heading Reference System，AHRS）辅助，其导航误差累积特性也导致其难以作为AUV长时间水下航行的精确导航手段[3]。长基线（Long Baseline，LBL）、超短基线（Ultrashort Baseline，USBL）等声学导航手段可在AUV水下航行过程中提供具备有界误差的精确导航结果。但LBL声学导航需要提前于海底布设多个声信标，使用USBL实现精确导航则需要AUV保持同支持母船的通讯，两者均使AUV作业范围受限严重[4]。海底地形匹配导航等地球物理导航手段，不依赖于卫星、声学信标或支持母船等外部辅助，不存在累积误差，可实现AUV长时间、全天候、远距离精确导航，已经得到了世界各国研究人员的广泛重视[5]。

目前，国内外研究人员已利用多波束测深探测仪（Multibeam Echosounder，MBES）针对海底地形匹配导航方法展开大量理论和试验研究[6-7]。海底地形匹配导航最早是通过卡尔曼滤波（Kalman Filter，KF）[8]、扩展卡尔曼滤波（Extended Kalman Filter，EKF）[9]、无迹卡尔曼滤波（Unscented Kalman Filter，UKF）[10]等参数滤波方法展开的。与传统KF方法中以观测到的水深值为测量更新不同，Li Peijua[8]等提出将迭代最近轮廓点（Iterated Closest Contour Point，ICCP）匹配结果与INS定位结果差值作为KF测量值，然后通过滤波器反馈校正及时校正SINS的累积误差。

随着计算机技术和非线性优化技术的不断进步，粒子滤波器（Particle Filter，PF）、质点滤波器（Point-Mass Filter，PMF）逐渐成为海底地形匹配导航问题的重要解决方案。Jindřich Duník[11]等提出通过 RaoBlackwellized质点滤波器解决海底地形匹配导航问题，其通过回放式仿真试验验证质点滤波器在解决地形匹配导航问题时具备较高精度，对质点初始化结果有极强鲁棒性，计算复杂度较小且可通过理论推导预测。Georgios Salavasidis[12]等提出以声学高度计为地形获取手段，通过PF方法实现为期77小时的AUV长航程精确水下导航。周天[13]等在用于海底地形匹配导航的PF 中引入了变栅格尺寸的KLD（Kullback–Leibler distance）方法，极大降低了算法运行所需的资源消耗。针对MBES测量过程中存在的地形畸变现象，Peng Dongdong[14]等提出利用Huber损失函数修改粒子权重计算方程，从而在重采样过程中减少离群值对估计值的影响。

由于具备对非高斯噪声和非线性运动模型的出色拟合能力，粒子滤波和质点滤波技术逐渐成为海底地形匹配导航方法研究的主流，而考虑 AUV机载计算机数据处理能力受限，质点滤波在海底地形匹配导航中具备更强的实用性。但如图1所示，MBES测绘得到的海底地形测点空间分布不规则，而先验地图则往往是网格化栅格数据，导致在质点权重计算过程中，实测地形测点与先验数据对应关系计算中存在大量插值过程，造成极大计算消耗。

图1 先验地形图与MBES实测地形数据Fig.1 A priori bathymetric map and bathymetric data measured using a MBES

为实现AUV的实时海底地形匹配精确导航，本文设计了一种海底地形匹配高效质点滤波导航方法。本文提出通过构建伪输入高斯过程（Sparse Pseudo-input Gaussian Processes，SPGPs）模型实现对海底地形深度的高效、准确估计，从而将MBES实时测绘地形数据网格化形成概率地图；提出适用于概率地图的质点权重计算模型，并完成了基于概率地图的海底地形匹配质点滤波算法框架设计。最后，通过船载MBES采集的海试数据，对所提出的基于 SPGPs的测深数据网格化方法和海底地形匹配质点滤波方法进行了回放式仿真数据验证。

1 测深数据网格化概率地图构建方法

实测海底地形数据的网格化是质点滤波器中最为耗时的过程。传统网格化方法包括反距离加权（Inverse Distance Weighted，IDW）插值[15]、克里金（Kriging）插值法[16]等，其中，IDW 插值忽略了地形测点之间的相关性关系，导致插值结果误差严重，而Kriging插值法虽然考虑了地形测点相关性，但其计算结构较IDW插值法更为复杂，计算效率较低。

本文提出通过 SPGPs回归方法实现网格化插值。SPGPs回归方法其实质是利用少量伪输入点代替全部历史数据实现高斯过程（Gaussian Process，GP）回归中的 GP模型训练[17]。因而本文首先对GP回归在网格化插值中的应用进行介绍。

GP模型可以看作一个输入为历史地形测深数据、输出为多元高斯分布均值和方差的函数。换而言之，在包含N个地形测点的海底地形测深数据集中，假设N个测点的平面坐标和深度分别为和，则可将其视为无限维度中从1个N变量高斯分布中取样的单点，即N个测点的深度总是满足1个联合高斯分布N(μ,Σ)，其中μ和 Σ 分别为高斯分布的均值和方差。在GP模型中，由于地形测深数据输入前会减去其地形深度均值，通常取μ=0，而Σ则由N个测点间相关性计算得到，且与测点水平位置的差值成正相关。

实现，该计算可以通过Gauss-Newton方法实现。

完成GP模型构建后，利用该模型实现网格点地形高程插值。假设网格点水平坐标为x*，其水深值z*满足以下多元高斯分布：

图2 高斯过程回归的地形水深估计流程图Fig.2 Flowchart of terrain depth estimate using the GP regression method

考虑采用全部历史MBES测深数据训练GP模型会造成巨量计算消耗，在GP回归算法的基础上，本文提出在全部历史 MBES测深数据中提取能够保留大部分信息的少量伪输入点，通过伪输入点训练 SPGPs模型，从而实现测深数据的高效网格化操作。

假设M个伪输入点（M远小于N）水平面坐标为，则通过最大化边缘概率：

即可同时求取X′和超参数θ。式（9）中，j∊[1,M]；IN为大小为N×N的单位矩阵；Λ=diag(λ)且λ={λ1,λ2,...,λN}，其中，。

在通过Gauss-Newton方法求解式（9）后，水平坐标为x*的网格插值点水深值z*满足均值为*μ，均值为Σ*的高斯分布，且：

式（10）和式（11）中：

2 质点滤波算法

本文所设计的海底地形匹配质点滤波算法框架如图3所示。

图3 质点滤波算法框架Fig.3 Framework of point-mass filter algorithm

传统质点滤波中通过计算MSD（Mean Square Displacement）算子实现质点权重计算。假设质点i在t时刻大小为N1×N2的网格化地图实测水深表示为h′(i,j)(i=1,2,...,N1,j=1,2,...,N2)，对应位置的先验地形图水深值为h(i,j)，则其质点i权重wi为

但是，以上方法忽略了实测地图网格化过程中不同网格节点的置信度差别。如式（11）所示，SPGPs回归方法的一大优势在于其可以在网格化插值过程中构建概率地图，即对各个插值节点的方差Σi,j进行估计。因而本文提出一种适用于概率地图的质点权重计算方法，即将式（14）改写为

式中，克罗内克函数δ(i,j)为

3 回放式仿真试验

如图4所示，于青岛中沙礁海域开展了船载MBES的数据获取海上试验。试验载体船与设备布置如图5所示。

图4 海试试验区域Fig.4 Sea trial area

图5 试验船只与设备布置Fig.5 Experimental vessel and equipment layout

如图6所示，试验使用T–sea CMBS200在线MBES作为海底地形数据测绘设备。T–sea CMBS200每次发射192个声学脉冲信号，测线宽度约为MBES距海底距离的10倍，水平波束宽度优于1°，垂直波束开角优于 2°，水平视场角≥140°，测距离≥300 m，距离分辨率优于2 cm，作业频率为4 Hz。试验过程中载体运动状态信息由图 7所示的StarNeto XW–GI5651导航系统给出，在GPS信号辅助下，该系统艏向角测量误差优于0.1°，横倾、纵倾角测量误差优于 0.01°，水平面定位误差小于1 m，作业频率为10 Hz。

图6 T–sea CMBS200在线多波束声呐Fig.6 T-sea CMBS200 online MBES

图7 StarNeto XW–GI5651导航系统Fig.7 StarNeto XW-GI5651 navigation system

采用海上试验数据开展回放式仿真试验。轨迹长度约为1 800 m，载体航速约为4 kn，数据更新频率为4 Hz，以GPS轨迹作为真实轨迹，惯性导航轨迹通过式（17）进行模拟。

在轨迹上每隔25 s选取1个地形导航点，并进行1次质点权重计算和重采样，输出海底地形匹配质点滤波导航结果。轨迹中共存在47个地形导航点，粒子数设定为500个，惯导系统初始误差约为9.5 m。

如图8所示，在第5个地形导航点后滤波器中质点分布高度收敛于载体实际位置处，质点滤波器输出结果保持稳定。如图9和图10所示，海底地形匹配质点滤波导航结果与 GPS结果高度吻合，在任务过程当中的47个地形导航点上，惯性导航系统和本文提出的地形匹配导航系统提供的 AUV定位误差均值分别为25.39 m和1.46 m，中位数分别为26.38 m和1.26 m。算法在MATLAB平台下执行耗时为69.63 s，且其中53.14 s用于概率地图构建，总耗时占任务时间比例少于8%。

图8 任务过程中质点分布变化Fig.8 Changes in mass point distribution during the mission

图9 导航系统输出轨迹结果Fig.9 Trajectories yielded by navigation systems

图10 任务过程中载体定位结果Fig.10 Location errors of the vessel during the mission

试验结果证明：所提出海底地形匹配高效质点滤波导航方法可以在低成本惯性导航系统辅助下为AUV提供在线的精确导航信息。

4 结论

本文提出了一种海底地形匹配高效质点滤波导航算法，并通过海测数据回放式仿真试验验证，得到以下结论：

1）相较于IDW插值和Kriging方法，SPGPs回归用于网格化插值可同时达到插值精度和计算效率的最优；

2）所提出海底地形匹配质点滤波导航方法可获得精确定位结果，其实时导航圆概率误差小于2 m；

3）所提出海底地形匹配高效质点滤波导航方法耗时远小于任务时常，可实现 AUV在线精确导航。