基于多传感器的紧耦合里程计方法、装置、设备及存储介质与流程

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


背景技术：

2.同时定位与建图技术(slam，simultaneous localization and mapping)目前已经广泛运用于自动驾驶、无人机、机器人、虚拟现实(vr，virtual reality)和增强现实(ar，augmented reality)等众多领域。
3.slam大致可分为前端里程计、后端(非线性)优化与回环检测三个部分。前端里程计主要负责处理各种传感器的原始数据，从而完成传感器的实时位姿估计与周围环境地图的实时构建。而后端(非线性)优化与回环检测则是为了消除前端里程计在位姿估计与构建周围环境地图时存在的误差。由于后端(非线性)优化与回环检测只能消除前端里程计的部分误差，因此，高精度的里程计对slam技术来说是十分重要的。
4.传统的里程计主要分为视觉里程计与激光里程计。视觉里程计通过处理图像信息完成传感器位姿的实时估计与周围环境地图的实时构建，而激光里程计则是通过处理激光点云数据完成实时位姿估计与地图构建。由于视觉里程计处理的是图像数据，因此，当视觉传感器处于纹理稀疏、光照明暗变化剧烈的场景中时，视觉里程计的系统性能会受到严重影响。激光里程计在较为空旷的场景中则会因采集的激光点云数据不足导致系统性能严重下降。因此，如何基于多传感器实现紧耦合里程计，以克服上述不同种类里程计存在的缺陷。


技术实现要素：

