CN113345020A - 一种动态场景下的即时定位方法、装置、设备和存储介质 - Google Patents

Abstract

本发明提供一种动态场景下的即时定位方法、装置、设备和存储介质，所述方法通过RGB‑D相机获取场景视频数据，通过5G通信将视频数据传输到云端进行处理，并反馈定位数据至终端；数据处理阶段，先进行图像帧筛选；在筛选出的图像帧中进行匹配点筛选，得到静态区域的特征匹配点对和三维空间点信息；通过参考帧模型或恒速度模型对位姿进行跟踪，并结合BA优化的方式调整位姿；对RGB图像进行语义分割，将分割得到的动态物体对应的深度信息从该帧深度图中移除，进行动态场景的静态三维稠密地图构建。本发明既可实现移动机器人在动态场景下的精准定位，又可避免引入过多的冗余信息，同时，通过5G通信及云端处理技术，使该系统可应用于低算力的终端设备。

Description

一种动态场景下的即时定位方法、装置、设备和存储介质

技术领域

本发明涉及AGV技术领域，具体的说是一种动态场景下的即时定位方法、装置、设备和存储介质。

背景技术

近年来，一些室内外服务机器人逐渐出现在日常生活中。例如，扫地机器人能够自动准确地定位，并通过高效地路径规划完成房间的清理；安保巡检机器人能够实现在人机共存的广场上自动巡视和安全检查，为人们的出行提供安全保障；Google的无人驾驶车辆能够在视觉传感器和全球定位系(Global Positioning System，GPS)的帮助下，建立起高精度地图以实现自动驾驶功能。这些相关领域蓬勃发展的背后均离不开一项重要的技术：即时定位与地图构建(Simultaneous Localization and Mapping,SLAM)，其中由于低成本易获取和丰富的信息数据等特点，基于视觉信息的SLAM技术(V-SLAM)尤其得到视。

同时，机器人即时定位与地图构建(SLAM)技术也是实现移动机器人自主运动的核心技术之一。当人类进入到陌生环境时，首先会观察并理解环境进而得到一些信息，通过这些信息来确定自身和环境中障碍物的位置，然后得到环境中可以通行的区域。对于机器人而言，SLAM技术就是让它们利用传感器去感知环境，建立环境的地图模型，并在运动过程中实现自身的定位。这些定位信息和环境地图，使后续高效的路径规划和动作决策成为可能。

目前大部分的SLAM技术均基于一个重要假设：环境是静态的。但是，真实的环境却是动态的，比如视野中移动的行人、门的开闭状态、可移动物体的不同位置。在动态环境下，移动的物体会给定位和地图构建带来极大的噪声和误差，甚至造成定位失败。例如，使用单目视觉进行定位时，当移动物体在视野中所占比例较大时，容易造成定位误差过大，如图1所示，较多的匹配点对落在动态物体上，会使定位误差逐渐累积。

普通的特征点法SLAM系统，一般采用连续的两帧图像进行特征匹配，以实现系统的初始化和位姿跟踪。但是当场景中存在动态物体时，由于相机帧率较高，对于连续的两帧图像，运动物体并没有发生明显的移动，难以消除动态物体的影响。另外，对于非连续的两帧图像，传统的匹配方式会造成大量的误匹配。连续图像帧及非连续图像帧的匹配区别如图2所示。

图2中A和B为场景中的部分运动区域，C为场景中的静态区域。图2(a)描述了连续图像帧之间的特征匹配。由于动态区域移动较小，因此A，B和C区域均能得到较好的特征匹配，但是会造成SLAM系统累计误差持续增大，影响定位精度。图2(b)为非连续图像帧之间的特征匹配。由于动态区域移动较大，使A和B区域的特征匹配发生错误，同样会影响系统定位精度。

发明内容

针对现有技术中在动态环境下普遍存在实时性较差和定位误差较大的不足之处，本发明要解决的技术问题是提供一种动态场景下的即时定位方法、装置、设备和存储介质。

本发明为实现上述目的所采用的技术方案是：一种动态场景下的即时定位与地图构建方法，包括以下步骤：

获取视频数据，从视频数据中进行图像帧筛选；

在筛选出的图像帧中进行匹配点筛选，得到静态区域的特征匹配点对和三维空间点信息；

通过参考帧模型或恒速度模型对位姿进行跟踪，并结合BA优化的方式调整位姿；

