一种用于视觉-惯性-激光融合的同步定位与建图方法与流程

本发明涉及机器人视觉、多传感器融合、同步定位与建图(SLAM)等技术领域，具体涉及多传感器融合的同步定位与建图方法。

背景技术：

同步定位与建图(SLAM)是一种机器人在未知环境中估计自身运动并且建立周围环境地图的技术。它在无人机、自动驾驶、移动机器人导航、虚拟现实和增强现实等领域有着广泛的应用。为了提高SLAM在不同环境下的定位精度和鲁棒性，人们做了大量的研究。由于SLAM通过安装在机器人上的传感器来感知自身的运动和周围的环境，许多研究试图通过集成多模态传感器来提高运动估计的性能。随着科技的发展，SLAM中常用的传感器体积越来越小，价格越来越低。这使得在一个设备上携带多个传感器，以可接受的价格和体积更好地进行定位和建图成为可能。

技术实现要素：

为了克服现有单传感器SLAM精度不高，在退化环境中容易发生丢失的问题，本发明提出了一种一种用于视觉-惯性-激光融合的同步定位与建图方法，结合来自相机、惯性测量单元(IMU)、激光雷达的测量数据实现高精度、高鲁棒性的带有回环和邻近检测及全局位姿优化的SALM系统。

为了解决上述的技术问题，本发明的技术方案如下：

一种用于视觉-惯性-激光融合的同步定位与建图方法，包括以下步骤：

1)视觉-惯性-激光里程计，过程如下：

假设相机内参以及三个传感器之间的外参已知，并且传感器之间经过时间同步，相机和激光雷达具有相同的频率；涉及到四个坐标系分别是世界坐标系W，相机坐标系C，惯性测量单元(IMU)坐标系I和激光雷达坐标系L；传感器坐标系C、I、L随着设备的运动而改变，Ci表示在ti时刻的相机坐标系；定义完成初始化后的激光雷达坐标系为世界坐标系；

首先，使用紧耦合的视觉惯性里程计方法估计相邻相机帧之间的相对运动，然后使用获得的动作近似注册这期间接收的点云，接着使用扫描匹配方法进一步优化估计的动作；

根据激光雷达的特性，激光点云被持续性地接收，并注册到坐标系Li中，其中ti是点i的接收时间，Li是ti时刻的激光雷达坐标系；定义tk为第k帧激光扫描的起始时间，tk+1为第k帧激光扫描的结束时间；令为在第k帧激光扫描期间接收到的点云集合；由于传感器经过时间同步，从视觉惯性里程计中得到tk到tk+1期间IMU的相对位姿变换使用激光雷达和IMU之间的外参，计算得到tk到tk+1期间激光雷达的相对位姿变换

其中是激光雷达和IMU之间的外参；

基于设备在一帧激光扫描期间以匀速运动的假设，使用线性插值获取从第k帧激光扫描的起始时刻到点i接收时的相对位姿变换：

然后将点i注册到Lk+1坐标系下：

其中是点i在坐标系Li中的齐次坐标；是点i在坐标系Lk+1中的齐次坐标；将点集中的所有点按照上式的方式投影到Lk+1坐标系下得到新的点集Pk；

根据点局部表面的平滑度从Pk中提取特征点，特征点分为边缘特征点和平面特征点；令i为Pk中的一点，且S是由激光雷达返回的同一次扫描中与点i连续的点的集合；定义点i局部表面的平滑度为：

其中|S|表示集合S中点的数量，边缘特征点有较小的平滑度，平面特征点有较大的平滑度；定义提取的特征点点集为Fk；

tk+1时刻预测的世界坐标系下的激光雷达位姿为：

其中是经过扫描匹配优化过的tk时刻激光雷达在世界坐标系下的位姿，而带有波浪标注的是没有经过扫描匹配优化的位姿或者变换；

使用估计的转换将Fk中的特征点投影到世界坐标系下；将投影后的特征点跟地图进行匹配，找到边缘特征点对应的边缘线，以及平面特征点对应的平面块；然后计算边缘特征点到边缘线的距离，以及平面特征点到平面块的距离，距离函数的形