5.为了解决上述技术问题中的至少一个，本公开提供了一种基于多传感器的紧耦合里程计方法、装置、设备及存储介质，能够有效提高整个里程计的系统性能。
6.本公开的第一方面提供了一种基于多传感器的紧耦合里程计方法，包括：
7.获取来自相机的图像帧、来自激光雷达的激光帧和来自imu的imu帧；
8.利用当前图像帧的预积分约束关系、当前图像帧的图像帧间约束关系和当前图像帧与激光帧间的约束关系更新系统状态变量，基于更新得到的系统状态变量，更新局部点云地图中激光点的rgb值和当前图像帧的图像特征点，所述当前图像帧的预积分约束关系通过当前图像帧关联的所述imu帧建立；
9.利用当前激光帧的预积分约束关系、当前激光帧的激光帧间约束关系和当前激光帧与图像帧间的约束关系更新系统状态变量，根据所述更新得到的系统状态变量将当前激光帧的激光点云数据合并到全局点云地图中，所述当前激光帧的预积分约束关系通过所述当前激光帧关联的所述imu帧建立，所述当前激光帧与图像帧间的约束关系通过所述当前激光帧的激光点与最邻近图像帧的图像特征点确定。
10.第一方面的一些可能的实现方式中，还包括：将所述图像帧和所述激光帧根据各
自的时间戳分别与所述imu帧关联。
11.第一方面的一些可能的实现方式中，所述将所述图像帧和所述激光帧根据各自的时间戳分别与imu帧关联，包括如下之一：
12.没有时间戳小于当前观测帧的保留观测帧但有时间戳小于当前观测帧的imu帧时，将时间戳小于当前观测帧的所有imu帧，和时间戳大于或等于当前观测帧的第一帧imu帧共同作为当前观测帧关联的imu帧；
13.有时间戳小于当前观测帧的保留观测帧时，寻找时间戳小于当前观测帧的最邻近观测帧，将时间戳大于该最邻近观测帧且时间戳小于当前观测帧间的所有imu帧和时间戳大于或等于当前观测帧的第一帧imu帧共同作为当前观测帧关联的imu帧；
14.没有时间戳小于当前观测帧的imu帧时，抛弃当前观测帧；
15.所述观测帧为所述图像帧或所述激光帧。
16.第一方面的一些可能的实现方式中，所述当前图像帧为初始图像帧时，所述当前图像帧的图像特征点通过如下方式获取：
17.根据相机与激光雷达间的外参将初始图像帧最邻近激光帧的激光点投影至初始图像帧中，以确定每个激光点的激光像素点；
18.将各个激光像素点划分到不同的图像块中；
19.根据每个图像块中激光像素点的质量和激光像素点所对应激光点的空间位置选取激光像素点作为初始图像帧的图像特征点。
20.第一方面的一些可能的实现方式中，所述当前图像帧不是初始图像帧时，所述当前图像帧的图像特征点通过方式更新：
21.根据所述相机与所述激光雷达间的外参，将所述当前图像帧最邻近激光帧的激光点投影至所述当前图像帧中，以确定每个所述激光点的激光像素点；
22.将每个所述激光像素点和所述当前图像帧中已存在的图像特征点划分到所述当前图像帧的不同图像块中；
23.根据每个所述图像块中激光像素点的质量、激光像素点所对应激光点的空间位置以及已存在的图像特征点的数量和质量，选取一个所述激光像素点或已存在的图像特征点作为所述图像块的当前图像特征点。
24.第一方面的一些可能的实现方式中，所述激光像素点的质量优劣根据以所述激光像素点为中心的图像块的像素梯度或图像熵值判断。
25.第一方面的一些可能的实现方式中，所述根据每个所述图像块中激光像素点的质量、激光像素点所对应激光点的空间位置以及已存在的图像特征点的数量和质量，选取一个所述激光像素点或已存在的图像特征点作为所述图像块的当前图像特征点，包括如下之一：
26.当所述图像块中有一个已存在的图像特征点时，保留所述已存在的图像特征点，以该已存在的图像特征点作为所述图像块的当前图像特征点；
27.当所述图像块中有多个已存在的图像特征点时，保留所述多个已存在的图像特征点中质量最好的图像特征点，以该质量最好的图像特征点作为所述图像块的当前图像特征点；
28.当所述图像块中没有已存在的图像特征点时，根据所述图像块中激光像素点的质
量和激光像素点所对应激光点的空间位置选取一个所述激光像素点作为所述图像块的图像特征点。
29.第一方面的一些可能的实现方式中，所述局部点云地图中激光点的rgb值通过如下方式更新：
30.根据所述相机与所述激光雷达间的外参，将所述局部点云地图中在当前图像帧视野范围内的激光点投影至当前图像帧，以确定所述局部点云地图中在当前图像帧视野范围内的激光点对应的像素坐标；
31.将所述当前图像帧中所述激光点对应的像素坐标处的rgb值附加给所述激光点。
32.第一方面的一些可能的实现方式中，所述当前图像帧不是初始图像帧时，所述局部点云地图中激光点的rgb值通过如下方式更新：
33.根据所述相机与所述激光雷达间的外参，将所述局部点云地图中在当前图像帧视野范围内的激光点投影至当前图像帧，以确定所述局部点云地图中在当前图像帧视野范围内的激光点对应的像素坐标；
34.将所述当前图像帧中所述激光点对应的像素坐标处的rgb值和所述激光点已附加的rgb值融合，并将所述激光点已附加的rgb值更新为融合得到的rgb值。
35.第一方面的一些可能的实现方式中，所述利用当前图像帧的预积分约束关系、当前图像帧的图像帧间约束关系和当前图像帧与激光帧间的约束关系更新系统状态变量，包括：
36.通过当前图像帧关联的所述imu帧建立当前图像帧与上一观测帧间的预积分约束关系，所述上一观测帧为时间戳与所述当前图像帧最接近的上一图像帧或上一激光帧；
37.获取当前图像帧与上一图像帧间的图像特征点匹配对；
38.根据所述图像特征点匹配对构建当前图像帧与上一图像帧间的图像帧间约束关系，利用当前图像帧与上一观测帧间的预积分约束关系和图像帧间约束关系执行第一联合优化，以更新系统状态变量；
39.根据所述第一联合优化更新得到的系统状态变量和所述图像特征点匹配对构建当前图像帧与最邻近激光帧间的约束关系，利用所述当前图像帧与上一观测帧间的预积分约束关系和当前图像帧与最邻近激光帧间的约束关系执行第二联合优化，以再次更新所述系统状态变量。
40.第一方面的一些可能的实现方式中，所述当前图像帧的图像帧间约束关系包括：当前图像帧与上一图像帧间的图像特征点匹配对的重投影残差和图像特征点匹配对对应的激光点与其在当前图像帧上投影所得的激光像素点间的rgb残差。
41.第一方面的一些可能的实现方式中，所述当前图像帧与激光帧间的约束关系包括：当前图像帧与最邻近激光帧间的3d点匹配对的三维坐标残差，所述3d点匹配对包括当前图像帧与上一图像帧间的图像特征点匹配对的三角化点和所述图像特征点匹配对对应的激光点。
42.第一方面的一些可能的实现方式中，所述利用当前激光帧的预积分约束关系、当前激光帧的激光帧间约束关系和当前激光帧与图像帧间的约束关系更新系统状态变量，包括：
43.通过当前激光帧关联的所述imu帧建立当前激光帧与上一观测帧间的预积分约束
关系，所述上一观测帧为时间戳与所述当前激光帧最接近的上一图像帧或上一激光帧；
44.获取当前激光帧的特征点匹配关系；
45.根据所述当前激光帧的特征点匹配关系构建当前激光帧的激光帧间约束关系，利用当前激光帧的所述预积分约束关系和所述激光帧间约束关系执行第三联合优化，以更新系统状态变量；
46.根据所述第三联合优化更新得到的系统状态变量和所述当前激光帧的特征点匹配关系构建当前激光帧与最邻近图像帧间的约束关系，利用当前激光帧的所述预积分约束关系及其与最邻近图像帧间的约束关系执行第四联合优化，以再次更新所述系统状态变量。
47.第一方面的一些可能的实现方式中，所述当前激光帧的激光帧间约束关系包括：当前激光帧的面特征点与匹配激光点所形成面的点面距离残差和当前激光帧的线特征点与匹配激光点所形成直线的点线距离残差。
48.第一方面的一些可能的实现方式中，所述当前激光帧与图像帧间的约束关系包括：所述当前激光帧与最邻近图像帧间的激光点匹配对的三维坐标残差；其中，所述激光点匹配对包括当前激光帧中的第一激光点和当前激光帧的最邻近图像帧中第一图像特征点所对应的激光点；其中，所述第一激光点投影至所述最邻近图像帧所获得的激光像素点与所述最邻近图像帧的第一图像特征点有重合。
49.本公开的第二方面提供了一种基于多传感器的紧耦合里程计装置，包括：
50.获取单元，用于获取来自相机的图像帧、来自激光雷达的激光帧和来自imu的imu帧；
51.第一状态更新单元，用于利用当前图像帧的预积分约束关系、当前图像帧的图像帧间约束关系和当前图像帧与激光帧间的约束关系更新系统状态变量，所述当前图像帧的预积分约束关系通过当前图像帧关联的所述imu帧建立；
52.第一地图更新单元，用于基于所述第一状态更新单元更新得到的系统状态变量，更新局部点云地图中激光点的rgb值；
53.图像特征点更新单元，用于基于所述第一状态更新单元更新得到的系统状态变量，更新当前图像帧的图像特征点；
54.第二状态更新单元，用于利用当前激光帧的预积分约束关系、当前激光帧的激光帧间约束关系和当前激光帧与图像帧间的约束关系更新系统状态变量，所述当前激光帧的预积分约束关系通过所述当前激光帧关联的所述imu帧建立，所述当前激光帧与图像帧间的约束关系通过所述当前激光帧的激光点与最邻近图像帧的图像特征点确定；
55.第二地图更新单元，用于根据所述更新得到的系统状态变量将当前激光帧的激光点云数据合并到全局点云地图中。
56.第二方面的一些可能的实现方式中，还包括：关联单元，用于将所述图像帧和所述激光帧根据各自的时间戳分别与所述imu帧关联。
57.第二方面的一些可能的实现方式中，所述图像特征点更新单元，具体用于在当前图像帧为初始图像帧时，通过如下方式获取所述当前图像帧的图像特征点：根据相机与激光雷达间的外参将初始图像帧最邻近激光帧的激光点投影至初始图像帧中，以确定每个激光点的激光像素点；将各个激光像素点划分到不同的图像块中；根据每个图像块中激光像
素点的质量和激光像素点所对应激光点的空间位置选取激光像素点作为初始图像帧的图像特征点。
58.第二方面的一些可能的实现方式中，所述图像特征点更新单元，具体用于在当前图像帧不是初始图像帧时，通过如下方式更新所述当前图像帧的图像特征点：
59.根据所述相机与所述激光雷达间的外参，将所述当前图像帧最邻近激光帧的激光点投影至所述初始图像帧中，以确定每个所述激光点的激光像素点；
60.将每个所述激光像素点和所述当前图像帧中已存在的图像特征点划分到所述当前图像帧的不同图像块中；
61.根据每个所述图像块中激光像素点的质量、激光像素点所对应激光点的空间位置以及已存在的图像特征点的数量和质量，选取一个所述激光像素点或已存在的图像特征点作为所述图像块的当前图像特征点。
62.第二方面的一些可能的实现方式中，所述激光像素点的质量优劣根据以所述激光像素点为中心的图像块的像素梯度或图像熵值判断。
63.第二方面的一些可能的实现方式中，第一地图更新单元，具体用于通过如下方式更新所述局部点云地图中激光点的rgb值：根据所述相机与所述激光雷达间的外参，将所述局部点云地图中在当前图像帧视野范围内的激光点投影至当前图像帧，以确定所述局部点云地图中在当前图像帧视野范围内的激光点对应的像素坐标；将所述当前图像帧中所述激光点对应的像素坐标处的rgb值附加给所述激光点。
64.第二方面的一些可能的实现方式中，第一地图更新单元，具体用于在所述当前图像帧不是初始图像帧时通过如下方式更新所述局部点云地图中激光点的rgb值：
65.根据所述相机与所述激光雷达间的外参，将所述局部点云地图中在当前图像帧视野范围内的激光点投影至当前图像帧，以确定所述局部点云地图中在当前图像帧视野范围内的激光点对应的像素坐标；
66.将所述当前图像帧中所述激光点对应的像素坐标处的rgb值和所述激光点已附加的rgb值融合，并将所述激光点已附加的rgb值更新为融合得到的rgb值。
67.第二方面的一些可能的实现方式中，所述第一状态更新单元，具体用于通过如下方式更新系统状态变量：
68.通过当前图像帧关联的所述imu帧建立当前图像帧与上一观测帧间的预积分约束关系，所述上一观测帧为时间戳与所述当前图像帧最接近的上一图像帧或上一激光帧；
69.获取当前图像帧与上一图像帧间的图像特征点匹配对；
70.根据所述图像特征点匹配对构建当前图像帧与上一图像帧间的图像帧间约束关系，利用当前图像帧与上一观测帧间的预积分约束关系和图像帧间约束关系执行第一联合优化，以更新系统状态变量；
71.根据所述第一联合优化更新得到的系统状态变量和所述图像特征点匹配对构建当前图像帧与最邻近激光帧间的约束关系，利用所述当前图像帧与上一观测帧间的预积分约束关系和当前图像帧与最邻近激光帧间的约束关系执行第二联合优化，以再次更新所述系统状态变量。
72.第二方面的一些可能的实现方式中，所述当前图像帧的图像帧间约束关系包括：当前图像帧与上一图像帧间的图像特征点匹配对的重投影残差和图像特征点匹配对对应
的激光点与其在当前图像帧上投影所得的激光像素点间的rgb残差。
73.第二方面的一些可能的实现方式中，所述当前图像帧与激光帧间的约束关系包括：当前图像帧与最邻近激光帧间的3d点匹配对的三维坐标残差，所述3d点匹配对包括当前图像帧与上一图像帧间的图像特征点匹配对的三角化点和所述图像特征点匹配对对应的激光点。
74.第二方面的一些可能的实现方式中，所述第二状态更新单元，具体用于通过如下方式更新系统状态变量：
75.通过当前激光帧关联的所述imu帧建立当前激光帧与上一观测帧间的预积分约束关系，所述上一观测帧为时间戳与所述当前激光帧最接近的上一图像帧或上一激光帧；
76.获取当前激光帧的特征点匹配关系；
77.根据所述当前激光帧的特征点匹配关系构建当前激光帧的激光帧间约束关系，利用当前激光帧的所述预积分约束关系和所述激光帧间约束关系执行第三联合优化，以更新系统状态变量；
78.根据所述第三联合优化更新得到的系统状态变量和所述当前激光帧的特征点匹配关系构建当前激光帧与最邻近图像帧间的约束关系，利用当前激光帧的所述预积分约束关系及其与最邻近图像帧间的约束关系执行第四联合优化，以再次更新所述系统状态变量。
79.第二方面的一些可能的实现方式中，所述当前激光帧的激光帧间约束关系包括：当前激光帧的面特征点与匹配激光点所形成面的点面距离残差和当前激光帧的线特征点与匹配激光点所形成直线的点线距离残差。
80.第二方面的一些可能的实现方式中，所述当前激光帧与图像帧间的约束关系包括：所述当前激光帧与最邻近图像帧间的激光点匹配对的三维坐标残差；其中，所述激光点匹配对包括当前激光帧中的第一激光点和当前激光帧的最邻近图像帧中第一图像特征点所对应的激光点；其中，所述第一激光点投影至所述最邻近图像帧所获得的激光像素点与所述最邻近图像帧的第一图像特征点有重合。
81.本公开的第三方面提供了一种电子设备，包括：
82.存储器，所述存储器存储执行指令；以及
83.处理器，所述处理器执行所述存储器存储的执行指令，使得所述处理器执行上述基于多传感器的紧耦合里程计方法。
84.本公开的第四方面提供了一种可读存储介质，所述可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现上述基于多传感器的紧耦合里程计方法。
85.本公开通过在激光帧间、图像帧间与激光和图像帧间建立不同的约束关系来共同更新维护同一个系统状态变量，不仅基于图像帧间的约束关系构建优化问题，而且可以在图像与激光帧间寻找约束关系构建优化问题，不仅可以根据激光帧间的约束关系构建优化问题，而且可以在激光与图像帧间寻找约束关系构建优化问题，能够有效提高各个传感器的耦合程度，从而有效降低传感器位姿估计与周围环境地图构建时存在的误差，提高整个里程计的系统性能。
附图说明
86.附图示出了本公开的示例性实施方式，并与其说明一起用于解释本公开的原理，其中包括了这些附图以提供对本公开的进一步理解，并且附图包括在本说明书中并构成本说明书的一部分。
87.图1是本公开的一个实施方式的基于多传感器的紧耦合里程计方法的流程示意图；
88.图2是本公开的一个实施方式中观测帧与imu帧的关联原理示意图；
89.图3是本公开的一个实施方式中获取初始图像帧的图像特征点的原理示意图；
90.图4是本公开的一个实施方式中使激光点空间分布均匀化的图像特征点选取方式的原理示意图；
91.图5是本公开的一个实施方式的利用当前图像帧的预积分约束关系、当前图像帧的图像帧间约束关系和当前图像帧与激光帧间的约束关系更新系统状态变量的流程示意图；
92.图6是本公开的一个实施方式在图像帧与激光帧间寻找约束关系构建优化问题的原理示意图；
93.图7是本公开的一个实施方式的利用当前激光帧的预积分约束关系、当前激光帧的激光帧间约束关系和当前激光帧与图像帧间的约束关系更新系统状态变量的流程示意图；
94.图8是本公开的一个实施方式在当前激光帧与图像帧间寻找约束关系构建优化问题的原理示意图；
95.图9是本公开的一个实施方式的采用处理系统的硬件实现方式的基于多传感器的紧耦合里程计装置的结构示意框图。
具体实施方式
96.下面结合附图和实施方式对本公开作进一步的详细说明。可以理解的是，此处所描述的具体实施方式仅用于解释相关内容，而非对本公开的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本公开相关的部分。
97.需要说明的是，在不冲突的情况下，本公开中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本公开的技术方案。
98.除非另有说明，否则示出的示例性实施方式/实施例将被理解为提供可以在实践中实施本公开的技术构思的一些方式的各种细节的示例性特征。因此，除非另有说明，否则在不脱离本公开的技术构思的情况下，各种实施方式/实施例的特征可以另外地组合、分离、互换和/或重新布置。
99.在附图中使用交叉影线和/或阴影通常用于使相邻部件之间的边界变得清晰。如此，除非说明，否则交叉影线或阴影的存在与否均不传达或表示对部件的具体材料、材料性质、尺寸、比例、示出的部件之间的共性和/或部件的任何其它特性、属性、性质等的任何偏好或者要求。此外，在附图中，为了清楚和/或描述性的目的，可以夸大部件的尺寸和相对尺寸。当可以不同地实施示例性实施例时，可以以不同于所描述的顺序来执行具体的工艺顺序。例如，可以基本同时执行或者以与所描述的顺序相反的顺序执行两个连续描述的工艺。
此外，同样的附图标记表示同样的部件。
100.当一个部件被称作“在”另一部件“上”或“之上”、“连接到”或“结合到”另一部件时，该部件可以直接在所述另一部件上、直接连接到或直接结合到所述另一部件，或者可以存在中间部件。然而，当部件被称作“直接在”另一部件“上”、“直接连接到”或“直接结合到”另一部件时，不存在中间部件。为此，术语“连接”可以指物理连接、电气连接等，并且具有或不具有中间部件。
101.本文使用的术语是为了描述具体实施例的目的，而不意图是限制性的。如这里所使用的，除非上下文另外清楚地指出，否则单数形式“一个(种、者)”和“所述(该)”也意图包括复数形式。此外，当在本说明书中使用术语“包含”和/或“包括”以及它们的变型时，说明存在所陈述的特征、整体、步骤、操作、部件、组件和/或它们的组，但不排除存在或附加一个或更多个其它特征、整体、步骤、操作、部件、组件和/或它们的组。还要注意的是，如这里使用的，术语“基本上”、“大约”和其它类似的术语被用作近似术语而不用作程度术语，如此，它们被用来解释本领域普通技术人员将认识到的测量值、计算值和/或提供的值的固有偏差。
102.相关技术简要说明：
103.相关技术1：《r3live:a robust,real-time,rgb-colored,lidar-inertial-visual tightly-coupled state estimation and mapping package》提出了一种基于视觉、激光与imu的紧耦合定位与建图方法，该方法包含两个里程计，分别是视觉惯性里程计(vio，visual-inertial odometry)与雷达惯性里程计(lio)，vio通过帧间光流追踪获得当前图像帧与上一图像帧间的图像特征点匹配点对，并根据匹配点对建立残差方程(重投影残差与rgb残差)，最后利用状态误差迭代卡尔曼滤波(esikf)进行迭代优化，从而更新系统状态变量(例如，旋转、位移、速度等信息)。lio利用增量k维树(ikd-tree)搜索获得当前激光帧点云与局部点云地图间的匹配关系，并根据匹配关系建立残差方程(点面残差)，然后利用esikf进行迭代优化并更新系统状态变量，最后利用优化完成后的系统状态变量将当前帧激光帧点云发布到全局点云地图中。该技术中，vio的图像特征点根据系统状态变量将激光点投影至图像帧中直接获得，获得的图像特征点与激光点会形成一一对应关系。在投影获得图像特征点时，没有考虑特征点的优劣情况，也没有将特征点进行均匀化处理，这些都会影响vio帧间光流追踪的精度。此外，该技术中，单独在vio的图像帧间寻找约束关系构建优化，再单独在lio中的激光帧间寻找约束关系构建优化，而没有激光帧与图像帧之间寻找约束关系构建优化，耦合程度不高。
104.相关技术2：《fast-livo:fast and tightly-coupled sparse-direct lidar-inertial-visual odometry》公开了一种基于视觉、激光与imu的紧耦合里程计技术。该相关技术与相关技术1一样，由vio与lio两个里程计组成，lio的实现过程与技术1的类似，vio是基于直接法的里程计。相关技术2中将激光点云根据系统状态变量投影至图像帧中寻找并记录各个激光点对应的图像块。在vio中，根据imu提供的先验位姿寻找在当前图像帧视场范围内的激光点，根据直接法建立当前帧视场范围内的激光点与当前图像帧间的图像块光度误差残差方程，最后利用esikf进行迭代优化并更新系统状态变量。由于vio中只根据图像块间的光度误差建立残差方程进行全局状态量的优化，因此，在光照明暗变化剧烈的场景中系统性能会有所下降。并且，该相关技术也没有在激光帧与图像帧间寻找约束关系
构建优化问题，仍存在耦合程度不高的缺陷。
105.相关技术3：中国专利cn111561923公开了一种基于多传感器融合的slam制图方法、系统，该技术以激光雷达为基准，标定相机、imu、全球导航卫星系统(gnss)和激光雷达之间的位置关系，得到标定信息。然后以gnss为基准，同步激光雷达、相机、imu的时间到当前gnss的时间系统。接着采集激光雷达、相机、imu、和gnss的数据，并根据这些数据实现实时的位姿解算与周围环境地图的构建。该技术与技术1、2相比新增了gnss，但是gnss卫星信号是从几万公里的天空发射下来的，这些信号到地面已很微弱，无法穿透建筑，因此，该技术仅适用于室外环境。此外，该技术也没有考虑在激光帧与图像帧间寻找约束关系构建优化问题，同样存在耦合程度不高的缺陷。
106.鉴于此，本公开提供了如下的基于多传感器的紧耦合里程计方法、装置、设备及存储介质，以克服相关技术中各传感器间耦合程度不高的缺陷，降低slam前端里程计实时位姿估计与周围环境地图实时构建的误差。
107.在对本公开的具体实施方式进行详细描述前，对本公开的一些相关细节做如下说明：
108.时间基准：由于相机、激光雷达与惯性测量单元(imu，inertial measurement unit)是三种不同类型的传感器，其输出数据的时间基准可能不同。本公开以imu的时间为基准，同步相机与激光雷达的时间到imu的时间系统；
109.激光点：本公开所描述的激光点均是指3d激光点。
110.坐标系：本公开所涉及的坐标系均为笛卡尔坐标系(右手系)；
111.本公开中，相机、激光雷达与imu对应着三个不同的坐标系，各个坐标系间相差一个旋转外参与平移外参，这些旋转外参与平移外参固定不变，并且不作为系统状态变量的一部分。本公开中，记与为激光雷达坐标系到imu坐标系的旋转外参与平移外参，为激光雷达坐标系到imu坐标系的旋转外参与平移外参，与为相机坐标系到imu坐标系的旋转外参与平移外参，
112.本公开中，将第一帧imu数据所对应的imu坐标系定义为世界坐标系(w)；
113.本公开中，系统状态变量可以包括如下之一或多项：imu坐标系到世界坐标系的旋转量与平移量、系统的速度、imu陀螺仪与加速度计零偏和世界坐标系下的重力加速度。本公开中，系统状态变量可以记为x，其中与是系统的imu坐标系到世界坐标系的旋转量与平移量，wv为系统的速度，bg与ba分别为imu陀螺仪与加速度计零偏，它们是由高斯噪声与驱动的随机游走，bg、ba、wg为世界坐标系下的重力加速度，系统状态变量在系统运行的每一时刻都在不断变化，因此当系统在不同时刻接收到imu帧、图像帧与激光帧时，各个imu帧、图像帧与激光帧都对应着自己的系统状态变量。
114.本公开中，当前图像帧是vio系统正在处理的图像帧，当前激光帧是lio系统正在处理的激光帧。
115.下文结合图1至图9对本公开的具体实施方式进行详细说明。
116.图1示出了本公开的一个实施方式的基于多传感器的紧耦合里程计方法的流程示意图。参见图1所示，一个实施方式中，基于多传感器的紧耦合里程计方法的示例性实现流
程s10可以包括如下步骤：
117.步骤s12，获取来自相机的图像帧、来自激光雷达的激光帧和来自imu的imu帧；
118.考虑到相机、激光雷达与imu的输出频率不同，同一时刻下不能同时接收到图像帧数据、激光帧数据与imu帧数据。因此，一些实施方式中，步骤s12还可以包括：对图像帧、激光帧和imu数据帧进行预处理，以使图像帧、激光帧和imu数据帧相对应。
119.一些实施方式中，对图像帧、激光帧和imu数据帧进行预处理，可以包括：将每一帧图像帧和每一帧激光帧分别根据各自的时间戳与imu帧关联。
120.一些实施方式中，可以将输出频率最高的imu帧根据时间戳的不同分别与图像帧与激光帧关联，这样，在接收到图像帧时，运行vio系统，处理图像帧和与图像帧相关联的imu帧；在接收到激光帧时，运行lio系统，处理激光帧和与激光帧相关联的imu帧。
121.一些实施方式中，图像帧与imu帧的关联方法和激光帧与imu帧的关联方法相同。
122.下面将图像帧与激光帧统称为观测帧，详述其与imu帧的关联方法。
123.图2示出了观测帧与imu帧的关联原理示意图。如图2所示，观测帧与imu帧的关联可以分为c1、c2、c3三种情况，这三种情况分别对应着三帧观测帧f1、f2、f3。一种实现方式中，观测帧与imu帧关联可以包括如下之一或多项：
124.c1：没有时间戳小于当前观测帧f1的imu帧时，可以将当前观测帧f1直接抛弃，并不进行后续步骤的处理。通常，类似f1这样的观测帧只会在系统刚开始运行时出现。
125.c2：没有时间戳小于当前观测帧f2的保留观测帧但有时间戳小于当前观测帧f2的imu帧时，保留观测帧f2，并进行后续步骤的处理。此时，可以将时间戳小于f2的所有imu帧和时间戳大于或等于f2的第一帧imu帧(也即，时间戳大于或等于f2的最邻近imu帧)共同作为f2的关联imu帧。如图2所示，观测帧f2位置处被虚线方框圈起来的三个imu帧便是f2的关联imu帧。和f1一样，类似f2这样的观测帧通常只会在系统刚开始运行时出现。
126.c3：有时间戳小于当前观测帧f3的保留观测帧f2时，保留观测帧f3，并进行后续步骤的处理。此时，寻找时间戳小于f3并且与f3时间戳最接近的观测帧，即寻找当前观测帧f3的最邻近观测帧，如图2所示，当前观测帧f3的最邻近观测帧为f2，然后将时间戳大于f2且时间戳小于f3的所有imu帧和时间戳大于或等于f3的第一帧imu帧(也即，时间戳大于或等于f3的最邻近imu帧)共同作为f3的关联imu帧，图2中f3位置处被虚线方框圈起来的imu帧便是f3的关联imu帧。通常，f3这样的观测帧在整个系统运行时最常见。
127.步骤s14，利用当前图像帧的预积分约束关系、当前图像帧的图像帧间约束关系和当前图像帧与激光帧间的约束关系更新系统状态变量；
128.一些实施方式中，当前图像帧是初始图像帧时，在步骤s14中或者步骤s14前需要执行系统初始化，系统初始化可以包括：利用初始图像帧对已接收到的所有激光帧中的激光点附加rgb值，同时获取初始图像帧的图像特征点。
129.整个系统初始化阶段要求系统处于静止状态，此时系统接收的所有激光帧与图像帧对应的系统状态变量都相同，因此，一些实施方式中，可直接根据相机与激光雷达间的外参将激光帧中的各个激光点投影至图像帧中，以为各个激光点附加rgb值，并且获取图像特征点。
130.本公开中，将系统初始化过程中成功获取图像特征点的图像帧称为初始图像帧，初始图像帧自动满足一个条件：在系统接收到初始图像帧以前已经有激光帧被接收。因为
只有当系统接收到激光帧后，才可以将激光帧的激光点投影至图像帧中完成初始化操作。
131.需要说明的是，系统初始化为可选步骤，仅在必要时执行。本公开中，只要有一帧图像帧成功获取图像特征点，初始化过程即可视为结束，此后的图像帧、激光帧均无需再执行系统初始化。
132.一些实施方式中，利用初始图像帧对已接收到的最邻近激光帧中的激光点附加rgb值，可以包括：根据相机与激光雷达间的外参，将系统初始化过程中已接收的所有激光帧中的激光点投影至初始图像帧以确定该激光点在初始图像帧中对应的像素坐标(即确定激光点的激光像素点)，将每个激光点在初始图像帧中对应的像素坐标处的rgb值附加至该激光点的数据中。
133.图3示出了获取初始图像帧的图像特征点的原理示意图。
134.若获取图像特征点时，直接选取每个图像块中质量最好的激光像素点作为图像特征点，虽然这样获得的图像特征点在图像帧中是均匀分布的，但图像特征点对应的激光点在空间中则不一定均匀分布。因此，在各个图像块中选取图像特征点时不仅要考虑各个激光像素点质量的优劣，而且还要考虑激光像素点所对应的激光点在空间中的分布情况。
135.一些实施方式中，获取初始图像帧的图像特征点，可以包括：步骤a1，根据相机与激光雷达间的外参将初始图像帧最邻近激光帧的激光点投影至初始图像帧中，以确定每个激光点在初始图像帧中对应的像素点，即，确定每个激光点的激光像素点，下文将激光点在初始图像帧中的对应像素点简称为激光像素点；步骤a2，将各个激光像素点划分到不同的图像块中；步骤a3，根据每个图像块中激光像素点的质量和激光像素点所对应激光点的空间位置选取激光像素点作为初始图像帧的图像特征点。如此，可以在获取图像特征点的同时均匀化图像特征点所对应激光点的空间分布和图像特征点在图像帧中的分布。
136.一些实施方式中，如图3所示，将各个激光像素点划分到不同图像块的过程可以包括：计算各个图像块(最开始时只存在一个图像块)中包含的所有激光像素点在x方向与y方向上坐标的方差，然后将图像块按照方差较大的方向进行等分，当图像块中只有一个激光像素点时，该图像块停止等分。最终，当图像块的数目达到所需图像特征点数目的阈值(例如，700个)时停止等分图像块。
137.一些实施方式中，也可以使用orb-slam2中的四叉树方法将激光像素点划分到不同的图像块中。
138.一些实施方式中，每个图像块中激光像素点的质量优劣可以根据以所述激光像素点为中心的图像块的像素梯度或图像熵值来判断。像素梯度是以激光像素点为中心的一个图像块中所有像素的梯度和，梯度越高激光像素点质量越好。图像熵值是以激光像素点为中心的一个图像块的熵值，熵值越高激光像素点的质量越好。
139.一些实施方式中，图像熵值可以通过下式(1)计算得到。
[0140][0141]
式(1)中，h为以激光像素点为中心的一个图像块的图像熵值，n为以激光像素点为中心的一个图像块所包含的像素点数目，下标i为像素点的序号，u为像素点坐标，i(
·
)表示u对应的像素点的灰度值，p(
·
)表示灰度值在图像块中出现的概率。如果图像块像素灰度值分布范围大，则熵值较大，认为图像块纹理信息丰富或图像像素梯度变化明显。如果图像块灰度值分布范围小，则熵值较小，认为该图像块无纹理信息或图像像素梯度变化不明
显。
[0142]
图4示出了使激光点空间分布均匀化的图像特征点选取方式的原理示意图。图4中，fc表示初始图像帧，初始图像帧的最邻近激光帧的激光点都分布在空间s中，p1、p2、p3为三个不同的激光像素点，a1、a2是根据图3所描述方法划分出来的不同的图像块。规定以各个图像特征点对应的激光点为中心的边长为5cm的正方体内不允许存在其他图像特征点对应的激光点。激光像素点p1为图像块a1中质量最好的激光像素点，且激光像素点p1不存在于任何以图像特征点对应的激光点为中心的边长为5cm的正方体内，所以p1被选为图像特征点，c1为以p1对应的激光点为中心的边长为5cm的正方体。在a2中，激光像素点p2的质量最好，但是p2对应的激光点位于c1中，而质量略低于p2的p3对应的激光点不存在于任何以图像特征点对应的激光点为中心的边长为5cm的正方体内，此时，不将p2选为图像特征点，而是将p3选为图像特征点。通过该方法获取图像帧的图像特征点，不仅图像特征点在图像帧中的分布是均匀的，而且图像特征点对应的激光点在空间中的分布也是均匀的。
[0143]
图5示出了一些实施方式的步骤s14的具体实现流程。如图5所示，步骤s14的具体实现流程可以包括如下步骤：
[0144]
步骤s52，通过当前图像帧关联的imu帧建立当前图像帧与上一观测帧间的预积分约束关系；
[0145]
具体地，根据当前图像帧关联的imu数据，确定当前图像帧对应的系统状态变量提供初值、以及当前图像帧与上一观测帧间的预积分约束关系；
[0146]
具体地，处理与当前图像帧相关联的imu数据，通过imu数据积分给当前图像帧对应的系统状态变量提供初值，并且根据当前图像帧关联的imu数据建立当前图像帧与上一观测帧间的预积分约束关系。本公开中，将当前图像帧与上一观测帧间的预积分约束关系记为r(x)。
[0147]
示例性地，imu数据的积分方法和当前图像帧与上一观测帧间的预积分约束建立方法与vins-mono论文中的相关描述一致。
[0148]
步骤s54，获取当前图像帧与上一图像帧间的图像特征点匹配对；
[0149]
一些实施方式中，可以通过光流追踪获得上一图像帧与当前图像帧间的特征点匹配对。当然在实际应用中，并不局限于通过光流追踪来获取帧间特征点匹配对，也可像svo的做法一样，通过直接法最小化特征点周围图像块的光度误差来获取图像帧间特征点匹配对。
[0150]
步骤s56，根据图像特征点匹配对，在上一图像帧与当前图像帧间构建优化问题，优化并更新系统状态变量。
[0151]
具体地，根据步骤s54获得的图像特征点匹配对构建当前图像帧与上一图像帧间的图像帧间约束关系，利用当前图像帧与上一观测帧间的预积分约束关系和图像帧间约束关系执行第一联合优化，以更新系统状态变量。
[0152]
一些实施方式中，当前图像帧的图像帧间约束关系包括：当前图像帧与上一图像帧间的图像特征点匹配对的重投影残差和图像特征点匹配对对应的激光点与其在当前图像帧上投影所得的激光像素点间的rgb残差。一些实施方式中，重投影残差可以通过式(2)确定，rgb残差可以通过式(3)确定。
[0153]
一些实施方式中，可以基于下式(2)～(3)，根据图像特征点匹配对构建上一图像
帧与当前图像帧间的残差方程：
[0154][0155]
o(x，wpi)＝o(wpi)-i(ui)i＝1、2...n(3)
[0156]
式(2)为图像特征点匹配对的重投影残差方程。式(2)中，n表示图像特征点匹配对的数目，下标i表示图像特征点匹配对的序号，u表示图像特征点匹配对中当前图像帧的图像特征点的像素坐标，wp表示图像特征点匹配对对应的激光点在世界坐标系下的坐标，k表示相机的内参矩阵，c(x，wpi，ui)为图像特征点匹配对的重投影残差，)为图像特征点匹配对的重投影残差，与是系统的imu坐标系到世界坐标系的旋转量与平移量，与为相机坐标系到imu坐标系的旋转外参与平移外参，为的转置，为的转置。
[0157]
式(3)为图像特征点匹配对对应的激光点与其在当前图像帧上投影所得的激光像素点间的rgb残差方程，wp的定义与式(2)中一致，ui表示图像特征点匹配对对应的激光点在当前图像帧上投影所得的激光像素点的像素坐标，o(
·
)表示wp对应的激光点被附加的rgb值，i(
·
)表示当前图像帧像素坐标u处的rgb值，o(x，wpi)为图像特征点匹配对对应的激光点与其在当前图像帧上投影所得的激光像素点间的rgb残差。
[0158]
一些实施方式中，步骤s56中，可以根据公式(2)～(3)两个残差方程构建的约束关系和当前图像帧与上一观测帧间的预积分约束关系，利用图优化的方法进行联合优化，以优化并更新系统状态变量。
[0159]
一些实施方式中，步骤s56中，可以以下式(4)所示函数为目标函数，根据公式(2)～(3)的残差方程与预积分约束关系进行联合优化，以优化并更新系统状态变量。
[0160][0161]
式(4)中，r(x)为当前图像帧与上一观测帧间的预积分约束关系，x为系统状态变量，c(x，wpi，ui)为图像特征点匹配对的重投影残差，o(x，wpi)为图像特征点匹配对对应的激光点与其在当前图像帧上投影所得的激光像素点间的rgb残差，wp表示图像特征点匹配对对应的激光点在世界坐标系下的坐标，ui表示图像特征点匹配对对应的激光点在当前图像帧上投影所得的激光像素点的像素坐标，n表示图像特征点匹配对的数目，下标i表示图像特征点匹配对的序号。
[0162]
步骤s58，在图像帧与激光帧间寻找约束关系构建优化问题，进一步优化并更新系统状态变量。
[0163]
一些实施方式中，可以根据步骤s56(即，第一联合优化)更新得到的系统状态变量和步骤s54得到的图像特征点匹配对构建当前图像帧与最邻近激光帧间的约束关系，利用当前图像帧与上一观测帧间的预积分约束关系和当前图像帧与最邻近激光帧间的约束关系执行第二联合优化，以再次更新系统状态变量。
[0164]
一些实施方式中，当前图像帧与激光帧间的约束关系包括：当前图像帧与最邻近激光帧间的3d点匹配对的三维坐标残差，3d点匹配对包括当前图像帧与上一图像帧间的图像特征点匹配对的三角化点和图像特征点匹配对对应的激光点。
[0165]
一些实施方式中，3d点匹配对的三维坐标残差可以通过式(5)确定。
[0166]
图6示出了在图像帧与激光帧间寻找约束关系构建优化问题的原理图。
[0167]
图6中，表示当前图像帧，表示的上一图像帧，与两帧间用虚线连接的图像特征点是步骤s32得到的图像特征点匹配对，一个图像特征点匹配对中的两个特征点对应同一激光点。
[0168]
通过s56优化更新后的系统状态变量，三角化与两帧间的图像特征点匹配对以确定三角化出来的点，三角化出来的点(即，与两帧间的图像特征点匹配对的三角化点)与图像特征点匹配对对应的激光点形成3d点匹配对，图6中用实线椭圆包裹住的点即为这种3d点匹配对。由于不同图像特征点匹配对对应的激光点可能来自于不同的激光帧，因此，通过三角化出来的点与图像特征点匹配对对应的激光点形成的3d点匹配对即可建立图像帧与激光帧间的联系。
[0169]
如果通过步骤s56优化更新后的系统状态变量精度很高，那么3d点匹配对间的距离应该很小，甚至为零。如果通过步骤s56优化更新后的系统状态变量精度不高，那么3d点匹配对间的间距就会比较大。因此，一些实施方式中，可通过3d点匹配对构建三维坐标残差方程，并根据残差方程构建的约束关系和当前图像帧与上一观测帧间的预积分约束关系，利用图优化的方法构建优化问题，优化并更新系统状态变量，提高系统状态变量的精度。
[0170]
一些实施方式中，3d点匹配对间的三维坐标残差方程可以表示为下式(5)。
[0171][0172]
式(5)中，n是3d点匹配对的数目，下标i是3d点匹配对的序号，wp是3d点匹配对中的图像特征点匹配对所对应激光点在世界坐标系下的坐标，cp是3d点匹配对中的图像特征点匹配对三角化出来的点在当前图像帧坐标系下的坐标，n(x，wpi，cpi)表示3d点匹配对间的三维坐标残差，与是系统的imu坐标系到世界坐标系的旋转量与平移量，与为相机坐标系到imu坐标系的旋转外参与平移外参。
[0173]
一些实施方式中，根据3d点匹配对的三维坐标残差方程构建的约束关系和当前图像帧与上一观测帧间的预积分约束关系执行联合优化时的目标函数可以为下式(6)：
[0174][0175]
式(6)中，n(x，wpi，cpi)表示3d点匹配对间的三维坐标残差，r(x)为当前图像帧与上一观测帧间的预积分约束关系，n是3d点匹配对的数目，下标i是3d点匹配对的序号，x为系统状态变量，wp是3d点匹配对中的图像特征点匹配对所对应激光点在世界坐标系下的坐标，cp是3d点匹配对中的图像特征点匹配对三角化出来的点在当前图像帧坐标系下的坐标。
[0176]
步骤s16，利用当前激光帧的预积分约束关系、当前激光帧的激光帧间约束关系和当前激光帧与图像帧间的约束关系更新系统状态变量；
[0177]
也即，步骤s1在激光帧间、激光与图像帧间寻找约束关系构建优化问题，优化并更新系统状态变量。
[0178]
图7示出了一些实施方式的步骤s16的具体实现流程。如图7所示，步骤s16的具体实现流程可以包括如下步骤：
[0179]
步骤s72，通过当前激光帧关联的imu帧建立当前激光帧与上一观测帧间的预积分约束关系；
[0180]
其中，上一观测帧为时间戳与当前激光帧最接近的上一图像帧或上一激光帧。
[0181]
一些实施方式中，步骤s72可以包括：根据当前激光帧关联的imu数据，确定当前激
光帧对应的系统状态变量提供初值、以及当前激光帧与上一观测帧的预积分约束关系。
[0182]
一些实施方式中，步骤s72的具体实现方式可以与步骤s52相同，不再赘述。
[0183]
步骤s74，获取当前激光帧的特征点匹配关系；
[0184]
一些实施方式中，步骤s74可以包括：提取当前激光帧的特征点，将当前激光帧提取的特征点通过kd-tree与多帧激光特征点的点云集进行搜索匹配，获得当前激光帧的特征点匹配关系。
[0185]
一些实施方式中，可以采用loam论文中的相关方式提取特征点，搜索匹配的策略也可以跟loam中一致。
[0186]
一些实施方式中，提取的特征点可以包括面特征点与线特征点。对于面特征点，搜索在多帧激光面特征点的点云集中离它最近的三个激光点；对于线特征点，可以搜索在多帧激光线特征点的点云集中离它最近的两个激光点。其中，多帧激光特征点的点云集由最近5帧激光帧维护更新。
[0187]
一些实施方式中，可以使用kd-tree进行特征点的搜索匹配。在实际运用中，kd-tree的实时更新会比较耗时，因此，还可使用实时更新效率较高的方法(例如，r3l1ve中运用对ikd-tree)来代替kd-tree。
[0188]
步骤s76，通过步骤s74中获得的激光特征点匹配关系，在激光帧间构建优化问题，优化并更新系统状态变量。
[0189]
一些实施方式中，步骤s76可以包括：根据当前激光帧的特征点匹配关系构建当前激光帧的激光帧间约束关系，利用当前激光帧的预积分约束关系和激光帧间约束关系执行第三联合优化，以更新系统状态变量。
[0190]
一些实施方式中，当前激光帧的激光帧间约束关系可以包括：当前激光帧的面特征点与其匹配的激光点所形成面的点面距离残差和当前激光帧的线特征点与其匹配的激光点所形成直线的点线距离残差。
[0191]
一些实施方式中，当前激光帧的激光帧间约束关系可以表示为下式(7)～(8)所示的残差方程。
[0192][0193][0194]
式(7)为当前激光帧的面特征点与它所匹配的距离最近的3个激光点所组成面的点面距离残差方程。式(7)中，n是面特征点的总个数，下标i是各个面特征点的序号，下标z是与面特征点匹配的3个距离最近激光点的序号(z＝1、2、3)，n是与面特征点匹配的3个距离最近激光点所组成面的法向量，是与各个面特征点匹配的距离最近3个激光点在世界坐标系下的坐标，
l
pi是各个面特征点在当前激光坐标系下的坐标。表示当前激光帧的面特征点与它所匹配的距离最近的3个激光点所组成面的点面距离残差。
[0195]
式(8)是当前激光帧的线特征点与它所匹配的距离最近的2个激光点所组成直线的点线距离残差方程。式(8)中，n是线特征点的总个数，与是与各个线特征点匹配的距离最近的2个激光点在世界坐标系下的坐标，下标i与
l
pi的定义与式(7)中定义相同，表示当前激光帧的线特征点与其匹配的距离最近的2个激光点所组成直线的点线距离残差。
[0196]
一些实施方式中，根据当前激光帧的特征点匹配关系构建的约束关系和当前激光帧与上一观测帧间的预积分约束关系执行联合优化时的目标函数可以为下式(9)：
[0197][0198]
式(9)中，r(x)为当前激光帧与上一观测帧间的预积分约束关系，x为系统状态变量，表示当前激光帧的线特征点与其匹配的距离最近的2个激光点所组成直线的点线距离残差，表示当前激光帧的面特征点与它所匹配的距离最近的3个激光点所组成面的点面距离残差，中的n为面特征点的总个数，中的n为线特征点的总个数。
[0199]
步骤s78，在当前激光帧与当前激光帧的最邻近图像帧间寻找约束关系并构建优化问题，以进一步优化与更新系统状态变量。
[0200]
一些实施方式中，步骤s78可以包括：根据第三联合优化更新得到的系统状态变量和当前激光帧的特征点匹配关系构建当前激光帧与最邻近图像帧间的约束关系，利用当前激光帧的预积分约束关系及其与最邻近图像帧间的约束关系执行第四联合优化，以再次更新系统状态变量，从而进一步提高系统状态变量的精度。
[0201]
一些实施方式中，当前激光帧与图像帧间的约束关系可以包括：当前激光帧与最邻近图像帧间的激光点匹配对的三维坐标残差，所述激光点匹配对包括当前激光帧中的第一激光点和当前激光帧的最邻近图像帧中第一图像特征点所对应的激光点，所述第一激光点投影至所述最邻近图像帧所获得的激光像素点与所述最邻近图像帧的第一图像特征点有重合。
[0202]
一些实施方式中，当前激光帧与最邻近图像帧间的激光点匹配对的三维坐标残差可以通过下文式(10)确定。
[0203]
图8示出了在当前激光帧与其最邻近图像帧间寻找约束关系构建优化问题的原理图。
[0204]
图8中，代表当前激光帧，代表上一激光帧，表示与当前激光帧时间戳最邻近的图像帧，表示的上一图像帧。图8中与两帧间用虚线连接的图像特征点是步骤s32得到的图像特征点匹配对。
[0205]
根据步骤s76优化得到的系统状态变量，将当前激光帧的激光点投影至寻找经过投影后与中的图像特征点有重合的当前帧激光点。由于每个图像特征点都有一个对应的激光点，因此，在找到经过投影后与图像特征点重合的当前帧激光点后，将图像特征点对应的激光点与投影后与图像特征点重合的当前帧激光点作为激光点匹配对并记录这种匹配关系。图8中用实线椭圆包裹住的点即为激光点匹配对。
[0206]
如图8所示，激光点匹配关系分为c1、c2、c3三种情况：
[0207]
c1中，激光点匹配对对应的图像特征点是中的图像特征点通过光流追踪得到的，所以该图像特征点对应的激光点可能不是观测到的，而是以前的激光帧观测到的。这种情况相当于通过当前激光帧的最邻近图像帧与甚至是以前的激光帧建立起了联系。
[0208]
c2中，激光点匹配点对对应的图像特征点是投影得到的，这种情况相当于通过
当前激光帧的最邻近图像帧与上一激光帧建立起了联系。
[0209]
c3中，多个当前帧激光点在投影后与同一个图像特征点重合，此时只保留与图像特征点对应的激光点距离最近的当前帧激光点与图像特征点对应的激光点间的匹配关系。
[0210]
如果通过步骤s76优化后的系统状态变量的精度很高，该步骤获得的激光点匹配对间的间距会很小，甚至趋近于零。如果通过步骤s76优化后的系统状态变量的精度不够高，激光点匹配对间的间距将会很大。因此，一些实施方式中，步骤s78中可通过激光点匹配对构建激光点间的三维坐标残差方程，并根据该激光点间的三维坐标残差方程构建的约束关系和当前激光帧与上一观测帧间的预积分约束关系，利用图优化的方法执行联合优化，以更新系统状态变量，从而进一步提高系统状态变量的精度。
[0211]
一些实施方式中，激光点匹配对间的三维坐标残差方程可以为式(10)所示的方程。
[0212][0213]
式(10)中，n为激光点匹配点对的数目，下标i为激光点匹配点对的序号，wpi与
l
pi为激光点匹配对，wpi为图像特征点对应的激光点，
l
pi为当前帧激光点，wpi位于世界坐标系中，
l
pi位于当前激光帧坐标系中。h(x，wpi，
l
pi)表示激光点匹配对间的三维坐标残差。
[0214]
一些实施方式中，步骤s78中根据激光点匹配对构建的约束关系和当前激光帧与上一观测帧间的预积分约束关系执行联合优化时的目标函数可以为下式(11)：
[0215][0216]
其中，r(x)为当前激光帧与上一观测帧间的预积分约束关系，x为系统状态变量，h(x，wpi，
l
pi)表示表示激光点匹配对间的三维坐标残差。
[0217]
步骤s18：基于步骤s14更新得到的系统状态变量，更新局部点云地图中激光点的rgb值和当前图像帧的图像特征点。
[0218]
一些实施方式中，可以根据步骤s14获得的系统状态变量，将局部点云地图中在当前图像帧视野范围内的激光点投影至当前图像帧中以确定当前图像帧中对应该激光点的像素坐标，将当前图像帧中对应该激光点的像素坐标处的rgb值附加给该激光点。通常，局部点云地图由最新处理完的几帧激光帧维护。
[0219]
一些实施方式中，如果局部点云地图中的一些激光点在处理当前图像帧以前的图像帧时就已经被附加了rgb值，可以将激光点已被附加的rgb值与当前图像帧中对应该激光点的像素坐标处的rgb值融合，并将该激光点附加的rg b值更新为融合得到的rgb值。例如，rgb值的融合方法可采用r3live论文中的相关公式描述的方法。
[0220]
更新当前图像帧的图像特征点的方法与前文系统初始化中获取初始图像帧中图像特征点的方法基本相同，不同在于在更新图像特征点之前当前图像帧中已存在从上一图像帧中通过光流法追踪过来的图像特征点，而没添加图像特征点之前初始图像帧是不存在图像特征点的。也就是说，一些实施方式中，在当前图像帧中添加图像特征点，可以包括：根据当前图像帧已存在的从上一图像帧中通过光流法追踪过来的图像特征点和当前图像帧最邻近激光帧在当前图像帧中的激光像素点，在当前图像帧中添加图像特征点。
[0221]
一些实施方式中，在当前图像帧中添加图像特征点，可以包括：先将当前图像帧最邻近激光帧的激光点投影至当前图像帧以获得当前图像帧最邻近激光帧在当前图像帧中的激光像素点，再将当前图像帧已存在的从上一图像帧中通过光流法追踪过来的图像特征
点和所述激光像素点划分到当前图像帧的不同图像块中，最后根据各个图像块中的图像特征点和激光像素点在当前图像帧的各个图像块中添加图像特征点。
[0222]
一些实施方式中，可以按照步骤s14中的划分方法将激光像素点与当前图像帧中已存在的从上一图像帧中通过光流法追踪过来的图像特征点一起划分到当前图像帧的不同图像块中。
[0223]
一些实施方式中，根据各个图像块中的图像特征点和激光像素点在当前图像帧的各个图像块中添加图像特征点，可以包括如下三种情况：
[0224]
情况一：当图像块中有一个已存在的图像特征点时，保留已存在的图像特征点，以该已存在的图像特征点作为所述图像块的当前图像特征点，也即，当图像块中只存在一个从上一图像帧中通过光流法追踪过来的图像特征点时，保留此图像特征点，并且不在该图像块中通过激光像素点添加图像特征点。
[0225]
情况二：当图像块中有多个已存在的图像特征点时保留多个已存在的图像特征点中质量最好的图像特征点，以该质量最好的图像特征点作为图像块的当前图像特征点，也即，当图像块中存在多个从上一图像帧中通过光流法追踪过来的图像特征点时，保留质量最好的图像特征点、删除其余的图像特征点，并且不在该图像块中通过激光像素点添加图像特征点。
[0226]
情况三：当图像块中没有已存在的图像特征点时根据图像块中激光像素点的质量和激光像素点所对应激光点的空间位置选取一个激光像素点作为图像块的图像特征点，也即，当图像块中不存在从上一图像帧中通过光流法追踪过来的图像特征点时，采用向初始图像帧中添加图像特征点的方法，通过激光像素点为当前图像帧添加图像特征点。
[0227]
步骤s110：根据步骤s16更新得到的系统状态变量将当前激光帧的激光点云数据合并到全局点云地图中；
[0228]
由于当前激光帧的所有激光点都位于激光坐标系下，所以根据s16得到的系统状态变量，可以将最新的激光点云数据(即，当前激光帧的激光点云)从当前激光帧坐标系转换到世界坐标系，然后将转换到世界坐标系下的当前激光帧的激光点云与全局点云地图融合，从而将当前激光帧的激光点云数据合并到全局点云地图中。
[0229]
图9是本公开的一个实施方式的采用处理系统的硬件实现方式的基于多传感器的紧耦合里程计装置的结构示意框图。
[0230]
该装置可以包括执行上述流程图中各个或几个步骤的相应模块。因此，可以由相应模块执行上述流程图中的每个步骤或几个步骤，并且该装置可以包括这些模块中的一个或多个模块。模块可以是专门被配置为执行相应步骤的一个或多个硬件模块、或者由被配置为执行相应步骤的处理器来实现、或者存储在计算机可读介质内用于由处理器来实现、或者通过某种组合来实现。
[0231]
该硬件结构可以利用总线架构来实现。总线架构可以包括任何数量的互连总线和桥接器，这取决于硬件的特定应用和总体设计约束。总线1200将包括一个或多个处理器1300、存储器1400和/或硬件模块的各种电路连接到一起。总线1200还可以将诸如外围设备、电压调节器、功率管理电路、外部天线等的各种其他电路1500连接。
[0232]
总线1200可以是工业标准体系结构(isa，industry standard architecture)总线、外部设备互连(pci，peripheral component)总线或扩展工业标准体系结构(eisa，
extended industry standard component)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，该图中仅用一条连接线表示，但并不表示仅有一根总线或一种类型的总线。
[0233]
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本公开的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本公开的实施方式所属技术领域的技术人员所理解。处理器执行上文所描述的各个方法和处理。例如，本公开中的方法实施方式可以被实现为软件程序，其被有形地包含于机器可读介质，例如存储器。在一些实施方式中，软件程序的部分或者全部可以经由存储器和/或通信接口而被载入和/或安装。当软件程序加载到存储器并由处理器执行时，可以执行上文描述的方法中的一个或多个步骤。备选地，在其他实施方式中，处理器可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行上述方法之一。
[0234]
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，可以具体实现在任何可读存储介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。
[0235]
就本说明书而言，“可读存储介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。可读存储介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(ram)，只读存储器(rom)，可擦除可编辑只读存储器(eprom或闪速存储器)，光纤装置，以及便携式只读存储器(cdrom)。另外，可读存储介质甚至可以是可在其上打印程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得程序，然后将其存储在存储器中。
[0236]
应当理解，本公开的各部分可以用硬件、软件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(pga)，现场可编程门阵列(fpga)等。
[0237]
本技术领域的普通技术人员可以理解实现上述实施方式方法的全部或部分步骤是可以通过程序来指令相关的硬件完成，程序可以存储于一种可读存储介质中，该程序在执行时，包括方法实施方式的步骤之一或其组合。
[0238]
此外，在本公开各个实施方式中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个可读存储介质中。存储介质可以是只读存储器，磁盘或光盘等。
[0239]
如图9所示，根据本公开的一些实施方式的基于多传感器的紧耦合里程计装置900可以包括：
[0240]
获取单元902，用于获取来自相机的图像帧、来自激光雷达的激光帧和来自imu的imu帧；
[0241]
第一状态更新单元904，用于利用当前图像帧的预积分约束关系、当前图像帧的图像帧间约束关系和当前图像帧与激光帧间的约束关系更新系统状态变量，所述当前图像帧的预积分约束关系通过当前图像帧关联的所述imu帧建立；
[0242]
第一地图更新单元906，用于基于所述第一状态更新单元更新得到的系统状态变量，更新局部点云地图中激光点的rgb值；
[0243]
图像特征点更新单元908，用于基于所述第一状态更新单元更新得到的系统状态变量，更新当前图像帧的图像特征点；
[0244]
第二状态更新单元910，用于利用当前激光帧的预积分约束关系、当前激光帧的激光帧间约束关系和当前激光帧与图像帧间的约束关系更新系统状态变量，所述当前激光帧的预积分约束关系通过所述当前激光帧关联的所述imu帧建立，所述当前激光帧与图像帧间的约束关系通过所述当前激光帧的激光点与最邻近图像帧的图像特征点确定；
[0245]
第二地图更新单元912，用于根据所述更新得到的系统状态变量将当前激光帧的激光点云数据合并到全局点云地图中。
[0246]
一些实施方式中，基于多传感器的紧耦合里程计装置900还包括：关联单元914，用于将所述图像帧和所述激光帧根据各自的时间戳分别与所述imu帧关联。
[0247]
一些实施方式中，图像特征点更新单元908，具体用于在当前图像帧为初始图像帧时，通过如下方式获取所述当前图像帧的图像特征点：根据相机与激光雷达间的外参将初始图像帧最邻近激光帧的激光点投影至初始图像帧中，以确定每个激光点的激光像素点；将各个激光像素点划分到不同的图像块中；根据每个图像块中激光像素点的质量和激光像素点所对应激光点的空间位置选取激光像素点作为初始图像帧的图像特征点。
[0248]
一些实施方式中，图像特征点更新单元908，具体用于在当前图像帧不是初始图像帧时，通过如下方式更新所述当前图像帧的图像特征点：根据所述相机与所述激光雷达间的外参，将所述当前图像帧最邻近激光帧的激光点投影至所述初始图像帧中，以确定每个所述激光点的激光像素点；将每个所述激光像素点和所述当前图像帧中已存在的图像特征点划分到所述当前图像帧的不同图像块中；根据每个所述图像块中激光像素点的质量、激光像素点所对应激光点的空间位置以及已存在的图像特征点的数量和质量，选取一个所述激光像素点或已存在的图像特征点作为所述图像块的当前图像特征点。
[0249]
一些实施方式中，激光像素点的质量优劣根据以所述激光像素点为中心的图像块的像素梯度或图像熵值判断。
[0250]
一些实施方式中，第一地图更新单元906，具体用于通过如下方式更新所述局部点云地图中激光点的rgb值：根据所述相机与所述激光雷达间的外参，将所述局部点云地图中在当前图像帧视野范围内的激光点投影至当前图像帧，以确定所述局部点云地图中在当前图像帧视野范围内的激光点对应的像素坐标；将所述当前图像帧中所述激光点对应的像素坐标处的rgb值附加给所述激光点。
[0251]
一些实施方式中，第一地图更新单元906，具体用于在当前图像帧不是初始图像帧
时通过如下方式更新所述局部点云地图中激光点的rgb值：根据所述相机与所述激光雷达间的外参，将所述局部点云地图中在当前图像帧视野范围内的激光点投影至当前图像帧，以确定所述局部点云地图中在当前图像帧视野范围内的激光点对应的像素坐标；将所述当前图像帧中所述激光点对应的像素坐标处的rgb值和所述激光点已附加的rgb值融合，并将所述激光点已附加的rgb值更新为融合得到的rgb值。
[0252]
一些实施方式中，第一状态更新单元904，具体用于通过如下方式更新系统状态变量：
[0253]
通过当前图像帧关联的所述imu帧建立当前图像帧与上一观测帧间的预积分约束关系，所述上一观测帧为时间戳与所述当前图像帧最接近的上一图像帧或上一激光帧；
[0254]
获取当前图像帧与上一图像帧间的图像特征点匹配对；
[0255]
根据所述图像特征点匹配对构建当前图像帧与上一图像帧间的图像帧间约束关系，利用当前图像帧与上一观测帧间的预积分约束关系和图像帧间约束关系执行第一联合优化，以更新系统状态变量；
[0256]
根据所述第一联合优化更新得到的系统状态变量和所述图像特征点匹配对构建当前图像帧与最邻近激光帧间的约束关系，利用所述当前图像帧与上一观测帧间的预积分约束关系和当前图像帧与最邻近激光帧间的约束关系执行第二联合优化，以再次更新所述系统状态变量。
[0257]
一些实施方式中，当前图像帧的图像帧间约束关系包括：当前图像帧与上一图像帧间的图像特征点匹配对的重投影残差和图像特征点匹配对对应的激光点与其在当前图像帧上投影所得的激光像素点间的rgb残差。
[0258]
一些实施方式中，当前图像帧与激光帧间的约束关系包括：当前图像帧与最邻近激光帧间的3d点匹配对的三维坐标残差，所述3d点匹配对包括当前图像帧与上一图像帧间的图像特征点匹配对的三角化点和所述图像特征点匹配对对应的激光点。
[0259]
一些实施方式中，第二状态更新单元910，具体用于通过如下方式更新系统状态变量：
[0260]
通过当前激光帧关联的所述imu帧建立当前激光帧与上一观测帧间的预积分约束关系，所述上一观测帧为时间戳与所述当前激光帧最接近的上一图像帧或上一激光帧；
[0261]
获取当前激光帧的特征点匹配关系；
[0262]
根据所述当前激光帧的特征点匹配关系构建当前激光帧的激光帧间约束关系，利用当前激光帧的所述预积分约束关系和所述激光帧间约束关系执行第三联合优化，以更新系统状态变量；
[0263]
根据所述第三联合优化更新得到的系统状态变量和所述当前激光帧的特征点匹配关系构建当前激光帧与最邻近图像帧间的约束关系，利用当前激光帧的所述预积分约束关系及其与最邻近图像帧间的约束关系执行第四联合优化，以再次更新所述系统状态变量。
[0264]
一些实施方式中，当前激光帧的激光帧间约束关系包括：当前激光帧的面特征点与匹配激光点所形成面的点面距离残差和当前激光帧的线特征点与匹配激光点所形成直线的点线距离残差。
[0265]
一些实施方式中，当前激光帧与图像帧间的约束关系包括：当前激光帧与最邻近
图像帧间的激光点匹配对的三维坐标残差；其中，所述激光点匹配对包括当前激光帧中的第一激光点和当前激光帧的最邻近图像帧中第一图像特征点所对应的激光点；其中，所述第一激光点投影至所述最邻近图像帧所获得的激光像素点与所述最邻近图像帧的第一图像特征点有重合。
[0266]
本公开还提供了一种电子设备，包括：存储器，存储器存储执行指令；以及处理器或其他硬件模块，处理器或其他硬件模块执行存储器存储的执行指令，使得处理器或其他硬件模块执行上述基于多传感器的紧耦合里程计方法。
[0267]
实际应用中，电子设备可应用于车辆、物流车、机器人或其他场景。
[0268]
本公开还提供了一种可读存储介质，可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现上述基于多传感器的紧耦合里程计方法。
[0269]
在本说明书的描述中，参考术语“一个实施方式/方式”、“一些实施方式/方式”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施方式/方式或示例描述的具体特征、结构、材料或者特点包含于本技术的至少一个实施方式/方式或示例中。在本说明书中，对上述术语的示意性表述不必须的是相同的实施方式/方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施方式/方式或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施方式/方式或示例以及不同实施方式/方式或示例的特征进行结合和组合。
[0270]
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本技术的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
[0271]
本领域的技术人员应当理解，上述实施方式仅仅是为了清楚地说明本公开，而并非是对本公开的范围进行限定。对于所属领域的技术人员而言，在上述公开的基础上还可以做出其它变化或变型，并且这些变化或变型仍处于本公开的范围内。