对所述筛选出的图像帧进行语义分割，将分割得到的动态物体对应的深度信息从该帧深度图中移除，进行动态场景的静态三维稠密地图构建。

所述从所述视频数据中进行图像帧筛选，具体为：

选取首帧图像与第n帧图像作为预筛选出的图像帧，其中，n为自然数，x-1为窗隔大小，且x≤8；

对该两帧图像进行区块划分，选取区块运动较大的两帧图像作为最终筛选结果，并持续更新图像窗口；

所述在筛选出的图像帧中进行匹配点筛选，包括以下步骤：

在筛选出的图像帧中，统计前后两帧图像中对应图像区块的匹配得分；

依据图像区块中特征匹配数量，设定判决阈值对网格区块中特征匹配是否正确进行判断，仅保留正确的匹配区块。

所述统计前后两帧图像中对应图像区块的匹配得分，具体为：

s＝|X|-1

其中s表示两帧图像中对应区域得分，|X|表示该区域中特征匹配点对的数量。

所述对网格区块中特征匹配是否正确进行判断，

其中，

表示网格对{i_k,j_k}中对应的特征点匹配数量，根据运动的平滑性及特征点匹配的一致性，s_ij值越大，表示网格A中特征匹配的正确性越高，反之，则正确性越低；

设置阈值t来对网格A中的特征匹配是否正确进行判断：

其中，α为常量，n表示表示以A为中心的网格中特征匹配的数量。

所述通过参考帧模型或恒速度模型对位姿进行跟踪，并结合BA优化的方式调整位姿，包括以下步骤：

优先使用恒速度模型进行位姿跟踪，若跟踪失败则采用参考帧模型进行跟踪，得到当前相机所在位置的绝对位姿；

对绝对位姿的计算结果执行局部BA优化，调整当前帧的位姿及地图点。

所述动态场景的静态三维稠密地图构建，具体为：

相机坐标系下的三维点云坐标(x,y,z)为：

其中，d与深度图在像素坐标(u,v)的值成正相关，f_x,f_y为相机在x,y方向上的焦距，c_x,c_y为像素坐标系相对于成像平面的平移量。

一种动态场景下的即时定位装置，包括：

数据获取及传输模块，用于获取RGB-D相机捕获到的视频数据，并通过5G通信发送到云端；

图像帧筛选模块，用于从所述视频数据中进行图像帧筛选；

特征点匹配模块，用于在筛选出的图像帧中进行匹配点筛选，得到静态区域的特征匹配点对和三维空间点信息；

跟踪模块，用于通过参考帧模型或恒速度模型对位姿进行跟踪，并结合BA优化的方式调整位姿；

地图构建模块，用于对所述图像帧筛选模块筛选出的图像帧进行语义分割，将分割得到的动态物体对应的深度信息从该帧深度图中移除，进行动态场景的静态三维稠密地图构建。

一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行上述技术方案所述的动态场景下的即时定位方法的步骤。

一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行上述技术方案所述的动态场景下的即时定位方法的步骤。

本发明具有以下优点及有益效果：

1、本发明既可确保图像中的动态物体出现显著运动，又可避免引入过多的冗余信息。并且，在为局部地图添加新的地图点时，能够排除场景中动态物体的影响。

2、本发明在定位过程中，相比于ORB-SLAM2及DynaSLAM所使用的方法，能够在实时性和定位精度上均有所提升。

3、本发明在场景重建过程中，使用Mask R-CNN检测运动物体，根据所提供的掩膜信息消除动态目标。

4、本发明基于5G通信技术进行数据传输，并结合云端处理技术进行数据处理，极大降低了对终端设备性能需求，可应用范围广、拓展性强。

附图说明

图1为动态物体对特征点匹配影响示意图；

图2为图像帧之间的特征匹配示意图；(a)连续帧匹配，(b)非连续帧匹配；

图3为本发明的SLAM算法流程图；

图4为本发明的图像区块划分流程图；

图5为本发明的图像网格划分示意图；(a)首帧图像，(b)第x帧图像；

图6为本发明的重投影误差示意图；

图7为实施本发明方法的硬件安装示意图；

图8为实施本发明方法的系统设计框图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

如图3所示，一种动态场景下的即时定位与地图构建方法，包括以下步骤：包括以下步骤：

S1、获取视频数据，从所述视频数据中进行图像帧筛选。

所有图像帧集合表示为F＝{f₁,f₂,...,f_N}，固定窗口图像帧集合为W＝{f₁,f₂,...,f₈}，其中

