[发明专利]基于多传感器的紧耦合里程计方法、装置、设备及存储介质

说明书

本公开提供了一种基于多传感器的紧耦合里程计方法、装置、设备及存储介质。本公开的一个实施例中，基于多传感器的紧耦合里程计方法，包括：获取来自相机的图像帧、来自激光雷达的激光帧和来自IMU的IMU帧；利用当前图像帧的预积分约束关系、当前图像帧的图像帧间约束关系和当前图像帧与激光帧间的约束关系更新系统状态变量，基于更新得到的系统状态变量，更新局部点云地图中激光点的RGB值和当前图像帧的图像特征点；利用当前激光帧的预积分约束关系、当前激光帧的激光帧间约束关系和当前激光帧与图像帧间的约束关系更新系统状态变量，根据所述更新得到的系统状态变量将当前激光帧的激光点云数据合并到全局点云地图中。本公开能够有效提高里程计耦合程度，降低传感器位姿估计与周围环境地图构建时存在的误差。

技术领域

本公开涉及自动驾驶技术领域，尤其涉及一种基于多传感器的紧耦合里程计方法、装置、设备及存储介质。

背景技术

同时定位与建图技术(SLAM，Simultaneous Localization And Mapping)目前已经广泛运用于自动驾驶、无人机、机器人、虚拟现实(VR，Virtual Reality)和增强现实(AR，Augmented Reality)等众多领域。

SLAM大致可分为前端里程计、后端(非线性)优化与回环检测三个部分。前端里程计主要负责处理各种传感器的原始数据，从而完成传感器的实时位姿估计与周围环境地图的实时构建。而后端(非线性)优化与回环检测则是为了消除前端里程计在位姿估计与构建周围环境地图时存在的误差。由于后端(非线性)优化与回环检测只能消除前端里程计的部分误差，因此，高精度的里程计对SLAM技术来说是十分重要的。

传统的里程计主要分为视觉里程计与激光里程计。视觉里程计通过处理图像信息完成传感器位姿的实时估计与周围环境地图的实时构建，而激光里程计则是通过处理激光点云数据完成实时位姿估计与地图构建。由于视觉里程计处理的是图像数据，因此，当视觉传感器处于纹理稀疏、光照明暗变化剧烈的场景中时，视觉里程计的系统性能会受到严重影响。激光里程计在较为空旷的场景中则会因采集的激光点云数据不足导致系统性能严重下降。因此，如何基于多传感器实现紧耦合里程计，以克服上述不同种类里程计存在的缺陷。

发明内容

为了解决上述技术问题中的至少一个，本公开提供了一种基于多传感器的紧耦合里程计方法、装置、设备及存储介质，能够有效提高整个里程计的系统性能。

本公开的第一方面提供了一种基于多传感器的紧耦合里程计方法，包括：

获取来自相机的图像帧、来自激光雷达的激光帧和来自IMU的IMU帧；

利用当前图像帧的预积分约束关系、当前图像帧的图像帧间约束关系和当前图像帧与激光帧间的约束关系更新系统状态变量，基于更新得到的系统状态变量，更新局部点云地图中激光点的RGB值和当前图像帧的图像特征点，所述当前图像帧的预积分约束关系通过当前图像帧关联的所述IMU帧建立；

利用当前激光帧的预积分约束关系、当前激光帧的激光帧间约束关系和当前激光帧与图像帧间的约束关系更新系统状态变量，根据所述更新得到的系统状态变量将当前激光帧的激光点云数据合并到全局点云地图中，所述当前激光帧的预积分约束关系通过所述当前激光帧关联的所述IMU帧建立，所述当前激光帧与图像帧间的约束关系通过所述当前激光帧的激光点与最邻近图像帧的图像特征点确定。

第一方面的一些可能的实现方式中，还包括：将所述图像帧和所述激光帧根据各自的时间戳分别与所述IMU帧关联。

第一方面的一些可能的实现方式中，所述将所述图像帧和所述激光帧根据各自的时间戳分别与IMU帧关联，包括如下之一：

没有时间戳小于当前观测帧的保留观测帧但有时间戳小于当前观测帧的IMU帧时，将时间戳小于当前观测帧的所有IMU帧，和时间戳大于或等于当前观测帧的第一帧IMU帧共同作为当前观测帧关联的IMU帧；