CN117824664B - 基于多波束测深声呐的自主无人系统主动slam方法 - Google Patents

Abstract

本发明公开了一种基于多波束测深声呐的自主无人系统主动SLAM方法。本发明首先搭载多波束测深声呐传感器的水下无人系统在预先规划的路径上进行海底探测任务，根据多波束测深声呐传感器的测深数据生成位姿图，得到全局地图；然后采用主动SLAM算法生成对应的重访候选点和勘探候选点及相应的重访和勘探动作；再通过计算效用函数，选择使效应函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作：当选择的目标点重访候选点并执行重访动作时，根据闭环检测的结果进行全局优化，根据全局优化的结果更新水下无人系统的位姿和全局地图后返回原来位置。本发明可以提高水下无人系统进行海底探测的效率和定位精度。

Description

基于多波束测深声呐的自主无人系统主动SLAM方法

技术领域

本发明属于声学SLAM技术和水下无人系统定位技术领域，具体为一种基于多波束测深声呐的自主无人系统主动SLAM方法。

背景技术

同步定位与地图构建(Simultaneous Localization and Mapping,SLAM)是指搭载特定传感器的主体在没有环境先验信息的情况下，在运动的过程中建立环境的模型，同时估计自身的运动。SLAM算法在陆地机器人的应用上已经相对成熟，但其在水下的应用还面临较大困难。光在水下的吸收、折射、散射、漫反射等原因导致视觉传感器在水下应用范围受限，水下SLAM的应用主要依靠声学传感器，多波束测深声呐可以为水下无人系统提供海底深度信息进而解算出其位姿。通常搭载多波束测深声呐的水下无人系统可以基于SLAM技术在水下进行探测，但在实际应用中，在海底地形平坦区域，测深信息较难分辨会造成水下无人系统错误识别闭环从而导致定位失败。而主动SLAM的作用则是赋予水下无人系统自主动作的能力，在海底地形平坦区域，通过主动决策选择目标动作，降低估计误差，提高自身定位精度。

相关文献公开了一种基于海底地形关键特征提取的水下同步定位与地图构建方法，该方法通过对环境特征进行实时观测和数据关联，同时检测闭环获得地形约束因子，对地形约束因子进行图优化从而得到实时的定位导航结果，但该方法不适用于海底地形平坦缺少环境特征的情况；另外有文献公开了一种融合前视声呐与ORB-SLAM3的水下SLAM方法，该方法在视觉跟踪有效时将前视声呐、双目相机及惯性测量单元的信息相融合求解因子图，当视觉跟踪丢失时通过因子图中的声呐和IMU继续起作用，该方法缺少主动选择传感器的策略，仅适用于小范围水下探测。综上所述，水下无人系统进行海底探测还存在着传感器不适配、导航定位精度不高、无法自主决策等问题尚未被解决。

发明内容

为了克服现有技术中存在的不足，本发明目的是提供一种基于多波束测深声呐的自主无人系统主动SLAM方法，采用多波束测深声呐实现水下无人系统在海底未知区域的自主探测，主动SLAM方法用于实现水下无人系统在海底探测过程中的有效定位，此方案能够在配备较少传感器情况下实现水下无人系统的自主导航与定位。

上述的目的通过以下技术方案实现：

本发明的一种基于多波束测深声呐的自主无人系统主动SLAM方法，包括以下步骤：

S1.搭载多波束测深声呐传感器的水下无人系统在预先规划的路径上进行海底探测任务，根据多波束测深声呐传感器的测深数据生成位姿图，得到全局地图；

S2.采用主动SLAM算法，根据预先规划路径和步骤S1得到的全局地图确定要访问的区域，并生成对应的重访候选点集合A和勘探候选点集合B及相应的重访和勘探动作；

S3.通过计算效用函数，从步骤S2得到的重访候选点集合A和勘探候选点集合B中选择使效应函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作：

S4.当步骤S3选择的目标点为勘探候选点并执行勘探动作时，水下无人系统到达该被选中的目标点，然后继续执行步骤S1；

当步骤S3选择的目标点为重访候选点并执行重访动作时，水下无人系统到达该被选中的目标点，然后进行闭环检测，根据闭环检测的结果进行全局优化，根据全局优化的结果更新水下无人系统的位姿和全局地图后返回原来位置。

进一步地，所述步骤S1具体包括以下步骤：

