CN115355901A - 一种融合动态目标感知的多机联合建图方法 - Google Patents

Abstract

本发明公开了一种融合动态目标感知的多机联合建图方法，属于三维视觉领域。首先，采用基于YOLACT和DeepSORT的多目标跟踪算法，在机器人端对连续采集的RGB图像帧进行潜在动态目标跟踪，并结合光流法分析目标运动状态，进而剔除动态特征点；其次，基于图像帧中的静态特征点和IMU数据，采用紧耦合的视觉惯性里程计进行相机位姿跟踪，并投影关键帧上的静态特征点到三维空间，从而增量式构建单机的静态子地图；再次，在服务器端接收子地图数据和关键帧数据以重建子地图，并基于词袋模型进行不同子地图之间的回环检测；最后，基于位姿图融合算法在服务器端实现全局一致的相机位姿估计和地图融合。本发明提升了多机协同视觉联合建图方法在动态场景下的鲁棒性，实现了全局一致的地图构建。

Description

一种融合动态目标感知的多机联合建图方法

技术领域

本发明属于三维视觉领域，涉及到一种基于视觉传感器和IMU传感器，并结合动态目标感知技术进行图像预处理的多机联合建图方法。

背景技术

高精度的地图信息是移动机器人进行路径规划等任务的基础，近年来，随着建图技术日趋成熟，该技术越来越多的被应用于救援、勘探、自动驾驶、虚拟现实领域。尽管目前的建图技术在无人巡检、环境建模等领域表现良好，但主要以单机建图方面的研究为主，难以直接应用于建图效率要求高的作业场景。

多机协同的建图技术能够实现多机间信息共享,加速全局地图融合速度，并通过机器人间数据的传输优化单机的位姿估计。由于此类技术具有效率高、环境适应能力强的优点，近年来基于激光雷达或视觉的多机协同系统不断涌现，并在环境纹理信息丰富的结构化环境中表现出优越性能。

基于激光雷达的多机联合建图系统相对精确，误差模型简单，但激光雷达价格昂贵，难以大规模应用；视觉传感器具有价格低廉、获取环境纹理信息丰富的特点，在重定位和场景分类上具有巨大优势；惯性导航算法具有定位频率高、不易受环境干扰等优点，但长时间运行易产生漂移，融合视觉和惯性传感器的优势，视觉惯性融合技术逐渐成为建图领域的研究热点。

现有的基于视觉惯性传感器的联合建图技术多注重单机位姿的优化和多机间计算资源的分配，然而，在实际应用中依旧面临诸多挑战：动态干扰环境下的精确协同定位、多机产生的大量数据如何高效利用和管理等。

如图1所示，一种存在动态目标干扰的小型室内工厂场景，该场景中动态目标随机运动造成的遮挡会干扰到图像静态特征点的匹配，动态特征点匹配的数量过多也会造成错误的位姿估计，从而降低地图构建的精度；同时，多个机器人数据的传输和管理也对多机协同建图系统的灵活性提出挑战。因此，如何解决以上问题，是实现机器人在此类场景中高效、准确的联合建图的关键问题。

发明内容

有鉴于此，本发明提出了一种融合动态目标感知的多机视觉联合建图方法，能够克服动态干扰，提升多机视觉联合建图系统在动态场景下的适应性。本发明首先采用一种融合动态目标感知的方法进行图像帧预处理，实现对动态目标的跟踪，并通过剔除动态特征点，提升后续单机地图构建精度；其次，在机器人端运行基于视觉惯性传感器的单机定位建图模块，实现机器人端位姿估计和单机子地图增量式更新；最后，通过WiFi上传更新后的单机子地图和更新后的关键帧数据到服务器端，在服务器端重新恢复子地图，并采用词袋模型进行回环检测，完成子地图的优化和不同机器人之间的位姿关联，进而生成全局地图。

本发明提出一种融合动态目标感知的多机联合建图方法，包括：

步骤1：融合动态目标感知的图像预处理阶段，该阶段连续采集RGB图像帧作为输入，完成对图像帧数据中动态干扰对象(设定为行人)的实例分割和持续跟踪，ORB特征点提取，静态和动态特征点的区分，并将图像帧和静态特征点传输到下一阶段。

步骤1.1：通过机器人上的相机连续获取RGB图像帧序列；基于深度学习神经网络框架YOLACT，对图像帧中的潜在的随机动态干扰对象进行实例分割和预测框提取，并对图像帧中分割的随机动态干扰对象覆盖掩膜；动态干扰对象由图像帧中的像素级的实例分割表示来表征；

