CN111258313B - 多传感器融合slam系统及机器人 - Google Patents

多传感器融合slam系统及机器人 Download PDF

Info

Publication number
CN111258313B
CN111258313B CN202010064034.2A CN202010064034A CN111258313B CN 111258313 B CN111258313 B CN 111258313B CN 202010064034 A CN202010064034 A CN 202010064034A CN 111258313 B CN111258313 B CN 111258313B
Authority
CN
China
Prior art keywords
pose
key frame
laser
feature points
visual
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.)
Active
Application number
CN202010064034.2A
Other languages
English (en)
Other versions
CN111258313A (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.)
Shenzhen Pudu Technology Co Ltd
Original Assignee
Shenzhen Pudu Technology 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 Shenzhen Pudu Technology Co Ltd filed Critical Shenzhen Pudu Technology Co Ltd
Priority to CN202010064034.2A priority Critical patent/CN111258313B/zh
Publication of CN111258313A publication Critical patent/CN111258313A/zh
Priority to PCT/CN2020/135026 priority patent/WO2021147546A1/zh
Priority to US17/793,770 priority patent/US20230066441A1/en
Priority to EP20916000.1A priority patent/EP4083739A4/en
Application granted granted Critical
Publication of CN111258313B publication Critical patent/CN111258313B/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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
    • B25J9/162—Mobile manipulator, movable base with manipulator arm mounted on it
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4808—Evaluating distance, position or velocity data
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/52—Surveillance or monitoring of activities, e.g. for recognising suspicious objects
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/82—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using neural networks

Abstract

本发明提供了一种多传感器融合SLAM系统及机器人，所述系统运行于移动机器人，包括：视觉惯性模块、激光扫描匹配模块、闭环检测模块、以及视觉激光图优化模块。根据本发明提供的多传感器融合SLAM系统及机器人，使用体素子图可减少激光匹配约束优化的计算量，使得位姿的计算更加准确，系统长时间运行累计的误差可以通过各模块充分融合而及时得到修复，并且整体提升了系统的鲁棒性及定位和建图的准确性。

Description

