CN115031718A - 一种多传感器融合的无人船同步定位与建图方法(slam)及*** - Google Patents

一种多传感器融合的无人船同步定位与建图方法(slam)及*** Download PDF

Info

Publication number
CN115031718A
CN115031718A CN202210575283.7A CN202210575283A CN115031718A CN 115031718 A CN115031718 A CN 115031718A CN 202210575283 A CN202210575283 A CN 202210575283A CN 115031718 A CN115031718 A CN 115031718A
Authority
CN
China
Prior art keywords
slam
point cloud
unmanned ship
mapping
laser radar
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
Application number
CN202210575283.7A
Other languages
English (en)
Other versions
CN115031718B (zh
Inventor
余道洋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhongyu Future (Hefei) Water Technology Co.,Ltd.
Original Assignee
Anhui Zhongkeheding Technology Development Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Anhui Zhongkeheding Technology Development Co ltd filed Critical Anhui Zhongkeheding Technology Development Co ltd
Priority to CN202210575283.7A priority Critical patent/CN115031718B/zh
Publication of CN115031718A publication Critical patent/CN115031718A/zh
Application granted granted Critical
Publication of CN115031718B publication Critical patent/CN115031718B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G01S17/894—3D imaging with simultaneous measurement of time-of-flight at a 2D array of receiver pixels, e.g. time-of-flight cameras or flash lidar
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种多传感器融合的无人船同步定位与建图方法(SLAM)，包括以下步骤：S1、通过固态激光雷达探测无人船前方的水面环境并生产第一点云数据；S2、对第一点云数据进行特征提取，提取角和面的特征用于SLAM的关键帧配准；S3、通过机械激光雷达对无人船周围进行探测并生产第二点云数据；S4、对第二点云数据进行特征提取，并与所述S2中提取的特征进行融合，形成关键帧；S5、SLAM***接收到S4中特征融合后的关键帧，并对关键帧进行处理以进行同步定位和地图构建，本发明通过优化SLAM算法，可以适应复杂的水面环境，使得无人船能够在复杂的水面环境上进行自主航向，而且在没有卫星信号的情况下，仍可以安全自主航向。

Description

一种多传感器融合的无人船同步定位与建图方法(SLAM)及 ***
技术领域
本发明涉及无人船自主循环技术领域，具体是一种多传感器融合的无人船同步定位与建图方法(SLAM)及***。
背景技术
由于水面对光线的反射、波浪的随机扰动、河岸建筑物遮挡以及雾气等对环境传感器的干扰，无人船艇在内河的完全自主巡航是国际性难题。当无人船艇的信号受到干扰或屏蔽时位置和状态信息丢失，美国麻省理工学院Wei Wang等人研发了“Roboat”无人船用于阿姆斯特丹城市水上交通，但是也遇到了桥梁和建筑物阻碍了GPS数据的接收，以及阳光直射时激光雷达观察到许多错误的数据等问题。国内有发明专利采用事先测绘桥梁精确位置，穿越前让无人船停泊在桥前固定经纬度、航向角的起始点，再按照固定航向角快速不受控的穿越(202110783834.4)。
因此，国内外现有技术难以解决无人船在水面信号受限时的定位与导航，无法完全自主巡航。机器人、无人机和无人车等无人***常用的同步定位与地图构建(Simultaneous Localization And Mapping,SLAM)方法解决导航定位和地图更新问题，但是现有SLAM的传感器对水面场景的复杂环境适应性较差，因此无人船在复杂水面环境自主巡航的关键是解决水面专用的SLAM技术。
发明内容
本发明的目的在于提供一种多传感器融合的无人船同步定位与建图方法(SLAM)及***，以解决上述背景技术中提出的问题。
为实现上述目的，本发明提供如下技术方案：
一种多传感器融合的无人船同步定位与建图方法(SLAM)，所述同步定位和地图构建的过程包括以下步骤：
S1、通过固态激光雷达探测无人船前方的水面环境并生产第一点云数据；
S2、对第一点云数据进行特征提取，提取角和面的特征用于SLAM的关键帧配准；
S3、通过机械激光雷达对无人船周围进行探测并生产第二点云数据；
S4、对第二点云数据进行特征提取，并与所述S2中提取的特征进行融合，形成关键帧；
S5、SLAM***接收到S4中特征融合后的关键帧，并对关键帧进行处理以进行同步定位和地图构建。
作为本发明进一步的方案：所述SLAM采用因子图优化方法，给定传感器测量值的船舶状态
Figure BDA0003661891910000021
所述SLAM***因子包括激光雷达里程计因子、GPS因子、回环因子；
其中：dx和dy表示无人船与当前观测地标之间的相对位置，γ表示无人船航向角。
作为本发明进一步的方案：所述因子使用启发式方法添加新节点
Figure BDA0003661891910000022
当船舶的位置或旋转变化超过用户定义的阈值时，添加新节点x，使用贝叶斯树的增量平滑和映射来优化***新节点后的因子图。
作为本发明进一步的方案：所述激光雷达里程计因子的构建包括以下步骤：
T1、在对固态激光雷达的原始点云进行特征提取的基础上，进一步提取点云图像的水面和岸边或其他水面固体物质的边缘特征，作为点云帧的选择标准，再进行帧间特征匹配流程；
T2、帧间特征配准算法是通过匹配激光扫描数据中两次相邻帧的特征，建立以欧式距离等为度量标准的目标函数，并求解以两次扫描点云之间的变换关系为优化变量的最小二乘问题，即平移和旋转变换，进而求得在两帧的间隔时间内移动机器人的局部位姿变换，进而求得在两帧t与t+1的间隔时间内无人船的局部位姿变换T(t,t+1)；
T3、对于t时刻和t+1时刻获得的相邻两帧激光雷达点云Ft、Ft+1，设在其中提取的特征角点集和特征平面点集分别为Ft c、Ft p，在时间t提取的所有特征组成一个激光雷达点云帧Ft，其中Ft＝{Ft c,Ft p}；
T4、当船只的姿态变化超过用户定义的阈值时，选择一个激光雷达帧Ft+1作为关键帧。选择的规则是当船舶的位置变化或旋转角度变化超过一定的阈值；
T5、两个关键帧之间的激光雷达帧将被丢弃，当选择一个激光雷达关键帧时，采用扫描匹配来计算新关键帧与先前子关键帧之间的相对变换，子关键帧是通过将前n个关键帧转换为全局帧来获得的，然后将这些变换的关键帧合并为像素地图Mt，Mt由两个子像素图组成，即上一步中提取的点和面两种类型的特征，这两个子像素图分别表示为
Figure BDA0003661891910000031
(点特征像素图)和
Figure BDA0003661891910000032
(面特征像素图)，特征集和像素图之间的关系可以表示为：
Figure BDA0003661891910000033
其中：
Figure BDA0003661891910000034
Figure BDA0003661891910000035
其中
Figure BDA0003661891910000036
和
Figure BDA0003661891910000037
是全局框架中变换的点和平面特征，新的激光雷达关键帧Ft+1使用IMU的初始猜测转换为全局帧，全局帧中变换后的新关键帧表示为'Ft+1，针对上述关键帧及像素集，使用最近邻搜索的方法进行扫描匹配，并获得他们之间的相对变换ΔTt,t+1，作为激光雷达里程计因子添加到因子图中。
作为本发明进一步的方案：所述GPS组合导航因子构建方法，在SLAM***中，无人船利用GPS、北斗和惯性导航组合模块的绝对测量值，组合导航的位姿信息源通过时间戳，融入至激光雷达里程计的点云帧中，并将其作为GPS因子纳入因子图中。
作为本发明进一步的方案：所述回环因子在SLAM***中的回环检测用于激光雷达里程计因子的校准，以消除时间累积的位姿漂移，所述回环因子关键帧匹配包括以下步骤：
L1、输入当前时刻的特征点云；
L2、在前一时刻的特征点云中搜索当前时刻特征点云的最邻近点和次邻近点；
L3、计算当前时刻特征点云中的点到近邻边缘线的距离；
L4、堆叠距离约束，并通过L-M算法迭代求解相对变化；
L5、获取迭代次数并判断迭代次数是否等于设定值且迭代次数小于最大值，若是则通过相对变化对当前时间的特征点云进行变化并执行步骤L1，若否则执行步骤L6；
L6、判断迭代次数是否等于最大值且距离均小于阈值，若否则执行步骤L4，若是则执行步骤L7；
L7、输出相对变化解。
作为本发明进一步的方案：所述SLAM地图构成是通过获取每个关键帧的准确姿势和每个关键帧对应的三维点云的位姿，通过坐标变化，将激光三维点云坐标有每时刻的激光坐标系转换到全局的地图坐标并拼接生产三维地图。
作为本发明进一步的方案：所述同步定位和地图构建后，在卫星信号正常时，无人船使用卫星和SLAM组合导航，并对无人船进行定位以及地图更新，在卫星信号缺少时，无法使用卫星导航时，自动切换至惯性导航模式并计算无人船位姿信息，作为SLAM的位姿校准和时间戳同步，通过SLAM***进行导航
一种多传感器的无人船同步定位与建图***，包括无人船，所述无人船上设有SLAM***，所述无人船上设有与SLAM***信号连通的固态激光雷达、低线束全景机械激光雷达、卫星和惯性导航模块。
与现有技术相比，本发明的有益效果是：本发明基于SLAM算法，对SLAM***合伙算法进行深度的优化和改进，使得本申请的无人船同步定位和地图构建的方法，可以适应复杂的水面环境，解决常规视觉和激光雷达融合传感器对光线干扰敏感和配准误差较大问题，使得无人船能够在复杂的水面环境上自动探测和识别障碍物及桥梁等目标物，进行自主航向，而且在没有卫星信号的情况下，通过SLAM技术仍可以安全自主航向。
附图说明
图1为本实施例因子图优化的内河无人船舶SLAM原理示意图；
图2、图3为本实施例水面桥梁的关键特征位姿转换示意；
图4为本实施例关键帧匹配算法流程示意图；
图5为本实施例三维点云地图拼接示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参阅图1-5，本发明实施例中，无人船同步定位与建图***，包括无人船，无人船上设有SLAM***，无人船上设有与SLAM***信号连通的固态激光雷达、低线束全景机械激光雷达、卫星和惯性导航模块，所述固态激光雷达用于探测无人船前方一定视角范围(例如120度)的水面环境，并生成点云数据，进一步对点云图像进行识别，识别水面目标的类型，如河岸、桥梁、浮标、船舶或其他障碍物，识别的结果作为无人船巡航场景自动切换，如穿越桥梁、障碍规，所述机械激光雷达用于探测无人船周边360度的全景水面环境，并生成点云数据
一种多传感器融合的无人船同步定位与建图方法(SLAM)，基于SLAM方法进行同步定位和地图构建，SLAM(simultaneous localization and mapping)是机器人、无人机和无人车等无人***常用的同步定位与地图构建，本实施例中的同步定位和地图构建基于SLAM***，其过程包括以下步骤：
S1、通过固态激光雷达探测无人船前方的水面环境并生产第一点云数据；
S2、对第一点云数据进行特征提取，提取角和面的特征用于SLAM的关键帧配准；
S3、通过机械激光雷达对无人船周围进行探测并生产第二点云数据；
S4、对第二点云数据进行特征提取，并与S2中提取的特征进行融合，融合后的数据同步时间戳，形成关键帧，作为关键帧用于SLAM***中的激光雷达里程计和回环检测配准，所述点云数据同时计算无人船两边和后部的河岸或者障碍物位置和距离，用于障碍规避或河岸靠边巡航；
S5、SLAM***接收到S4中特征融合后的关键帧，并对关键帧进行处理以进行同步定位和地图构建。
SLAM由固态激光雷达、机械激光雷达、组合导航传感器组成，采用因子图优化方法。因子图优化SLAM方法指在估计给定传感器测量值的船舶状态
Figure BDA0003661891910000051
dx和dy表示无人船与当前观测地标之间的相对位置，γ表示无人船航向角，为了无缝地结合来自各种传感器的测量，本发明利用来建模这个问题，SLAM***采用因子图建模方法，SLAM***的三类因子是激光雷达里程计因子、GPS因子以及回环因子，因子是为了限制内存使用并提高定位***的效率，使用启发式方法向图中添加新节点
Figure BDA0003661891910000061
仅当船舶的位置或旋转变化超过用户定义的阈值时，才会添加新节点x，再使用贝叶斯树的增量平滑和映射来优化***新节点后的因子图。
激光雷达里程计因子的构建包括以下步骤：
T1、在对固态激光雷达的原始点云进行特征提取的基础上，进一步提取点云图像的水面和岸边或其他水面固体物质的边缘特征，作为点云帧的选择标准，再进行帧间特征匹配流程；
T2、帧间特征配准算法是通过匹配激光扫描数据中两次相邻帧的特征，建立以欧式距离等为度量标准的目标函数，并求解以两次扫描点云之间的变换关系为优化变量的最小二乘问题，即平移和旋转变换，进而求得在两帧的间隔时间内移动机器人的局部位姿变换，在本实施例中，如图2所示为水面桥梁的激光雷达点云图像及关键特征，图3为角和面随时间的位姿变换关系，圆点表示特征角点，三角点表示特征平面点，桥梁上的箭头表示匹配关系，桥梁下的箭头表示t时刻到t+1时刻待求解的位姿变换T(t,t+1)，进而求得在两帧t与t+1的间隔时间内无人船的局部位姿变换T(t,t+1)；
T3、对于t时刻和t+1时刻获得的相邻两帧激光雷达点云Ft、Ft+1，设在其中提取的特征角点集和特征平面点集分别为Ft c、Ft p，在时间t提取的所有特征组成一个激光雷达点云帧Ft，其中Ft＝{Ft c,Ft p}；
T4、为了提供计算的效率和速度，SLAM通常采用了关键帧选择的概念，这在视觉SLAM领域中得到了广泛的应用，当船只的姿态变化超过用户定义的阈值时，选择一个激光雷达帧Ft+1作为关键帧。选择的规则是当船舶的位置变化或旋转角度变化超过一定的阈值；
T5、两个关键帧之间的激光雷达帧将被丢弃，当选择一个激光雷达关键帧时，采用扫描匹配来计算新关键帧与先前子关键帧之间的相对变换，子关键帧是通过将前n个关键帧转换为全局帧来获得的，然后将这些变换的关键帧合并为像素地图Mt，Mt由两个子像素图组成，即上一步中提取的点和面两种类型的特征，这两个子像素图分别表示为
Figure BDA0003661891910000071
(点特征像素图)和
Figure BDA0003661891910000072
(面特征像素图)，特征集和像素图之间的关系可以表示为：
Figure BDA0003661891910000073
其中：
Figure BDA0003661891910000074
Figure BDA0003661891910000075
其中Ft c和'Ft p是全局框架中变换的点和平面特征，新的激光雷达关键帧Ft+1使用IMU的初始猜测转换为全局帧，全局帧中变换后的新关键帧表示为'Ft+1，针对上述关键帧及像素集，使用最近邻搜索的方法进行扫描匹配，并获得他们之间的相对变换ΔTt,t+1，作为激光雷达里程计因子添加到因子图中。
所述GPS组合导航因子构建方法，在SLAM***中，无人船可以通过使用激光雷达里程计因子来实现低漂移状态估计，但定位***在长时间的导航任务中仍然存在漂移问题。因此，利用GPS、北斗和惯性导航组合模块的绝对测量值，组合导航的位姿信息源通过时间戳，融入至激光雷达里程计的点云帧中，并将其作为GPS因子纳入因子图中。
所述回环因子构建方法，SLAM主要是针对没有先验信息的未知环境进行定位与建图，现有的SLAM***具备对已形成地图的历史场景进行判断和识别的能力，通常也被称为回环闭合检测，由于无人船在内河的作业通常是顺序执行不重复路径，因此可理解为“无穷的走廊”，本申请称为顺序SLAM，在该***中的回环检测主要用于激光雷达里程计因子的校准，以消除时间累积的位姿漂移，具体算法原理如图4所示，回环因子在SLAM***中的回环检测用于激光雷达里程计因子的校准，以消除时间累积的位姿漂移，回环因子关键帧匹配包括以下步骤：
L1、输入当前时刻的特征点云；
L2、在前一时刻的特征点云中搜索当前时刻特征点云的最邻近点和次邻近点；
L3、计算当前时刻特征点云中的点到近邻边缘线的距离；
L4、堆叠距离约束，并通过L-M算法迭代求解相对变化；
L5、获取迭代次数并判断迭代次数是否等于设定值且迭代次数小于最大值，若是则通过相对变化对当前时间的特征点云进行变化并执行步骤L1，若否则执行步骤L6；
L6、判断迭代次数是否等于最大值且距离均小于阈值，若否则执行步骤L4，若是则执行步骤L7；
L7、输出相对变化解。
由于使用了因子图，回环检测可以无缝地整合到SLAM***中，因子图主要是通过帧间配准得到的实时位姿估计结果和观测约束条件建立，因子图由点和边构成，其中点表示无人船不同时刻的位姿，边表示不同时刻位姿之间的约束因子，基于欧几里德距离的简单但有效的闭环检测方法。
当新的状态Xi+1被添加到因子图中时，如图1所示，搜索图并找到距离Xi+1一定距离内的先验状态。例如，x3是返回的候选状态，提取子关键帧{F3-m，…，F3，…，F3+m}，并将它们合并到一个局部像素图中，如果可以在Fi+1和像素图之间找到成功的匹配，就可以得到相对变换ΔT3,i+1，将其作为循环闭合因子添加到图表中。
基于SLAM的导航地图的拼接，经过上述图优化的过程，每个关键帧的姿态都是最优的，已知关键帧的准确位姿和每个关键帧对应的三维点云的位姿，通过坐标变换，将激三维光点云坐标由每时刻的激光坐标系，转换到全局的地图坐标系，即完成了三维地图的拼接。船载固态和机械激光雷达的坐标系标定，标定的目的是以正向安装的固态激光雷达坐标系为基准，将机械激光雷达的点云转换到该坐标系下。坐标转换关系可以用旋转矩阵R和平移向量T表示，通过Levenberg-Marquart算法最终计算出R和T两个坐标系之间的旋转矩阵和平移向量。
组合导航在卫星信号正常时用于无人船的导航、定位及地图更新，在卫星信号缺少时，自动切换至惯性导航模式并计算无人船位姿信息，作为SLAM的位姿校准和时间戳同步
一种多传感器的无人船组合导航方法，在卫星信号正常时，无人船使用卫星和SLAM组合导航，并对无人船进行定位以及地图更新，在卫星信号缺少时，无法使用卫星导航时，自动切换至惯性导航模式并计算无人船位姿信息，作为SLAM的位姿校准和时间戳同步，通过SLAM***进行导航。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (9)

1.一种多传感器融合的无人船同步定位与建图方法(SLAM)，其特征在于，所述同步定位和地图构建的过程包括以下步骤：
S1、通过固态激光雷达探测无人船前方的水面环境并生产第一点云数据；
S2、对第一点云数据进行特征提取，提取角和面的特征用于SLAM的关键帧配准；
S3、通过机械激光雷达对无人船周围进行探测并生产第二点云数据；
S4、对第二点云数据进行特征提取，并与所述S2中提取的特征进行融合，形成关键帧；
S5、SLAM***接收到S4中特征融合后的关键帧，并对关键帧进行处理以进行同步定位和地图构建。
2.根据权利要求1所述的一种多传感器融合的无人船同步定位与建图方法(SLAM)，其特征在于，所述SLAM采用因子图优化方法，给定传感器测量值的船舶状态
Figure FDA0003661891900000011
所述SLAM***因子包括激光雷达里程计因子、GPS因子、回环因子；
其中：dx和dy表示无人船与当前观测地标之间的相对位置，γ表示无人船航向角。
3.根据权利要求2所述的一种多传感器融合的无人船同步定位与建图方法(SLAM)，其特征在于，所述因子使用启发式方法添加新节点
Figure FDA0003661891900000012
当船舶的位置或旋转变化超过用户定义的阈值时，添加新节点x，使用贝叶斯树的增量平滑和映射来优化***新节点后的因子图。
4.根据权利要求2所述的一种多传感器融合的无人船同步定位与建图方法(SLAM)，其特征在于，所述激光雷达里程计因子的构建包括以下步骤：
T1、在对固态激光雷达的原始点云进行特征提取的基础上，进一步提取点云图像的水面和岸边或其他水面固体物质的边缘特征，作为点云帧的选择标准，再进行帧间特征匹配流程；
T2、帧间特征配准算法是通过匹配激光扫描数据中两次相邻帧的特征，建立以欧式距离等为度量标准的目标函数，并求解以两次扫描点云之间的变换关系为优化变量的最小二乘问题，即平移和旋转变换，进而求得在两帧的间隔时间内移动机器人的局部位姿变换，进而求得在两帧t与t+1的间隔时间内无人船的局部位姿变换T(t,t+1)；
T3、对于t时刻和t+1时刻获得的相邻两帧激光雷达点云Ft、Ft+1，设在其中提取的特征角点集和特征平面点集分别为Ft c、Ft p，在时间t提取的所有特征组成一个激光雷达点云帧Ft，其中Ft＝{Ft c,Ft p}；
T4、当船只的姿态变化超过用户定义的阈值时，选择一个激光雷达帧Ft+1作为关键帧。选择的规则是当船舶的位置变化或旋转角度变化超过一定的阈值；
T5、两个关键帧之间的激光雷达帧将被丢弃，当选择一个激光雷达关键帧时，采用扫描匹配来计算新关键帧与先前子关键帧之间的相对变换，子关键帧是通过将前n个关键帧转换为全局帧来获得的，然后将这些变换的关键帧合并为像素地图Mt，Mt由两个子像素图组成，即上一步中提取的点和面两种类型的特征，这两个子像素图分别表示为
Figure FDA0003661891900000021
(点特征像素图)和
Figure FDA0003661891900000022
(面特征像素图)，特征集和像素图之间的关系可以表示为：
Figure FDA0003661891900000023
其中：
Figure FDA0003661891900000024
Figure FDA0003661891900000025
其中Ft c和'Ft p是全局框架中变换的点和平面特征，新的激光雷达关键帧Ft+1使用IMU的初始猜测转换为全局帧，全局帧中变换后的新关键帧表示为'Ft+1，针对上述关键帧及像素集，使用最近邻搜索的方法进行扫描匹配，并获得他们之间的相对变换ΔTt,t+1，作为激光雷达里程计因子添加到因子图中。
5.根据权利要求2所述的一种多传感器融合的无人船同步定位与建图方法(SLAM)，其特征在于，所述GPS组合导航因子构建方法，在SLAM***中，无人船利用GPS、北斗和惯性导航组合模块的绝对测量值，组合导航的位姿信息源通过时间戳，融入至激光雷达里程计的点云帧中，并将其作为GPS因子纳入因子图中。
6.根据权利要求2所述的一种多传感器融合的无人船同步定位与建图方法(SLAM)，其特征在于，所述回环因子在SLAM***中的回环检测用于激光雷达里程计因子的校准，以消除时间累积的位姿漂移，所述回环因子关键帧匹配包括以下步骤：
L1、输入当前时刻的特征点云；
L2、在前一时刻的特征点云中搜索当前时刻特征点云的最邻近点和次邻近点；
L3、计算当前时刻特征点云中的点到近邻边缘线的距离；
L4、堆叠距离约束，并通过L-M算法迭代求解相对变化；
L5、获取迭代次数并判断迭代次数是否等于设定值且迭代次数小于最大值，若是则通过相对变化对当前时间的特征点云进行变化并执行步骤L1，若否则执行步骤L6；
L6、判断迭代次数是否等于最大值且距离均小于阈值，若否则执行步骤L4，若是则执行步骤L7；
L7、输出相对变化解。
7.根据权利要求1所述的一种多传感器融合的无人船同步定位与建图方法(SLAM)，其特征在于，所述SLAM地图构成是通过获取每个关键帧的准确姿势和每个关键帧对应的三维点云的位姿，通过坐标变化，将激光三维点云坐标有每时刻的激光坐标系转换到全局的地图坐标并拼接生产三维地图。
8.根据权利要求1所述的一种多传感器融合的无人船同步定位与建图方法(SLAM)，其特征在于，所述同步定位和地图构建后，在卫星信号正常时，无人船使用卫星和SLAM组合导航，并对无人船进行定位以及地图更新，在卫星信号缺少时，无法使用卫星导航时，自动切换至惯性导航模式并计算无人船位姿信息，作为SLAM的位姿校准和时间戳同步，通过SLAM***进行导航。
9.用于权利要求1的一种多传感器融合的无人船同步定位与建图***，包括无人船，其特征在于，所述无人船上设有SLAM***，所述无人船上设有与SLAM***信号连通的固态激光雷达、低线束全景机械激光雷达、卫星和惯性导航模块。
CN202210575283.7A 2022-05-25 2022-05-25 一种多传感器融合的无人船同步定位与建图方法(slam)及*** Active CN115031718B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210575283.7A CN115031718B (zh) 2022-05-25 2022-05-25 一种多传感器融合的无人船同步定位与建图方法(slam)及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210575283.7A CN115031718B (zh) 2022-05-25 2022-05-25 一种多传感器融合的无人船同步定位与建图方法(slam)及***

Publications (2)

Publication Number Publication Date
CN115031718A true CN115031718A (zh) 2022-09-09
CN115031718B CN115031718B (zh) 2023-10-31

Family

ID=83122050

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210575283.7A Active CN115031718B (zh) 2022-05-25 2022-05-25 一种多传感器融合的无人船同步定位与建图方法(slam)及***

Country Status (1)

Country Link
CN (1) CN115031718B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116736327A (zh) * 2023-08-10 2023-09-12 长沙智能驾驶研究院有限公司 定位数据优化方法、装置、电子设备和可读存储介质
CN117215309A (zh) * 2023-09-27 2023-12-12 河南工业大学 无人车粮库清扫***的定位与建图方法、定位与建图装置

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180075643A1 (en) * 2015-04-10 2018-03-15 The European Atomic Energy Community (Euratom), Represented By The European Commission Method and device for real-time mapping and localization
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法
CN113340295A (zh) * 2021-06-16 2021-09-03 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN113670316A (zh) * 2021-08-27 2021-11-19 广州市工贸技师学院（广州市工贸高级技工学校） 基于双雷达的路径规划方法、***、存储介质及电子设备
CN114080625A (zh) * 2020-06-19 2022-02-22 深圳市大疆创新科技有限公司 绝对位姿确定方法、电子设备及可移动平台

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180075643A1 (en) * 2015-04-10 2018-03-15 The European Atomic Energy Community (Euratom), Represented By The European Commission Method and device for real-time mapping and localization
CN114080625A (zh) * 2020-06-19 2022-02-22 深圳市大疆创新科技有限公司 绝对位姿确定方法、电子设备及可移动平台
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法
CN113340295A (zh) * 2021-06-16 2021-09-03 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN113670316A (zh) * 2021-08-27 2021-11-19 广州市工贸技师学院（广州市工贸高级技工学校） 基于双雷达的路径规划方法、***、存储介质及电子设备

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116736327A (zh) * 2023-08-10 2023-09-12 长沙智能驾驶研究院有限公司 定位数据优化方法、装置、电子设备和可读存储介质
CN116736327B (zh) * 2023-08-10 2023-10-24 长沙智能驾驶研究院有限公司 定位数据优化方法、装置、电子设备和可读存储介质
CN117215309A (zh) * 2023-09-27 2023-12-12 河南工业大学 无人车粮库清扫***的定位与建图方法、定位与建图装置

Also Published As

Publication number Publication date
CN115031718B (zh) 2023-10-31

Similar Documents

Publication Publication Date Title
CN110850403B (zh) 一种多传感器决策级融合的智能船水面目标感知识别方法
Wu et al. Survey of underwater robot positioning navigation
CN102042835B (zh) 自主式水下机器人组合导航***
CN107167826B (zh) 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位***及方法
CN115031718B (zh) 一种多传感器融合的无人船同步定位与建图方法(slam)及***
Chambers et al. Perception for a river mapping robot
EP2687868B1 (en) Systems and methods for correlating reduced evidence grids
Ribas et al. Underwater SLAM in a marina environment
EP2834668A1 (en) Method for localizing a vehicle equipped with two lidar systems
US11580688B2 (en) High-definition city mapping
CN112880678A (zh) 一种复杂水域环境中无人艇航行规划方法
CN115790571A (zh) 基于异构无人***相互观测的同时定位与地图构建方法
Wang et al. Monocular visual SLAM algorithm for autonomous vessel sailing in harbor area
CN114077249B (zh) 一种作业方法、作业设备、装置、存储介质
CN115574816A (zh) 仿生视觉多源信息智能感知无人平台
CN114593739A (zh) 基于视觉检测与参考线匹配的车辆全局定位方法及装置
Ma et al. RoLM: Radar on LiDAR map localization
CN117470259A (zh) 一种子母式空地协同多传感器融合三维建图***
CN112747749A (zh) 一种基于双目视觉和激光融合定位导航***
CN116380079A (zh) 一种融合前视声呐与orb-slam3的水下slam方法
Jabbour et al. Backing up GPS in urban areas using a scanning laser
KR102624644B1 (ko) 벡터 맵을 이용한 이동체의 맵 매칭 위치 추정 방법
Krishnaswamy et al. Sensor fusion for GNSS denied navigation
Tamjidi et al. 6-DOF pose estimation of an autonomous car by visual feature correspondence and tracking
Yun et al. Multi-hypothesis outdoor localization using multiple visual features with a rough map

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
CB02 Change of applicant information
CB02 Change of applicant information

Address after: 230031 room 304-1, R & D building, Hefei Institute of technology innovation engineering, Chinese Academy of Sciences, 2666 Xiyou Road, high tech Zone, Hefei, Anhui Province

Applicant after: Anhui Zhongke Heding Intelligent Technology Development Co.,Ltd.

Address before: 230031 room 304-1, R & D building, Hefei Institute of technology innovation engineering, Chinese Academy of Sciences, 2666 Xiyou Road, high tech Zone, Hefei, Anhui Province

Applicant before: Anhui zhongkeheding Technology Development Co.,Ltd.

TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20230809

Address after: Room 1157, Building A, J1, Phase II, Innovation Industrial Park, No. 2800, Chuangxin Avenue, High tech Zone, Hefei, Anhui 230088

Applicant after: Hefei Henghao Intelligent Technology Partnership (L.P.)

Address before: 230031 room 304-1, R & D building, Hefei Institute of technology innovation engineering, Chinese Academy of Sciences, 2666 Xiyou Road, high tech Zone, Hefei, Anhui Province

Applicant before: Anhui Zhongke Heding Intelligent Technology Development Co.,Ltd.

GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20240425

Address after: Room 766-3, 7th Floor, Building A3A4, Zhong'an Chuanggu Science and Technology Park, No. 900 Wangjiang West Road, High tech Zone, Hefei City, Anhui Province, 230088

Patentee after: Zhongyu Future (Hefei) Water Technology Co.,Ltd.

Country or region after: China

Address before: Room 1157, Building A, J1, Phase II, Innovation Industrial Park, No. 2800, Chuangxin Avenue, High tech Zone, Hefei, Anhui 230088

Patentee before: Hefei Henghao Intelligent Technology Partnership (L.P.)

Country or region before: China