S11.多波束测深声呐传感器测得多波束测深声呐数据由一系列测量点组成，每个测量点由波束长度r和回波角α计算；为水下无人系统的位姿，其中x、y、z分别为水下无人系统在大地坐标系下东、北、深轴上的坐标，/>θ、ψ分别为水下无人系统在大地坐标系下东、北、深轴上的欧拉角；测量点(x_D,y_D,z_D)的计算如下：

其中x_D、y_D、z_D分别表示测量点在大地坐标系下东、北、深轴上的坐标；T表示转换矩阵且T由如下的表达式给出：

S12.生成位姿图：位姿图包括节点和边，其中节点表示水下无人系统在t时刻的姿态；边包括里程计关联、弱数据关联和闭环检测，里程计关联由惯性导航系统产生，弱数据关联由稀疏伪输入高斯过程回归方法构建，闭环由地形匹配过程检测；

S13.根据位姿图，建立全局地图。

进一步地，所述步骤S2具体包括以下步骤：

S21.在已访问区域中，从全局地图中地形信息丰富的区域生成可能重访候选点，具体地，以100个多波束测深声呐的测量值为一个单元计算地形信息，I表示已访问区域中所有可能重访候选点的集合：

I＝{I_i|i＝1,2,…,k}I_i＝{x_i,y_i,c_Ti}

其中，k表示已访问区域中所有可能重访候选点的个数，x_i、y_i为可能重访候选点I_i在大地坐标系下东、北轴上的坐标，c_Ti表示可能重访候选点I_i区域的地形信息，c_Ti的计算如下：

其中，P、Q分别为候选单元的长和宽，p为不大于P的一个正整数、q为不大于Q的一个正整数，x_pq、y_pq为候选单元内测量点在大地坐标系下东、北轴上的坐标，h_pq表示候选单元内测量点(x_pq,y_pq)对应位置的海拔，||·||为欧几里得范数；

S22.从步骤S21生成的可能重访候选点集合I中，将满足水下无人系统的位置与可能重访候选点之间的距离在3w以下条件的点加入重访候选点集合A，即：

其中w为多波束测深声呐的测量宽度；

S23.从预先规划的路径中生成勘探候选点，每个重访候选点A_i都有一个对应的勘探候选点B_i，令水下无人系统到重访候选点A_i与到对应的勘探候选点B_i间的距离相等，即将满足这个条件的勘探候选点加入勘探候选点集合B。

进一步地，所述步骤S3具体包括以下步骤：

S31.效用函数用来描述水下无人系统的位姿不确定性和地图不确定性，其计算如下所示：

其中，a表示期望的动作，a_*表示期望动作a产生的效用，x为水下无人系统的姿态，m为可能受动作a影响的地图，u为水下无人系统历史姿态变换，z为历史测量值，为预测测量值，H[·]表示熵，P(·)表示概率分布；

H[P(x)]由香农熵公式计算：

其中，p_i表示离散变量x的概率分布，也就是说p_i≥0，对于

由于联合概率分布P(x，m|u，z)和难以计算，因此，假设水下无人系统的姿态不确定性和地图的不确定性是相互独立的，即：

H[P(x,m|u,z)]≈H[P(x|u,z)]+H[P(m|x,(u,z))]

因此得到效用函数如下：

S32.使用步骤S31得到的效用函数分别计算水下无人系统访问重访候选点A和勘探候选点B各自产生的位姿与地图不确定性；

S33.选择使效用函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作。

进一步地，步骤S4所述采用地形匹配过程检测闭环，具体包括：以位姿协方差∑_t对水下无人系统的当前姿态x_t进行采样，获得采样点N为采样点的个数；采样点与水下无人系统的最远距离为d_max，搜索范围d_r为：

然后根据搜索范围获得历史轨迹点，并使用获得的历史轨迹点与相应的测量值构造局部八叉树地图m_octo；对每个采样点在局部八叉树地图m_octo上生成多波束测深声呐的模拟测量值/>n为测量点的个数；利用当前实际测量值通过以下计算得到匹配似然值l_i：

其中，σ_z为测量噪声方差；

如果所有样本中的最大匹配似然值满足l_i＞0.8，则认为闭合环路被检测到，然后更新水下无人系统的位姿：

其中，为更新的水下无人系统的姿态。

有益效果：与现有技术相比，本发明具有以下有益效果：

1.本发明以多波束测深声呐为水下无人系统进行海底探测的主要传感器，多波束测深声呐的宽覆盖率可以使水下无人系统大范围的探索海底未知区域，提高了水下无人系统进行海底探测的效率。