式如下：

将Fk中所有点到各自对应的匹配的距离和写成下面的非线性函数：

通过上面的式子，将扫描匹配转换为一个非线性优化问题，并通过最小化d求解；通过高斯-牛顿法求解这个问题，得到优化后的tk+1时刻的雷达位姿最后使用优化后的变换将第k帧激光扫描中接收到的点云注册到世界坐标系下。

2)鲁棒性增强处理，过程如下：

当设备处于缺少结构化信息的环境中时，扫描匹配会失效；当设备处于弱纹理环境中或者发生剧烈运动等情况时，视觉惯性里程计会失效；

当其中一种情况发生时，系统会自动调整剩余可工作模块承担起定位和建图工作；

在视觉惯性里程计模块失效的情况下，该模块不再输出动作近似，相反地，它会发送一个信号使得扫描到扫描匹配的模块开始工作；在该模块中，首先从当前帧的点云中提取特征点，然后在上一帧的点云中找到与其匹配的边缘线或平面块，然后计算特征点到它的匹配的距离，最后通过最小化所有特征点到他们匹配的距离和来恢复两帧之间的相对运动；

当处于缺少结构化信息的场景中时，无法提取到足够的几何特征，因此无法有效优化视觉惯性里程计输出的动作近似；在这种情况下，视觉惯性里程计的输出将跳过扫描匹配模块，而直接用于将点云注册到地图中；当设备回到正常的环境当中，所有模块会逐渐恢复正常工作；

3)回环及邻近检测，过程如下：

回环和邻近检测用于移除里程计误差的累积，从而进一步提高定位精度；回环检测是基于视觉外观的，而邻近检测是基于激光扫描的；

为了进行回环和邻近检测，需要维护一个关键帧数据库，每个关键帧包括当前帧在世界坐标系中的位姿、对应的图片以及点云；

基于外观的回环检测是通过DBoW2实现的，受到相机视角的限制，只有当设备处于相同或相近位置且朝向相同时才能够检测到回环；由于本方法采用的3D激光雷达具有360度的视角，邻近检测不受到朝向的限制，也就是说只要设备处于相同或相近的位置，就有可能检测到邻近；

邻近检测的过程如下，先遍历关键帧数据库，根据关键帧在世界坐标系下的位姿计算每个关键帧和当前关键帧之间的相对位姿；如果计算得到平移的距离小于R1，使用之前计算的相对位姿作为先验，然后用扫描匹配进一步更新两者之间的相对位姿；如果更新后两个关键帧之间的距离小于R2，R2＜R1，则认为检测到了邻近；其中R1，R2为经验值；如果超过一个关键帧满足这个要求，则选择较早的作为邻近帧，以最大程度地移除误差累积；为了减少不必要地约束，当前关键帧的前n个关键帧不用来进行回环和邻近检测；

4)六自由度的全局优化，过程如下：

当检测到回环或者邻近后，添加回环边或者邻近边到位姿图中，然后进行6自由度的位姿优化；每个关键帧对应位姿图中的一个顶点，存在下面三种边连接顶点：

4-1)序列化边

一个顶点与它前面的若干个顶点之间通过序列化边进行连接；序列化边的值为两个顶点之间的相对位姿变换，通过视觉-惯性-激光里程计直接获得；连接顶点i和顶点j的序列化边的值为：

其中和分别为从里程计直接获得的关键帧i在世界坐标系下的位置和朝向；

4-2)回环边

如果顶点有回环连接，该顶点会通过回环边和回环顶点建立连接；回环边拥有如上式所定义的6自由度的相对位姿变换；它的值通过pnp求解得到；

4-3)邻近边

如果顶点存在邻近连接，邻近边会在该顶点和邻近顶点之间建立连接，邻近边的值通过点云到点云的匹配得到；

帧i和帧j之间的边的误差为：

整个位姿图的序列化边、回环边和邻近边通过最小化下面的代价函数优化：

