一种基于声呐的水下机器人连续占据建图方法

技术领域

本发明属于水下机器人感知技术领域，主要涉及一种基于声呐的水下机器人连续占据建图方法。

背景技术

近年来，世界各国对海洋安全、海洋资源和海洋经济等方面日益重视，高精度的海底地图可以为执行海洋任务的水下自主航行器(AUV)等海洋机器人提供导航和路径规划的依据和参考，具备十分重要的意义。目前海底地图的大规模构建主要是通过卫星遥感的方法实现的，但分辨率约为5公里，只有小部分海底环境地图是通过现代水下传感设备精确构建的。

水下环境光线不足、海水传导率较高，光学传感器与激光雷达难以在AUV上使用，用于水下建图的传感器主要为声呐。近年来，国内外围绕基于声呐图像的水下建图方法已经进行了一系列的研究工作[1]。国外关于水下建图方法的研究工作主要集中于美国、澳大利亚和西班牙等国家，国内相应的研究机构有中国海洋大学、哈尔滨工程大学等[2]。目前所研究的水下建图方法主要适用于人工结构的水下环境，如水池、码头等，且主要基于人工设定的路标或者是水下环境四周明显的点、线等几何特征实现AUV的建图，如特征地图、拓扑地图和语义地图等。然而，对于缺乏有效环境特征点的复杂海底环境，利用明显的几何特征建立其环境模型是非常困难的，因此以上几种地图很难在海底环境应用。

目前对AUV构建自然海域的地形地图的研究还相对较少，在大尺度、自然海域的实时建图应用场景下，现有方法以水深测量地图(bathymetric map)为代表，这类水下占据栅格地图的分辨率/精度偏低，容易受到测量异常值和噪声影响，对未知区域和被占据区域的占据情况和不确定性没有预测能力，多用于精度和航行时间要求不高的近海任务场景。

连续占据建图(Continuous Occupancy Mapping，COM)是一种基于位置的概率性描述的连续建图技术，采用了监督学习方法，可以得到任意位置的占据水平及其不确定性[3][4]。即使在观测相对较少的情况下，该方法也能在遮挡区域和传感器之间提供含关联方差的占据概率推断，非常适用于未知环境和非结构化环境中的机器人定位导航和路径规划。与传统离散的占据栅格地图相比，它不再受限于离散的、分辨率固定的网格，可以生成任意分辨率的地图，地图也更精确，对观测噪声、异常值等因素也更具鲁棒性。

连续占据建图技术目前仍处于研究起步阶段，主要面向基于激光雷达和深度摄像机RGB-D的地面机器人任务场景，而相对复杂的水下场景鲜有涉及，目前国内外关于如何基于声呐进行水下连续占据建图的研究则更是十分少见。

[参考文献]

[1]Wilson T,Williams S B.Adaptive path planning for depth-constrainedbathymetric mapping with an autonomous surface vessel[J].Journal of FieldRobotics,2018,35(3):345-358.

[2]张书景.大尺度环境中自主式水下机器人同时定位与地图构建算法研究[博士学位论文],青岛:中国海洋大学,2014.

[3]O'Callaghan S T,Ramos F T.Gaussian process occupancy maps[J].International Journal of Robotics Research,2012,31(1):42-62.

[4]C.Rasmussen&C.Williams,Gaussian Processes for Machine Learning,MITPress,2006.

发明内容

本发明的目的在于，针对目前复杂海洋环境建图方法中存在的问题，提供一种基于声呐的水下机器人连续占据建图方法，将高斯过程连续占据建图(Gaussian ProcessContinuous Occupancy Mapping,GPCOM)技术应用到水下声呐建图，结合Rao-Blackwellized粒子滤波(RBPF)方法，最终通过多个有效粒子构建的局部地图融合得到全局连续占据地图。

为了解决上述问题，本发明提供了如下技术方案：

一种基于声呐的水下机器人连续占据建图方法，包括以下步骤：

步骤1：初始化Rao-Blackwellized粒子滤波中的粒子数量M、粒子状态和迭代次数N

步骤2：通过AUV运动模型和步骤1中所述的扩展卡尔曼滤波，预测更新每个粒子携带的机器人位置和姿态信息；

步骤3：滤除声呐数据中的周围杂波和海底回波成分；采用光线投射法(Raycasting)以固定分辨率λ

具体地说，在一帧由n

步骤4：将步骤3得到的离散地图数据集

具体地说，将子数据集

给定训练数据集

其中σ

训练数据集

对于占据地图二值分类问题，将以上求得的高斯过程回归结果压缩到[0,1]区间即可得到高斯过程分类结果。本发明中可以使用概率最小二乘函数：

其中Φ(·)为累积高斯分布，参数α和β可通过交叉验证方法确定。

于是可以得到每个粒子对应的子数据集