2.本发明提出的与水下无人系统位姿与地图不确定性相关的效用函数，可以使水下无人系统能够在重访和勘探任务之间做出合理决策，达到真正自主的作用。

3.本发明提出的主动SLAM方法可以使水下无人系统即使在海底地形平坦的区域，也可以避免因地形误匹配造成的错误闭环，从而有效估计出水下无人系统的位置，提高水下无人系统的定位精度。

附图说明

图1为本发明流程图；

图2为候选点选择示意图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

如图1所示，基于多波束测深声呐的自主无人系统主动SLAM方法，包括预先设定任务区域，进行路径规划；传感器信息输入生成位姿图，建立全局地图；生成重访候选点A和勘探候选点B；效用函数评估；执行相应动作。具体包括以下步骤：

S1.搭载多波束测深声呐传感器的水下无人系统在预先规划的路径上进行海底探测任务前，预先设定好任务区域，以常用“割草机”模式进行路径规划，如图2所示，此过程仅在任务开始前设置一次；水下无人系统进行海底探测任务开始，来自多波束测深声呐传感器的信息输入，根据其测深数据生成位姿图，建立全局地图；

具体包括以下步骤：

S11.多波束测深声呐传感器测得多波束测深声呐数据由一系列测量点组成，每个测量点由波束长度r和回波角α计算；为水下无人系统的位姿，其中x、y、z分别为水下无人系统在大地坐标系下东、北、深轴上的坐标，/>θ、ψ分别为水下无人系统在大地坐标系下东、北、深轴上的欧拉角；测量点(x_D,y_D,z_D)的计算如下：

其中x_D、y_D、z_D分别表示测量点在大地坐标系下东、北、深轴上的坐标；T表示转换矩阵且T由如下的表达式给出：

S12.生成位姿图：位姿图包括节点和边，其中节点表示水下无人系统在t时刻的姿态；边包括里程计关联、弱数据关联和闭环检测，里程计关联由惯性导航系统产生，弱数据关联由稀疏伪输入高斯过程(Sparse Pseudo-input GaussianProcesses,SPGPs)回归方法构建，闭环由地形匹配过程检测；

S13.根据位姿图，建立全局地图。

S2.采用主动SLAM算法，根据预先规划路径和步骤S1得到的全局地图确定要访问的区域，并生成对应的重访候选点集合A和勘探候选点集合B及相应的重访和勘探动作；如图2所示，提出两类供水下无人系统行动选择的候选点：a.从已访问区域中选择A类候选点进行重访；b.从预先规划的路径中选择B类候选点进行勘探。具体包括以下步骤：

S21.在已访问区域中，从全局地图中地形信息丰富的区域生成可能重访候选点，具体地，以100个多波束测深声呐的测量值为一个单元计算地形信息，I表示已访问区域中所有可能重访候选点的集合：

I＝{I_i|i＝1,2,…,k}I_i＝{x_i,y_i,c_Ti}

其中，k表示已访问区域中所有可能重访候选点的个数，x_i、y_i为可能重访候选点I_i在大地坐标系下东、北轴上的坐标，c_Ti表示可能重访候选点I_i区域的地形信息，c_Ti的计算如下：

其中，P、Q分别为候选单元的长和宽，p为不大于P的一个正整数、q为不大于Q的一个正整数，x_pq、y_pq为候选单元内测量点在大地坐标系下东、北轴上的坐标，h_pq表示候选单元内测量点(x_pq,y_pq)对应位置的海拔，||·||为欧几里得范数；

S22.如果重访候选点距离过远，将会降低水下无人系统探测任务的效率。因此，从步骤S21生成的可能重访候选点集合I中，将满足水下无人系统的位置与可能重访候选点之间的距离在3w以下条件的点加入重访候选点集合A，如图2中A₁～A₈，即：

其中w为多波束测深声呐的测量宽度；

S23.从预先规划的路径中生成勘探候选点，每个重访候选点A_i都有一个对应的勘探候选点B_i，令水下无人系统到重访候选点A_i与到对应的勘探候选点B_i间的距离相等，即如图2中/>将满足这个条件的勘探候选点加入勘探候选点集合B。

S3.生成候选点后，通过计算效用函数，从步骤S2得到的重访候选点集合A和勘探候选点集合B中选择使效应函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作：

具体包括以下步骤：