为消除动态物体的影响，引入自适应窗隔匹配模型。该方法通过帧间筛选策略将窗口中首帧图像与第x(x≤8)帧图像进行特征匹配，x-1为窗隔大小，窗口大小设置为固定值8。

选取窗口W中两帧图像f₁、f_x提取均匀分布的ORB特征点，并使用暴力匹配法获得初步的匹配关系。然后进行图像区域划分，算法主要流程如图4。该算法将图像帧划分为16*16大小的像素块，以f_x图像左上角像素块为起点，在f₁图像中执行匹配搜索，搜索步长设置为8，优先选择含匹配角点的像素块。匹配正确之后以该像素块为基准，在横向和纵向依次进行匹配扩展，逐渐扩大区域范围，且该区域块能够占据图像区域的绝大部分，则视为静态区域块，记作T区域。匹配搜索过程中检测到不连续区块，则将其单独标记，并对该区域进行局部扩展，且该区域占据图像范围较小，视其为动态区域块，记为F区域。实际应用中，设定T区域必定出现且唯一，F区域可为0个或者多个。

得到F及T区域的对应关系后，采用最小二乘法对F区域及T区域求对应的平移量分别为t_F及t_T，当0≤||t_F||₂≤0.2||t_T||₂或1.8||t_T||₂≤||t_F||₂≤3||t_T||₂，则表明该两帧图像符合筛选条件。

S2、采用基于网格的概率运动统计方式进行匹配点筛选，获得静态区域的特征匹配点对和三维空间点信息。

S2-1、在筛选出的图像帧中，统计前后两帧图像中对应图像区块的匹配得分。

根据GMS(Grid-based motion statistics)特征匹配算法的原理，有下式：

s＝|X|-1

其中s表示两帧图像中对应区域得分，|X|表示该区域中特征匹配点对的数量，如果一个特征匹配是正确的，那么它的周围依然存在其它的特征匹配点对，此时s的值较大。

对于连续的两个图像帧，如图2(a)所示。区域A、B、C中均可得到较多的匹配点对，故使得动态区域A、B造成假得分的情况。假设A，B和C区域中的特征匹配点对得分为s_A，s_B，s_C，那么有公式：

s_A≈s_B≈s_C

在这种情况下，无法通过设定合适的阈值将动态区域(区域A和B)和静态区域(区域C)的特征匹配区分开。故GMS算法不适用于场景中动态物体移动较小的情况。

本发明使用改进的GMS特征匹配算法来获得高质量的匹配点对，该算法是一种高效且实时的匹配点筛选算法，其基于运动网格统计模型对原始的特征匹配进行筛选，得到静态区域的特征匹配。

以图2(b)为例，静态区域C有较高匹配得分。但是，区域A和B受到动态物体的影响发生了明显变化，使得这些区域的特征匹配出现较大的错误，s_A和s_B的值很小，则有公式：

s_A≈s_B＜＜

这样就可以通过设置合适的阈值，消除动态物体上特征匹配点的影响，解决假得分的现象获得静态区域的特征匹配点集S。

S2-2、依据图像区块中特征匹配数量，设定判决阈值对网格区块中特征匹配是否正确进行判断，仅保留正确的匹配区块。

为提高特征匹配点对的筛选速度，将图像进行网格划分。由网格的可分性度量值

可知，P正比于网格数量K及网格区域中特征点数量M，且K与M互补。故综合考虑特征匹配点对的筛选速度和可分精度，采用增大网格数量K的方式，即缩小网格的大小，将其设置为10*10。然后，直接判断每个网格中所有特征匹配点对的正确性，如图5所示。

图5为经过窗隔匹配筛选的两帧图像，(a)为窗隔模型中的首帧图像，(b)为第x帧图像，在以网格A为中心的3*3网格中，统计左右图像对中区域A的匹配得分s_ij，其定义如下：

其中，

表示网格对{i_k,j_k}中对应的特征点匹配数量，根据运动的平滑性及特征点匹配的一致性，s_ij值越大，表示网格A中特征匹配的正确性越高，反之，则正确性越低。此时，设置阈值t来对网格A中的特征匹配是否正确进行判断。

其中，α为常量(一般设置为6)，n表示表示以A为中心的3*3的网格中特征匹配的数量。

