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

Abstract

本发明公开了一种基于声呐的水下机器人连续占据建图方法，包括步骤：A.初始化粒子滤波器中的粒子数量和状态；B.预测更新AUV的位姿信息；C.声呐数据预处理，获取包含均匀采样点位置和占据状态的数据集；D.为每个粒子分配子数据集，基于高斯过程分类模型得到局部高斯过程连续占据地图；E.通过各个粒子所构建地图和实际测量的偏差计算更新所有粒子的权重；F.进行序列重要性重采样，获取有效粒子，并将有效粒子构建的地图融合得到局部占据地图；G.将每个建图周期的局部占据地图按照位姿时序融合为全局连续占据地图。本发明方法提高了对声呐观测噪声和异常值的鲁棒性并具有较高的建图精度；可以预测不能直接观测区域的占据情况和不确定性，意义重大。

Description

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

技术领域

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

背景技术

近年来，世界各国对海洋安全、海洋资源和海洋经济等方面日益重视，高精度的海底地图可以为执行海洋任务的水下自主航行器(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_cnt，以AUV接收到n_scan帧声呐扫描数量为一个建图周期，每个扫描周期以最后一帧的时间为周期时间戳。对于第k个建图周期，其对应的粒子状态包含状态向量为x_ekf(k)＝[z(k),v_b(k),w_b(k)]^T，即可由深度计(DM)、多普勒速度计(DVL)等机载设备直接观测的AUV绝对深度、机体系b上的速度和加速度，该向量由所有粒子共享的扩展卡尔曼滤波器跟踪；每个粒子状态还包含不可直接观测到的状态向量

该向量由粒子滤波器估计，其中前三项分别表示AUV的世界坐标系w水平方向x和y坐标和姿态角，后两项分别表示第i个(1≤i≤M)粒子携带的GPCOM地图和该粒子对应的权重。GPCOM地图初始化为占据值取值为0的连续场。

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

步骤3：滤除声呐数据中的周围杂波和海底回波成分；采用光线投射法(Raycasting)以固定分辨率λ_z从声源出发沿着各波束方向进行均匀采样，根据各波束测量的环境占据情况，得到包含环境位置和占据值的数据；

具体地说，在一帧由n_beam个波束组成的声呐扫描中，对于某个方位角为ψ_q(k),q∈{1,...,n_beam}的声呐波束，从声呐数据中读取该波束对应的声呐测量距离，若该波束的测量距离r_q(k)等于声呐最大探测距离r^max，则将该波束上的均匀采样点均定义为“自由点”，即未被占据状态，占据值设置为o_free＝-1；若r(k)小于r^max，则将该波束末端的采样点记为“占据点”，即被占据状态，占据值设置为o_occ＝+1，并将该波束的信号源和该占据点之间的所有均匀采样点均设为自由点；将超出声呐最大探测距离r^max和在声呐探测范围内但无法直接测量到的均匀采样点记为占据状态未知，并设置占据值为o_unknown＝0。于是得到一帧声呐扫描中n_beam个波束实际的距离测量值r(k)、方位测量值ψ_sonar(k)，将在这些波束上的均匀采样点由声呐极坐标系转换到声呐笛卡尔坐标系，再转换到世界笛卡尔坐标系，得到包含N个采样点位置x_j和相应占据值o_j＝{-1,+1}的离散数据集

其中j∈{1,...,N}；

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

随机采样，得到M个包含n＝N/M个采样点数据的子数据集

为每个粒子分配一个子数据集

并使用高斯过程分类方法(GPC)，建立高斯过程分类模型，对声呐扫描覆盖范围内的区域进行连续性的占据值分类及不确定性预测，从而更新每个粒子携带的GPCOM地图mⁱ(k)；

具体地说，将子数据集

作为训练数据，假设该数据满足一个高斯过程

其中x和x′均为输入向量，

为均值函数，

为预先定义的非平稳协方差函数，即核函数，非平稳核函数相对不够平滑，更适合地形起伏较多从而导致地图占据和非占据状态突变频繁的水下地图构建场景。

给定训练数据集

和一个待预测的查询点

则查询点占据值o_*的后验仍是一个高斯过程，可表示为：

其中：

其中σ_n为输入噪声的标准差，K(·,·)为向量之间的协方差矩阵，I为n阶单位阵。

训练数据集

确定后，先根据预先的定义直接计算各个K(·,·)，然后按照各自的定义计算均值和协方差，其中核函数的超参数可通过最大化边缘对数似然方法求得，从而可以唯一地确定一个高斯过程。

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

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

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

的高斯过程分类模型，通过预测未采样和未探测区域的占据状态和不确定性，从而得到每个粒子构建的GPCOM地图mⁱ(k)。

步骤5：对于每个粒子构建的GPCOM地图mⁱ(k)，利用光线投射法沿着每个波束查询GPCOM地图，找到该波束上占据值最大的点，并计算从声呐中心到该点的距离

并与每个波束的实际测量值r_q(k)对比，得到偏差，计算相应的粒子权重并更新，其中每个粒子在第k个建图周期的权重正比于期望值

和实际值r_q(k)的偏差：

其中σ_r为声呐的距离测量噪声的标准差；

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

个权重较高的粒子及其构建的GPCOM地图，并将其融合为该建图周期的局部GPCOM地图m(k)；

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

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

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

附图说明

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

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

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

具体实施方式

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

本发明的原理准备如下：

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

Rao-Blackwellized粒子滤波(RBPF)：

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

本发明的具体步骤包括：

步骤1，初始化Rao-Blackwellized粒子滤波中的粒子数量M、粒子状态和迭代次数N_cnt，以AUV接收到n_scan帧声呐扫描数量为一个建图周期，每个扫描周期以最后一帧的时间为周期时间戳。对于第k个建图周期，其对应的粒子状态包含状态向量为x_ekf(k)＝[z(k),v_b(k),w_b(k)]^T，即可由深度计(DM)、多普勒速度计(DVL)等机载设备直接观测的AUV绝对深度、机体系b上的速度和加速度，该向量由所有粒子共享的扩展卡尔曼滤波器跟踪；每个粒子状态还包含不可直接观测到的状态向量

该向量由粒子滤波器估计，其中前三项分别表示AUV的世界坐标系w水平方向x和y坐标和姿态角，后两项分别表示第i个(1≤i≤M)粒子携带的GPCOM地图和该粒子对应的权重。GPCOM地图初始化为占据值取值为0的连续场。数目为M的粒子集合表示为：

其中P_IDi表示粒子的ID编号。

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

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

其中噪声N(k)＝[N_z N_v N_w]^T，N_v和N_w为速度和加速度噪声，N_z为深度测量噪声，控制输入u(k)包含滚转角、俯仰角和偏航速率，滚转角和俯仰角噪声忽略不计，J(k)和H(k)分别为f关于状态x_ekf(k)和噪声N(k)的雅克比矩阵，P(k)和Q(k)＝diag{σ_vσ_w}分别为状态噪声和测量噪声协方差矩阵。

通过从EKF中随机采样世界坐标系x和y方向的速度噪声N_vx、N_vy以及偏航速率噪声

预测每个粒子状态：

其中：

其中Δt为时间间隔，R(k)为机体系到世界系的旋转矩阵，由滚转角φ_w(k)、俯仰角θ_w(k)和偏航角ψ_w(k)定义，P_v(k)为速度噪声的协方差矩阵，偏航速率

可由控制输入u(k)中得到。

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

z(k)＝C(k)x_ekf(k)+ε(k)，

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

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

步骤3，滤除声呐数据中的周围杂波和海底回波成分；采用光线投射法(Raycasting)以固定分辨率λ_z从声源出发沿着各波束方向进行均匀采样，根据各波束测量的环境占据情况，得到包含环境位置和占据值的数据；

具体地说，如图2和图3所示，在一帧由n_beam个波束组成的声呐扫描中，对于某个方位角为ψ_q(k),q∈{1,...,n_beam}的声呐波束，从声呐数据中读取该波束对应的声呐测量距离，若该波束的测量距离r_q(k)等于声呐最大探测距离r^max，则将该波束上的均匀采样点均定义为“自由点”，即未被占据状态，占据值设置为o_free＝-1；若r(k)小于r^max，则将该波束末端的采样点记为“占据点”，即被占据状态，占据值设置为o_occ＝+1，并将该波束的信号源和该占据点之间的所有均匀采样点均设为自由点；将超出声呐最大探测距离r^max和在声呐探测范围内但无法直接测量到的均匀采样点记为占据状态未知，并设置占据值为o_unknown＝0。于是得到一帧声呐扫描中n_beam个波束实际的距离测量值r(k)、方位测量值ψ_sonar(k)，将在这些波束上的均匀采样点由声呐极坐标系转换到声呐笛卡尔坐标系，再转换到世界笛卡尔坐标系，得到包含N个采样点位置x_j和相应占据值o_j＝{-1,+1}的离散数据集

其中j∈{1,...,N}；

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

随机采样，得到M个包含n＝N/M个采样点数据的子数据集

为每个粒子分配一个子数据集

并使用高斯过程分类方法(GPC)，建立高斯过程分类模型，对声呐扫描覆盖范围内的区域进行连续性的占据值分类及不确定性预测，从而更新每个粒子携带的GPCOM地图mⁱ(k)；

具体地说，将子数据集

作为训练数据，假设该数据满足一个高斯过程

其中x和x′均为输入向量，

为均值函数，

为预先定义的非平稳协方差函数，即核函数，非平稳核函数相对不够平滑，更适合地形起伏较多从而导致地图占据和非占据状态突变频繁的水下地图构建场景。

给定训练数据集

和一个待预测的查询点

则查询点占据值o_*的后验仍是一个高斯过程，可表示为：

其中：

其中σ_n为输入噪声的标准差，k(·,·)为向量之间的协方差矩阵，I为n阶单位阵。

训练数据集

确定后，先根据预先的定义直接计算各个K(·,·)，然后按照各自的定义计算均值和协方差，从而唯一地确定一个高斯过程。

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

其中距离r＝|x_*-x|，σ_κ和

分别为核函数的信号方差和尺度因子。相比于常用的高斯核函数，该非平稳核函数相对不够平滑，更适合地形起伏较多从而导致地图占据和非占据状态突变频繁的水下地图构建场景。

核函数的超参数

可通过最大化边缘对数似然方法求得：

其中K为训练数据集

的n阶协方差矩阵。

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

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

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

的高斯过程分类模型，通过预测未采样和未探测区域的占据状态和不确定性，从而得到每个粒子构建的GPCOM地图mⁱ(k)。

步骤5，对于每个粒子构建的GPCOM地图mⁱ(k)，利用光线投射法沿着每个波束查询GPCOM地图，找到该波束上占据值最大的点，并计算从声呐中心到该点的距离

并与每个波束的实际测量值r_q(k)对比，得到偏差，计算相应的粒子权重并更新，其中每个粒子在第k个建图周期的权重正比于期望值

和实际值r_q(k)的偏差：

其中σ_r为声呐的距离测量噪声的标准差；

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

个权重较高的粒子及其构建的GPCOM地图，并将其融合为该建图周期的局部GPCOM地图m(k)；

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

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

和子数据集

为例，有：

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

和

分别表示数据集

对应GPCOM地图的高斯过程均值和核函数。

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

Claims (4)

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

步骤1：初始化Rao-Blackwellized粒子滤波器中的粒子数量和状态；

步骤2：预测更新水下机器人的位姿信息；

步骤3：声呐数据预处理，获取包含均匀采样点位置和占据状态的数据集；

步骤4：为每个粒子分配子数据集，通过训练建立高斯过程分类模型，得到局部高斯过程连续占据地图；

步骤5：通过各个粒子所构建地图和实际测量的偏差计算更新所有粒子的权重；

步骤6：进行序列重要性重采样，获取有效粒子，并将有效粒子构建的地图融合得到局部占据地图；

步骤7：将每个建图周期的局部占据地图按照位姿时序融合为全局连续占据地图；

具体的：

步骤1：初始化Rao-Blackwellized粒子滤波中的粒子数量M、粒子状态和迭代次数N_cnt，以AUV接收到n_scan帧声呐扫描数量为一个建图周期，每个扫描周期以最后一帧的时间为周期时间戳；对于第k个建图周期，其对应的粒子状态包含状态向量为x_ekf(k)＝[z(k),v_b(k),w_b(k)]^T，其内元素依次为可由机载设备直接观测的AUV绝对深度、机体系b上的速度和加速度，该向量由所有粒子共享的扩展卡尔曼滤波器跟踪；每个粒子状态还包含不可直接观测到的状态向量

该向量由粒子滤波器估计，其中前三项分别表示AUV的世界坐标系w水平方向x、y坐标和姿态角，后两项分别表示第i个粒子携带的GPCOM地图和该粒子对应的权重，其中 1 ≤ i ≤ M ；GPCOM地图初始化为占据值取值为0的连续场；

步骤2：预测更新每个粒子携带的机器人位置和姿态信息；

步骤3：滤除声呐数据中的周围杂波和海底回波成分；采用光线投射法Ray casting以固定分辨率λ_z从声源出发沿着各波束方向进行均匀采样，根据各波束测量的环境占据情况，得到包含环境位置和占据值的数据；将在这些波束上的均匀采样点由声呐极坐标系转换到声呐笛卡尔坐标系，再转换到世界笛卡尔坐标系，得到包含N个采样点位置x_j和相应占据值o_j＝{-1,+1}的离散数据集

其中j∈{1,...,N}；

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

随机采样，得到M个包含n＝N/M个采样点数据的子数据集

为每个粒子分配一个子数据集

并使用高斯过程分类方法GPC，建立高斯过程分类模型，对声呐扫描覆盖范围内的区域进行连续性的占据值分类及不确定性预测，从而更新每个粒子携带的GPCOM地图mⁱ(k)；

步骤5：对于每个粒子构建的GPCOM地图mⁱ(k)，利用光线投射法沿着每个波束查询GPCOM地图，找到该波束上占据值最大的点，并计算从声呐中心到该点的距离

并与每个波束的实际测量值r_q(k)对比，得到偏差，计算相应的粒子权重并更新，其中每个粒子在第k个建图周期的权重正比于期望值

和实际值r_q(k)的偏差；

步骤6：权重更新完毕后，对所有完成位姿和权重更新的粒子进行序列重要性重采样SIR，达到迭代次数N_cnt后，提取

个权重较高的粒子及其构建的GPCOM地图，并将其融合为该建图周期的局部GPCOM地图m(k)；

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

2.根据权利要求1所述的基于声呐的水下机器人连续占据建图方法，其特征在于，所述的步骤3中根据各波束测量的环境占据情况，得到包含环境位置和占据值的数据；具体为：在一帧由n_beam个波束组成的声呐扫描中，对于某个方位角为ψ_q(k),q∈{1,...,n_beam}的声呐波束，从声呐数据中读取该波束对应的声呐测量距离，若该波束的测量距离r_q(k)等于声呐最大探测距离r^max，则将该波束上的均匀采样点均定义为“自由点”，即未被占据状态，占据值设置为o_free＝-1；若r(k)小于r^max，则将该波束末端的采样点记为“占据点”，即被占据状态，占据值设置为o_occ＝+1，并将该波束的信号源和该占据点之间的所有均匀采样点均设为自由点；将超出声呐最大探测距离r^max和在声呐探测范围内但无法直接测量到的均匀采样点记为占据状态未知，并设置占据值为o_unknown＝0；于是得到一帧声呐扫描中n_beam个波束实际的距离测量值r(k)、方位测量值ψ_sonar(k)及占据值数据。

3.根据权利要求1所述的基于声呐的水下机器人连续占据建图方法，其特征在于，所述的步骤2中的AUV位姿预测更新直接通过步骤1中所述的扩展卡尔曼滤波器和AUV自身的运动模型得到。

4.根据权利要求1所述的基于声呐的水下机器人连续占据建图方法，其特征在于，所述声呐为机械扫描式声呐、多波束前视成像声呐或其他类型的声呐。

Images