多传感器融合SLAM系统及机器人
技术领域
本发明涉及机器人技术领域，特别涉及一种多传感器融合SLAM系统及机器人。
背景技术
SLAM技术在过去的几十年里得到了广泛的研究，尽管SLAM技术解决了机器人在未知环境同步定位与地图构建，但在处理多样化的环境和长时间连续运行方面，任然存在挑战。SLAM可以运行在各种各样的传感器上。在过去的几年里，基于激光雷达的SLAM系统因其对环境变化的鲁棒性而比基于视觉的系统更受欢迎。然而，纯激光雷达系统有其不足之处，它们在隧道或走廊等重复结构的环境中会失效。
发明内容
在这些有挑战的环境中，定位和建图需要充分利用各种传感器的优点。
鉴于此，本发明的目的在于提供一种多传感器融合SLAM系统及机器人，整体提升系统的鲁棒性及定位和建图的准确性。
为了实现上述目的，本发明实施方式提供如下技术方案：
本发明提供一种多传感器融合SLAM系统，所述系统运行于移动机器人，包括：
激光扫描匹配模块，使用所述位姿信息作为初值，将激光扫描的点云与体素地图匹配求解出高级位姿，根据所述高级位姿将所述点云融入所述体素地图，并衍生新的体素子图，所述激光扫描匹配模块产生激光匹配约束；
闭环检测模块，使用深度神经网络提取视觉关键帧图像描述符，将所述图像描述符与之前的关键帧的所述图象描述符进行比较，判断是否存在闭环，如果存在闭环，则采用n点透视法确定两个所述关键帧的位姿变换，根据所述位姿变换和所述体素子图求解闭环位姿约束，并将所述闭环位姿约束发送至所述激光扫描匹配模块；以及
视觉激光图优化模块，在发生所述闭环后，根据所述位姿信息、所述激光匹配约束以及所述闭环位姿约束，修正系统累计误差。
在这种情况下，使用体素子图可减少激光匹配约束优化的计算量，使得位姿的计算更加准确，系统长时间运行累计的误差可以通过各模块充分融合而及时得到修复，并且整体提升了系统的鲁棒性及定位和建图的准确性。
其中，采用迭代最近点算法将激光扫描的点云与体素地图匹配求解出所述高级位姿。
其中，所述视觉惯性模块包括视觉前端单元、IMU预积分单元和滑动窗口优化单元，所述视觉前端单元用于选定所述关键帧，所述IMU预积分单元生成IMU观测值，所述滑动窗口优化单元用于联合优化视觉重投影误差、惯性测量误差和里程测量误差。
由此，IMU预积分单元可将重力加速度对位姿和速度的影响去除，使得新定义的IMU观测值与初值积分的位姿和速度无关，在优化过程中无需反复重新积分，加快优化速度，进而可提高滑动窗口优化单元计算相邻帧的预积分增量、预积分误差的雅各比矩阵和协方差矩阵的效率；滑动窗口优化单元采用窗口优化而不是全局优化，可显著降低计算量，保证计算速度，视觉惯性模块可以为激光扫描匹配模块输出实时准确地位姿信息。
其中，所述视觉前端单元采用单目相机或者双目相机作为输入，所述单目相机或者双目相机采集初始图像，所述视觉前端单元通过KLT稀疏光流算法跟踪各个帧的特征点，所述视觉前端单元包括检测器，所述检测器检测角点特征，保持每个所述初始图像中的所述特征点为最小数量，所述检测器用于设置相邻两个所述特征点之间的最小像素间隔，所述视觉前端单元去除所述特征点的畸变，并使用随机抽样一致算法和基本矩阵模型剔除错误匹配的所述特征点，将正确匹配的所述特征点投影到单位球面上。
由此，可以是特征点便于进一步优化。
其中，所述选定关键帧，具体包括：判断当前帧和最新的所述关键帧之间的被跟踪的所述特征点的平均视差是否超过阈值，如果超过第一阈值，则将所述当前帧视为新的关键帧，如果所述帧的跟踪的特征点的数量低于第二阈值，则将所述帧视为所述新的关键帧。
在这种情况下，避免了完全丢失特征跟踪。
其中，所述激光扫描匹配模块包括激光雷达，所述激光雷达获取扫描点，根据所述位姿信息和IMU观测值对所述扫描点执行变换，将其转换为当前时刻的所述机器人所在坐标系下的三维点云。
由此，可避免由于激光雷达的旋转速度相对机器人移动速度较慢时，产生的非常严重的运动畸变，显著提升了位姿估计的精度。
其中，所述闭环检测模块包括相似性检测单元、视觉位姿求解单元和激光位姿求解单元，所述相似性检测单元提取当前关键帧图像描述符，并与关键帧数据集中的关键帧图像描述符做对比，选出最相似的相似关键帧，将所述相似关键帧插入所述关键帧数据集，所述视觉位姿求解单元通过快速特征点提取和描述算法匹配所述当前关键帧和所述相似关键帧的特征点，使用随机抽样一致算法和基本矩阵模型剔除错误匹配的所述特征点，当正确匹配的所述特征点达到第三阈值，使用随机抽样一致算法和n点透视法求解所述当前关键帧到所述相似关键帧的相对位姿变换，所述激光位姿求解单元选择所述当前关键帧以及所述相似关键帧相关联的两个所述体素子图，将所述相对位姿变换作为初值，采用迭代最近点算法匹配所述两个体素子图，得到最终相对位姿变换。
在这种情况下，可以显著提升对机器人视角变化、环境亮度变化和弱纹理等情况下的闭环检测的速度和准确性。
本发明还提供一种机器人，所述机器人包括如上所述的多传感器融合SLAM系统。
根据本发明所提供的多传感器融合SLAM系统及机器人，使用体素子图可减少激光匹配约束优化的计算量，使得位姿的计算更加准确，系统长时间运行累计的误差可以通过各模块充分融合而及时得到修复，并且整体提升了系统的鲁棒性及定位和建图的准确性。
附图说明
图1示出了本发明的实施方式所涉及的多传感器融合SLAM系统的示意图；
图2示出了本发明的实施方式所涉及的多传感器融合SLAM系统的视觉惯性模块的示意图；
图3示出了本发明的实施方式所涉及的多传感器融合SLAM系统的闭环检测模块的示意图。
具体实施方式
以下，参考附图，详细地说明本发明的优选实施方式。在下面的说明中，对于相同的部件赋予相同的符号，省略重复的说明。另外，附图只是示意性的图，部件相互之间的尺寸的比例或者部件的形状等可以与实际的不同。
本发明实施方式涉及多传感器融合SLAM系统及机器人。
如图1所示，多传感器融合SLAM系统100运行于移动机器人。包括：视觉惯性模块10、激光扫描匹配模块20、闭环检测模块30、视觉激光图优化模块40。视觉惯性模块10，用于输出位姿信息。激光扫描匹配模块20使用所述位姿信息作为初值，将激光扫描的点云与体素地图匹配求解出高级位姿，根据所述高级位姿将所述点云融入所述体素地图，并衍生新的体素子图，所述激光扫描匹配模块产生激光匹配约束。闭环检测模块30使用深度神经网络提取视觉关键帧图像描述符，将所述图像描述符与之前的关键帧的所述图象描述符进行比较，判断是否存在闭环，如果存在闭环，则采用n点透视法(PnP)确定两个所述关键帧的位姿变换，根据所述位姿变换和所述体素子图求解闭环位姿约束，并将所述闭环位姿约束发送至所述激光扫描匹配模块。视觉激光图优化模块20在发生所述闭环后，根据所述位姿信息、所述激光匹配约束以及所述闭环位姿约束，修正系统累计误差。在这种情况下，使用体素子图可减少激光匹配约束优化的计算量，使得位姿的计算更加准确，系统长时间运行累计的误差可以通过各模块充分融合而及时得到修复，并且整体提升了系统的鲁棒性及定位和建图的准确性。
在本实施方式中，采用迭代最近点算法(ICP)将激光扫描的点云与体素地图匹配求解出所述高级位姿。
如图2所示，在本实施方式中，视觉惯性模块10包括视觉前端单元11、IMU预积分单元12和滑动窗口优化单元13。视觉前端单元11用于选定所述关键帧。IMU预积分单元12生成IMU观测值。滑动窗口优化单元13用于联合优化视觉重投影误差、惯性测量误差和里程测量误差。由此，IMU预积分单元可将重力加速度对位姿和速度的影响去除，使得新定义的IMU观测值与初值积分的位姿和速度无关，在优化过程中无需反复重新积分，加快优化速度，进而可提高滑动窗口优化单元计算相邻帧的预积分增量、预积分误差的雅各比矩阵和协方差矩阵的效率；滑动窗口优化单元采用窗口优化而不是全局优化，可显著降低计算量，保证计算速度，视觉惯性模块可以为激光扫描匹配模块输出实时准确地位姿信息。
在本实施方式中，视觉前端单元11采用单目相机或者双目相机作为输入。所述单目相机或者双目相机采集初始图像。视觉前端单元11通过KLT稀疏光流算法跟踪各个帧的特征点。视觉前端单元11包括检测器。所述检测器检测角点特征，保持每个所述初始图像中的所述特征点为最小数量。所述检测器用于设置相邻两个所述特征点之间的最小像素间隔。视觉前端单元11去除所述特征点的畸变，并使用随机抽样一致算法(RANSAC)和基本矩阵模型剔除错误匹配的所述特征点，将正确匹配的所述特征点投影到单位球面上。由此，可以使特征点便于进一步优化。
在本实施方式中，优选地，每个图像中特征点的最小数量为100–300。
在本实施方式中，所述选定关键帧，具体包括：
判断当前帧和最新的所述关键帧之间的被跟踪的所述特征点的平均视差是否超过阈值，如果超过第一阈值，则将所述当前帧视为新的关键帧，如果所述帧的跟踪的特征点的数量低于第二阈值，则将所述帧视为所述新的关键帧。在这种情况下，避免了完全丢失特征跟踪。
在本实施方式中，滑动窗口优化单元13的滑动窗口中的状态变量为:
Figure BDA0002375409050000061
Figure BDA0002375409050000062
Figure BDA0002375409050000063
其中,xk对应第k帧图像的IMU状态，包括IMU在世界坐标系下的位置、速度、姿态，以及IMU坐标系下的加速度计偏置和陀螺仪偏置。n和m分别滑动窗口中关键帧数量和特征点数量。λl表示第l个特征点在其第一次观测帧上的逆深度。
Figure BDA0002375409050000064
表示相机到IMU的外参，包括位置、姿态和时延。
为计算滑动窗口中的状态变量，使用非线性优化方式求解公式。其中，
Figure BDA0002375409050000065
和
Figure BDA0002375409050000066
分别表示IMU、视觉和里程测量误差。表示窗口内所有IMU测量，C表示窗口内至少观测到两次的特征点，O表示窗口内所有的里程测量。是由边缘化得到的先验信息。
Figure BDA0002375409050000067
在一些示例中，为保证窗口长度固定，需要丢弃多余的关键帧。但为了保留被丢弃关键帧对窗口内其他帧的约束，需要将被丢弃的信息转化为先验信息，转化过程称为边缘化，具体由舒尔补计算实现。
在本实施方式中，激光扫描匹配模块20包括激光雷达。所述激光雷达获取扫描点，根据所述位姿信息和IMU观测值对所述扫描点执行变换，将其转换为当前时刻的所述机器人所在坐标系下的三维点云。由此，可避免由于激光雷达的旋转速度相对机器人移动速度较慢时，产生的非常严重的运动畸变，显著提升了位姿估计的精度。
在一些示例中，激光雷达可选择二维或三维激光。为减小三维激光匹配的计算量需要对其提取几何特征，包括边缘点和平面点。
如图3所示，在本实施方式中，闭环检测模块30包括相似性检测单元31、视觉位姿求解单元32和激光位姿求解单元33。相似性检测单元31提取当前关键帧图像描述符，并与关键帧数据集中的关键帧图像描述符做对比，选出最相似的相似关键帧，将所述相似关键帧插入所述关键帧数据集。视觉位姿求解单元32通过快速特征点提取和描述算法匹配所述当前关键帧和所述相似关键帧的特征点，使用随机抽样一致算法(RANSAC)和基本矩阵模型剔除错误匹配的所述特征点，当正确匹配的所述特征点达到第三阈值，使用随机抽样一致算法(RANSAC)和n点透视法(PnP)求解所述当前关键帧到所述相似关键帧的相对位姿变换。激光位姿求解单元33选择所述当前关键帧以及所述相似关键帧相关联的两个所述体素子图，将所述相对位姿变换作为初值，采用迭代最近点算法(ICP)匹配所述两个体素子图，得到最终相对位姿变换。在这种情况下，可以显著提升对机器人视角变化、环境亮度变化和弱纹理等情况下的闭环检测的速度和准确性。
在一些示例中，当求解内点数量大于设定阈值才认为当前回环检查有效。
在本实施方式中，第一阈值、第二阈值、第三阈值可以是预设设定的任意值。
本发明实施方式还涉及一种机器人。所述机器人包括如上所述的多传感器融合SLAM系统100。关于多传感器融合SLAM系统100，在此不做赘述。在这种情况下，整体提升了机器人SLAM系统的鲁棒性及定位和建图的准确性。
以上所述的实施方式，并不构成对该技术方案保护范围的限定。任何在上述实施方式的精神和原则之内所作的修改、等同更换和改进等，均应包含在该技术方案的保护范围之内。