对于静态区域，如图5中的白色方框所示，网格A可以获得较高的匹配得分s_ij。而对如图5中的黑色方框所示的动态区域，由于存在运动物体，使得左图像提取的特征点无法在右图像中找到对应的匹配点，甚至会在右图像中找到错误的匹配点，所以该区域的匹配得分值s_ij低。因此，由上式可根据区域内匹配特征点的数量设置自适应阈值。若s_ij≥t，则区域匹配正确，保留匹配点；若s_ij＜t，则区域匹配错误，舍弃匹配点。通过自适应阈值判断匹配点对可以剔除动态区域的误匹配，得到静态区域的匹配点对，提高全局的匹配正确率。

S3、通过参考帧模型或恒速度模型对位姿进行跟踪，并结合BA优化的方式调整位姿。

S3-1、优先使用恒速度模型进行位姿跟踪，若跟踪失败则采用参考帧模型进行跟踪，得到当前相机所在位置的绝对位姿。

对于参考帧模型，设置最新的关键帧作为参考帧，对参考帧和当前帧进行窗隔匹配，如果参考帧匹配的特征点在局部地图中存在对应的三维点，就将这些三维点投影到当前帧。将上一帧的位姿信息作为当前帧位姿的初始值，根据BA优化得到当前帧的位姿信息与观测到的三维地图点。与参考帧模型不同的是，恒速模型假设两帧之间的运动与之前两帧相同，以此来设定BA优化的位姿初始值。

S3-2、对绝对位姿的计算结果执行局部BA优化，调整当前帧的位姿及地图点。

本发明使用自适应窗隔匹配模型进行系统的初始化，初始化成功后，得到当前帧的位姿信息，并把当前帧设置为关键帧和参考帧。当新的图像帧创建后，得到n组匹配成功的静态点对，其对应于三维空间点P_i，p_i1、p_i2(i∈n)为该点在参考帧和当前帧中的投影，p_i2为在初始位姿下空间点P_i在当前帧中的投影，e_i为重投影误差，如图6所示。然后，则可通过BA优化方式，即最小化重投影误差求解相机位姿。引入李代数中ξ表示相机位姿，有如下关系：

其中^运算符号表示将李代数元素转换为变换矩阵。该点重投影误差可表示为：

e_i＝p_i2-exp(ξ^)p_i1

把n组匹配点的误差项平方求和，构建最小二乘问题，并使其最小化：

使用列文伯格-马夸尔特优化算法求解上式，可求得最优相机位姿ξ^*。

S4、用Mask R-CNN进行图像的语义分割，将分割得到的动态物体对应的深度信息从该帧的深度图中移除，进行动态场景的静态三维稠密地图构建。

采用Mask R-CNN对图像帧进行语义分割。使用在COCO数据集上训练的深度模型，该模型可以分割常见的运动物体(例如人、自行车、汽车、火车、卡车、鸟、马、羊等)。Mask R-CNN算法的输入是原始的RGB图像，输出图像中包含矩形选择框、类别标签以及实例的掩膜区域。然而，在本系统的应用中仅需要用到实例的掩膜信息，因此在算法中移除了关于类别预测的模块。

由于Mask R-CNN是在Faster R-CNN的框架下进行设计的，因此相对于其它语义分割算法具有较快的预测速度，在GPU设备上预测COCO数据集上的一张图片的语义信息仅需200ms。为了提高系统运行速度，将语义分割部分作为一个单独的线程运行。假设输入图像大小为m×n×3，那么输出矩阵的大小为m×n×l，其中l表示图像中目标的数目。对于图像的每一个通道进行语义分割，最终得到单帧图像中所有的动态目标。

利用深度学习算法得到动态物体的掩膜信息，根据对应关系可移除动态物体在深度图中的深度数据，进而得到的关键帧位姿信息进行静态三维稠密地图构建。

为了保持系统的运行速度，将可视化点云信息界面作为一个单独的线程运行，当图像帧为关键帧时，将此时的深度图和RGB图像结合相机内部参数恢复得到相机坐标系下的三维点云坐标(x,y,z)，如下式所示：

其中，d与深度图在像素坐标(u,v)的值成正相关，f_x,f_y为相机在x,y方向上的焦距，c_x,c_y为像素坐标系相对于成像平面的平移量，由相机自身决定，可以用相机标定的方式得到。

在静态地图重建中，单独开一个线程执行重建，缓存接收的重建数据，确保系统实时持续运行。

