基于立体视觉、惯性、激光雷达的同步定位与建图系统及方法

本发明属于机器人位置服务的。更具体地，本发明涉及一种基于立体视觉、惯性、激光雷达的同步定位与建图系统及其同步定位与建图方法；可扩展用到无人驾驶、人工智能、室内定位、工业互联网和无线通信等领域；是一种可以在不受控制的场景中工作的自动激光雷达-照相机校准方法。

背景技术：

1、一、相关技术发展背景介绍：

2、同步定位与建图(slam)解决了建立未知环境地图同时估计机器人状态的问题，在过去几十年里得到了广泛关注和深入研究，但在处理不同的环境和长期连续操作时仍存在很多问题。

3、根据移动机器人搭载的传感器不同，可以将slam分为激光雷达(lidar)slam和视觉slam；为了更好地适应环境，目前还出现了多传感器融合的slam。

4、激光slam与视觉slam相比更具环境适应性，但激光slam在重复结构(例如隧道或者走廊)中失效。因此在复杂环境中定位与建图极具挑战性，需要融合使用多种传感器才能满足需求。但是目前还没有很好的解决方案。

5、传统的惯性测量单元(imu)测量估计值易漂移。

6、二、现有技术中检索到以下相关文献：

7、1、“fusion of lidar data and multispectral imagery for effectivebuilding detection based on graph and connected component analysis”；

8、该相关文献提出了一种基于图的数据驱动方法，该方法将lidar数据和多光谱图像融合在一起。其存在的问题是：这是一种简单的外部校准方法，受环境影响大。

9、2、“real-time probabilistic fusion of sparse 3d lidar and densestereo”；

10、该相关文献提出的技术方案是：3d-lidar数据与立体图像的融合。立体深度估计的优点在于它能够通过利用立体匹配技术来生成周围环境的密集深度图。其存在的问题是：密集的立体深度估计在计算上非常复杂。这是由于需要匹配立体图像中的对应点；此外，例如，由于明亮区域中像素值的饱和，使用立体图像的密集深度估计会受到图像传感器动态范围的限制。

11、最接近的现有技术及其存在的技术问题是：

12、当前的vio文献介绍了不同的构架来融合视觉和惯性数据。文献将不同的方法分为紧耦合系统和松耦合系统。其中，紧耦合系统将视觉信息和惯性测量被联合优化；松耦合系统将imu看成为一个分离的模块，并与一个视觉状态估计器融合到一起。该方法还可进一步分为基于滤波的方法和基于图优化的方法。紧耦合的优化方法是利用迭代地最小化残差的优势达到更好的精度和鲁棒性，同时具有更高的计算代价。

13、相关文献中示出了双目相机和一个激光扫描仪组合方案，利用vo进行运动估计，通过匹配激光帧来精确修正，使用多分辨率栅格地图表示。

14、还有文献提出使用更具鲁棒性的vio替代vo，即vloam(visual-lidar odometryand mapping，视觉激光雷达测距和测绘)，其使用一个imu，一个单目相机和一个激光扫描仪，采用松耦合结构，相机和lidar模块的信息可被用于修正imu偏置。

技术实现思路

1、本发明提供一种基于立体视觉、惯性、激光雷达的同步定位与建图系统，其目的是采用紧耦合的组合方式将立体视觉惯性里程计(vio)与lidar建图融合，同时利用lidar增强闭环监测功能。

2、为了实现上述目的，本发明采取的技术方案为：

3、本发明的基于立体视觉、惯性、激光雷达的同步定位与建图系统，设置四个模块，分别是视觉前段模块、vio后端模块、回环检测模块和lidar建图模块。

4、所述的视觉前端模块从立体相机获取立体图像对，执行帧到帧跟踪和立体匹配，并输出立体匹配作为视觉测量，分别发送给vio后端模块和回环检测模块。

5、所述的vio后端模块采用立体vio进行立体匹配和imu测量，在姿势图上执行imu(惯性测量单元)预积分和紧耦合的固定滞后平滑；所述的vio后端模块以imu速率向所述的lidar建图模块输出vio姿态；所述的vio后端模块以摄像机速率向所述的回环检测模块输出vio姿态。