步骤1.2：提取图像帧中ORB特征点，计算特征点的描述子；

步骤1.3：基于步骤1.2提取的ORB特征点视觉特征，使用光流法提取图像帧中的动态特征点，若提取的动态特征点在步骤1.1中动态干扰对象实例分割掩膜范围内，则将其判断为表征随机动态干扰对象的动态特征点，ORB特征点视觉特征点中除了表征随机动态干扰对象的动态特征点的其他特征点即为静态特征点。

步骤2：利用机器人端连续获取的IMU传感器数据和预处理后的图像数据，在机器人端运行视觉惯性里程计实现当前时刻机器人位姿估计，同时运行稀疏点云地图重建模块，利用机器人当前更新的关键帧Ki的位姿，将当前关键帧中的静态特征点映射到子地图中，实现单机子地图增量式构建，同时将新增的关键帧和新增的子地图数据通过WiFi传输到服务器端。

步骤2.1：持续接收IMU和预处理后图像数据，并将预处理后的图像帧和IMU数据时间戳同步，其中，预处理后图像数据指原始图像帧以及静态特征点；从预处理后的图像帧中选取匹配到足够数量静态特征点的两个连续图像帧为初始关键帧K₀和K₁，三角化两个初始关键帧上的静态特征点，进而初始化子地图；

步骤2.2：根据时间戳同步后的图像帧和IMU数据，构建视觉惯性里程计，估计机器人位姿；

步骤2.3：运行稀疏点云构建模块，根据图像帧间的共视关系更新关键帧，并利用更新后关键帧Ki上的静态特征点和2.2估计得到的机器人位姿信息生成新的地图点，将新的地图点映射到三维的子地图中，不断更新子地图；

步骤2.4：将更新后的关键帧和更新后的子地图数据上传服务器；

步骤3：服务器端全局一致的位姿关联和联合建图，该阶段用于接收机器人端上传的子地图和关键帧数据，在服务器端重新恢复子地图，并采用词袋模型进行回环检测，完成子地图的优化和不同机器人之间的位姿关联，生成全局地图。

步骤3.1：持续接收机器人端的位姿和关键帧数据，在服务器端重新恢复子地图；

步骤3.2：采用词袋模型进行回环检测，将上传的关键帧数据和服务器端保存的关键帧序列进行匹配，若当前上传的关键帧，与匹配到的、服务器中保存在关键帧序列中的关键帧，均在当前关键帧所在的子地图内，则采用BA方法对子地图进行地图点和关键帧位姿的优化；若匹配到的关键帧不在当前上传关键帧所在的子地图内，则计算两个匹配关键帧之间的三维变换关系，通过该三维变换关系，将子地图转换到同一坐标系下，生成全局地图，采用BA方法进行全局地图关键帧位姿优化和地图点优化；

步骤3.3：对3.2生成的全局地图进行冗余关键帧和地图点的剔除，遍历全局地图范围内的所有关键帧，根据特征点的共视冗余度剔除冗余的关键帧和重复的地图点，运行全局BA方法，优化并更新全局地图和机器人位姿。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1是本发明所涉及的动态场景；

图2是本发明的整体框架示意图；

图3是本发明多目标跟踪流程；

图4是本发明为动态目标跟踪结果；

图5是本发明视觉惯性里程计算法流程；

图6是本发明多机联合地图构建结果。

具体实施方式

以下给出一个或多个方面的主要概述以应对这些方面的基本理解。此概述不能将所有构想的方面进行详述，其唯一的目的是要以简化形式给出一个或多个方面的一些概念以为稍后给出的更加详细的描述之序。注意，以下结合附图和具体实施例描述的诸方面仅是示例性的，而不应被理解为对本发明的保护范围进行任何限制。

图2展示本发明多机联合建图系统框架，其中将EuRoC数据集中的MH01和MH02作为两个机器人端采集到的数据序列，一个台式机作为服务器端，两台笔记本电脑作为机器人端，且均安装Ubuntu 18.04、ROS Melodic。机器人端和服务器端之间，数据均通过WiFi传输。一种融合动态目标感知的多机联合建图方法主要分三个阶段：融合动态目标感知的图像预处理阶段、机器人端位姿估计和单机子地图维护阶段、服务器端全局一致的位姿关联和联合建图阶段。

