基于信息互通的视觉与激光雷达融合建图定位方法及系统与流程

本发明涉及定位建图，具体地说是一种基于信息互通的视觉与激光雷达融合建图定位方法及系统。

背景技术：

1、目前，包括移动机器人在内的智能体实现室内自主导航大多依赖已知地图。通过slam(simultaneous localization and mapping)，即同步建图定位技术，智能体利用传感器(激光雷达、视觉传感器等)获取周围环境信息，构建临时局部环境地图，同时求取智能体在该环境地图中的位姿，并最终获得导航用全局地图。

2、基于多线激光雷达的同步建图定位技术依靠激光雷达主动测量的特性，可以远距离捕捉环境的细节，对建图轨迹及技巧要求不高。但是，此类方法在无结构环境，例如长走廊或平坦的空地，建图定位效果会大幅劣化。视觉建图定位技术采用视觉传感器，价格相比多线激光雷达普遍更为低廉，且获取场景信息更为丰富，地点识别能力较强，尤其适合纹理丰富的场景。因此学术界及工业界均对视觉建图定位技术给予了极大的关注。然而，视觉建图定位技术对光照变化非常敏感，仅采用单目相机常常面临初始化困难的问题，且难以应对纹理单一及快速运动的建图场景。因此催生了将激光雷达建图定位技术与视觉建图定位技术进行耦合的尝试，以达到各取所长、各补所短的效果。同时，通过惯性测量单元(imu)的加持，进一步提高建图定位的稳健性和准确性。

3、激光雷达建图定位技术和视觉建图定位技术发展至今日，都已各自具备完善成熟的算法体系和关键技术思路，也陆续有新的算法及经典算法的优化版本出现。然而，雷达视觉融合算法并不多，且激光雷达建图定位和视觉建图定位两方向各自的技术积累很少被挖掘利用，且存在如下缺点：

4、①现有的雷达视觉融合建图定位算法对视觉传感器的种类有一定的限制，仅可采用单目相机或双目相机。

5、②现有的雷达视觉融合建图定位算法大多基于一种激光雷达建图定位技术与一种视觉建图定位技术的组合，定制化较强，再拓展可能性小，无法利用最新的、性能更优的激光或视觉建图定位技术进行验证并提升效果。

6、③也有雷达视觉融合建图定位技术利用了激光雷达建图定位线程与视觉建图定位线程的信息互通，但互通信息有限，仅包括回环信息和里程计信息，且对获得的信息不加判断并缺少进一步的反馈机制，比如激光雷达建图定位线程只要收到视觉建图定位线程发送的里程计信息，就直接使用。若此时视觉建图定位因为纹理缺失，效果退化，视觉里程计信息不准确，会造成雷达视觉融合建图定位效果的劣化甚至建图失败。

7、故如何快速高效的建立雷达视觉融合建图定位，实现复杂场景下稳定精确、高鲁棒性的建图定位是目前亟待解决的技术问题。

技术实现思路

1、本发明的技术任务是提供一种基于信息互通的视觉与激光雷达融合建图定位方法及系统，来解决如何快速高效的建立雷达视觉融合建图定位，实现复杂场景下稳定精确、高鲁棒性的建图定位的问题。

2、本发明的技术任务是按以下方式实现的，一种基于信息互通的视觉与激光雷达融合建图定位方法，该方法是以一台智能体设备为载体，在智能体设备部署计算单元，计算单元上部署激光雷达点云节点、激光雷达里程计节点、视觉里程计节点、视觉辅助激光雷达回环节点以及激光雷达回环的视觉验证节点，以ros(robot operating system，机器人操作系统)节点及发布topic话题的形式进行激光雷达建图定位线程与视觉建图定位线程之间的信息互通，同时判断信息优劣，实现视觉slam(simultaneous localization andmapping，同步建图定位)及激光slam算法的融合。

3、作为优选，智能体设备还包括视觉传感器、激光雷达传感器以及imu(inertialmeasurement unit，惯性测量单元)的移动机器人设备；其中，计算单元上部署任意独立的激光雷达建图定位技术环境以及视觉建图定位技术环境且计算单元具有ros系统。