步骤5：对于每个粒子构建的GPCOM地图m

其中σ

步骤6：权重更新完毕后，对所有完成位姿和权重更新的粒子进行序列重要性重采样(Sequential Importance Resampling,SIR)，达到迭代次数N

步骤7：将每个建图周期构建的局部GPCOM地图m(k)按AUV的位姿时序进行全局融合，最终可得到全局GPCOM地图m。

与现有技术相比，本发明的有益效果为：

本发明实现了一种基于声呐的水下机器人连续占据建图方法，与传统离散占据栅格建图方法相比，基于高斯过程分类的水下连续建图方法利用了海底环境的空间依赖关系，提高了对声呐观测噪声和异常值的鲁棒性，同时保证了较高的建图精度；可以预测不能直接观测区域的占据情况和不确定性，这对海洋机器人在未知非结构化环境中的定位导航和路径规划至关重要；采用Rao-Blackwellized粒子滤波方法，将观测结果与每个粒子构建的局部GPCOM地图进行对比加权和序列重要性重采样，提高了建图精度；在每个建图周期内，通过采集的声呐数据构建训练数据集，并根据粒子数量划分子数据集，由局部GPCOM地图融合全局地图，复杂度由O(n

附图说明

图1为本发明方法中的声呐、IMU等在水下机器人上的安装位置及其坐标系示意图。

图2为本发明方法中声呐使用光线投射法的示意图。

图3为本发明方法中声呐测量物体距离和方位示意图。

具体实施方式

下面结合附图对本发明方法的实施方式做详细说明。

本发明的原理准备如下：

在水下无人潜航器(以下简称“AUV”)上，声呐按图1安装在机身的头部，深度计和惯性测量单元IMU按图1安装在机身的中心位置，多普勒速度计DVL按图1安装在机身的头部，各自的坐标系如图1所示，各坐标系之间存在欧氏变换关系。

Rao-Blackwellized粒子滤波(RBPF)：

该方法引入了Rao-Blackwellized分解思想，是一种混合滤波算法，它将机器人在线估计问题在形式上分解为可用粒子滤波(PF)估计的部分和可用扩展卡尔曼滤波(EKF)估计的部分，打破了使用单一滤波算法局限性，成为机器人定位与地图构建领域的重要方法之一。

本发明的具体步骤包括：

步骤1，初始化Rao-Blackwellized粒子滤波中的粒子数量M、粒子状态和迭代次数N

其中P

步骤2，通过AUV运动模型和步骤1中所述的扩展卡尔曼滤波，预测更新每个粒子携带的机器人位置和姿态信息；

为便于描述，本具体实施例采用匀速运动模型，状态预测方程为：

其中噪声N(k)＝[N

通过从EKF中随机采样世界坐标系x和y方向的速度噪声N

其中：

其中Δt为时间间隔，R(k)为机体系到世界系的旋转矩阵，由滚转角φ

预测过程完成后，EKF状态更新过程可表示为：

z(k)＝C(k)x

其中z(k)为关于绝对深度、机体系速度和加速度的测量，C(k)为观测矩阵，在本实施例中为单位阵，ε(k)为测量噪声。

最终可得到保存在控制输入向量u(k)的滚转角、俯仰角和偏航速率，从而计算出每个粒子携带的机器人位置和姿态信息。

步骤3，滤除声呐数据中的周围杂波和海底回波成分；采用光线投射法(Raycasting)以固定分辨率λ

具体地说，如图2和图3所示，在一帧由n

步骤4，将步骤3得到的离散地图数据集

具体地说，将子数据集

给定训练数据集

其中σ

训练数据集

本实施例中采用了非平稳的Matern核函数：

其中距离r＝|x

核函数的超参数

其中K为训练数据集

对于占据地图二值分类问题，将以上求得的高斯过程回归结果压缩到[0,1]区间即可得到高斯过程分类结果。本实施例中使用了概率最小二乘函数：

其中Φ(·)为累积高斯分布，参数α和β可通过交叉验证方法确定。本实施例中采用了LOO(leave-one-out)交叉验证方法。

于是可以得到每个粒子对应的子数据集

步骤5，对于每个粒子构建的GPCOM地图m

其中σ

步骤6，权重更新完毕后，对所有完成位姿和权重更新的粒子进行序列重要性重采样(Sequential Importance Resampling,SIR)，达到迭代次数N

步骤7，将每个建图周期构建的局部GPCOM地图m(k)按AUV的位姿时序进行全局融合，最终可得到全局GPCOM地图m。

本实施例中采用BCM(Bayesian committee machine)方法由局部地图超参数以概率方式近似全局地图的超参数，从而实现融合，得到全局GPCOM地图及其不确定性。以数据集

其中p为子数据集的个数，

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是局限性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护范围之内。