步骤1：融合动态目标感知的图像预处理阶段运行如下若干步骤：基于机器人端连续获取的RGB图像帧数据，针对动态场景中随机的动态干扰对象，假设潜在的随机动态干扰对象为行人，对每帧图像均进行图像预处理用于分割并跟踪动态干扰对象；提取ORB视觉特征点，并区分动态视觉特征点和静态视觉特征点，通过剔除动态特征点，提升后续定位与建图的精度。

步骤1.1：通过机器人上的相机连续获取RGB图像帧序列，对图像帧中的潜在的随机动态干扰对象进行实例分割和目标跟踪，具体为：

基于深度学习神经网络框架YOLACT，对图像帧中的潜在的随机动态干扰对象进行实例分割和预测框提取，并对图像帧中的随机动态干扰对象的分割结果覆盖掩膜；其中，动态干扰对象由图像帧中的像素级的实例分割结果表示来表征；

对提取到动态干扰对象的预测框采用DeepSORT进行目标跟踪，对预测到的行人目标进行跟踪并生成对应的行人ID，算法流程如图3所示；

步骤1.2：提取图像帧中ORB特征点，并采用BRIEF特征描述算法计算特征点的描述子；

步骤1.3：基于步骤1.2提取的ORB视觉特征，采用光流法和步骤1.1所述的随机动态目标实例分割相结合的方法，提取动态特征点并剔除，具体为：

使用光流法提取图像帧中的动态特征点；

若提取的动态特征点在步骤1.1中动态干扰对象实例分割掩膜范围内，则将其判断为表征随机动态干扰对象的动态特征点，ORB视觉特征点中除了表征随机动态干扰对象的动态特征点的其他特征点即为静态特征点；

将原始图像帧和静态特征点数据输入单机定位与建图模块。

步骤2：机器人端相机位姿估计和单机子地图维护阶段运行如下若干步骤：输入预处理后的图像数据和IMU传感器采集的线性加速度和角速度数据，运行视觉惯性里程计跟踪相机位姿，同时运行稀疏点云地图重建模块，利用机器人当前更新的关键帧的位姿，将当前关键帧中的静态特征点映射到子地图中，实现单机子地图的增量式构建，并通过光束平差优化算法优化子地图和机器人位姿；同时将更新后的子地图数据和更新后的关键帧数据上传服务器端。

其中，机器人端向服务器端传输的更新后的关键帧数据和子地图数据包括：新增的关键帧K_i，新增的的地图点系列L_i。机器人端以固定的频率分批传输到服务器端。

步骤2.1：持续接收IMU和预处理后图像数据，并将当前接收到第i个的图像帧I_i和IMU数据时间戳同步，其中，预处理后图像数据指原始图像帧以及静态特征点；从预处理后的图像帧中选取匹配到足够数量静态特征点的两个连续图像帧为初始关键帧，三角化静态特征点，进而初始化子地图；

步骤2.2：根据时间戳同步后的图像帧和IMU数据，构建视觉惯性里程计(如图5所示)，对当前图像帧I_i和参考关键帧K_j构建视觉重投影误差模型，并对当前图像帧I_i和参考关键帧K_j时间范围内的IMU数据进行预积分，构建IMU误差模型，通过构建非线性优化函数联合优化视觉重投影误差和IMU误差，对当前帧机器人位姿进行估计，具体为：

构建当前图像帧I_t和参考关键帧K_j的视觉重投影误差模型，根据匹配到的特征点集合x(这里x是当前图像帧I_t和参考关键帧K_j匹配到的所有特征点的集合)，通过最小化特征点集合x的重投影误差，计算图像帧I_t的位姿。视觉的重投影误差模型，定义如下：

其中x_k是第k个地图点在当前图像帧像素坐标系下匹配到的特征点的坐标，∑为求和符号,

则是第k个地图点在当前图像帧相机坐标系C下的3D坐标(坐标系坐标)，ρ()是鲁棒核函数，

是当前图像帧相机坐标系下第k个地图点根据相机位姿(包括相机的旋转和平移)，映射到像素坐标系下的投影坐标。

对当前图像帧I_i和参考关键帧K_j时间范围内的IMU数据进行预积分，根据陀螺仪的输出建立IMU坐标系，通过加速度计的输出计算得到机器人载体自身的位姿、速度信息，构建IMU的预积分计算值和测量值之差的残差模型；其中，IMU器件包含加速度计和陀螺仪，可以实时测量载体相对惯性坐标系的线性加速度和角速度，并通过积分计算出机器人自身的位姿和速度信息。

通过视觉特征匹配点对的重投影误差和IMU误差构建非线性优化函数，以优化机器人的位姿，速度，和IMU漂移。视觉与IMU联合优化的整体的估计状态向量：

