CN116052046A - 一种基于目标跟踪的动态场景双目视觉slam方法 - Google Patents

Abstract

本发明公开了一种基于目标跟踪的动态场景双目视觉SLAM方法，属于同步定位与地图构建领域，在远端采用嵌入式设备控制安装在运动体上的随动云台相机并采集图像，在服务端使用嵌入式设备部署算法，算法采用结合目标跟踪的SLAM算法排除视野内运动体自身的遮挡机构和动态物体的干扰，减小位姿计算和地图构建的误差。本发明不仅能够对动态物体进行目标检测与分割，还能够对视野遮挡物体进行分割，基于轻量目标检测网络和分布式的结构，减少资源消耗，使得算法可以部署在嵌入式设备上。

Description

一种基于目标跟踪的动态场景双目视觉SLAM方法

技术领域

本发明涉及同步定位与地图构建领域，尤其是一种基于目标跟踪的动态场景双目视觉SLAM方法。

背景技术

同步定位与地图构建(SLAM，Simultaneous Localization and Mapping)是指机器人在未知环境运动，在运动过程中不断检测周围环境，并根据环境中的特征，感知自身运动位姿，同时构建稀疏点云地图的算法。目前，同步定位与地图构建技术在自动驾驶、无人机、虚拟现实技术、增强现实技术等领域都有广泛的应用，为感知自身位姿提供了算法支持。

其中，SLAM算法中按工作场景又分为静态场景SLAM和动态场景SLAM。静态场景SLAM算法占大多数，指的是机器人工作环境为纯静态场景，比如ORBSLAM、VINS-MONO、LSDSLAM等。它们在静态环境下的位姿计算与地图构建非常的精确，但是在动态环境下会受动态物体的干扰而出现很大的误差。而动态场景SLAM针对动态环境对算法进行了改进以排除动态物体的干扰，以达到精确地位姿估计，比如DS-SLAM、DynaSLAM、VDO-SLAM等。

在文献[C.Yu et al.,"DS-SLAM:A Semantic Visual SLAM towards DynamicEnvironments,"2018IEEE/RSJ International Conference on Intelligent Robots andSystems(IROS),2018,pp.1168-1174,doi:10.1109/IROS.2018.8593691.]中提出了DS-SLAM，它是以ORBSLAM2为基础框架，结合SegNet网络作为语义分割网络的动态场景SLAM。在获取到新的图像帧时，使用语义分割网络对图像进行语义分割，将潜在运动物体区域分割出来。提取特征点后，使用随机抽样一致(RANSAC，Random Sample Consensus)计算投影点到极线距离以检查特征点移动一致性，筛选出外点。如果潜在动态物体上的外点很多，那么该物体被视为动态的，上面所有特征点都被标记为外点，然后删除所有外点。

在文献[B.Bescos,J.M.Fácil,J.Civera and J.Neira,"DynaSLAM:Tracking,Mapping,and Inpainting in Dynamic Scenes,"in IEEE Robotics and AutomationLetters,vol.3,no.4,pp.4076-4083,Oct.2018,doi:10.1109/LRA.2018.2860039.]中提出了DynaSLAM，它是以ORBSLAM2为框架的动态场景SLAM算法，使用Mask-RCNN作为语义分割网络。在获取到新的图像帧时，使用语义分割网络对图像语义分割，将“人”等明显动态区域直接删除，对剩下的区域进行轻量跟踪；对潜在运动物体(比如椅子、书本)进行处理：对当前帧，选择与之重叠度最大的5帧计算每个关键点到当前帧的投影和投影深度，同时生成三维点，计算关键点与三维点形成的夹角，若大于30°则认为该点被挡住，认为是动态点，不做处理。然后使用区域增长算法从动态特征点得到动态物体的掩膜。对动态物体遮挡的背景，使用前20帧的信息进行修复。

VDO-SLAM原理跟DynaSLAM差不多，也是用Mask-RCNN进行语义分割，不同的是区分动态物体的方法：VDOSLAM使用密集光流跟踪物体，使用场景流区分物体动态静态。

上述算法都只对外部动态物体做目标检测与分割，如果画面中出现视野遮挡，则不能准确将其分割出来，而这些视野遮挡相对于静态场景仍属于动态物体。另外，上述算法都使用了语义分割网络用于检测和分割动态物体，虽然这样精度高，但是实时性差，对于轻量级设备占用太多资源。