Claims (7)

1.一种多传感器融合SLAM系统，其特征在于，所述系统运行于移动机器人，包括：
视觉惯性模块，用于输出位姿信息；
激光扫描匹配模块，使用所述位姿信息作为初值，将激光扫描的点云与体素地图匹配求解出高级位姿，根据所述高级位姿将所述点云融入所述体素地图，并衍生新的体素子图，所述激光扫描匹配模块产生激光匹配约束；
闭环检测模块，使用深度神经网络提取视觉关键帧图像描述符，将所述图像描述符与之前的关键帧的所述图像描述符进行比较，判断是否存在闭环，如果存在闭环，则采用n点透视法确定两个所述关键帧的位姿变换，根据所述位姿变换和所述体素子图求解闭环位姿约束，并将所述闭环位姿约束发送至所述激光扫描匹配模块；
所述闭环检测模块包括相似性检测单元、视觉位姿求解单元和激光位姿求解单元，所述相似性检测单元提取当前关键帧图像描述符，并与关键帧数据集中的关键帧图像描述符做对比，选出最相似的相似关键帧，将所述相似关键帧插入所述关键帧数据集，所述视觉位姿求解单元通过快速特征点提取和描述算法匹配所述当前关键帧和所述相似关键帧的特征点，使用随机抽样一致算法和基本矩阵模型剔除错误匹配的所述特征点，当正确匹配的所述特征点达到第三阈值，使用随机抽样一致算法和n点透视法求解所述当前关键帧到所述相似关键帧的相对位姿变换，所述激光位姿求解单元选择所述当前关键帧以及所述相似关键帧相关联的两个所述体素子图，将所述相对位姿变换作为初值，采用迭代最近点算法匹配所述两个体素子图，得到最终相对位姿变换，以及
视觉激光图优化模块，在发生所述闭环后，根据所述位姿信息、所述激光匹配约束以及所述闭环位姿约束，修正系统累计误差。
2.如权利要求1所述的多传感器融合SLAM系统，其特征在于，采用迭代最近点算法将激光扫描的点云与体素地图匹配求解出所述高级位姿。
3.如权利要求1所述的多传感器融合SLAM系统，其特征在于，所述视觉惯性模块包括视觉前端单元、IMU预积分单元和滑动窗口优化单元，所述视觉前端单元用于选定所述关键帧，所述IMU预积分单元生成IMU观测值，所述滑动窗口优化单元用于联合优化视觉重投影误差、惯性测量误差和里程测量误差。
4.如权利要求3所述的多传感器融合SLAM系统，其特征在于，所述视觉前端单元采用单目相机或者双目相机作为输入，所述单目相机或者双目相机采集初始图像，所述视觉前端单元通过KLT稀疏光流算法跟踪各个帧的特征点，所述视觉前端单元包括检测器，所述检测器检测角点特征，保持每个所述初始图像中的所述特征点为最小数量，所述检测器用于设置相邻两个所述特征点之间的最小像素间隔，所述视觉前端单元去除所述特征点的畸变，并使用随机抽样一致算法和基本矩阵模型剔除错误匹配的所述特征点，将正确匹配的所述特征点投影到单位球面上。
5.如权利要求4所述的多传感器融合SLAM系统，其特征在于，所述选定关键帧，具体包括：判断当前帧和最新的所述关键帧之间的被跟踪的所述特征点的平均视差是否超过阈值，如果超过第一阈值，则将所述当前帧视为新的关键帧，如果所述帧的跟踪的特征点的数量低于第二阈值，则将所述帧视为所述新的关键帧。
6.如权利要求3所述的多传感器融合SLAM系统，其特征在于，所述激光扫描匹配模块包括激光雷达，所述激光雷达获取扫描点，根据所述位姿信息和IMU观测值对所述扫描点执行变换，将其转换为当前时刻的所述机器人所在坐标系下的三维点云。
7.一种机器人，其特征在于，所述机器人包括如权利要求1-6任一项所述的多传感器融合SLAM系统。
CN202010064034.2A 2020-01-20 2020-01-20 多传感器融合slam系统及机器人 Active CN111258313B (zh)