其中优化函数第一部分为当前第i帧图像帧的视觉残差项∑E_proj(j)，表示特征点在相机下地图点投影产生的残差，第二部分是当前第i帧图像帧和参考关键帧j之间的IMU运动模型的残差，表示两帧图像之间IMU产生的残差，θ包含当前的机器人的旋转，平移，速度，IMU偏置信息，使用最小二乘法实现非线性优化，进而通过得到的θ获得机器人位姿。

步骤2.3：运行稀疏点云构建模块，根据图像帧间的共视关系更新关键帧，并利用新增的关键帧上的静态特征点和步骤2.2估计得到的机器人位姿信息生成新的地图点，将新的地图点映射到三维的子地图中，不断更新子地图；

关键帧更新策略：根据图像帧间的共视关系判断当前帧是否可作为关键帧插入前j个关键帧的关键帧序列，具体的，当基于当前帧I_i观测到的特征点L_i数量少于参考关键帧K_j的共视特征点数量的90％，或者距离插入上一帧关键帧K_j已经跟踪了20个图像帧的位姿而没有插入关键帧，则当前帧可作为关键帧插入前j个关键帧的关键帧序列。

步骤2.4：通过WiFi上传更新后的子地图数据和关键帧数据到服务器端。

步骤3：服务器端全局一致的位姿关联和联合建图，该阶段接收机器人端上传的更新后的子地图数据和更新后的关键帧数据，完成子地图的优化和不同机器人之间的位姿关联和子地图融合，生成全局地图。

其中，服务器端管理的数据内容主要包括：各个机器人端上传的子地图序列M＝{M₀,M₁,…,M_n,}(恢复的子地图中包含地图点和关键帧序列，关键帧与关键帧之间的IMU约束，关键帧和地图点之间的观测模型)、一个全局地图(包含一系列的关键帧数据和地图点，以及关键帧与关键帧之间的IMU约束，关键帧和地图点之间的观测模型)。

步骤3.1：服务器端通过WiFi接收机器人端上传的更新后的关键帧和子地图数据，并在服务器端重新恢复子地图，其中，接收机器人端的数量不做限制，允许其在任务期间动态加入，并为每个新连接的机器人在服务器端恢复新加入的子地图。

步骤3.2：采用基于视觉特征的词袋模型进行回环检测，将上传的关键帧数据和服务器端保存的历史关键帧序列K＝{K₀,K₁,…,K_n,}进行匹配，完成子地图的回环检测及优化，或者不同子地图之间的地图融合，生成全局地图。具体为：

若当前上传的关键帧K_i与匹配到服务器端保存的关键帧序列的关键帧K_j，均在当前关键帧所在的子地图M_c内，则采用BA方法对子地图进行地图点和关键帧位姿的优化；

若匹配到的关键帧K_j不在当前上传关键帧K_i所在的子地图M_c内，则计算两个匹配关键帧之间的三维变换关系T_ij，通过该三维变换关系，将子地图M_c和关键帧K_j所在的子地图M_f转换到同一坐标系下，生成全局地图，采用BA方法进行全局地图关键帧位姿优化和地图点优化；

步骤3.3：对3.2生成的全局地图进行冗余关键帧和地图点的剔除，遍历全局地图范围内的所有关键帧，根据特征点的共视冗余程度(关键帧中有90％的地图点在其他关键帧中可以找到)剔除冗余的关键帧，和重复的地图点(将地图点投影到关键帧，通过特征匹配方式选取被观测次数多的地图点)，运行全局BA方法，优化并更新全局地图和机器人位姿。

定期剔除全局地图中多余的关键帧，并执行全局BA优化降低相机的累积漂移，这里每个全局地图中的关键帧KF_k的冗余值计算公式为：

其中

是关键帧KF_k观测到的地图点，函数τ(obs(LM))计算每个地图点LM的观测次数，观察次数越多，冗余值越大，若冗余值超过阈值，则判断该关键帧为多余的关键帧，并在全局地图的关键帧序列中删除该关键帧及与其关联的地图点，最后执行全局BA优化降低全局地图和机器人位姿的累积漂移。

图6为系统生成的全局一致性地图。

为了使得文章的解释更简单化，已上述的图文描述为一系列步骤，但是应该理解并领会，这些方法不受操作的次序所限制，因为按照一个或多个步骤进行实施，一些动作可按不同的顺序发生，但本领域技术人员可以理解其动作发生的原理。