S31.效用函数用来描述水下无人系统的位姿不确定性和地图不确定性，其计算如下所示：

其中，a表示期望的动作，a_*表示期望动作a产生的效用，x为水下无人系统的姿态，m为可能受动作a影响的地图，u为水下无人系统历史姿态变换，z为历史测量值，为预测测量值，H[·]表示熵，P(·)表示概率分布；

H[P(x)]由香农熵公式计算：

其中，p_i表示离散变量x的概率分布，也就是说p_i≥0，对于

由于联合概率分布P(x,m|u,z)和难以计算，因此，假设水下无人系统的姿态不确定性和地图的不确定性是相互独立的，即：

H[P(x,m|u,z)]≈H[P(x|u,z)]H[P(m|x,(u,z))]

因此得到效用函数如下：

S32.使用步骤S31得到的效用函数分别计算水下无人系统访问重访候选点A和勘探候选点B各自产生的位姿与地图不确定性；

S33.选择使效用函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作。

S4.当步骤S3选择的目标点为勘探候选点并执行勘探动作时，水下无人系统到达该被选中的勘探候选点，然后继续执行步骤S1；

当步骤S3选择的目标点为重访候选点并执行重访动作时，水下无人系统到达该被选中的重访候选点，然后进行闭环检测，根据闭环检测的结果进行全局优化，根据全局优化的结果更新水下无人系统的位姿和全局地图后返回原来位置。

所述采用地形匹配过程检测闭环，具体包括：以位姿协方差∑_t对水下无人系统的当前姿态x_t进行采样，获得采样点N为采样点的个数；采样点与水下无人系统的最远距离为d_max，搜索范围d_r为：

然后根据搜索范围获得历史轨迹点，并使用获得的历史轨迹点与相应的测量值构造局部八叉树地图m_octo；对每个采样点在局部八叉树地图m_octo上生成多波束测深声呐的模拟测量值/>n为测量点的个数；利用当前实际测量值通过以下计算得到匹配似然值l_i：

其中，σ_z为测量噪声方差；

如果所有样本中的最大匹配似然值满足l_i＞0.8，则认为闭合环路被检测到，然后更新水下无人系统的位姿：

其中，为更新的水下无人系统的姿态。

为了验证本发明方法的有效性，开展基于多波束测深声呐的自主无人系统主动SLAM方法实验。将本发明方法分别与航迹推算方法(DR)和基于粒子滤波的SLAM方法(PFSLAM)对比，比较平均定位误差。实验结果如下所示。

表1 DR、PF SLAM和本发明方法的平均定位误差

	DR	PF SLAM	本发明方法
				平均定位误差(m)	10.64	4.27	1.69

从表1来看，本发明方法的平均定位误差小于航迹推算方法和基于粒子滤波的SLAM方法，提高了水下无人系统的定位精度。

综上所述，本发明采用多波束测深声呐为水下无人系统进行海底探测的主要传感器，其宽覆盖率可以提高水下无人系统进行海底探测的效率，同时本发明提出的主动SLAM方法能够有效地解决水下无人系统定位失败的问题，通过计算效用函数可以为水下无人系统提供合理决策，避免因地形误匹配造成的错误闭环，从而减少水下无人系统的定位误差，提高了水下无人系统的定位精度。

Claims (2)

1.一种基于多波束测深声呐的自主无人系统主动SLAM方法，其特征在于，包括以下步骤：

S1.搭载多波束测深声呐传感器的水下无人系统在预先规划的路径上进行海底探测任务，根据多波束测深声呐传感器的测深数据生成位姿图，得到全局地图；

S2.采用主动SLAM算法，根据预先规划路径和步骤S1得到的全局地图确定要访问的区域，并生成对应的重访候选点集合A和勘探候选点集合B及相应的重访和勘探动作，具体包括以下步骤：

S21.在已访问区域中，从全局地图中地形信息丰富的区域生成可能重访候选点，具体地，以100个多波束测深声呐的测量值为一个单元计算地形信息，I表示已访问区域中所有可能重访候选点的集合：

I＝{I_i|i＝1,2,...,k}I_i＝{x_i,y_i,c_Ti}

其中，k表示已访问区域中所有可能重访候选点的个数，x_i、y_i为可能重访候选点I_i在大地坐标系下东、北轴上的坐标，c_Ti表示可能重访候选点I_i区域的地形信息，c_Ti的计算如下：