发明内容

本发明需要解决的技术问题是提供一种基于目标跟踪的动态场景双目视觉SLAM方法，针对工程机械室外作业的工作环境，采用分布式结构，在远端采用嵌入式设备控制双目随动云台相机并采集图像，在服务端使用嵌入式设备部署算法，排除视野内运动体自身的遮挡机构和动态物体的干扰，减小位姿计算和地图构建的误差。

为解决上述技术问题，本发明所采用的技术方案是：

一种基于目标跟踪的动态场景双目视觉SLAM方法，在远端采用嵌入式设备控制安装在运动体上的随动云台相机并采集图像，在服务端使用嵌入式设备部署算法，算法采用结合目标跟踪的SLAM算法排除视野内运动体自身的遮挡机构和动态物体的干扰；所述SLAM方法包括以下步骤：

步骤1，SLAM算法双目初始化与目标检测网络读取训练模型信息；

步骤2，远程随动采集双目彩色视频图像；

步骤3，目标跟踪网络对双目彩色视频图像进行目标检测及跟踪，将结果送入SLAM线程作后续处理；

步骤4，SLAM线程对双目彩色图像进行预处理，提取角点后，使用光流匹配双目关键点并计算深度信息；

步骤5，使用步骤3中的目标跟踪结果，将关键点分类为静态点和待筛选点；

步骤6，对步骤5中的静态点进行对极几何约束去除多余外点，使用PnP算法求解位姿信息；

步骤7，对步骤5中的待筛选点，使用动态点筛选方法筛选出真正的动态点；

步骤8，对步骤7中动态点进行删除，并删除当前帧与该点的共视关系，对静态点进行合并；

步骤9，更新当前帧的信息，包括关键点、地图点以及其共视关系；基于位姿进行局部优化；如果当前帧是关键帧，则进行回环检测。

本发明技术方案的进一步改进在于：在步骤2中，在服务端操作人员使用携带IMU的VR设备控制随动云台相机的云台旋转，在远端使用树莓派连接随动云台相机，控制随动云台相机采集双目彩色视频图像，并通过ROS传回到服务端完成双目彩色视频图像的采集；VR设备上的IMU测量模型为：

其中，

为角速度，ω^b为角速度真值，b^g为角速度零偏、n^g为角速度噪声，

为加速度、q_bw为world到body系的旋转矩阵、a^w为加速度真值、g^w为重力加速度、b^a为加速度零偏、n^a为加速度噪声；

角速度与欧拉角的转换关系为：

其中，φ为偏航角yaw，θ为俯仰角pitch，ψ为滚转角roll；ω_x、ω_y、ω_z分别是角速度在xyz轴分解的向量；

则随动云台相机两舵机的输入脉冲数分别为：

其中，P_⊥和P_∥分别为随动云台相机两舵机的输入脉冲，DEG为步进角。

本发明技术方案的进一步改进在于：在步骤3中，目标跟踪网络是基于yolov5s的目标检测网络和卡尔曼滤波，具体包括以下步骤：

3.1，目标检测网络对图像中的局部运动机构进行检测，得到一组矩形框；

3.2，对步骤3.1输出结果的新目标构建一个卡尔曼滤波器，并使用矩形框坐标信息进行初始化；

其中，状态量设置为

状态转移矩阵设置为

由于没有输入量，u＝0，观测量设置为

观测矩阵设置为

另外，对运动体上特殊场景遮挡物体“第一人称挖掘机臂”、“第一人称消防水枪”由于其启动位置的固定性，预设其初始位置，即：

3.3，对步骤3.1输出结果的旧目标进行跟踪，使用其卡尔曼滤波器输出的预测值对步骤3.1的所有输出进行IOU匹配：

3.4，对步骤3.3中匹配结果大于阈值T的目标标记为跟踪成功，使用其矩形框作为最终结果并修正卡尔曼滤波器；

3.5，对步骤3.3中匹配结果小于阈值T的目标的跟踪失败Flag自增，若跟踪失败Flag大于阈值F，则释放该卡尔曼滤波器的内存，否则使用卡尔曼滤波器输出的预测值作为最终结果；

3.6，若步骤3.1目标检测网络未输出到任何矩形框，则所有已有的目标的跟踪失败Flag自增，若跟踪失败Flag大于F，则释放对应卡尔曼滤波器的内存，否则使用卡尔曼滤波器输出的预测值作为最终结果。