6、所述的回环检测模块进行视觉回环检测和初始环路约束估计，并通过对稀疏点云icp提取进一步精细配准；约束所有lidar姿势的全局位姿图被增量式优化，以获取全局校正的轨迹和实时的lidar姿势校正；所述的lidar姿势校正被发送至所述的lidar建图模块，以进行地图更新和重新定位。

7、所述的lidar建图模块使用来自vio的运动估计并执行lidar去扭曲(dewarping)和扫描帧到地图的配准。

8、为了实现与上述技术方案相同的发明目的，本发明还提供了以上所述的基于立体视觉、惯性、激光雷达的同步定位与建图系统的同步定位与建图方法，其技术方案是：

9、所述的视觉前段模块对立体视觉进行立体匹配，使用kanade lucas tomasi(klt)特征跟踪器来跟踪先前立体匹配中的所有特征点，无论是在左图像还是右图像中；只有当他们都被跟踪才会输出；

10、如果跟踪的立体匹配数低于阈值，使用shi-tomashi角点检测器进行特征提取，然后进行特征消除过程，删除与任何小于阈值的现有特征的像素距离的特征；

11、orb描述子后计算所有幸存特征，然后进行立体暴力匹配以获得新的匹配结果；

12、所述的同步定位与建图系统统通过对第一帧立体视觉进行立体匹配来对同步定位与建图系统初始化。

13、所述的vio后端模块的立体视觉惯性里程计的目标是在相对较高的频率上提供实时准确的状态估计，作为lidar建图算法的运动模型输入；

14、在位姿图上运行的紧耦合固定滞后平滑器，在精度和效率之间可获得很好的权衡；

15、基于优化的方法通常允许多次线性化以接近全局最小值，固定滞后位姿图优化器进一步限定了变量的最大数目，降低了计算复杂度；

16、由于差的视觉测量会导致系统不收敛，因此对视觉测量强制执行严格的异常值拒绝机制，系统通过检查平均重投影误差来消除异常值。

17、所述的lidar建图模块(激光雷达建图)在进行激光雷达点提取和扫描帧点云到地图配准之前，使用高频imu速率vio姿态作为运动；

18、将扫描x表示为从一个完整的激光雷达旋转获得的点云；经过去扭曲后从x中提取几何特征，包括锐边点和平面点；

19、然后，基于当前扫描到地图的特征点(所有先前的特征点)，通过最小化特征点形成的欧几里德距离残差，将其作为一个优化问题来解决。

20、所述的回环检测基于雷达增强，其目的是通过对全局位姿图进行优化来消除漂移，全局位姿图结合了lidar建图的回环约束和相对变换信息；

21、为了更好地辅助激光雷达制图，校正后的激光雷达姿态被实时发送回来，以便新扫描的特征点被配准到重新访问的地图上；

22、所述的回环检测采用视觉词袋回环检测和pnp回环约束公式的基础上增加icp对齐，采用增量求解器isam2对全局姿态图进行优化，实现了实时性。

23、本发明采用上述技术方案，在多个典型环境下开展测试验证，包括无特色的走廊、杂乱无章的高台、隧道和室外环境等；所有这些序列的数据收集在同一点开始和结束；基于最终漂移误差(fde)对里程计(lidar建图姿态)进行评估；测试结果表明，与loam相比，该方法可以生成更准确的地图和更低的fde(最终漂移误差)；先一步将其扩展到一个紧密耦合的框架中，以便从lidar建图得到的精确姿态估计，可以用于imu偏差校正；在循环闭合中对扫描帧之间的稀疏特征点进行icp精细化，获得更好的回环约束；紧耦合的立体vio，有效消除了imu时间累计误差和偏差；使用稀疏3d功能进行地图配准的lidar建图，高效实现了全局地图的优化和更新；在复杂环境下更具鲁棒性；通过将立体视觉惯性里程计(vio)与lidar相融合共同实现定位与建图，实现在不同环境中的长期稳定运行。