[发明专利]一种用于视觉-惯性-激光融合的同步定位与建图方法

说明书

一种用于视觉‑惯性‑激光融合的同步定位与建图方法，主要涉及多传感器融合、SLAM等技术领域。为了解决单传感器SLAM在定位和建图中精度低、易丢失的问题，本发明提出了一种鲁棒的高精度的视觉、惯性、激光雷达融合的SLAM系统。本发明通过激光雷达的扫描匹配进一步优化紧耦合的视觉惯性里程计，得到更加精确的定位结果。并且当相机或者激光雷达发生退化，导致视觉惯性模块或者扫描匹配模块不能正常工作时，系统自动整合剩余可工作模块，维持稳定的位姿估计。为了移除累计误差，添加了基于外观的回环检测和基于点云的邻近匹配，之后进行六自由度的位姿优化，以保持全局一致性。本发明能够得到鲁棒的高精度的定位和建图效果。

技术领域

本发明涉及机器人视觉、多传感器融合、同步定位与建图(SLAM)等技术领域，具体涉及多传感器融合的同步定位与建图方法。

背景技术

同步定位与建图(SLAM)是一种机器人在未知环境中估计自身运动并且建立周围环境地图的技术。它在无人机、自动驾驶、移动机器人导航、虚拟现实和增强现实等领域有着广泛的应用。为了提高SLAM在不同环境下的定位精度和鲁棒性，人们做了大量的研究。由于SLAM通过安装在机器人上的传感器来感知自身的运动和周围的环境，许多研究试图通过集成多模态传感器来提高运动估计的性能。随着科技的发展，SLAM中常用的传感器体积越来越小，价格越来越低。这使得在一个设备上携带多个传感器，以可接受的价格和体积更好地进行定位和建图成为可能。

发明内容

为了克服现有单传感器SLAM精度不高，在退化环境中容易发生丢失的问题，本发明提出了一种用于视觉-惯性-激光融合的同步定位与建图方法，结合来自相机、惯性测量单元(IMU)、激光雷达的测量数据实现高精度、高鲁棒性的带有回环和邻近检测及全局位姿优化的SALM系统。

为了解决上述的技术问题，本发明的技术方案如下：

一种用于视觉-惯性-激光融合的同步定位与建图方法，包括以下步骤：

1)视觉-惯性-激光里程计，过程如下：

假设相机内参以及三个传感器之间的外参已知，并且传感器之间经过时间同步，相机和激光雷达具有相同的频率；涉及到四个坐标系分别是世界坐标系W，相机坐标系C，惯性测量单元(IMU)坐标系I和激光雷达坐标系L；传感器坐标系C、I、L随着设备的运动而改变，C_i表示在t_i时刻的相机坐标系；定义完成初始化后的激光雷达坐标系为世界坐标系；

首先，使用紧耦合的视觉惯性里程计方法估计相邻相机帧之间的相对运动，然后使用获得的动作近似注册这期间接收的点云，接着使用扫描匹配方法进一步优化估计的动作；

根据激光雷达的特性，激光点云被持续性地接收，并注册到坐标系L_i中，其中t_i是点i的接收时间，L_i是t_i时刻的激光雷达坐标系；定义t_k为第k帧激光扫描的起始时间，t_k+1为第k帧激光扫描的结束时间；令为在第k帧激光扫描期间接收到的点云集合；由于传感器经过时间同步，从视觉惯性里程计中得到t_k到t_k+1期间IMU的相对位姿变换使用激光雷达和IMU之间的外参，计算得到t_k到t_k+1期间激光雷达的相对位姿变换

其中是激光雷达和IMU之间的外参；

基于设备在一帧激光扫描期间以匀速运动的假设，使用线性插值获取从第k帧激光扫描的起始时刻到点i接收时的相对位姿变换：

然后将点i注册到L_k+1坐标系下: