CN117824664A - 基于多波束测深声呐的自主无人系统主动slam方法 - Google Patents
基于多波束测深声呐的自主无人系统主动slam方法 Download PDFInfo
- Publication number
- CN117824664A CN117824664A CN202410246817.0A CN202410246817A CN117824664A CN 117824664 A CN117824664 A CN 117824664A CN 202410246817 A CN202410246817 A CN 202410246817A CN 117824664 A CN117824664 A CN 117824664A
- Authority
- CN
- China
- Prior art keywords
- unmanned system
- underwater unmanned
- point
- candidate
- revisit
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 60
- 238000001514 detection method Methods 0.000 claims abstract description 27
- 230000000694 effects Effects 0.000 claims abstract description 8
- 238000005457 optimization Methods 0.000 claims abstract description 8
- 238000005259 measurement Methods 0.000 claims description 16
- 238000005070 sampling Methods 0.000 claims description 15
- 238000004364 calculation method Methods 0.000 claims description 6
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000000875 corresponding effect Effects 0.000 description 17
- 238000012876 topography Methods 0.000 description 7
- 230000000007 visual effect Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 239000002245 particle Substances 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000031700 light absorption Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
- G01S15/06—Systems determining the position data of a target
- G01S15/08—Systems for measuring distance only
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/88—Sonar systems specially adapted for specific applications
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Acoustics & Sound (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本发明公开了一种基于多波束测深声呐的自主无人系统主动SLAM方法。本发明首先搭载多波束测深声呐传感器的水下无人系统在预先规划的路径上进行海底探测任务，根据多波束测深声呐传感器的测深数据生成位姿图，得到全局地图；然后采用主动SLAM算法生成对应的重访候选点和勘探候选点及相应的重访和勘探动作；再通过计算效用函数，选择使效应函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作：当选择的目标点重访候选点并执行重访动作时，根据闭环检测的结果进行全局优化，根据全局优化的结果更新水下无人系统的位姿和全局地图后返回原来位置。本发明可以提高水下无人系统进行海底探测的效率和定位精度。
Description
技术领域
本发明属于声学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和回波角α计算；为水下无人系统的位姿，其中分别为水下无人系统在大地坐标系下东、北、深轴上的坐标，，/>分别为水下无人系统在大地坐标系下东、北、深轴上的欧拉角；测量点/>的计算如下：
，
其中分别表示测量点在大地坐标系下东、北、深轴上的坐标；T表示转换矩阵且T由如下的表达式给出：
，
S12.生成位姿图：位姿图包括节点和边，其中节点表示水下无人系统在/>时刻的姿态；边包括里程计关联、弱数据关联和闭环检测，里程计关联由惯性导航系统产生，弱数据关联由稀疏伪输入高斯过程回归方法构建，闭环由地形匹配过程检测；
S13.根据位姿图，建立全局地图。
进一步地，所述步骤S2具体包括以下步骤：
S21.在已访问区域中，从全局地图中地形信息丰富的区域生成可能重访候选点，具体地，以100个多波束测深声呐的测量值为一个单元计算地形信息，表示已访问区域中所有可能重访候选点的集合：
，
其中，k表示已访问区域中所有可能重访候选点的个数，、/>为可能重访候选点在大地坐标系下东、北轴上的坐标，/>表示可能重访候选点/>区域的地形信息，/>的计算如下：
，
其中，P、Q分别为候选单元的长和宽，p为不大于P的一个正整数、q 为不大于Q的一个正整数，为候选单元内测量点在大地坐标系下东、北轴上的坐标，/>表示候选单元内测量点/>对应位置的海拔，/>为欧几里得范数；
S22.从步骤S21生成的可能重访候选点集合中，将满足水下无人系统的位置与可能重访候选点之间的距离在/>以下条件的点加入重访候选点集合A，即：
，
其中为多波束测深声呐的测量宽度，/>为t时刻水下无人系统在大地坐标系下东轴上的坐标；/>为t时刻水下无人系统在大地坐标系下北轴上的坐标；
S23.从预先规划的路径中生成勘探候选点，每个重访候选点都有一个对应的勘探候选点/>，令水下无人系统到重访候选点/>与到对应的勘探候选点/>间的距离相等，即，将满足这个条件的勘探候选点加入勘探候选点集合B。
进一步地，所述步骤S3具体包括以下步骤：
S31.效用函数用来描述水下无人系统的位姿不确定性和地图不确定性，其计算如下所示：
，
其中，表示期望的动作，/>表示期望动作/>产生的效用，/>为水下无人系统的姿态，/>为可能受动作/>影响的地图，/>为水下无人系统历史姿态变换，/>为历史测量值，/>为预测测量值，/>表示熵，/>表示概率分布；
由香农熵公式计算：
，
其中，表示离散变量/>的概率分布，也就是说/>，对于/>；
由于联合概率分布和/>难以计算，因此，假设水下无人系统的姿态不确定性和地图的不确定性是相互独立的，即：
，
因此得到效用函数如下：
；
S32.使用步骤S31得到的效用函数分别计算水下无人系统访问重访候选点A和勘探候选点B各自产生的位姿与地图不确定性；
S33.选择使效用函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作。
进一步地，步骤S4所述采用地形匹配过程检测闭环，具体包括：以位姿协方差对水下无人系统的当前姿态/>进行采样，获得采样点/>，N为采样点的个数；采样点与水下无人系统的最远距离为/>，搜索范围/>为：
，
然后根据搜索范围获得历史轨迹点，并使用获得的历史轨迹点与相应的测量值构造局部八叉树地图；对每个采样点/>，在局部八叉树地图/>上生成多波束测深声呐的模拟测量值/>，n为测量点的个数；利用当前实际测量值，通过以下计算得到匹配似然值/>：
，
其中，为测量噪声方差；
如果所有样本中的最大匹配似然值满足，则认为闭合环路被检测到，然后更新水下无人系统的位姿：
，
其中，为更新的水下无人系统的姿态。
有益效果：与现有技术相比，本发明具有以下有益效果：
1.本发明以多波束测深声呐为水下无人系统进行海底探测的主要传感器，多波束测深声呐的宽覆盖率可以使水下无人系统大范围的探索海底未知区域，提高了水下无人系统进行海底探测的效率。
2.本发明提出的与水下无人系统位姿与地图不确定性相关的效用函数，可以使水下无人系统能够在重访和勘探任务之间做出合理决策，达到真正自主的作用。
3.本发明提出的主动SLAM方法可以使水下无人系统即使在海底地形平坦的区域，也可以避免因地形误匹配造成的错误闭环，从而有效估计出水下无人系统的位置，提高水下无人系统的定位精度。
附图说明
图1为本发明流程图；
图2为候选点选择示意图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明。
如图1所示，基于多波束测深声呐的自主无人系统主动SLAM方法，包括预先设定任务区域，进行路径规划；传感器信息输入生成位姿图，建立全局地图；生成重访候选点A和勘探候选点B；效用函数评估；执行相应动作。具体包括以下步骤：
S1.搭载多波束测深声呐传感器的水下无人系统在预先规划的路径上进行海底探测任务前，预先设定好任务区域，以常用“割草机”模式进行路径规划，如图2所示，此过程仅在任务开始前设置一次；水下无人系统进行海底探测任务开始，来自多波束测深声呐传感器的信息输入，根据其测深数据生成位姿图，建立全局地图；
具体包括以下步骤：S11.多波束测深声呐传感器测得多波束测深声呐数据由一系列测量点组成，每个测量点由波束长度r和回波角α计算；为水下无人系统的位姿，其中/>分别为水下无人系统在大地坐标系下东、北、深轴上的坐标，，分别为水下无人系统在大地坐标系下东、北、深轴上的欧拉角；测量点的计算如下：
，
其中分别表示测量点在大地坐标系下东、北、深轴上的坐标；T表示转换矩阵且T由如下的表达式给出：
，
S12.生成位姿图：位姿图包括节点和边，其中节点表示水下无人系统在/>时刻的姿态；边包括里程计关联、弱数据关联和闭环检测，里程计关联由惯性导航系统产生，弱数据关联由稀疏伪输入高斯过程（Sparse Pseudo-input GaussianProcesses, SPGPs）回归方法构建，闭环由地形匹配过程检测；
S13.根据位姿图，建立全局地图。
S2.采用主动SLAM算法，根据预先规划路径和步骤S1得到的全局地图确定要访问的区域，并生成对应的重访候选点集合A和勘探候选点集合B及相应的重访和勘探动作；如图2所示，提出两类供水下无人系统行动选择的候选点：a.从已访问区域中选择A类候选点进行重访；b.从预先规划的路径中选择B类候选点进行勘探。具体包括以下步骤：
S21.在已访问区域中，从全局地图中地形信息丰富的区域生成可能重访候选点，具体地，以100个多波束测深声呐的测量值为一个单元计算地形信息，表示已访问区域中所有可能重访候选点的集合：
，
其中，k表示已访问区域中所有可能重访候选点的个数，、/>为可能重访候选点在大地坐标系下东、北轴上的坐标，/>表示可能重访候选点/>区域的地形信息，/>的计算如下：
，
其中，P、Q分别为候选单元的长和宽，p为不大于P的一个正整数、q 为不大于Q的一个正整数，为候选单元内测量点在大地坐标系下东、北轴上的坐标，/>表示候选单元内测量点/>对应位置的海拔，/>为欧几里得范数；
S22.从步骤S21生成的可能重访候选点集合中，将满足水下无人系统的位置与可能重访候选点之间的距离在/>以下条件的点加入重访候选点集合A，即：
，
其中为多波束测深声呐的测量宽度，/>为t时刻水下无人系统在大地坐标系下东轴上的坐标；/>为t时刻水下无人系统在大地坐标系下北轴上的坐标；
S23.从预先规划的路径中生成勘探候选点，每个重访候选点都有一个对应的勘探候选点/>，令水下无人系统到重访候选点/>与到对应的勘探候选点/>间的距离相等，即，将满足这个条件的勘探候选点加入勘探候选点集合B。
S3. 生成候选点后，通过计算效用函数，从步骤S2得到的重访候选点集合A和勘探候选点集合B中选择使效应函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作：
具体包括以下步骤：
S31.效用函数用来描述水下无人系统的位姿不确定性和地图不确定性，其计算如下所示：
，
其中，表示期望的动作，/>表示期望动作/>产生的效用，/>为水下无人系统的姿态，/>为可能受动作/>影响的地图，/>为水下无人系统历史姿态变换，/>为历史测量值，/>为预测测量值，/>表示熵，/>表示概率分布；
由香农熵公式计算：
，
其中，表示离散变量/>的概率分布，也就是说/>，对于/>；
由于联合概率分布和/>难以计算，因此，假设水下无人系统的姿态不确定性和地图的不确定性是相互独立的，即：
，
因此得到效用函数如下：
；
S32.使用步骤S31得到的效用函数分别计算水下无人系统访问重访候选点A和勘探候选点B各自产生的位姿与地图不确定性；
S33.选择使效用函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作。
S4.当步骤S3选择的目标点为勘探候选点并执行勘探动作时，水下无人系统到达该被选中的勘探候选点，然后继续执行步骤S1；
当步骤S3选择的目标点为重访候选点并执行重访动作时，水下无人系统到达该被选中的重访候选点，然后进行闭环检测，根据闭环检测的结果进行全局优化，根据全局优化的结果更新水下无人系统的位姿和全局地图后返回原来位置。
所述采用地形匹配过程检测闭环，具体包括：以位姿协方差对水下无人系统的当前姿态/>进行采样，获得采样点/>，N为采样点的个数；采样点与水下无人系统的最远距离为/>，搜索范围/>为：
，
然后根据搜索范围获得历史轨迹点，并使用获得的历史轨迹点与相应的测量值构造局部八叉树地图；对每个采样点/>，在局部八叉树地图/>上生成多波束测深声呐的模拟测量值/>，n为测量点的个数；利用当前实际测量值，通过以下计算得到匹配似然值/>：
，
其中，为测量噪声方差；
如果所有样本中的最大匹配似然值满足，则认为闭合环路被检测到，然后更新水下无人系统的位姿：
，
其中，为更新的水下无人系统的姿态。
为了验证本发明方法的有效性，开展基于多波束测深声呐的自主无人系统主动SLAM方法实验。将本发明方法分别与航迹推算方法（DR）和基于粒子滤波的SLAM方法（PFSLAM）对比，比较平均定位误差。实验结果如下所示。
表1 DR、PF SLAM和本发明方法的平均定位误差：
| DR | PF SLAM | 本发明方法 | |
| 平均定位误差（m） | 10.64 | 4.27 | 1.69 |
从表1来看，本发明方法的平均定位误差小于航迹推算方法和基于粒子滤波的SLAM方法，提高了水下无人系统的定位精度。
综上所述，本发明采用多波束测深声呐为水下无人系统进行海底探测的主要传感器，其宽覆盖率可以提高水下无人系统进行海底探测的效率，同时本发明提出的主动SLAM方法能够有效地解决水下无人系统定位失败的问题，通过计算效用函数可以为水下无人系统提供合理决策，避免因地形误匹配造成的错误闭环，从而减少水下无人系统的定位误差，提高了水下无人系统的定位精度。
Claims (5)
1.一种基于多波束测深声呐的自主无人系统主动SLAM方法，其特征在于，包括以下步骤：
S1.搭载多波束测深声呐传感器的水下无人系统在预先规划的路径上进行海底探测任务，根据多波束测深声呐传感器的测深数据生成位姿图，得到全局地图；
S2.采用主动SLAM算法，根据预先规划路径和步骤S1得到的全局地图确定要访问的区域，并生成对应的重访候选点集合A和勘探候选点集合B及相应的重访和勘探动作；
S3.通过计算效用函数，从步骤S2得到的重访候选点集合A和勘探候选点集合B中选择使效应函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作：
S4.当步骤S3选择的目标点为勘探候选点并执行勘探动作时，水下无人系统到达该被选中的勘探候选点，然后继续执行步骤S1；
当步骤S3选择的目标点为重访候选点并执行重访动作时，水下无人系统到达该被选中的重访候选点，然后进行闭环检测，根据闭环检测的结果进行全局优化，根据全局优化的结果更新水下无人系统的位姿和全局地图后返回原来位置。
2.根据权利要求1所述的基于多波束测深声呐的自主无人系统主动SLAM方法，其特征在于，所述步骤S1具体包括以下步骤：
S11.多波束测深声呐传感器测得多波束测深声呐数据由一系列测量点组成，每个测量点由波束长度r和回波角α计算；为水下无人系统的位姿，其中/>分别为水下无人系统在大地坐标系下东、北、深轴上的坐标，/>分别为水下无人系统在大地坐标系下东、北、深轴上的欧拉角；测量点/>的计算如下：
，
其中分别表示测量点在大地坐标系下东、北、深轴上的坐标；T表示转换矩阵且T由如下的表达式给出：
，
S12.生成位姿图：位姿图包括节点和边，其中节点表示水下无人系统在/>时刻的姿态；边包括里程计关联、弱数据关联和闭环检测，里程计关联由惯性导航系统产生，弱数据关联由稀疏伪输入高斯过程回归方法构建，闭环由地形匹配过程检测；
S13.根据位姿图，建立全局地图。
3.根据权利要求1所述的基于多波束测深声呐的自主无人系统主动SLAM方法，其特征在于，所述步骤S2具体包括以下步骤：
S21.在已访问区域中，从全局地图中地形信息丰富的区域生成可能重访候选点，具体地，以100个多波束测深声呐的测量值为一个单元计算地形信息，表示已访问区域中所有可能重访候选点的集合：
，
其中，k表示已访问区域中所有可能重访候选点的个数，、/>为可能重访候选点/>在大地坐标系下东、北轴上的坐标，/>表示可能重访候选点/>区域的地形信息，/>的计算如下：
，
其中，P、Q分别为候选单元的长和宽，p为不大于P的一个正整数、q 为不大于Q的一个正整数，为候选单元内测量点在大地坐标系下东、北轴上的坐标，/>表示候选单元内测量点/>对应位置的海拔，/>为欧几里得范数；
S22.从步骤S21生成的可能重访候选点集合中，将满足水下无人系统的位置与可能重访候选点之间的距离在/>以下条件的点加入重访候选点集合A，即：
，
其中为多波束测深声呐的测量宽度，/>为t时刻水下无人系统在大地坐标系下东轴上的坐标；/>为t时刻水下无人系统在大地坐标系下北轴上的坐标；
S23.从预先规划的路径中生成勘探候选点，每个重访候选点都有一个对应的勘探候选点/>，令水下无人系统到重访候选点/>与到对应的勘探候选点/>间的距离相等，即，将满足这个条件的勘探候选点加入勘探候选点集合B。
4.根据权利要求1所述的基于多波束测深声呐的自主无人系统主动SLAM方法，其特征在于，所述步骤S3具体包括以下步骤：
S31.效用函数用来描述水下无人系统的位姿不确定性和地图不确定性，其计算如下所示：
，
其中，表示期望的动作，/>表示期望动作/>产生的效用，/>为水下无人系统的姿态，/>为可能受动作/>影响的地图，/>为水下无人系统历史姿态变换，/>为历史测量值，/>为预测测量值，/>表示熵，/>表示概率分布；
由香农熵公式计算：
，
其中，表示离散变量/>的概率分布，也就是说/>，对于/>；
由于联合概率分布和/>难以计算，因此，假设水下无人系统的姿态不确定性和地图的不确定性是相互独立的，即：
，
因此得到效用函数如下：
；
S32.使用步骤S31得到的效用函数分别计算水下无人系统访问重访候选点A和勘探候选点B各自产生的位姿与地图不确定性；
S33.选择使效用函数最小的候选点作为目标点，水下无人系统访问该目标点并执行该目标点对应的动作。
5.根据权利要求1所述的基于多波束测深声呐的自主无人系统主动SLAM方法，其特征在于，步骤S4所述闭环检测是采用地形匹配过程检测闭环，具体包括：以位姿协方差对水下无人系统的当前姿态/>进行采样，获得采样点/>，N为采样点的个数；采样点与水下无人系统的最远距离为/>，搜索范围/>为：
，
然后根据搜索范围获得历史轨迹点，并使用获得的历史轨迹点与相应的测量值构造局部八叉树地图；对每个采样点/>，在局部八叉树地图/>上生成多波束测深声呐的模拟测量值/>，n为测量点的个数；利用当前实际测量值，通过以下计算得到匹配似然值/>：
，
其中，为测量噪声方差；
如果所有样本中的最大匹配似然值满足，则认为闭合环路被检测到，然后更新水下无人系统的位姿：
，
其中，为更新的水下无人系统的姿态。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410246817.0A CN117824664B (zh) | 2024-03-05 | 2024-03-05 | 基于多波束测深声呐的自主无人系统主动slam方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410246817.0A CN117824664B (zh) | 2024-03-05 | 2024-03-05 | 基于多波束测深声呐的自主无人系统主动slam方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117824664A true CN117824664A (zh) | 2024-04-05 |
| CN117824664B CN117824664B (zh) | 2024-05-28 |
Family
ID=90523120
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410246817.0A Active CN117824664B (zh) | 2024-03-05 | 2024-03-05 | 基于多波束测深声呐的自主无人系统主动slam方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117824664B (zh) |
Citations (21)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5002141A (en) * | 1989-12-08 | 1991-03-26 | Toledo Scale Corporation | Vehicle weighing in motion apparatus and method |
| JP2006023194A (ja) * | 2004-07-08 | 2006-01-26 | Alfa Vision Kk | マルチビーム測深方法 |
| CN102621994A (zh) * | 2012-04-05 | 2012-08-01 | 北京理工大学 | 一种地球同步轨道合成孔径雷达覆盖中国全境的控制方法 |
| WO2014209344A1 (en) * | 2013-06-28 | 2014-12-31 | Intel Corporation | Systems and methods for revisit location detection |
| CN107579759A (zh) * | 2017-09-19 | 2018-01-12 | 清华大学 | 一种无人机卫星通信系统中天线波束的稳定方法及装置 |
| CN109186610A (zh) * | 2018-10-15 | 2019-01-11 | 哈尔滨工程大学 | 一种auv地形匹配导航的鲁棒bslam方法 |
| CN110320520A (zh) * | 2019-06-26 | 2019-10-11 | 哈尔滨工程大学 | 一种测深信息同步定位与建图的鲁棒后端图优化方法 |
| CN110441760A (zh) * | 2019-09-16 | 2019-11-12 | 河海大学 | 一种基于先验地形图的大范围海底地形图拓展构图方法 |
| DE102019114366A1 (de) * | 2018-09-18 | 2020-03-19 | GM Global Technology Operations LLC | Systeme und verfahren zur verwendung nichtlinearer modellprädiktiver regelung (model predictive control, mpc) für autonome systeme |
| CN111220146A (zh) * | 2019-12-10 | 2020-06-02 | 哈尔滨工程大学 | 一种基于高斯过程回归学习的水下地形匹配定位方法 |
| CN111693047A (zh) * | 2020-05-08 | 2020-09-22 | 中国航空工业集团公司西安航空计算技术研究所 | 一种高动态场景下的微小型无人机视觉导航方法 |
| CN112146654A (zh) * | 2020-08-25 | 2020-12-29 | 浙江大学 | 基于关键约束帧的前视成像声呐水下定位与导航方法 |
| US20210031891A1 (en) * | 2019-07-30 | 2021-02-04 | Schlumberger Technology Corporation | System and method for autonomous exploration for mapping underwater environments |
| CN112785702A (zh) * | 2020-12-31 | 2021-05-11 | 华南理工大学 | 一种基于2d激光雷达和双目相机紧耦合的slam方法 |
| WO2022022256A1 (zh) * | 2020-07-31 | 2022-02-03 | 湖北亿咖通科技有限公司 | 回环检测方法及系统、可读存储介质、电子设备 |
| CA3202821A1 (en) * | 2021-01-12 | 2022-07-21 | John Zelek | System and method of hybrid scene representation for visual simultaneous localization and mapping |
| CN114998540A (zh) * | 2022-06-12 | 2022-09-02 | 哈尔滨工程大学 | 一种智慧城市传感器探测主动同步定位与建图方法 |
| CN115031733A (zh) * | 2022-05-06 | 2022-09-09 | 哈尔滨工程大学 | 一种水下机器人多波束测深主动同步定位与建图方法 |
| WO2022253027A1 (zh) * | 2021-05-31 | 2022-12-08 | 江苏科技大学 | 基于高斯分布聚类的水下声呐图像匹配方法 |
| CN116592897A (zh) * | 2023-07-17 | 2023-08-15 | 河海大学 | 基于位姿不确定性的改进orb-slam2定位方法 |
| DE102022114780A1 (de) * | 2022-06-13 | 2023-12-14 | Daimler Truck AG | Verfahren zum autonomen Fahrbetrieb eines Fahrzeugs |
-
2024
- 2024-03-05 CN CN202410246817.0A patent/CN117824664B/zh active Active
Patent Citations (21)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5002141A (en) * | 1989-12-08 | 1991-03-26 | Toledo Scale Corporation | Vehicle weighing in motion apparatus and method |
| JP2006023194A (ja) * | 2004-07-08 | 2006-01-26 | Alfa Vision Kk | マルチビーム測深方法 |
| CN102621994A (zh) * | 2012-04-05 | 2012-08-01 | 北京理工大学 | 一种地球同步轨道合成孔径雷达覆盖中国全境的控制方法 |
| WO2014209344A1 (en) * | 2013-06-28 | 2014-12-31 | Intel Corporation | Systems and methods for revisit location detection |
| CN107579759A (zh) * | 2017-09-19 | 2018-01-12 | 清华大学 | 一种无人机卫星通信系统中天线波束的稳定方法及装置 |
| DE102019114366A1 (de) * | 2018-09-18 | 2020-03-19 | GM Global Technology Operations LLC | Systeme und verfahren zur verwendung nichtlinearer modellprädiktiver regelung (model predictive control, mpc) für autonome systeme |
| CN109186610A (zh) * | 2018-10-15 | 2019-01-11 | 哈尔滨工程大学 | 一种auv地形匹配导航的鲁棒bslam方法 |
| CN110320520A (zh) * | 2019-06-26 | 2019-10-11 | 哈尔滨工程大学 | 一种测深信息同步定位与建图的鲁棒后端图优化方法 |
| US20210031891A1 (en) * | 2019-07-30 | 2021-02-04 | Schlumberger Technology Corporation | System and method for autonomous exploration for mapping underwater environments |
| CN110441760A (zh) * | 2019-09-16 | 2019-11-12 | 河海大学 | 一种基于先验地形图的大范围海底地形图拓展构图方法 |
| CN111220146A (zh) * | 2019-12-10 | 2020-06-02 | 哈尔滨工程大学 | 一种基于高斯过程回归学习的水下地形匹配定位方法 |
| CN111693047A (zh) * | 2020-05-08 | 2020-09-22 | 中国航空工业集团公司西安航空计算技术研究所 | 一种高动态场景下的微小型无人机视觉导航方法 |
| WO2022022256A1 (zh) * | 2020-07-31 | 2022-02-03 | 湖北亿咖通科技有限公司 | 回环检测方法及系统、可读存储介质、电子设备 |
| CN112146654A (zh) * | 2020-08-25 | 2020-12-29 | 浙江大学 | 基于关键约束帧的前视成像声呐水下定位与导航方法 |
| CN112785702A (zh) * | 2020-12-31 | 2021-05-11 | 华南理工大学 | 一种基于2d激光雷达和双目相机紧耦合的slam方法 |
| CA3202821A1 (en) * | 2021-01-12 | 2022-07-21 | John Zelek | System and method of hybrid scene representation for visual simultaneous localization and mapping |
| WO2022253027A1 (zh) * | 2021-05-31 | 2022-12-08 | 江苏科技大学 | 基于高斯分布聚类的水下声呐图像匹配方法 |
| CN115031733A (zh) * | 2022-05-06 | 2022-09-09 | 哈尔滨工程大学 | 一种水下机器人多波束测深主动同步定位与建图方法 |
| CN114998540A (zh) * | 2022-06-12 | 2022-09-02 | 哈尔滨工程大学 | 一种智慧城市传感器探测主动同步定位与建图方法 |
| DE102022114780A1 (de) * | 2022-06-13 | 2023-12-14 | Daimler Truck AG | Verfahren zum autonomen Fahrbetrieb eines Fahrzeugs |
| CN116592897A (zh) * | 2023-07-17 | 2023-08-15 | 河海大学 | 基于位姿不确定性的改进orb-slam2定位方法 |
Non-Patent Citations (2)
| Title |
|---|
| 韩月;陈鹏云;沈鹏;: "基于高斯和粒子滤波的AUV水下地形辅助导航方法", 无人系统技术, no. 01, 15 January 2020 (2020-01-15), pages 48 - 54 * |
| 马腾, 等: "AUV的图优化海底地形同步定位与建图方法", 导航定位与授时, vol. 7, no. 2, 31 March 2020 (2020-03-31), pages 42 - 49 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117824664B (zh) | 2024-05-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107314768B (zh) | 水下地形匹配辅助惯性导航定位方法及其定位系统 | |
| Johannsson et al. | Imaging sonar-aided navigation for autonomous underwater harbor surveillance | |
| CN108871351B (zh) | 一种auv海底地形匹配的动态路径规划方法 | |
| US6728608B2 (en) | System and method for the creation of a terrain density model | |
| CN109186610B (zh) | 一种auv地形匹配导航的鲁棒bslam方法 | |
| Park et al. | Radar localization and mapping for indoor disaster environments via multi-modal registration to prior LiDAR map | |
| CN106772524A (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
| CN109375646A (zh) | 基于fmsrupf算法的auv对接回收自主导航方法 | |
| CN114596360B (zh) | 一种基于图拓扑的双阶段主动即时定位与建图算法 | |
| CN113532439B (zh) | 输电线路巡检机器人同步定位与地图构建方法及装置 | |
| Wang et al. | Multipath parallel ICCP underwater terrain matching algorithm based on multibeam bathymetric data | |
| CN108061897A (zh) | 一种基于前视声呐的水下结构化环境线特征提取方法 | |
| CN116295426A (zh) | 基于三维重建质量反馈的无人机自主探索方法及装置 | |
| CN113532438B (zh) | 一种大初始定位误差下的改进iccp地形匹配方法 | |
| Garcia et al. | Portable multi-hypothesis Monte Carlo localization for mobile robots | |
| Caglioti et al. | Cooperative, distributed localization in multi-robot systems: a minimum-entropy approach | |
| Burguera et al. | Probabilistic sonar scan matching for robust localization | |
| Ma et al. | A contour-based path planning method for terrain-aided navigation systems with a single beam echo sounder | |
| CN112581610B (zh) | 从多波束声呐数据中建立地图的鲁棒优化方法和系统 | |
| Wang et al. | Research on autonomous planning method based on improved quantum Particle Swarm Optimization for Autonomous Underwater Vehicle | |
| Roman | Self consistent bathymetric mapping from robotic vehicles in the deep ocean | |
| CN117824664B (zh) | 基于多波束测深声呐的自主无人系统主动slam方法 | |
| CN116660980A (zh) | 一种基于改进程函方程的微地震定位方法 | |
| CN111307136A (zh) | 一种双智能水下机器人水下航行地形匹配导航方法 | |
| Zhang et al. | Particle Filtering SLAM algorithm for urban pipe leakage detection and localization |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |