一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法与流程

本发明涉及计算机视觉、机器人等技术领域，具体涉及一种基于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数据再一次的紧耦合，得到一个将更为准确的系统的初始位姿，利用此初始位姿对激光雷阵帧的位姿进行优化，得到一个较准确的激光雷达帧的位姿，最后利用闭环检测，对上述的激光雷达帧的位姿进一步的优化，得到最终激光雷帧位姿，利用此激光雷达帧的位姿构建高精度地图；本发明将三种传感器获得的数据进行融合，获得了激光雷达帧位姿更加的准确，有效提高了定位系统的鲁棒性、稳定性。

尽管为示例目的，已经公开了本发明的优选实施方式，但是本领域的普通技术人员将意识到，在不脱离由所附的权利要求书公开的本发明的范围和精神前提下，各种改进、增加以及取代是可能的。