argmin(q，t)||∑(i，j)∈Ari，j+∑(i，j)∈Bri，j+∑(i，j)∈Cri，j|| (4.3)

其中A是序列化边的集合，B是回环边的集合，C是邻近边的集合；随着时间的增加，位姿图的大小不断增加，由于位姿图的优化时间和图的大小成正相关，降采样过程被用于限制关键帧数据库的大小；在降采用过程中，具有回环或者邻近约束的关键帧被保留下来，其余与周围关键帧距离太近的关键帧则有可能会被移除。

本发明的有益效果为：

附图说明

图1是整个系统的结构框图。

图2是扫描匹配模块的结构框图。

图3是视觉惯性里程计模块失效时，系统的结构框图。

图4是扫描匹配模块失效时，系统的结构框图。

图5是邻近检测的示意图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1～图5，一种用于视觉-惯性-激光融合的同步定位与建图方法，包括以下步骤：

1)视觉-惯性-激光里程计，过程如下：

假设相机内参以及三个传感器之间的外参已知，并且传感器之间经过时间同步，相机和激光雷达具有相同的频率；本方法涉及到四个坐标系，分别是世界坐标系W，相机坐标系C，惯性测量单元(IMU)坐标系I和激光雷达坐标系L；传感器坐标系C、I、L随着设备的运动而改变，Ci表示在ti时刻的相机坐标系；定义完成初始化后的激光雷达坐标系为世界坐标系；

首先，使用紧耦合的视觉惯性里程计方法估计相邻相机帧之间的相对运动，然后使用获得的动作近似注册这期间接收的点云，接着使用扫描匹配方法进一步优化估计的动作；

扫描匹配模块的步骤如图2所示；根据激光雷达的特性，激光点云被持续性地接收，并注册到坐标系Li中，其中ti是点i的接收时间，Li是ti时刻的激光雷达坐标系；定义tk为第k帧激光扫描的起始时间，tk+1为第k帧激光扫描的结束时间；令为在第k帧激光扫描期间接收到的点云集合；由于传感器经过时间同步，从视觉惯性里程计中得到tk到tk+1期间IMU的相对位姿变换使用激光雷达和IMU之间的外参，计算得到tk到tk+1期间激光雷达的相对位姿变换

其中是激光雷达和IMU之间的外参；

基于设备在一帧激光扫描期间以匀速运动的假设，使用线性插值获取从第k帧激光

扫描的起始时刻到点i接收时的相对位姿变换：

然后将点i注册到Lk+1坐标系下：

其中是点i在坐标系Li中的齐次坐标；是点i在坐标系Lk+1中的齐次坐标；将点集中的所有点按照上式的方式投影到Lk+1坐标系下得到新的点集Pk；

根据点局部表面的平滑度从Pk中提取特征点，特征点分为边缘特征点和平面特征点；令i为Pk中的一点，且S是由激光雷达返回的同一次扫描中与点i连续的点的集合；定义点i局部表面的平滑度为：

其中|S|表示集合S中点的数量，边缘特征点有较小的平滑度，平面特征点有较大的平滑度；定义提取的特征点点集为Fk；

tk+1时刻预测的世界坐标系下的激光雷达位姿为：

其中是经过扫描匹配优化过的tk时刻激光雷达在世界坐标系下的位姿，而带有波浪标注的是没有经过扫描匹配优化的位姿或者变换；

使用估计的转换将Fk中的特征点投影到世界坐标系下；将投影后的特征点跟地图进行匹配，找到边缘特征点对应的边缘线，以及平面特征点对应的平面块；然后计算边缘特征点到边缘线的距离，以及平面特征点到平面块的距离，距离函数的形式如下：

将Fk中所有点到各自对应的匹配的距离和写成下面的非线性函数：

通过上面的式子，将扫描匹配转换为一个非线性优化问题，并通过最小化d求解；通过高斯-牛顿法求解这个问题，得到优化后的tk+1时刻的雷达位姿最后使用优化后的变换将第k帧激光扫描中接收到的点云注册到世界坐标系下。

2)鲁棒性增强处理，过程如下：