本发明技术方案的进一步改进在于：在步骤7中，所述动态点筛选方法包括以下步骤：

7.1，使用对极几何约束对静态点求取本质矩阵E，

7.2，对步骤7.1的本质矩阵E使用SVD奇异值分解计算旋转矩阵R和平移向量t：

其中，R₁、R₂均是一种可能的旋转矩阵、

均是一种可能的平移矩阵；

求得2组R₁、

R₂、

取其中点深度大于零的解即为待筛选点的位姿；

7.3，使用步骤7.2中的位姿信息作为基准，将步骤5中待筛选点分类为动态点和静态点：

根据本质矩阵E求得点x₁的极线l₁＝[X,Y,Z]^T＝Ex₁；

点x₂到极线l₁的距离公式为：

其中，x₁和x₂是一对匹配点，F是基础矩阵，X、Y是极线的参数，T代表矩阵的转置；

若D大于阈值D_th则被认为是动态点，若D小于阈值D_th则被认为是静态点。

由于采用了上述技术方案，本发明取得的技术进步是：

1、本发明不仅能够对动态物体进行目标检测与分割，还能够对视野遮挡物体进行分割，基于轻量目标检测网络和分布式的结构，减少资源消耗，使得算法可以部署在嵌入式设备上。

2、本发明针对工程机械室外作业的工作环境，提出一种轻量的视野遮挡的动态场景SLAM算法，算法采用分布式结构，远端采用嵌入式设备控制双目云台相机并采集图像，服务端使用嵌入式设备部署算法，算法采用结合目标跟踪的SLAM算法排除视野内运动体自身的遮挡机构和动态物体的干扰，减小位姿计算和地图构建的误差。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图；

图1是本发明提供的一种基于目标跟踪的动态场景双目视觉SLAM方法的流程图；

图2是本发明实施例中基于YOLOv5s和卡尔曼滤波的多目标跟踪算法流程图；

图3是本发明实施例中剔除和筛选动态特征点的原理图；

图4是本发明实施例中硬件部署示意图。

具体实施方式

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

本申请实施例通过提供一种基于目标跟踪的动态场景双目视觉SLAM方法，解决了现有技术中存在的“如果画面中出现相对于静态场景仍属于动态物体的视野遮挡，则不能准确将其分割出来；使用语义分割网络用于检测和分割动态物体时实时性差，对于轻量级设备占用太多资源。”的问题，大体思路为：目标检测+卡尔曼滤波+SLAM算法。

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

本申请发明人在研究过程中发现：大多数SLAM算法在静态场景下有很好的定位表现，但在动态场景下会出现误差，这是因为大多数算法都是依靠静态物体上的特征点进行辅助定位，如果这些特征点来自动态物体，那么特征点在帧间的移动将会异于其他静态特征点移动的趋势，虽然有RANSAC算法可以抵抗少量外点的影响，但如果动态特征点的数量很多，与静态特征点相同数量级，那么RANSAC算法将会失效。目前大部分动态SLAM算法都是基于语义分割网络的，通过语义分割网络获得动态物体的区域以达到去除动态特征点的目的。但是由于语义分割网络消耗资源大，往往不能实时运行，或者占用资源很大，不能部署在大多数嵌入式设备。因此本发明侧重重心在于剔除动态目标干扰的同时，节省计算资源，使得算法可以部署在嵌入式设备(体型小、计算能力相对较差、但是便于移动的小型计算机)。

针对工程机械室外作业的工作环境，本发明的整个硬件系统包括服务端和远端；

所述服务端由嵌入式设备、操作人员、VR设备构成，操作人员穿戴VR设备，VR设备上设置有IMU模块，设置在远端的随动云台相机会随VR设备的姿态进行调整，嵌入式设备接收到远端的图像数据运行算法进行处理。

所述远端由运动体和随动云台相机构成，在施工场景中，所述运动体包括挖掘机、消防机器人等，所述随动云台相机安装在运动体上方居中位置，会被运动体自身一部分机构(如：第一人称挖掘机臂、第一人称消防水枪)遮挡，所述随动云台相机由两个舵机和双目相机组成；所述舵机会随着服务端VR设备上的IMU运动而运动。因此，随动云台相机可以随操作人员的头部运动而运动，这样就可以在运动体不动的情况下，观察四周情况。

服务端和远端通过无线局域网进行通讯，随动云台相机接收到IMU的信号做相应动作，随动云台相机也会把采集到的双目图像通过无线局域网发送给服务端的嵌入式设备。

服务端的嵌入式设备设置有所需的算法，算法由SLAM算法部分和目标跟踪算法部分构成。采集到的双目图像同时送进SLAM算法部分和目标跟踪算法部分中进行处理。SLAM算法部分对每个输入的双目图像，输出一个位姿。对一组输入的图像，输出一组位姿，即这个运动体的运动轨迹。

具体的：

如图1所示，一种基于目标跟踪的动态场景双目视觉SLAM方法，在远端采用嵌入式设备控制安装在运动体上的随动云台相机并采集图像；在服务端使用嵌入式设备部署算法，算法采用结合目标跟踪的SLAM算法排除视野内运动体自身的遮挡机构和动态物体的干扰；所述SLAM方法包括以下步骤：

步骤1，SLAM算法双目初始化与目标检测网络读取训练模型信息；

步骤2，远程随动采集双目彩色视频图像；

在服务端操作人员使用携带IMU的VR设备控制随动云台相机的云台旋转，在远端使用树莓派连接随动云台相机，控制随动云台相机采集双目彩色视频图像，并通过ROS传回到服务端完成双目彩色视频图像的采集；

VR设备上IMU测量模型为：

其中，

为角速度，ω^b为角速度真值，b^g为角速度零偏、n^g为角速度噪声，

为加速度、q_bw为world到body系的旋转矩阵、a^w为加速度真值、g^w为重力加速度、b^a为加速度零偏、n^a为加速度噪声；

角速度与欧拉角的转换关系为：

其中，φ为偏航角yaw，θ为俯仰角pitch，ψ为滚转角roll；ω_x、ω_y、ω_z分别是角速度在xyz轴分解的向量；

则随动云台相机两舵机的输入脉冲数分别为：

其中，P_⊥和P_∥分别为随动云台相机两舵机的输入脉冲，DEG为步进角。

步骤3，目标跟踪网络(基于yolov5s的目标检测网络和卡尔曼滤波)对双目彩色视频图像进行目标检测及跟踪，将结果送入SLAM线程作后续处理；

目标检测及跟踪处理过程为：目标跟踪网络接收到双目图像后，会对图像中的目标物体进行检测。目标物体包括动态物体(人、车、火焰)和运动体自身的遮挡物体(第一人称挖掘机臂、第一人称消防水枪)。因为这些目标物体对于SLAM算法都属于动态的，其上面的特征点会干扰SLAM算法的定位精度。检测完，目标检测网络会输出目标物体的矩形框信息(中心点坐标、宽、高)。使用一组卡尔曼滤波器对这些矩形框进行跟踪：如果这个矩形框是之前未跟踪到的，那么为其新初始化一个卡尔曼滤波器；如果这个矩形框信息与之前的卡尔曼滤波器跟踪的矩形框匹配上了，那么更新这个卡尔曼滤波器。即每个卡尔曼滤波器跟踪一个矩形框。匹配方法是IOU匹配。最终对输入的每个双目图像，目标跟踪部分会输出一组目标物体的矩形框信息，用于SLAM算法。也就是说目标检测+卡尔曼滤波跟踪＝目标跟踪。

如图2所示，具体包括以下步骤：

3.1，YOLOv5s初始化加载模型后挂起，当检测到队列中存在图像时，目标跟踪线程会进入循环，图像被送入YOLOv5s后，YOLOv5s输出一组矩形框。

3.2，为新目标分配卡尔曼滤波器的内存，使用当前的矩形框信息初始化状态参数和观测参数；

具体的预测状态量为cx、cy、w、h、dx、dy、dw、dh，分别代表矩形框中心点横纵坐标、宽、高以及这些量的微分量。

观测量为cx、cy、w、h，分别代表矩形框中心点横纵坐标、宽、高。即：

其中，

另外，对运动体上特殊场景遮挡物体“第一人称挖掘机臂”、“第一人称消防水枪”由于其启动位置的固定性，预设其初始位置，即：

3.3，对于步骤3.1输出结果的旧目标，使用其卡尔曼滤波器输出的预测值对步骤3.1的所有输出进行IOU匹配，即：

3.4，若3.3中最大IOU匹配值大于0.4，则认为匹配成功；最大匹配矩形框即为目标跟踪结果，同时用于修正卡尔曼滤波器。

3.5，若3.3中最大IOU匹配值小于0.4，则认为匹配失败。使用卡尔曼滤波器提供的预测值信息作为最终跟踪的矩形框。同时跟踪丢失Flag自增，当跟踪丢失Flag大于12时，即表示为12帧跟踪丢失，此时判定为目标跟丢，释放该目标对应的卡尔曼滤波器内存。

3.6，若3.1中，YOLOv5s因为画面模糊而未能检测到任何物体，未能提供任何矩形框时。所有目标的卡尔曼滤波器的跟踪丢失Flag自增，当跟踪丢失Flag大于12时，即表示为12帧跟踪丢失，此时判定为目标跟丢，释放该目标对应的卡尔曼滤波器内存。剩余的卡尔曼滤波器各自提供一个预测值作为目标跟踪最终矩形框。

3.7，重复以上步骤，跟踪所有潜在动态新物体、旧物体；并将最终输出的矩形框和目标标签信息传送到SLAM算法部分作后续处理。

步骤4，SLAM线程对双目彩色图像进行灰度化等预处理，提取角点后，使用光流匹配双目关键点并计算深度信息；

具体的：

4.1将接收到的图像存储到队列中，SLAM线程调用图像时，会先统计队列中图像数量，如果数量为0则等待；如果数量大于1，则逐个删除多余图像数据，只使用最新的图像，使系统最接近于实时。这样做的优点在于可以让系统处理紧跟实时图像，缺点是每帧的图像间隔可能不一致。

4.2将双目RGB图像灰度化后限制对比度自适应直方图均衡化(CLAHE)；具体包括以下步骤：

4.2.1，将图像灰度化后调整大小；

4.2.2，图像分块填充，计算每一块直方图二维数组；

4.2.3，对直方图进行对比度限制处理；削弱高频率灰度值的频数，使图像灰度均衡，极端亮区域和极端暗区域被限制；

4.2.4，使用限制的直方图生成直方图均衡映射函数；

4.2.5，判断像素点的位置，使用双线性插值得到最终图像。

4.3，提取处理后图像的FAST角点，并对角点进行四叉树均匀化处理；

具体包括以下步骤：

4.3.1，在图像中选取像素p，假设它的亮度为I_p；

4.3.2，设置一个阈值T，以像素p为中心，选取半径为3的圆上的16个像素点；

4.3.3，加入选取的圆上，有连续N个点的亮度大于I_p+T或者小于I_p-T，那么像素p可以被认为是FAST角点；

4.3.4，重复循环3.1到3.3，对每一个像素执行相同的操作，得到整张图像的FAST角点；

4.3.5，将整张图像作为一个节点node；

4.3.6，遍历所有节点node；

4.3.7，如果node内FAST角点数量大于等于1，则将该node分裂成4个node；如果node内FAST角点数量为0，则将该node删除；

4.3.8，重复3.6、3.7，直到node数量大于200；

4.3.9，每个node保留1个FAST角点，删除其余角点。

4.4，使用LK光流法跟踪并匹配角点；该算法基于灰度不变假设，即：

I(x+dx,y+dy,t+dt)＝I(x,y,t)

其中，I为灰度值，x、y、t分别是角点横纵坐标和时间。

记

为I_x，

为I_y，

为I_t；

为像素在x方向的速度，记为u，

为像素在y方向的速度，记为v。则上式变为：

若窗口内像素具有相同的运动，设窗口大小为w×w则有w²个方程：

解超定方程

即

这样可以得到左目角点在右目大致位置，将左右目角点进行匹配。

4.5，使用三角测量算法计算角点深度；

设外参为R、t，像素坐标系2D点为u，待求的3D点为X。

则有su＝PX，其中s为深度，P为投影矩阵P＝KT

u^{^}su＝u^{^}PX＝0

只有前两个方程是线性独立的，解X还需要额外一对方程，即：

使用SVD即可解得X，即得到3D点坐标。

步骤5，使用步骤3中的目标跟踪结果，将关键点分类为静态点和待筛选点；

具体的，使用目标跟踪网络提供的矩形框信息，将图像中的角点分类为两类，一类为矩形框外部的点，为静态点；一类为矩形框内部的点，为待筛选点。

步骤6，对步骤5中的静态点进行对极几何约束去除多余外点，使用PnP算法求解位姿信息；

