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

权利要求书

1.一种基于多传感器的紧耦合里程计方法，其特征在于，包括：

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

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

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

2.根据权利要求1所述基于多传感器的紧耦合里程计方法，其特征在于，还包括：将所述图像帧和所述激光帧根据各自的时间戳分别与所述IMU帧关联。

3.根据权利要求2所述基于多传感器的紧耦合里程计方法，其特征在于，所述将所述图像帧和所述激光帧根据各自的时间戳分别与IMU帧关联，包括如下之一：

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

有时间戳小于当前观测帧的保留观测帧时，寻找时间戳小于当前观测帧的最邻近观测帧，将时间戳大于该最邻近观测帧且时间戳小于当前观测帧间的所有IMU帧和时间戳大于或等于当前观测帧的第一帧IMU帧共同作为当前观测帧关联的IMU帧；

没有时间戳小于当前观测帧的IMU帧时，抛弃当前观测帧；

所述观测帧为所述图像帧或所述激光帧。

4.根据权利要求1所述基于多传感器的紧耦合里程计方法，其特征在于，所述当前图像帧为初始图像帧时，所述当前图像帧的图像特征点通过如下方式获取：

根据相机与激光雷达间的外参将初始图像帧最邻近激光帧的激光点投影至初始图像帧中，以确定每个激光点的激光像素点；

将各个激光像素点划分到不同的图像块中；

根据每个图像块中激光像素点的质量和激光像素点所对应激光点的空间位置选取激光像素点作为初始图像帧的图像特征点。