Priority Applications (4)

Application Number Priority Date Filing Date Title
CN202010064034.2A CN111258313B (zh) 2020-01-20 2020-01-20 多传感器融合slam系统及机器人
PCT/CN2020/135026 WO2021147546A1 (zh) 2020-01-20 2020-12-09 多传感器融合slam系统、多传感器融合方法及机器人、介质
US17/793,770 US20230066441A1 (en) 2020-01-20 2020-12-09 Multi-sensor fusion slam system, multi-sensor fusion method, robot, and medium
EP20916000.1A EP4083739A4 (en) 2020-01-20 2020-12-09 MULTIPLE SENSOR FUSION SLAM SYSTEM, MULTIPLE SENSOR FUSION METHOD, ROBOT AND SUPPORT

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010064034.2A CN111258313B (zh) 2020-01-20 2020-01-20 多传感器融合slam系统及机器人

Publications (2)

Publication Number Publication Date
CN111258313A CN111258313A (zh) 2020-06-09
CN111258313B true CN111258313B (zh) 2022-06-07

Family

ID=70947053

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010064034.2A Active CN111258313B (zh) 2020-01-20 2020-01-20 多传感器融合slam系统及机器人

Country Status (4)

Country Link
US (1) US20230066441A1 (zh)
EP (1) EP4083739A4 (zh)
CN (1) CN111258313B (zh)
WO (1) WO2021147546A1 (zh)