4、作为优选，激光雷达点云节点用于获取并发布激光雷达slam矫正畸变后的点云；具体如下：

5、在单目模式下，将点云与图像对齐，获得图像特征点对应像素的实际三维坐标，避免单目模式下，需要智能体设备移动，利用相邻图像帧视差进行初始化且初始化困难的问题；

6、在双目模式下，双目相机虽可通过左右目视差获得深度信息，但获得深度范围受双目相机基线限制，获得深度准确度受光照影响较大；因此，通过激光雷达点云与左目图像的对齐，补盲双目相机深度并有效提升深度信息准确度。

7、作为优选，激光雷达里程计节点用于获取并发布激光雷达slam里程计，并提供给视觉slam线程，进行坐标转换，替代imu信息辅助初始化，因此无需特定的初始化动作和轨迹来保证imu的充分激励，避免初始化过程中没有位姿信息的情况以及初始化困难的问题；

8、其中，坐标转换公式如下：

9、twv＝twl*tlv；

10、其中，twv∈se(3)，twl∈se(3)，tlv∈se(3)，twv、twl以及tlv分别作为视觉传感器位姿(v–visual，w–world，wv–由视觉传感器坐标系向世界坐标系的转换)、激光雷达位姿(l–lidar，wl–由激光雷达坐标系向世界坐标系的转换)以变换矩阵形式的表示以及视觉传感器与激光雷达外参转换矩阵(lv–由视觉传感器坐标系向激光雷达坐标系的转换)；se(3)是指李群se(3)，指三维空间上的刚性变换；

11、在视觉slam线程帧间跟踪丢失的情况下(主要由于当前帧与上一帧没有足够的特征匹配)，利用激光雷达里程计信息恢复跟踪，即利用位姿信息，获得当前帧特征点对应的世界坐标系下三维位置信息，并与丢失跟踪前上一帧生成的地图点进行对齐，从而得到两帧间的特征匹配，恢复跟踪；

12、对激光雷达slam里程计的准确度，基于如下标准进行判断：

13、判断是否有跳变，公式如下：

14、

15、其中，表示激光雷达slam里程计估计的相邻帧对应的智能体设备位置差，即i时刻激光雷达slam里程计估计位置与(i-1)时刻激光雷达slam里程计估计位置距离小于阈值d，即认为激光雷达slam里程计未发生不合理的跳变；例如假设智能体以1m/s的速度行进，位姿计算和发布的频率为15hz，则可设置d为0.067m；

16、ba(bundle adjustment,光束平差法)优化残差则认为优化问题收敛，所得位姿估计较为可信；其中，作为根据场景预设的阈值。

17、作为优选，视觉里程计节点用于获取并发布视觉slam里程计，并提供给激光slam线程，经过坐标系转换，作为激光雷达slam扫描匹配(scan matching)的初始位姿，从而应对智能体剧烈运动以及imu信息缺失的情况；其中，坐标系转换公式如下：

18、twl＝twv*tvl；

19、其中，tvl∈se(3)由视觉传感器与激光雷达外参标定获得(vl–由激光雷达坐标系向视觉传感器坐标系的转换)；

20、对视觉slam里程计的准确度，基于如下标准进行判断：

21、判断是否有跳变，公式如下：

22、

23、若视觉slam里程计估计的相邻帧对应的智能体设备位置差，即i时刻视觉slam里程计估计位置与(i-1)时刻视觉slam里程计估计位置距离小于阈值d，即认为视觉slam里程计未发生不合理的跳变；

24、ba优化残差则认为优化问题收敛，所得位姿估计较为可信；其中，作为根据场景预设的阈值；

25、判断当前关键帧是否跟踪到足够的特征点

26、ntrack>αthre·ntotal；

27、若当前关键帧跟踪到的特征点数ntrack大于αthre·ntotal(αthre<1–预设的百分比,ntotal–每一帧提取的特征点总数)，则认为帧间跟踪稳定，后经计算和优化获得的位姿可信。

28、作为优选，视觉辅助激光回环节点的视觉slam线程发布检测到回环的当前图像帧与回环帧的时间戳，依照时间戳获取对应的点云关键帧，并进行icp配准；确定回环，则激光雷达slam线程进行优化，完成回环。