具体的，对步骤5分类的静态点使用PnP求解位姿。设齐次坐标：3D点P(X,Y,Z,1)，2D点p(u,v,1)。设变换为p＝TP，即：

设

上式转化为

即

则需要6对点求解。

实际求解位姿时并不是只使用6对点，这样会浪费大量角点信息，实际中往往采用RANSAC结合PnP方式转化为一个最小二乘问题，解出一个使损失函数最小的值即为静态点的位姿。

步骤7，对步骤5中的待筛选点，使用动态点筛选方法筛选出真正的动态点；所述动态点筛选方法包括以下步骤：

7.1，使用对极几何约束对静态点求取本质矩阵E，

具体的，由于待筛选点基本都是来自于潜在动态物体，其运动趋势与静态点趋势不一致，因此在步骤4.5三角测量时，不一定能准确计算深度。因此使用对极几何约束，使用2D点以及双目外参求解位姿，如图3所示。

设s为深度，p₁、p₂为两帧匹配的2D点，P为匹配的3D点，K为内参矩阵，R为旋转矩阵、t为平移矩阵。则有：

s₁p₁＝KP、s₂p₂＝K(RP+t)

尺度意义下相等：p₁；KP、p₂；K(RP+t)

设归一化相机坐标系x₁＝K^-1p₁、x₂＝K^-1p₂，则：

x₂；K^-1K(RP+t)；RP+t；Rx₁+t

x₂；Rx₁+t

t^{^}x₂；t^{^}Rx₁

其中，E＝t^R为基础矩阵。

由对极约束

得：

e₁u₁u₂+e₂u₁v₁+e₃u₁+e₄u₂v₁+e₅v₁v₂+e₆v₁+e₇u₂+e₈v₂+e₉＝0

令

将上式转化为：

由于尺度等价性，自由度可减1，只需要8对点即可求解出E。

7.2，对步骤7.1的本质矩阵E使用SVD奇异值分解计算旋转矩阵R和平移向量t：

其中，R₁、R₂均是一种可能的旋转矩阵、

均是一种可能的平移矩阵；

求得2组R₁、

R₂、

取其中点深度大于零的解即为待筛选点的位姿。

7.3，使用步骤7.2中的位姿信息作为基准，将步骤5中待筛选点分类为动态点和静态点：

根据本质矩阵E求得点x₁的极线l₁＝[X,Y,Z]^T＝Ex₁；

点x₂到极线l₁的距离公式为：

其中，x₁和x₂是一对匹配点，F是基础矩阵，X、Y是极线的参数，T代表矩阵的转置；

若D大于阈值D_th则被认为是动态点，若D小于阈值D_th则被认为是静态点。

步骤8，对步骤7中动态点进行删除，并删除当前帧与该点的共视关系，对静态点进行合并；

具体的，使用步骤6中求解的位姿为基准，依据步骤7中求解的待筛选点的位姿，筛选得到动态角点。将动态角点删除，将矩形框内部剩余点与静态点合并，以达到去除动态点干扰的目的。

步骤9，更新当前帧的信息，包括关键点、地图点以及其共视关系；基于位姿进行局部优化；如果当前帧是关键帧，则进行回环检测。

实施例

如图4所示，远端的作业端使用随动云台相机和树莓派构成，服务端使用嵌入式设备部署算法，整套算法通过ROS进行消息通信。树莓派系统为Ubuntu18.04、ROS；嵌入式设备配置为Intel i5-10400、Ubuntu18.04、ROS，随动云台相机由oak相机和两个舵机构成。远端载体实验小车为非必要选项。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (4)

1.一种基于目标跟踪的动态场景双目视觉SLAM方法，其特征在于：在远端采用嵌入式设备控制安装在运动体上的随动云台相机并采集图像，在服务端使用嵌入式设备部署算法，算法采用结合目标跟踪的SLAM算法排除视野内运动体自身的遮挡机构和动态物体的干扰；所述SLAM方法包括以下步骤：

步骤1，SLAM算法双目初始化与目标检测网络读取训练模型信息；

步骤2，远程随动采集双目彩色视频图像；

步骤3，目标跟踪网络对双目彩色视频图像进行目标检测及跟踪，将结果送入SLAM线程作后续处理；

步骤4，SLAM线程对双目彩色图像进行预处理，提取角点后，使用光流匹配双目关键点并计算深度信息；