Families Citing this family (32)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111258313B (zh) * 2020-01-20 2022-06-07 深圳市普渡科技有限公司 多传感器融合slam系统及机器人
CN111879306B (zh) * 2020-06-17 2022-09-27 杭州易现先进科技有限公司 视觉惯性定位的方法、装置、系统和计算机设备
CN113819905A (zh) * 2020-06-19 2021-12-21 北京图森未来科技有限公司 一种基于多传感器融合的里程计方法及装置
CN113945206B (zh) * 2020-07-16 2024-04-19 北京图森未来科技有限公司 一种基于多传感器融合的定位方法及装置
CN112083726B (zh) * 2020-09-04 2021-11-23 湖南大学 一种面向园区自动驾驶的双滤波器融合定位系统
CN112304307A (zh) * 2020-09-15 2021-02-02 浙江大华技术股份有限公司 一种基于多传感器融合的定位方法、装置和存储介质
CN112241002B (zh) * 2020-10-11 2022-10-18 西北工业大学 一种基于Karto SLAM的鲁棒闭环检测新方法
CN112596064B (zh) * 2020-11-30 2024-03-08 中科院软件研究所南京软件技术研究院 激光与视觉融合的一体化室内机器人全局定位方法
CN112525202A (zh) * 2020-12-21 2021-03-19 北京工商大学 一种基于多传感器融合的slam定位导航方法及系统
CN113267178B (zh) * 2021-03-25 2023-07-07 浙江大学 一种基于多传感器融合的模型位姿测量系统与方法
CN112985416B (zh) * 2021-04-19 2021-07-30 湖南大学 激光与视觉信息融合的鲁棒定位和建图方法及系统
CN113624221B (zh) * 2021-06-30 2023-11-28 同济人工智能研究院(苏州)有限公司 一种融合视觉与激光的2.5d地图构建方法
CN113483755B (zh) * 2021-07-09 2023-11-07 北京易航远智科技有限公司 一种基于非全局一致地图的多传感器组合定位方法及系统
CN115700507B (zh) * 2021-07-30 2024-02-13 北京小米移动软件有限公司 地图更新方法及其装置
CN113837277B (zh) * 2021-09-24 2022-11-18 东南大学 一种基于视觉点线特征优化的多源融合slam系统
CN114418927B (zh) * 2021-11-09 2023-06-16 四川大学 一种基于空间关系特征匹配的闭环检测方法及系统
CN114694013B (zh) * 2022-04-11 2022-11-15 北京理工大学 一种分布式多机协同视觉slam方法与系统
CN114643447A (zh) * 2022-05-10 2022-06-21 安徽理工大学 一种机器人定点焊接辅助装置
CN115115702A (zh) * 2022-05-24 2022-09-27 广东人工智能与先进计算研究院 自主定位方法、装置、设备及计算机可读存储介质
CN115267796B (zh) * 2022-08-17 2024-04-09 深圳市普渡科技有限公司 定位方法、装置、机器人和存储介质
CN115205470B (zh) * 2022-09-06 2023-02-21 深圳市其域创新科技有限公司 续扫重定位方法、装置、设备、存储介质及三维续扫方法
CN115222776B (zh) * 2022-09-19 2022-12-27 中国人民解放军国防科技大学 匹配辅助视觉目标跟踪方法、装置、电子设备和存储介质
CN116026335B (zh) * 2022-12-26 2023-10-03 广东工业大学 一种适用于未知室内环境的移动机器人定位方法与系统
CN116222543B (zh) * 2023-04-26 2023-07-28 齐鲁工业大学(山东省科学院) 用于机器人环境感知的多传感器融合地图构建方法及系统
CN116468858B (zh) * 2023-06-19 2023-08-15 中色蓝图科技股份有限公司 一种基于人工智能的地图融合方法及系统
CN116758161A (zh) * 2023-06-26 2023-09-15 北京道仪数慧科技有限公司 移动端空间数据生成方法及空间感知移动端
CN116698046B (zh) * 2023-08-04 2023-12-01 苏州观瑞汽车技术有限公司 一种物业室内服务机器人建图定位和回环检测方法及系统
CN117128951B (zh) * 2023-10-27 2024-02-02 中国科学院国家授时中心 适用于自动驾驶农机的多传感器融合导航定位系统及方法
CN117329928B (zh) * 2023-11-30 2024-02-09 武汉阿内塔科技有限公司 一种基于多元情报融合的无人机综合探测方法和系统
CN117308900B (zh) * 2023-11-30 2024-02-09 中国矿业大学 井下运输车移动测量系统、运载通行状态模拟与监测方法
CN117433511B (zh) * 2023-12-20 2024-03-12 绘见科技(深圳)有限公司 一种多传感器融合定位方法
CN117761717A (zh) * 2024-02-21 2024-03-26 天津大学四川创新研究院 一种自动回环三维重建系统及运行方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105856230A (zh) * 2016-05-06 2016-08-17 简燕梅 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法
CN109631855A (zh) * 2019-01-25 2019-04-16 西安电子科技大学 基于orb-slam的高精度车辆定位方法
US10390003B1 (en) * 2016-08-29 2019-08-20 Perceptln Shenzhen Limited Visual-inertial positional awareness for autonomous and non-autonomous device
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN111275763A (zh) * 2020-01-20 2020-06-12 深圳市普渡科技有限公司 闭环检测系统、多传感器融合slam系统及机器人

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104764457B (zh) * 2015-04-21 2017-11-17 北京理工大学 一种用于无人车的城市环境构图方法
US20170374342A1 (en) * 2016-06-24 2017-12-28 Isee, Inc. Laser-enhanced visual simultaneous localization and mapping (slam) for mobile devices
JP2020003830A (ja) * 2016-09-26 2020-01-09 株式会社日立製作所 移動体制御装置および移動体制御システム
US10151588B1 (en) * 2016-09-28 2018-12-11 Near Earth Autonomy, Inc. Determining position and orientation for aerial vehicle in GNSS-denied situations
CN108073167A (zh) * 2016-11-10 2018-05-25 深圳灵喵机器人技术有限公司 一种基于深度相机与激光雷达的定位与导航方法
EP3574285A4 (en) * 2017-01-27 2020-12-02 Kaarta, Inc. LASER SCAN DEVICE WITH REAL-TIME ESTIMATE OF CLEAN MOTION ONLINE
CN107450577A (zh) * 2017-07-25 2017-12-08 天津大学 基于多传感器的无人机智能感知系统和方法
CN107246876B (zh) * 2017-07-31 2020-07-07 中北润良新能源汽车(徐州)股份有限公司 一种无人驾驶汽车自主定位与地图构建的方法及系统
CN108665540A (zh) * 2018-03-16 2018-10-16 浙江工业大学 基于双目视觉特征和imu信息的机器人定位与地图构建系统
CN109144057A (zh) * 2018-08-07 2019-01-04 上海大学 一种基于实时环境建模和自主路径规划的导盲车
CN109917786A (zh) * 2019-02-04 2019-06-21 浙江大学 一种面向复杂环境作业的机器人感知系统及系统运行方法
CN109828588A (zh) * 2019-03-11 2019-05-31 浙江工业大学 一种基于多传感器融合的机器人室内路径规划方法
CN109974712A (zh) * 2019-04-22 2019-07-05 广东亿嘉和科技有限公司 一种基于图优化的变电站巡检机器人建图方法
CN110262546B (zh) * 2019-06-18 2021-07-20 武汉大学 一种隧道智能无人机巡检方法
CN110428467B (zh) * 2019-07-30 2020-05-19 四川大学 一种相机、imu和激光雷达联合的机器人定位方法
CN110706279B (zh) * 2019-09-27 2021-09-07 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111258313B (zh) * 2020-01-20 2022-06-07 深圳市普渡科技有限公司 多传感器融合slam系统及机器人

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105856230A (zh) * 2016-05-06 2016-08-17 简燕梅 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法
US10390003B1 (en) * 2016-08-29 2019-08-20 Perceptln Shenzhen Limited Visual-inertial positional awareness for autonomous and non-autonomous device
CN109631855A (zh) * 2019-01-25 2019-04-16 西安电子科技大学 基于orb-slam的高精度车辆定位方法
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN111275763A (zh) * 2020-01-20 2020-06-12 深圳市普渡科技有限公司 闭环检测系统、多传感器融合slam系统及机器人