29、更优地，激光雷达回环的视觉验证节点具体如下：

30、当视觉slam线程未发布检测到回环的信息，激光雷达slam线程通过icp配准检测到回环，则发布检测到回环的当前点云关键帧与回环帧的时间戳；

31、依照时间戳，利用视觉slam线程的关键帧集，查看相应的图像帧的匹配情况：

32、若特征点匹配小于阈值nthre，则拒绝此次检测到的回环，发挥视觉更善于识别地点的优势，避免错误回环；同时，icp阈值的选取对于激光雷达回环检测结果影响很大，利用视觉验证激光雷达回环检测，弱化icp阈值选取的影响和省去按照场景进行阈值选取需要的工作量。

33、一种基于信息互通的视觉与激光雷达融合建图定位系统，该系统是一台智能体设备为载体，智能体设备包括计算单元、视觉传感器、激光雷达传感器以及imu的移动机器人设备；其中，计算单元上部署任意独立的激光雷达建图定位技术环境以及视觉建图定位技术环境且计算单元具有ros系统；

34、计算单元上还部署激光雷达点云节点、激光雷达里程计节点、视觉里程计节点、视觉辅助激光雷达回环节点以及激光雷达回环的视觉验证节点；

35、其中，激光雷达点云节点用于获取并发布激光雷达slam矫正畸变后的点云；

36、激光雷达里程计节点用于获取并发布激光雷达slam里程计，并提供给视觉slam线程，进行坐标转换，替代imu信息辅助初始化；

37、视觉里程计节点用于获取并发布视觉slam里程计，并提供给激光slam线程，经过如下坐标系转换，作为激光雷达slam扫描匹配(scan matching)的初始位姿，从而应对智能体剧烈运动以及imu信息缺失的情况；

38、视觉辅助激光雷达回环节点用于通过视觉slam线程发布检测到回环的当前图像帧与回环帧的时间戳，依照时间戳获取对应的点云关键帧，并进行icp配准；确定回环，则激光雷达slam线程进行优化，完成回环；

39、激光雷达回环的视觉验证节点用于当视觉slam线程未发布检测到回环的信息，激光雷达slam线程通过icp配准检测到回环，则发布检测到回环的当前点云关键帧与回环帧的时间戳，并依照时间戳，利用视觉slam线程的关键帧集，查看相应的图像帧的匹配情况。

40、一种电子设备，包括：存储器和至少一个处理器；

41、其中，所述存储器上存储有计算机程序；

42、所述至少一个处理器执行所述存储器存储的计算机程序，使得所述至少一个处理器执行如上述的基于信息互通的视觉与激光雷达融合建图定位方法。

43、一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，所述计算机程序可被处理器执行以实现如上述的基于信息互通的视觉与激光雷达融合建图定位方法。

44、本发明的基于信息互通的视觉与激光雷达融合建图定位方法及系统具有以下优点：

45、(一)本发明基于雷达建图定位线程与视觉建图定位线程进行信息互通的思路，有选择性的、智能的挖掘利用视觉与激光雷达建图定位的有用信息，充分且少冗余的利用和组合最先进的slam技术，快速高效的建立雷达视觉融合建图定位的方案，具有性能高、拓展性强的优点，可有效利用激光雷达及视觉传感器建图定位各自的优势、弥补劣势，实现复杂场景下稳定精确、高鲁棒性的建图定位；

46、(二)本发明通过融合激光雷达与视觉建图定位技术，有效的利用了两种传感器的优势、弥补各自的劣势，既可以应对无结构环境，例如长走廊或平坦的空地等激光雷达建图定位效果会大幅劣化的场景，又能够应对纹理单一及快速运动等对于视觉建图定位技术具有很大挑战性的情形，实现复杂场景下稳定精确、高鲁棒性的建图定位；

47、(三)本发明保证了激光雷达建图定位线程与视觉建图定位线程的信息充分互通，并设计了信息的判断、选择以及反馈机制，保证信息质量，继而可在复杂场景下获得较优的建图定位效果。