如图7所示，实施本发明方法所设计的AGV硬件装配示意图，无须外部辅助材质，在动态场景中依靠RGB-D相机1采集到的数据便可实现精确定位及地图构建，并且通过微型PC2及5G无线通信组件3可实现相机数据的发送、定位及建图数据的接收，提高了设备的应用灵活性。

如图8所示，实施本发明方法的系统设计框图，采用底层终端处理与云端算法处理相结合的设计，使整个系统设计高内聚、低耦合，提高了稳定性。

Claims (10)

1.一种动态场景下的即时定位方法，其特征在于，包括以下步骤：

获取视频数据，从视频数据中进行图像帧筛选；

在筛选出的图像帧中进行匹配点筛选，得到静态区域的特征匹配点对和三维空间点信息；

通过参考帧模型或恒速度模型对位姿进行跟踪，并结合BA优化的方式调整位姿；

对所述筛选出的图像帧进行语义分割，将分割得到的动态物体对应的深度信息从该帧深度图中移除，进行动态场景的静态三维稠密地图构建。

2.根据权利要求1所述的一种动态场景下的即时定位方法，其特征在于，所述从所述视频数据中进行图像帧筛选，具体为：

选取首帧图像与第n帧图像作为预筛选出的图像帧，其中，n为自然数，x-1为窗隔大小，且x≤8；

对该两帧图像进行区块划分，选取区块运动较大的两帧图像作为最终筛选结果，并持续更新图像窗口。

3.根据权利要求1所述的一种动态场景下的即时定位方法，其特征在于，所述在筛选出的图像帧中进行匹配点筛选，包括以下步骤：

在筛选出的图像帧中，统计前后两帧图像中对应图像区块的匹配得分；

依据图像区块中特征匹配数量，设定判决阈值对网格区块中特征匹配是否正确进行判断，仅保留正确的匹配区块。

4.根据权利要求3所述的一种动态场景下的即时定位方法，其特征在于，所述统计前后两帧图像中对应图像区块的匹配得分，具体为：

s＝|X|-1

其中s表示两帧图像中对应区域得分，|X|表示该区域中特征匹配点对的数量。

5.根据权利要求3所述的一种动态场景下的即时定位方法，其特征在于，所述对网格区块中特征匹配是否正确进行判断，

其中，

表示网格对{i_k,j_k}中对应的特征点匹配数量，根据运动的平滑性及特征点匹配的一致性，s_ij值越大，表示网格A中特征匹配的正确性越高，反之，则正确性越低；

设置阈值t来对网格A中的特征匹配是否正确进行判断：

其中，α为常量，n表示表示以A为中心的网格中特征匹配的数量。

6.根据权利要求1所述的一种动态场景下的即时定位方法，其特征在于，所述通过参考帧模型或恒速度模型对位姿进行跟踪，并结合BA优化的方式调整位姿，包括以下步骤：

优先使用恒速度模型进行位姿跟踪，若跟踪失败则采用参考帧模型进行跟踪，得到当前相机所在位置的绝对位姿；

对绝对位姿的计算结果执行局部BA优化，调整当前帧的位姿及地图点。

7.根据权利要求1所述的一种动态场景下的即时定位方法，其特征在于，所述动态场景的静态三维稠密地图构建，具体为：

相机坐标系下的三维点云坐标(x,y,z)为：

其中，d与深度图在像素坐标(u,v)的值成正相关，f_x,f_y为相机在x,y方向上的焦距，c_x,c_y为像素坐标系相对于成像平面的平移量。

8.一种动态场景下的即时定位装置，其特征在于，包括：

数据获取及传输模块，用于获取RGB-D相机捕获到的视频数据，并通过5G通信发送到云端；

图像帧筛选模块，用于从所述视频数据中进行图像帧筛选；

特征点匹配模块，用于在筛选出的图像帧中进行匹配点筛选，得到静态区域的特征匹配点对和三维空间点信息；

跟踪模块，用于通过参考帧模型或恒速度模型对位姿进行跟踪，并结合BA优化的方式调整位姿；

地图构建模块，用于对所述图像帧筛选模块筛选出的图像帧进行语义分割，将分割得到的动态物体对应的深度信息从该帧深度图中移除，进行动态场景的静态三维稠密地图构建。

9.一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如权利要求1至7中任一项所述的动态场景下的即时定位方法的步骤。

10.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1至7中任一项所述的动态场景下的即时定位方法的步骤。

Images