Also Published As

Publication number Publication date
EP4083739A1 (en) 2022-11-02
EP4083739A4 (en) 2024-01-24
WO2021147546A1 (zh) 2021-07-29
US20230066441A1 (en) 2023-03-02
CN111258313A (zh) 2020-06-09

Similar Documents

Publication Publication Date Title
CN111258313B (zh) 多传感器融合slam系统及机器人
CN111275763B (zh) 闭环检测系统、多传感器融合slam系统及机器人
US11668571B2 (en) Simultaneous localization and mapping (SLAM) using dual event cameras
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN112634451B (zh) 一种融合多传感器的室外大场景三维建图方法
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN110044354A (zh) 一种双目视觉室内定位与建图方法及装置
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN112219087A (zh) 位姿预测方法、地图构建方法、可移动平台及存储介质
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN111366153B (zh) 一种激光雷达与imu紧耦合的定位方法
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN111932674A (zh) 一种线激光视觉惯性系统的优化方法
CN113052908A (zh) 一种基于多传感器数据融合的移动机器人位姿估计算法
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN114485640A (zh) 基于点线特征的单目视觉惯性同步定位与建图方法及系统
CN115371665A (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN112945233B (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN116007609A (zh) 一种多光谱图像和惯导融合的定位方法和计算系统
CN116468786A (zh) 一种面向动态环境的基于点线联合的语义slam方法
CN112432653B (zh) 基于点线特征的单目视觉惯性里程计方法
CN114445591A (zh) 地图构建方法、系统、设备以及计算机存储介质
Xia et al. YOLO-Based Semantic Segmentation for Dynamic Removal in Visual-Inertial SLAM
CN117611762B (zh) 一种多层级地图构建方法、系统及电子设备

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