CN111983639B - 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法 - Google Patents
一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法 Download PDFInfo
- Publication number
- CN111983639B CN111983639B CN202010860418.5A CN202010860418A CN111983639B CN 111983639 B CN111983639 B CN 111983639B CN 202010860418 A CN202010860418 A CN 202010860418A CN 111983639 B CN111983639 B CN 111983639B
- Authority
- CN
- China
- Prior art keywords
- frame
- data
- pose
- laser radar
- imu
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 230000008878 coupling Effects 0.000 claims abstract description 13
- 238000010168 coupling process Methods 0.000 claims abstract description 13
- 238000005859 coupling reaction Methods 0.000 claims abstract description 13
- 238000005259 measurement Methods 0.000 claims abstract description 12
- 238000001514 detection method Methods 0.000 claims abstract description 11
- 230000000007 visual effect Effects 0.000 claims description 27
- 238000007781 pre-processing Methods 0.000 claims description 21
- 238000005457 optimization Methods 0.000 claims description 17
- 238000012545 processing Methods 0.000 claims description 15
- 230000011218 segmentation Effects 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 4
- 240000007651 Rubus glaucus Species 0.000 claims description 3
- 235000011034 Rubus glaucus Nutrition 0.000 claims description 3
- 235000009122 Rubus idaeus Nutrition 0.000 claims description 3
- 230000003287 optical effect Effects 0.000 claims description 3
- 230000001360 synchronised effect Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000011161 development Methods 0.000 description 2
- 230000018109 developmental process Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000007792 addition Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000000059 patterning Methods 0.000 description 1
- 238000003825 pressing Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- 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/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- 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
- 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Image Processing (AREA)
Abstract
本发明涉及一种基于Multi‑Camera/Lidar/IMU的多传感器SLAM方法，由多目相机获得的多张图像数据与IMU惯性测量单元获得的数据进行紧耦合联合初始化，获得***的初始位姿；由激光雷达传感器获得激光雷达帧的点云数据，对点云数据进行预处理，将点云划分为强角点、弱角点、强平面点、弱平面点；通过***的初始位姿对激光雷达帧位姿进行优化；通过闭环检测对激光雷达帧位姿进行进一步优化；利用优化后的激光雷达帧位姿进行地图拼接。本发明将三种传感器获得的数据进行融合，获得了激光雷达帧位姿更加的准确，有效提高了定位***的鲁棒性、稳定性。
Description
技术领域
本发明涉及计算机视觉、机器人等技术领域，具体涉及一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法。
背景技术
近些年，无人驾驶行业又逐渐走进大众视野。SLAM可以使移动车辆在GPS无法正常工作的环境下实现定位，并追踪辨识动态车辆与行人为实现智能避障，辅助驾驶和自主导航提供可能。
在进行定位和构图的时候，SLAM主要是借助与传感器来获得原始的数据，同时会伴有IMU的辅助。在现有发展的情况下，SLAM有两种实现形式：一种是以激光雷达为主的激光SLAM；另一种是以摄像头为主的视觉SLAM。激光SLAM目前主要应用在室内以及范围较小的环境。视觉SLAM要广泛，也更能适应室外环境，但是对光的依赖性强，在光线暗或是无纹理区域无法工作。
随着无人驾驶的迅速发展，为了适应市场环境，无人驾驶的使用场景越来越丰富，既要适应室内环境，又要适应室外环境；室内结构化环境较为理想；室外光照变化剧烈，车辆行人等动态目标较多，重复及杂乱纹理场景较多，给无人驾驶的量产落地带来巨大挑战。因此使用单一传感器进行定位构图的方式已无法满足复杂场景下高精度地图的构建需求。
所以有必要提出一种新的方案，将多种传感器数据以进行融合，以解决室外复杂环境下环境地图的构建问题。
发明内容
针对目前单一的传感器无法满足室外复杂的环境场景，本发明提出了一种一种基于Multi-Cameral/Lidar/IMU的多传感器SLAM方法。
本发明提出一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法，包括以下步骤：
S1：由多目相机获得的多张图像数据与IMU惯性测量单元获得的数据进行紧耦合联合初始化，获得***的初始位姿；
S2:由激光雷达传感器获得激光雷达帧的点云数据，对点云数据进行预处理，将点云划分为强角点、弱角点、强平面点、弱平面点；
S3:通过***的初始位姿对激光雷达帧位姿进行优化；
S4：通过闭环检测对激光雷达帧位姿进行进一步优化；
S5：利用优化后的激光雷达帧位姿进行地图拼接。
优选的，获得***的初始化位姿包括如下步骤：
S11：通过树莓派进行多目相机的硬同步触发；
S12：从多目相机中获得多目图像与IMU数据，通过时间戳将多目图像数据与IMU数据进行对齐；
S13：多线程进行多目图像的视觉特征跟踪，并将跟踪的结果进行保存；
S14：对多目图像数据以及IMU数据进行预处理；
S15：对多目图像数据以及IMU数据进行联合初始化，将多目相机获得多张图像信息分别与IMU数据进行紧耦合处理，将处理得到的数据再一次和IMU数据进行紧耦合处理，得到多目的紧耦合视觉惯性里程计；
S16：对多目的紧耦合视觉惯性里程计进行优化，同时进行边缘化处理；
S17：得到优化后的多目视觉惯性里程计，获得***初始化的位姿，为下一步做准备。
优选的，多线程进行多目图像的视觉特征跟踪，主要使用OpenCV中的光流跟踪方法。
优选的，对多目图像数据以及IMU数据进行预处理包括如下步骤：
S141：首先取出相邻两帧间的IMU数据，进行***初始化位姿估计，将初始的偏航角置零；
S142：对IMU数据预处理主要进行帧间的IMU预积分，同时估计积分后的测量方差与预积分值对状态变量的雅克比矩阵；
S143：对多目图像数据预处理：首先要进行关键帧的判断，根据特征点的ID判断是否将特征压入特征链表中，根据前后帧的平均视差判断是否将该帧置为关键帧，从而确定后端滑动窗口优化的位置。
优选的，对多目的紧耦合视觉惯性里程计进行优化的步骤包括：
S161:基于滑动窗口的后端优化，使用开源的优化库Ceres进行状态量的优化，通过计算图像的重投影误差与IMU测量残差构建代价函数，使用非线性总体最小二乘方法进行优化；
S162：边缘化处理，根据当前帧是否为关键帧决定边缘化滑动窗口内的数据帧。
优选的，对激光雷达点云数据进行预处理的方法包括：
S21：接收激光雷达传感器原始16线点云数据；
S22：计算每一Scan的扫描起始角与终止角，根据每点的坐标值计算点的垂直角度与线号、水平角度与列号，保存成一个完整的Scan；
S23：根据同一列相邻上下两点的坐标计算两者之间的平整程度，从而确定该点是否为地面点；
S24：发布分割后点云，以及分割信息；
S25：根据分割后的点云，计算各个点的曲率；
S26：对于每一线的点云，用曲率进行排序，根据曲率的值将点云划分为强角点、弱角点、强平面点与弱平面点，并发布。
优选的，通过***的初始位姿对激光雷达帧位姿进行优化步骤包括：
S31：接收优化完成的视觉惯性里程计、关键帧的位姿、关键帧、预处理的点云数据、IMU数据保存至相应的队列中；
S32：通过各数据时间戳对齐各数据帧，并由此构建闭环检测关键帧，同时处理分割的点云数据，对齐数据并变化坐标系，由激光雷达坐标系转化至多目相机的坐标系下，并保存至相应的列队容器中；同时在激光雷达数据的对列中压入关键帧的索引；
S33：对激光雷达帧数据进行处理，得到优化后的激光雷达帧的位姿。
优选的，对激光雷达帧数据处理的步骤包括：
S331：取出关键帧的索引，对于第一帧关键帧，初始化其姿态，即将姿态欧拉角都置零；
S332：当索引大于1时，取出当前帧的前一帧，并在当前帧与前一帧的时间戳之间获取相应的激光雷达帧数据与IMU帧数据；
S333：获取前一帧的视觉惯性里程计位姿；
S334：控制当前帧与前一帧的时间戳之间，只有一帧激光雷达数据参与构建云地图，由于激光雷达帧与当前帧之间存在时间间隔，因此位姿并不相同，利用IMU帧数据进行推位，利用推位得到的较为准确的位姿将点云转化到世界坐标系下；
S335：对推位得到的激光雷达帧位姿进行进一步的优化。
优选的，对激光雷达帧的位姿进一步优化的方法包括：
S3351：将世界坐标系下的点云数据转换至前一帧的坐标系下，同时保存至点云容器中；
S3352：当点云容器的容量大于1时，取出当前帧的激光雷达的点云，同时取出当前帧视觉惯性里程计位姿，并对该位姿进行初步的纠正，为后续当前帧到局部地图的优化提供一个较为准确的初值；
S3353：利用当前帧邻近20帧的点云数据进行构建局部地图；
S3354：进行当前帧到局部地图的优化，根据预处理激光雷达帧的点云数据得到的角点与面点，利用当前帧与局部地图中的两类点构建点点最小距离的代价函数，进行高斯牛顿的迭代优化，得到优化后的激光雷达帧的位姿。
优选的，通过闭环检测对激光雷达帧位姿进行进一步优化的步骤包括：
S41：使用DBoW视觉词典检测与当前帧较为相似的闭环候选帧，使用RANSAC算法计算当前帧到闭环候选帧的位姿变换，同时计算两者之间的共视点，当共视点数大于阈值时，保存闭环信息用于闭环优化。
S42:对于检测到的闭环信息，使用位姿图优化的方式进行位姿调整，将优化后的位姿更新到激光雷达位姿中。
本发明的有益效果是：
本发明多目相机可以获得多张图像数据，通过多张图像数据分别与IMU数据进行紧耦合处理，得到多个***的初始位姿，进而对每一个位姿和IMU数据再一次的紧耦合，得到一个将更为准确的***的初始位姿，利用此初始位姿对激光雷阵帧的位姿进行优化，得到一个较准确的激光雷达帧的位姿，最后利用闭环检测，对上述激光雷达帧的位姿进一步的优化，得到最终激光雷帧位姿，利用此激光雷达帧的位姿构建高精度地图；本发明将三种传感器获得的数据进行融合，获得了激光雷达帧位姿更加的准确，有效提高了定位***的鲁棒性、稳定性。
附图说明
图1一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法流程图；
具体实施方式
以下将结合附图所示的具体实施方式对本发明进行详细描述，但这些实施方式并不限制本发明，本领域的普通技术人员根据这些实施方式所做出的结构、方法、或功能上的变换均包含在本发明的保护范围内。
如图1所示，一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法，包括以下步骤：
S1：由多目相机获得的多张图像数据与IMU惯性测量单元获得的数据进行紧耦合联合初始化，获得***的初始位姿；
获得***初始化位姿包括如下步骤：
S11：通过树莓派进行多目相机的硬同步触发；
S12：从多目相机中获得多目图像数据与IMU数据，通过时间戳将多目图像数据与IMU数据进行对齐；
有一具体的实施例，在***前后左右均安装有多目相机，在前后两侧为双目相机，左右两侧为单目相机；这样***在拍摄的时候可以同时获得六张不同的位置图像信息，可以全方位的对周围的环境进行拍摄。
S13：多线程进行多目图像的视觉特征跟踪，主要使用OpenCV中的光流跟踪方法。
S14：对多目图像数据以及IMU数据进行预处理；
对多目图像数据以及IMU数据进行预处理包括如下步骤：
S141：首先取出相邻两帧间的IMU数据，进行***初始化位姿估计，将初始的偏航角置零；
S142：对IMU数据预处理主要进行帧间的IMU预积分，同时估计积分后的测量方差与预积分值对状态变量的雅克比矩阵；
S143：对多目图像预处理：首先要进行关键帧的判断，根据特征点的ID判断是否将特征压入特征链表中，根据前后帧的平均视差判断是否将该帧置为关键帧，从而确定后端滑动窗口优化的位置。
因为IMU视觉惯性测量单元和多目相机采集的信号类型不同且采样率差异巨大，为了同步两者的数据一般需要在图像的两帧之间对IMU数据进行积分。同时估计积分后的测量方差与预积分值对状态变量的雅克比矩阵。
S15：对多目图像以及IMU数据进行联合初始化，将多目相机获得多张图像信息分别与IMU数据进行紧耦合处理，将处理得到的数据再一次和IMU数据进行紧耦合处理，得到多目的紧耦合视觉惯性里程计；
S16：对多目的紧耦合视觉惯性里程计进行优化，同时进行边缘化处理；
基于滑动窗口的后端优化，使用开源的优化库Ceres进行状态量的优化，通过计算图像的重投影误差与IMU测量残差构建代价函数，使用非线性总体最小二乘方法进行优化；边缘化处理根据当前帧是否为关键帧决定边缘化滑动窗口内的数据帧。
S17：得到优化后的多目视觉惯性里程计，获得***初始化的位姿，为下一步做准备。
S2:由激光雷达传感器获得激光雷达帧的点云数据，并对点云数据进行预处理，将点云划分为强角点、弱角点、强平面点、弱平面点；
对激光雷达点云数据进行预处理的方法包括：
S21：接收激光雷达传感器原始16线点云数据；
S22：计算每一Scan的扫描起始角与终止角，根据每点的坐标值计算点的垂直角度与线号、水平角度与列号，保存成一个完整的Scan；
S23：根据同一列相邻上下两点的坐标计算两者之间的平整程度，从而确定该点是否为地面点；
S24：发布分割后点云，以及分割信息；
S25：根据分割后的点云，计算各个点的曲率；
S26：对于每一线的点云，用曲率进行排序，根据曲率的值将点云划分为强角点、弱角点、强平面点与弱平面点，并发布。
S3:通过***的初始位姿对激光雷达帧位姿进行优化；
S31：接收优化完成的视觉惯性里程计、关键帧的位姿、关键帧、预处理的点云数据、IMU数据保存至相应的队列中；
S32：通过各数据时间戳对齐各数据帧，并由此构建闭环检测帧，同时处理分割的点云数据，对齐数据并变化坐标系，由激光雷达坐标系转化至多目相机的坐标系下，并保存至相应的列队容器中；同时在激光雷达数据的列中压入关键帧的索引；
S33：对激光雷达帧数据进行处理，得到优化后的激光雷达帧的位姿。
对激光雷达帧数据处理的步骤包括：
S331：取出关键帧的索引，对于第一关帧关键帧，初始化其姿态，即将姿态欧拉角都置零；
S332：当索引大于1时，取出当前帧的前一帧，并在当前帧与前一帧的时间戳之间获取相应的激光雷达帧与IMU帧数据；
S333：获取前一帧的视觉惯性里程计位姿；
S334：控制当前帧与前一帧的时间戳之间，只有一帧激光雷达数据参与构建云地图，由于激光雷达帧与当前帧之间存在时间间隔，因此位姿并不相同，利用IMU帧数据进行推位，利用推位得到的较为准确的位姿将点云转化到世界坐标系下；
S335：将推位得到的激光雷达帧位姿进行进一步的优化。
对激光雷达帧的位姿进一步优化的方法包括：
S3351：将世界坐标系下的点云数据转换至前一帧的坐标系下，同时保存至点云容器中；
S3352：当点云容器的容量大于1时，取出当前帧的激光雷达的点云，同时当前帧的视觉惯性里程计位姿，并对该位姿进行初步的纠正，为后续当前帧到局部地图的优化提供一个较为准确的初值；
S3353：利用当前帧邻近20帧的点云数据进行构建局部地图；
S3354：进行当前帧到局部地图的优化，根据预处理激光雷达帧的点云数据得到的角点与面点，利用当前帧与局部地图中的两类点构建点点最小距离的代价函数，进行高斯牛顿的迭代优化，得到优化后的激光雷达帧的位姿。
S4：通过闭环检测对激光雷达帧位姿进行进一步优化；
对激光雷达帧位姿进一步优化的步骤包括：
S41：使用DBoW视觉词典检测与当前帧较为相似的闭环候选帧，使用RANSAC算法计算当前帧到闭环候选帧的位姿变换，同时计算两者之间的共视点，当共视点数大于阈值时，保存闭环信息用于闭环优化；
S42:对于检测到的闭环信息，使用位姿图优化的方式进行位姿调整，将优化后的位姿更新到激光雷达位姿中。
S5：利用优化后的激光雷达帧位姿进行地图拼接。
本发明多目相机可以获得多张图像数据，通过多张图像数据分别与IMU数据进行紧耦合处理，得到多个***的初始位姿，进而对每一个位姿和IMU数据再一次的紧耦合，得到一个将更为准确的***的初始位姿，利用此初始位姿对激光雷阵帧的位姿进行优化，得到一个较准确的激光雷达帧的位姿，最后利用闭环检测，对上述的激光雷达帧的位姿进一步的优化，得到最终激光雷帧位姿，利用此激光雷达帧的位姿构建高精度地图；本发明将三种传感器获得的数据进行融合，获得了激光雷达帧位姿更加的准确，有效提高了定位***的鲁棒性、稳定性。
尽管为示例目的，已经公开了本发明的优选实施方式，但是本领域的普通技术人员将意识到，在不脱离由所附的权利要求书公开的本发明的范围和精神前提下，各种改进、增加以及取代是可能的。
Claims (8)
1.一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法，其特征在于：包括以下步骤：
S1：由多目相机获得的多张图像数据与IMU惯性测量单元获得的数据进行紧耦合联合初始化，获得***的初始位姿；
S2:由激光雷达传感器获得激光雷达帧的点云数据，对点云数据进行预处理，将点云划分为强角点、弱角点、强平面点、弱平面点；
S3:通过***的初始位姿对激光雷达帧位姿进行优化；
S4：通过闭环检测对激光雷达帧位姿进行进一步优化；
S5：利用优化后的激光雷达帧位姿进行地图拼接；
其中，步骤S3通过***的初始位姿对激光雷达帧位姿进行优化步骤包括：
S31：接收优化完成的视觉惯性里程计、关键帧的位姿、关键帧、预处理的点云数据、IMU数据保存至相应的队列中；
S32：通过各数据时间戳对齐各数据帧，并由此构建闭环检测关键帧，同时处理分割的点云数据，对齐数据并变化坐标系，由激光雷达坐标系转化至多目相机的坐标系下，并保存至相应的列队容器中；同时在激光雷达数据的对列中压入关键帧的索引；
S33：对激光雷达帧数据进行处理，得到优化后的激光雷达帧的位姿；
步骤S33对激光雷达帧数据处理的步骤包括：
S331：取出关键帧的索引，对于第一帧关键帧，初始化其姿态，即将姿态欧拉角都置零；
S332：当索引大于1时，取出当前帧的前一帧，并在当前帧与前一帧的时间戳之间获取相应的激光雷达帧数据与IMU帧数据；
S333：获取前一帧的视觉惯性里程计位姿；
S334：控制当前帧与前一帧的时间戳之间，只有一帧激光雷达数据参与构建云地图，由于激光雷达帧与当前帧之间存在时间间隔，因此位姿并不相同，利用IMU帧数据进行推位，利用推位得到的较为准确的位姿将点云转化到世界坐标系下；
S335：对推位得到的激光雷达帧位姿进行进一步的优化。
2.根据权利要求1所述一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法，其特征在于：获得***的初始化位姿包括如下步骤：
S11：通过树莓派进行多目相机的硬同步触发；
S12：从多目相机中获得多目图像与IMU数据，通过时间戳将多目图像数据与IMU数据进行对齐；
S13：多线程进行多目图像的视觉特征跟踪，并将跟踪的结果进行保存；
S14：对多目图像数据以及IMU数据进行预处理；
S15：对多目图像数据以及IMU数据进行联合初始化，将多目相机获得多张图像信息分别与IMU数据进行紧耦合处理，将处理得到的数据再一次和IMU数据进行紧耦合处理，得到多目的紧耦合视觉惯性里程计；
S16：对多目的紧耦合视觉惯性里程计进行优化，同时进行边缘化处理；
S17：得到优化后的多目视觉惯性里程计，获得***初始化的位姿，为下一步做准备。
3.根据权利要求2所述一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法，其特征在于：多线程进行多目图像的视觉特征跟踪，主要使用OpenCV中的光流跟踪方法。
4.根据权利要求2所述一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法，其特征在于：对多目图像数据以及IMU数据进行预处理包括如下步骤：
S141：首先取出相邻两帧间的IMU数据，进行***初始化位姿估计，将初始的偏航角置零；
S142：对IMU数据预处理主要进行帧间的IMU预积分，同时估计积分后的测量方差与预积分值对状态变量的雅克比矩阵；
S143：对多目图像数据预处理：首先要进行关键帧的判断，根据特征点的ID判断是否将特征压入特征链表中，根据前后帧的平均视差判断是否将该帧置为关键帧，从而确定后端滑动窗口优化的位置。
5.根据权利要求2所述一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法，其特征在于：对多目的紧耦合视觉惯性里程计进行优化的步骤包括：
S161:基于滑动窗口的后端优化，使用开源的优化库Ceres进行状态量的优化，通过计算图像的重投影误差与IMU测量残差构建代价函数，使用非线性总体最小二乘方法进行优化；
S162：边缘化处理，根据当前帧是否为关键帧决定边缘化滑动窗口内的数据帧。
6.根据权利要求1所述一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法，其特征在于：对激光雷达点云数据进行预处理的方法包括：
S21：接收激光雷达传感器原始16线点云数据；
S22：计算每一Scan的扫描起始角与终止角，根据每点的坐标值计算点的垂直角度与线号、水平角度与列号，保存成一个完整的Scan；
S23：根据同一列相邻上下两点的坐标计算两者之间的平整程度，从而确定该点是否为地面点；
S24：发布分割后点云，以及分割信息；
S25：根据分割后的点云，计算各个点的曲率；
S26：对于每一线的点云，用曲率进行排序，根据曲率的值将点云划分为强角点、弱角点、强平面点与弱平面点，并发布。
7.根据权利要求1所述一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法，其特征在于：对激光雷达帧的位姿进一步优化的方法包括：
S3351：将世界坐标系下的点云数据转换至前一帧的坐标系下，同时保存至点云容器中；
S3352：当点云容器的容量大于1时，取出当前帧的激光雷达的点云，同时取出当前帧视觉惯性里程计位姿，并对该位姿进行初步的纠正，为后续当前帧到局部地图的优化提供一个较为准确的初值；
S3353：利用当前帧邻近20帧的点云数据进行构建局部地图；
S3354：进行当前帧到局部地图的优化，根据预处理激光雷达帧的点云数据得到的角点与面点，利用当前帧与局部地图中的两类点构建点点最小距离的代价函数，进行高斯牛顿的迭代优化，得到优化后的激光雷达帧的位姿。
8.根据权利要求1所述一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法，其特征在于：通过闭环检测对激光雷达帧位姿进行进一步优化的步骤包括：
S41：使用DBoW视觉词典检测与当前帧较为相似的闭环候选帧，使用RANSAC算法计算当前帧到闭环候选帧的位姿变换，同时计算两者之间的共视点，当共视点数大于阈值时，保存闭环信息用于闭环优化，
S42:对于检测到的闭环信息，使用位姿图优化的方式进行位姿调整，将优化后的位姿更新到各激光雷达位姿中。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010860418.5A CN111983639B (zh) | 2020-08-25 | 2020-08-25 | 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010860418.5A CN111983639B (zh) | 2020-08-25 | 2020-08-25 | 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111983639A CN111983639A (zh) | 2020-11-24 |
| CN111983639B true CN111983639B (zh) | 2023-06-02 |
Family
ID=73443183
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010860418.5A Active CN111983639B (zh) | 2020-08-25 | 2020-08-25 | 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111983639B (zh) |
Families Citing this family (26)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112197770B (zh) * | 2020-12-02 | 2021-03-12 | 北京欣奕华数字科技有限公司 | 一种机器人的定位方法及其定位装置 |
| CN112461230B (zh) * | 2020-12-07 | 2023-05-09 | 优必康(青岛)科技有限公司 | 机器人重定位方法、装置、机器人和可读存储介质 |
| CN112649016B (zh) * | 2020-12-09 | 2023-10-03 | 南昌大学 | 一种基于点线初始化的视觉惯性里程计方法 |
| CN112785702B (zh) * | 2020-12-31 | 2023-06-20 | 华南理工大学 | 一种基于2d激光雷达和双目相机紧耦合的slam方法 |
| CN112965063B (zh) * | 2021-02-11 | 2022-04-01 | 深圳市安泽智能机器人有限公司 | 一种机器人建图定位方法 |
| CN112991515B (zh) * | 2021-02-26 | 2022-08-19 | 山东英信计算机技术有限公司 | 一种三维重建方法、装置及相关设备 |
| CN112907491B (zh) * | 2021-03-18 | 2023-08-22 | 中煤科工集团上海有限公司 | 一种适用于井下巷道的激光点云回环检测方法及*** |
| CN113066105B (zh) * | 2021-04-02 | 2022-10-21 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及*** |
| CN113129377B (zh) * | 2021-04-23 | 2024-05-03 | 南京航空航天大学 | 一种三维激光雷达快速鲁棒slam方法和装置 |
| CN113432600B (zh) * | 2021-06-09 | 2022-08-16 | 北京科技大学 | 基于多信息源的机器人即时定位与地图构建方法及*** |
| CN113432595A (zh) * | 2021-07-07 | 2021-09-24 | 北京三快在线科技有限公司 | 设备状态获取方法、装置、计算机设备及存储介质 |
| CN114088104B (zh) * | 2021-07-23 | 2023-09-29 | 武汉理工大学 | 一种自动驾驶场景下的地图生成方法 |
| CN113721260B (zh) * | 2021-08-26 | 2023-12-12 | 南京邮电大学 | 一种激光雷达、双目相机和惯导的在线联合标定方法 |
| CN113776517B (zh) * | 2021-09-03 | 2024-05-31 | Oppo广东移动通信有限公司 | 地图生成方法、装置、***、存储介质及电子设备 |
| CN113776519B (zh) * | 2021-09-14 | 2022-10-21 | 西南科技大学 | 一种无光动态开放环境下agv车辆建图与自主导航避障方法 |
| CN115371699B (zh) * | 2021-09-30 | 2024-03-15 | 达闼科技（北京）有限公司 | 视觉惯性里程计方法、装置及电子设备 |
| CN114199235B (zh) * | 2021-11-29 | 2023-11-03 | 珠海一微半导体股份有限公司 | 一种基于扇面深度相机的定位***及定位方法 |
| CN114170280B (zh) * | 2021-12-09 | 2023-11-28 | 北京能创科技有限公司 | 基于双窗口的激光里程计方法、***、装置 |
| CN114234967B (zh) * | 2021-12-16 | 2023-10-20 | 浙江大学 | 一种基于多传感器融合的六足机器人定位方法 |
| CN114370871A (zh) * | 2022-01-13 | 2022-04-19 | 华南理工大学 | 一种可见光定位与激光雷达惯性里程计的紧耦合优化方法 |
| CN114088087B (zh) * | 2022-01-21 | 2022-04-15 | 深圳大学 | 无人机gps-denied下高可靠高精度导航定位方法和*** |
| CN114648584B (zh) * | 2022-05-23 | 2022-08-30 | 北京理工大学前沿技术研究院 | 一种用于多源融合定位的鲁棒性控制方法和*** |
| CN115280960B (zh) * | 2022-07-08 | 2024-06-07 | 江苏大学 | 一种基于田间视觉slam的联合收获机转向控制方法 |
| CN115560744A (zh) * | 2022-08-22 | 2023-01-03 | 深圳市普渡科技有限公司 | 机器人以及基于多传感器的三维建图方法、存储介质 |
| CN115311353B (zh) * | 2022-08-29 | 2023-10-10 | 玩出梦想(上海)科技有限公司 | 一种多传感器多手柄控制器图优化紧耦合追踪方法及*** |
| CN117554984A (zh) * | 2023-11-08 | 2024-02-13 | 广东科学技术职业学院 | 一种基于图像理解的单线激光雷达室内slam定位方法及*** |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108401461A (zh) * | 2017-12-29 | 2018-08-14 | 深圳前海达闼云端智能科技有限公司 | 三维建图方法、装置、***、云端平台、电子设备和计算机程序产品 |
| CN108827315A (zh) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | 基于流形预积分的视觉惯性里程计位姿估计方法及装置 |
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN111337947A (zh) * | 2020-05-18 | 2020-06-26 | 深圳市智绘科技有限公司 | 即时建图与定位方法、装置、***及存储介质 |
| CN111402339A (zh) * | 2020-06-01 | 2020-07-10 | 深圳市智绘科技有限公司 | 一种实时定位方法、装置、***及存储介质 |
| CN111561923A (zh) * | 2020-05-19 | 2020-08-21 | 北京数字绿土科技有限公司 | 基于多传感器融合的slam制图方法、*** |
-
2020
- 2020-08-25 CN CN202010860418.5A patent/CN111983639B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108401461A (zh) * | 2017-12-29 | 2018-08-14 | 深圳前海达闼云端智能科技有限公司 | 三维建图方法、装置、***、云端平台、电子设备和计算机程序产品 |
| CN108827315A (zh) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | 基于流形预积分的视觉惯性里程计位姿估计方法及装置 |
| CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
| CN111337947A (zh) * | 2020-05-18 | 2020-06-26 | 深圳市智绘科技有限公司 | 即时建图与定位方法、装置、***及存储介质 |
| CN111561923A (zh) * | 2020-05-19 | 2020-08-21 | 北京数字绿土科技有限公司 | 基于多传感器融合的slam制图方法、*** |
| CN111402339A (zh) * | 2020-06-01 | 2020-07-10 | 深圳市智绘科技有限公司 | 一种实时定位方法、装置、***及存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111983639A (zh) | 2020-11-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111983639B (zh) | 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法 | |
| US20230260151A1 (en) | Simultaneous Localization and Mapping Method, Device, System and Storage Medium | |
| CN110009739B (zh) | 移动摄像机的数字视网膜的运动特征的提取与编码方法 | |
| CN110033489B (zh) | 一种车辆定位准确性的评估方法、装置及设备 | |
| CN109029433B (zh) | 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法 | |
| CN111882612B (zh) | 一种基于三维激光检测车道线的车辆多尺度定位方法 | |
| CN111024066B (zh) | 一种无人机视觉-惯性融合室内定位方法 | |
| CN109509230A (zh) | 一种应用于多镜头组合式全景相机的slam方法 | |
| CN103886107B (zh) | 基于天花板图像信息的机器人定位与地图构建*** | |
| CN111862673B (zh) | 基于顶视图的停车场车辆自定位及地图构建方法 | |
| CN110044354A (zh) | 一种双目视觉室内定位与建图方法及装置 | |
| CN113706626B (zh) | 一种基于多传感器融合及二维码校正的定位与建图方法 | |
| CN111275763A (zh) | 闭环检测***、多传感器融合slam***及机器人 | |
| CN113658337B (zh) | 一种基于车辙线的多模态里程计方法 | |
| CN112734765A (zh) | 基于实例分割与多传感器融合的移动机器人定位方法、***及介质 | |
| CN115936029A (zh) | 一种基于二维码的slam定位方法及装置 | |
| CN112669354A (zh) | 一种基于车辆非完整约束的多相机运动状态估计方法 | |
| CN110827353A (zh) | 一种基于单目摄像头辅助的机器人定位方法 | |
| CN114413958A (zh) | 无人物流车的单目视觉测距测速方法 | |
| CN114234967A (zh) | 一种基于多传感器融合的六足机器人定位方法 | |
| CN112432653B (zh) | 基于点线特征的单目视觉惯性里程计方法 | |
| CN117470259A (zh) | 一种子母式空地协同多传感器融合三维建图*** | |
| CN112945233A (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
| CN116804553A (zh) | 基于事件相机/imu/自然路标的里程计***及方法 | |
| CN116468786A (zh) | 一种面向动态环境的基于点线联合的语义slam方法 |
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 | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20240409 Address after: Room 509-2, 5th Floor, Building 1, Dingchuang Wealth Center, Cangqian Street, Yuhang District, Hangzhou City, Zhejiang Province, 311100 Patentee after: Hangzhou Yixun Technology Co.,Ltd. Country or region after: China Address before: Room 303-5, block B, building 1, 268 Shiniu Road, nanmingshan street, Liandu District, Lishui City, Zhejiang Province 323000 Patentee before: Zhejiang Guangpo Intelligent Technology Co.,Ltd. Country or region before: China |