其中，P、Q分别为候选单元的长和宽，p为不大于P的一个正整数、q为不大于Q的一个正整数，x_pq、y_pq为候选单元内测量点在大地坐标系下东、北轴上的坐标，h_pq表示候选单元内测量点(x_pq,y_pq)对应位置的海拔，‖·‖为欧几里得范数；

S22.从步骤S21生成的可能重访候选点集合I中，将满足水下无人系统的位置与可能重访候选点之间的距离在3w以下条件的点加入重访候选点集合A，即：

其中w为多波束测深声呐的测量宽度，x_t为t时刻水下无人系统在大地坐标系下东轴上的坐标；y_t为t时刻水下无人系统在大地坐标系下北轴上的坐标；

S23.从预先规划的路径中生成勘探候选点，每个重访候选点A_i都有一个对应的勘探候选点B_i，令水下无人系统到重访候选点A_i与到对应的勘探候选点B_i间的距离相等，即将满足这个条件的勘探候选点加入勘探候选点集合B；

S3.通过计算效用函数，从步骤S2得到的重访候选点集合A和勘探候选点集合B中选择使效应函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作，具体包括以下步骤：

S31.效用函数用来描述水下无人系统的位姿不确定性和地图不确定性，其计算如下所示：

其中，a表示期望的动作，a*表示期望动作a产生的效用，x为水下无人系统的姿态，m为可能受动作a影响的地图，u为水下无人系统历史姿态变换，z为历史测量值，为预测测量值，H[·]表示熵，P(·)表示概率分布；

H[P(x)]由香农熵公式计算：

其中，p_i表示离散变量x的概率分布，也就是说p_i≥0，对于

由于联合概率分布P(x,m|u,z)和难以计算，因此，假设水下无人系统的姿态不确定性和地图的不确定性是相互独立的，即：

H[P(x,m|u,z)]≈H[P(x|u,z)]+H[P(m|x,(u,z))]

因此得到效用函数如下：

S32.使用步骤S31得到的效用函数分别计算水下无人系统访问重访候选点A和勘探候选点B各自产生的位姿与地图不确定性；

S33.选择使效用函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作；

S4.当步骤S3选择的目标点为勘探候选点并执行勘探动作时，水下无人系统到达该被选中的目标点，然后继续执行步骤S1；

当步骤S3选择的目标点为重访候选点并执行重访动作时，水下无人系统到达该被选中的目标点，然后进行闭环检测，根据闭环检测的结果进行全局优化，根据全局优化的结果更新水下无人系统的位姿和全局地图后返回原来位置，所述闭环检测是采用地形匹配过程检测闭环，具体包括：以位姿协方差∑_t对水下无人系统的当前姿态x_t进行采样，获得采样点 N为采样点的个数；采样点与水下无人系统的最远距离为d_max，搜索范围d_r为：

然后根据搜索范围获得历史轨迹点，并使用获得的历史轨迹点与相应的测量值构造局部八叉树地图m_octo；对每个采样点在局部八叉树地图m_octo上生成多波束测深声呐的模拟测量值/>n为测量点的个数；利用当前实际测量值通过以下计算得到匹配似然值l_i：

其中，σ_z为测量噪声方差；

如果所有样本中的最大匹配似然值满足l_i>0.8，则认为闭合环路被检测到，然后更新水下无人系统的位姿：

其中，为更新的水下无人系统的姿态。

2.根据权利要求1所述的一种基于多波束测深声呐的自主无人系统主动SLAM方法，其特征在于，所述步骤S1具体包括以下步骤：

S11.多波束测深声呐传感器测得多波束测深声呐数据由一系列测量点组成，每个测量点由波束长度r和回波角α计算；为水下无人系统的位姿，其中x、y、z分别为水下无人系统在大地坐标系下东、北、深轴上的坐标，/>θ、ψ分别为水下无人系统在大地坐标系下东、北、深轴上的欧拉角；测量点(x_D,y_D,z_D)的计算如下：

其中x_D、y_D、z_D分别表示测量点在大地坐标系下东、北、深轴上的坐标；T表示转换矩阵且T由如下的表达式给出：

S12.生成位姿图：位姿图包括节点和边，其中节点表示水下无人系统在t时刻的姿态；边包括里程计关联、弱数据关联和闭环检测，里程计关联由惯性导航系统产生，弱数据关联由稀疏伪输入高斯过程回归方法构建，闭环由地形匹配过程检测；

S13.根据位姿图，建立全局地图。