当设备处于缺少结构化信息的环境中时，扫描匹配会失效；当设备处于弱纹理环境中或者发生剧烈运动等情况时，视觉惯性里程计会失效；

当其中一种情况发生时，系统会自动调整剩余可工作模块承担起定位和建图工作；

在视觉惯性里程计模块失效的情况下，该模块不再输出动作近似，相反地，它会发送一个信号使得扫描到扫描匹配的模块开始工作，如图3所示；在该模块中，首先从当前帧的点云中提取特征点，然后在上一帧的点云中找到与其匹配的边缘线或平面块，然后计算特征点到它的匹配的距离，最后通过最小化所有特征点到他们匹配的距离和来恢复两帧之间的相对运动；

当处于缺少结构化信息的场景中时，无法提取到足够的几何特征，因此无法有效优化视觉惯性里程计输出的动作近似；在这种情况下，视觉惯性里程计的输出将跳过扫描匹配模块，而直接用于将点云注册到地图中，如图4所示；当设备回到正常的环境当中，所有模块会逐渐恢复正常工作；

3)回环及邻近检测，过程如下：

回环和邻近检测用于移除里程计误差的累积，从而进一步提高定位精度；回环检测是基于视觉外观的，而邻近检测是基于激光扫描的；

为了进行回环和邻近检测，需要维护一个关键帧数据库，每个关键帧包括当前帧在世界坐标系中的位姿、对应的图片以及点云；

基于外观的回环检测是通过DBoW2实现的，受到相机视角的限制，只有当设备处于相同(或相近)位置且朝向相同时才能够检测到回环；由于本方法采用的3D激光雷达具有360度的视角，邻近检测不受到朝向的限制，也就是说只要设备处于相同或(相近的)位置，就有可能检测到邻近；

邻近检测的过程如下，先遍历关键帧数据库，根据关键帧在世界坐标系下的位姿计算每个关键帧和当前关键帧之间的相对位姿；如果计算得到平移的距离小于R1，使用之前计算的相对位姿作为先验，然后用扫描匹配进一步更新两者之间的相对位姿；如果更新后两个关键帧之间的距离小于R2，(R24R1)，则认为检测到了邻近；其中R1，R2为经验值；如果超过一个关键帧满足这个要求，则选择较早的作为邻近帧，以最大程度地移除误差累积；为了减少不必要地约束，当前关键帧的前n个关键帧不用来进行回环和邻近检测；

4)六自由度的全局优化，过程如下：

当检测到回环或者邻近后，添加回环边或者邻近边到位姿图中，然后进行6自由度的位姿优化；每个关键帧对应位姿图中的一个顶点，存在下面三种边连接顶点：

4-1)序列化边

一个顶点与它前面的若干个顶点之间通过序列化边进行连接；序列化边的值为两个顶点之间的相对位姿变换，通过视觉-惯性-激光里程计直接获得；连接顶点i和顶点j的序列化边的值为：

其中和分别为从里程计直接获得的关键帧i在世界坐标系下的位置和朝向；

4-2)回环边

如果顶点有回环连接，该顶点会通过回环边和回环顶点建立连接；回环边拥有如上式所定义的6自由度的相对位姿变换；它的值通过pnp求解得到；

4-3)邻近边

如果顶点存在邻近连接，邻近边会在该顶点和邻近顶点之间建立连接，邻近边的值通过点云到点云的匹配得到；

帧i和帧j之间的边的误差为：

整个位姿图的序列化边、回环边和邻近边通过最小化下面的代价函数优化：

argmin(q，t)||∑(i，j)∈Ari，j+∑(i，j)∈Bri，j+∑(i，j)∈Cri，j|| (4.3)

其中A是序列化边的集合，B是回环边的集合，C是邻近边的集合；随着时间的增加，位姿图的大小不断增加，由于位姿图的优化时间和图的大小成正相关，降采样过程被用于限制关键帧数据库的大小；在降采用过程中，具有回环或者邻近约束的关键帧被保留下来，其余与周围关键帧距离太近的关键帧则有可能会被移除。