尽管已对本发明说明性的具体实施方式逐步进行了描述，以便于本技术领域的技术人员能够进行领会，但是本发明不仅限于具体实施方式的范围，本领域技术人员可以在权利要求的范围内做出各种变形或修改，只要各种变化只要在所附的权利要求限定和确定的本发明精神和范围内。

Claims (4)

1.一种融合动态目标感知的多机联合建图方法，其特征在于，该方法包括如下步骤：

步骤1：融合动态目标感知的图像预处理阶段，该阶段连续采集RGB图像帧作为输入，完成对图像帧数据中动态干扰对象的实例分割和持续跟踪，ORB特征点提取，静态和动态特征点的区分，并将图像帧和静态特征点传输到下一阶段；

步骤2：利用机器人端连续获取的IMU传感器数据和预处理后的图像数据，在机器人端运行视觉惯性里程计实现当前时刻机器人位姿估计，同时运行稀疏点云地图重建模块，利用机器人当前更新的关键帧Ki的位姿，将当前关键帧中的静态特征点映射到子地图中，实现单机子地图增量式构建，同时将新增的关键帧和新增的子地图数据通过WiFi传输到服务器端；

步骤3：服务器端全局一致的位姿关联和联合建图，该阶段用于接收机器人端上传的子地图和关键帧数据，在服务器端重新恢复子地图，并采用词袋模型进行回环检测，完成子地图的优化和不同机器人之间的位姿关联，生成全局地图。

2.根据权利要求1所述的一种融合动态目标感知的多机联合建图方法，其特征在于，步骤1进一步包括如下步骤：

步骤1.1：通过机器人上的相机连续获取RGB图像帧序列；基于深度学习神经网络框架YOLACT，对图像帧中的潜在的随机动态干扰对象进行实例分割和预测框提取，并对图像帧中分割的随机动态干扰对象覆盖掩膜；动态干扰对象由图像帧中的像素级的实例分割表示来表征；

步骤1.2：提取图像帧中ORB特征点，计算特征点的描述子；

步骤1.3：基于步骤1.2提取的ORB特征点，使用光流法提取图像帧中的动态特征点，若提取的动态特征点在步骤1.1中动态干扰对象实例分割掩膜范围内，则将其判断为表征随机动态干扰对象的动态特征点，ORB特征点中除了表征随机动态干扰对象的动态特征点的其他特征点即为静态特征点。

3.根据权利要求1所述的一种融合动态目标感知的多机联合建图方法，其特征在于，步骤2进一步包括如下步骤：

步骤2.1：持续接收IMU和预处理后图像数据，并将预处理后的图像帧和IMU数据时间戳同步，其中，预处理后图像数据指原始图像帧以及静态特征点；从预处理后的图像帧中选取匹配到足够数量静态特征点的两个连续图像帧为初始关键帧K₀和K₁，三角化两个初始关键帧上的静态特征点，进而初始化子地图；

步骤2.2：根据时间戳同步后的图像帧和IMU数据，构建视觉惯性里程计，估计机器人位姿；

步骤2.3：运行稀疏点云构建模块，根据图像帧间的共视关系更新关键帧，并利用更新后关键帧K_i上的静态特征点和2.2估计得到的机器人位姿信息生成新的地图点，将新的地图点映射到三维的子地图中，不断更新子地图；

步骤2.4：将更新后的关键帧和更新后的子地图数据上传服务器。

4.根据权利要求1所述的一种融合动态目标感知的多机联合建图方法，其特征在于，步骤3进一步包括如下步骤：

步骤3.1：持续接收机器人端的位姿和关键帧数据，在服务器端重新恢复子地图；

步骤3.2：采用词袋模型进行回环检测，将上传的关键帧数据和服务器端保存的关键帧序列进行匹配，若当前上传的关键帧，与匹配到的、服务器中保存在关键帧序列中的关键帧，均在当前关键帧所在的子地图内，则采用BA方法对子地图进行地图点和关键帧位姿的优化；若匹配到的关键帧不在当前上传关键帧所在的子地图内，则计算两个匹配关键帧之间的三维变换关系，通过该三维变换关系，将子地图转换到同一坐标系下，生成全局地图，采用BA方法进行全局地图关键帧位姿优化和地图点优化；

步骤3.3：对3.2生成的全局地图进行冗余关键帧和地图点的剔除，遍历全局地图范围内的所有关键帧，根据特征点的共视冗余度剔除冗余的关键帧和重复的地图点，运行全局BA方法，优化并更新全局地图和机器人位姿。

Images