步骤5，使用步骤3中的目标跟踪结果，将关键点分类为静态点和待筛选点；

步骤6，对步骤5中的静态点进行对极几何约束去除多余外点，使用PnP算法求解位姿信息；

步骤7，对步骤5中的待筛选点，使用动态点筛选方法筛选出真正的动态点；

步骤8，对步骤7中动态点进行删除，并删除当前帧与该点的共视关系，对静态点进行合并；

步骤9，更新当前帧的信息，包括关键点、地图点以及其共视关系；基于位姿进行局部优化；如果当前帧是关键帧，则进行回环检测。

2.根据权利要求1所述的一种基于目标跟踪的动态场景双目视觉SLAM方法，其特征在于：在步骤2中，在服务端操作人员使用携带IMU的VR设备控制随动云台相机的云台旋转，在远端使用树莓派连接随动云台相机，控制随动云台相机采集双目彩色视频图像，并通过ROS传回到服务端完成双目彩色视频图像的采集；VR设备上的IMU测量模型为：

其中，

为角速度，ω^b为角速度真值，b^g为角速度零偏、n^g为角速度噪声，

为加速度、q_bw为world到body系的旋转矩阵、a^w为加速度真值、g^w为重力加速度、b^a为加速度零偏、n^a为加速度噪声；

角速度与欧拉角的转换关系为：

其中，φ为偏航角yaw，θ为俯仰角pitch，ψ为滚转角roll；ω_x、ω_y、ω_z分别是角速度在xyz轴分解的向量；

则随动云台相机两个舵机的输入脉冲数分别为：

其中，P_⊥和P_∥分别为随动云台相机两舵机的输入脉冲，DEG为步进角。

3.根据权利要求1所述的一种基于目标跟踪的动态场景双目视觉SLAM方法，其特征在于：在步骤3中，目标跟踪网络是基于yolov5s的目标检测网络和卡尔曼滤波，具体包括以下步骤：

3.1，目标检测网络对图像中的局部运动机构进行检测，得到一组矩形框；

3.2，对步骤3.1输出结果的新目标构建一个卡尔曼滤波器，并使用矩形框坐标信息进行初始化；

其中，状态量设置为

状态转移矩阵设置为

由于没有输入量，u＝0，观测量设置为

观测矩阵设置为

另外，对运动体上特殊场景遮挡物体“第一人称挖掘机臂”、“第一人称消防水枪”由于其启动位置的固定性，预设其初始位置，即：

3.3，对步骤3.1输出结果的旧目标进行跟踪，使用其卡尔曼滤波器输出的预测值对步骤3.1的所有输出进行IOU匹配：

3.4，对步骤3.3中匹配结果大于阈值T的目标标记为跟踪成功，使用其矩形框作为最终结果并修正卡尔曼滤波器；

3.5，对步骤3.3中匹配结果小于阈值T的目标的跟踪失败Flag自增，若跟踪失败Flag大于阈值F，则释放该卡尔曼滤波器的内存，否则使用卡尔曼滤波器输出的预测值作为最终结果；

3.6，若步骤3.1目标检测网络未输出到任何矩形框，则所有已有的目标的跟踪失败Flag自增，若跟踪失败Flag大于F，则释放对应卡尔曼滤波器的内存，否则使用卡尔曼滤波器输出的预测值作为最终结果。

4.根据权利要求1所述的一种基于目标跟踪的动态场景双目视觉SLAM方法，其特征在于：在步骤7中，所述动态点筛选方法包括以下步骤：

7.1，使用对极几何约束对静态点求取本质矩阵E，

7.2，对步骤7.1的本质矩阵E使用SVD奇异值分解计算旋转矩阵R和平移向量t：

其中，R₁、R₂均是一种可能的旋转矩阵、

均是一种可能的平移矩阵；

求得2组R₁、

R₂、

取其中点深度大于零的解即为待筛选点的位姿；

7.3，使用步骤7.2中的位姿信息作为基准，将步骤5中待筛选点分类为动态点和静态点：

根据本质矩阵E求得点x₁的极线l₁＝[X,Y,Z]^T＝Ex₁；

点x₂到极线l₁的距离公式为：

其中，x₁和x₂是一对匹配点，F是基础矩阵，X、Y是极线的参数，T代表矩阵的转置；

若D大于阈值D_th则被认为是动态点，若D小于阈值D_th则被认为是静态点。

Images