基于激光雷达和相机融合的增强型里程计方法

本发明属于自动驾驶，具体涉及一种基于激光雷达和相机融合的增强型里程计方法。

背景技术：

1、自动驾驶汽车集成了环境感知、无线通信、智能计算等功能，为用户提供了优质的服务和便捷的出行体验。移动定位作为自动驾驶领域最重要的功能之一，引起了学者们的广泛关注。移动定位的其中一种方法是使用全球卫星导航系统(gnss)。尽管先进的gnss可以提供精确到几厘米的定位，但由于信号延迟和服务质量等问题使得自动驾驶汽车不能完全依赖gnss。移动定位的另一种方法是里程计方法，它使用车辆自身传感器的数据跟踪车辆的运动，并估计车辆相对于上一时刻的位置以及方向角的变化。

2、随着相机和激光雷达在现代车辆中的普及，视觉里程计和激光雷达里程计成为了主流的里程计方法。视觉里程计和激光雷达里程计分别通过分析由于车辆运动导致的相邻两帧的图像变化和激光雷达点云变化来估计自动驾驶汽车的运动轨迹。然而，由于相机生成的图像缺乏深度信息，仅使用图像在世界坐标系下对车辆进行精确定位存在困难；由于激光雷达点云的稀疏性，在连续帧中点云特征的错误关联可能会导致车辆的位姿估计出现漂移。因此，为了提高自动驾驶汽车的综合感知能力，将相机与激光雷达融合的视觉和激光雷达融合里程计变得越来越重要。视觉和激光雷达融合里程计可分为增强型视觉里程计、增强型激光雷达里程计、并发视觉和激光雷达融合里程计。增强型视觉里程计采用视觉里程计作为其主要框架，并使用从激光雷达测量中获得的深度信息增强其估计结果；增强型激光雷达里程计使用激光雷达估计车辆运动，同时使用相机进行漂移校正；并发视觉和激光雷达里程计同时结合了激光雷达和图像数据进行运动估计。

3、虽然这些经典的融合里程计方法有其自身优势，但同时也存在一些固有的缺陷。激光雷达辅助视觉的增强型视觉里程计不执行图像与激光雷达点云之间的特征关联，而仅仅使用激光雷达提取的深度信息。由于激光雷达点云的稀疏性，许多图像中的像素点没有被分配深度值，因此需要额外的插值方法来弥补缺失的深度值。类似地，视觉辅助激光雷达的增强型激光雷达里程计的局限性在于优化过程中不能同时使用不同的传感器测量值。此外，大多数并发视觉和激光雷达里程计忽略了传感器性能与环境之间的相关性，并且不能动态调整激光雷达和相机的权重，这可能导致某些传感器的模态得到过度自信的估计。

技术实现思路

1、为了克服现有技术的不足，本发明提供了一种基于激光雷达和相机融合的增强型里程计方法，首先利用深度完成网络将相机生成的图像数据转换为伪激光雷达点云数据，使得图像数据与激光雷达点云数据具有一致的表示形式。其次，使用了贝叶斯推断将伪激光雷达点云数据与激光雷达点云数据进行融合，通过计算两者的重投影误差和置信度图生成融合点云数据。最后，将融合点云作为里程计的输入，实现了均衡考虑激光雷达与相机权重的里程计。本发明相较于只使用相机转换的伪激光雷达点云数据平均轨迹误差降低了约90％，相较于只使用激光雷达点云数据平均轨迹误差降低了约40％；相较于最优竞争方法，平均轨迹误差降低了约45％，有效提升了里程计的估计精度。

2、本发明解决其技术问题所采用的技术方案如下：

3、步骤1：将激光雷达生成的点云l转换为稀疏深度图s，并将稀疏深度图s和相机生成的图像i输入到深度完成网络中得到密集深度图d，最后将密集深度图d反向投影到3d空间转换为伪激光雷达点云p；

4、步骤2：根据激光雷达点云l与伪激光雷达点云p的重投影误差e，分别计算激光雷达点云l与伪激光雷达点云p置信度图cl与cp，并以此为依据计算融合点云f；

5、步骤3，使用融合点云f作为一个任意的激光雷达里程计框架输入，估计当前车辆的位姿。

6、进一步地，所述步骤1具体为：

7、步骤1-1：将激光雷达与相机进行对齐；

8、将激光雷达生成的点云l＝[xl,yl,zl]t按照已知的相机内参矩阵转换为稀疏深度图s＝[ul,vl,1]t，公式如下：

9、

10、其中表示激光雷达坐标系到相机坐标系的转换矩阵，rrect是校正矩阵，是相机坐标系到图像坐标系的转换矩阵；

11、步骤1-2：对不包含深度值的像素进行深度估计，将得到的稀疏深度图s和图像i输入到到与penet类似的深度完成网络中，从而得到密集深度图d；

12、步骤1-3：将密集深度图d反向投影到3d空间中转换为伪激光雷达点云p。

13、进一步地，所述步骤2具体为：

14、使用贝叶斯推理融合激光雷达和相机，通过计算重投影误差e估计它们的置信度图，公式如下：

15、

16、其中x表示当前点云在激光雷达坐标系中的3d坐标，表示当前点云在图像坐标系中的2d坐标，m表示当前点云中点的总数；

17、每个点云的置信度图计算为c＝e-λe，其中λ为归一化常数；如果计算的深度为负，则置信度设置为零；通过贝叶斯推理，计算融合点云f的输出如下：

18、

19、进一步地，所述步骤3中采用loam框架进行位姿估计。

20、本发明的有益效果如下：

21、1、本发明大幅提高了里程计对于车辆位姿估计的精度与稳定性，解决了激光雷达和相机融合里程计性能欠佳、图像与激光雷达点云数据不能充分融合、不同环境下某类传感器模态得到过度自信的估计的问题。

22、2、本发明相较于只使用相机转换的伪激光雷达点云数据平均轨迹误差降低了约90％，相较于只使用激光雷达点云数据平均轨迹误差降低了约40％；相较于最优竞争方法，平均轨迹误差降低了约45％，有效提升了里程计的估计精度。

技术特征：

1.一种基于激光雷达和相机融合的增强型里程计方法，其特征在于，包括如下步骤：

2.根据权利要求1所述的一种基于激光雷达和相机融合的增强型里程计方法，其特征在于，所述步骤1具体为：

3.根据权利要求2所述的一种基于激光雷达和相机融合的增强型里程计方法，其特征在于，所述步骤2具体为：

4.根据权利要求3所述的一种基于激光雷达和相机融合的增强型里程计方法，其特征在于，所述步骤3中采用loam框架进行位姿估计。

技术总结本发明公开了一种基于激光雷达和相机融合的增强型里程计方法，首先利用深度完成网络将相机生成的图像数据转换为伪激光雷达点云数据，使得图像数据与激光雷达点云数据具有一致的表示形式。其次，使用了贝叶斯推断将伪激光雷达点云数据与激光雷达点云数据进行融合，通过计算两者的重投影误差和置信度图生成融合点云数据。最后，将融合点云作为里程计的输入，实现了均衡考虑激光雷达与相机权重的里程计。本发明相较于只使用相机转换的伪激光雷达点云数据平均轨迹误差降低了约90％，相较于只使用激光雷达点云数据平均轨迹误差降低了约40％；相较于最优竞争方法，平均轨迹误差降低了约45％，有效提升了里程计的估计精度。技术研发人员：荀毅杰,董浩,马小川,刘家佳,毛伯敏,郭鸿志受保护的技术使用者：西北工业大学太仓长三角研究院技术研发日：技术公布日：2024/9/17