基于目标检测的动态鲁棒视觉SLAM

陈天阳，卜方玲，刘志鸿，程翰琳

(武汉大学 电子信息学院，湖北 武汉 430072)

0 引 言

传统视觉SLAM理论的基本条件之一是假设环境中不存在运动物体[1]，这在实验室场景下容易满足。然而，动态环境普遍出现于自动驾驶等潜在应用[2]，随着相关需求的扩张，该假设难以支撑SLAM技术真正走向应用。因此，研究动态场景下鲁棒的视觉SLAM具有重要意义。

近年来动态视觉SLAM的研究思路可以总结为运动分割[3]，即将环境中的运动物体排除在定位与建图之外。在这一思路之下又有多种具体方法可供选择：Kundu等[4]利用动态特征违背对极几何约束的特性，判定与整体位姿之差超出阈值的点为动态，但该方法在环境中动态点较多时失效；Sabzevari等[5]通过增加轮式编码器、IMU等设备以增强相机的外部传感器约束，但这类方法计算复杂且应用场景受限。除此以外，深度学习也被广泛应用于运动分割。这些方法[6,7]大多通过图像实例分割剔除动态特征点，在室内动态环境下取得了很好的效果，但这类方法无法实时运行，也不符合需求。

本文致力于实现高动态条件下的单目相机实时鲁棒自定位，主要贡献点在于解决了两个难题。第一，系统运行的实时性与定位的精确性之间的矛盾；第二，当前缺乏符合条件的室外高动态场景作为评估数据集。对于第一个问题，现有的视觉SLAM大多使用实例分割算法以实现较精准的图像构建，但无法满足实时性；本文因为不考虑建图，所以选择了对特征点分类较为粗糙但速度更快的目标检测算法YOLOv4[8]作为语义前端。为避免特征点的粗糙分类可能导致的不利影响，对动态特征点的判断策略进行了优化。对于第二个问题，当前最常用的视觉SLAM数据集均有帧率低、动态目标少的缺点，且录制环境与国内城市场景有较大不同。为此，本文提供了一个录制于武汉市的室外数据集，动态目标丰富、环境多变复杂，较好地满足了本文的研究需求。

1 视觉SLAM与视觉里程计

SLAM指搭载传感器的主体在没有先验信息的情况下建立周围环境的模型，并于运动过程中估计自身位置。根据所用感知设备的不同主要分为视觉和激光SLAM，前者以图像序列为输入，后者以激光雷达采集的点云为输入。经典视觉SLAM框架如图1所示。

图1 经典视觉SLAM框架

图1中的视觉里程计[9]根据相邻图像之间的视差估计相机运动并计算局部地图，又被称为前端；非线性优化模块接受不同时刻视觉里程计输出的相机位姿和回环检测信息并进行联合优化，得到全局一致的轨迹和地图，又被称为后端。本文主要聚焦视觉里程计，一个典型的视觉里程计如图2所示。

图2 经典视觉里程计

图2中C0…Cn表示对应时刻的相机位姿，Pa,…Pd表示相机观测到的路标点。在k-1和k时刻，相机各自拍得一帧图像分别记为Ik-1和Ik，由于相机处于运动状态，这两帧图像间存在视角差。例如，Ck-1和Ck分别观测到路标Pa,Pb,Pc和Pc,Pd， 根据可被两帧共同观测的Pc在两帧中坐标的差异可以计算出两帧间的位姿变换。记相机两时刻间的位姿变换为Tk,k-1∈R4×4

式中：Rk,k-1∈R3×3是旋转矩阵，tk,k-1∈R3×1是平移向量，则两帧间相机位姿可以由公式Cn=Cn-1·Tn,n-1相联系。如果能得到相机在其整个运动过程中的变换矩阵集合Tn,0={T1,0,T2,1,…,Tn,n-1}， 那么整个时段内相机在每一时刻的位姿即可解得。

以图3为例对帧间相机运动进行数学表达。已知I1，I2是相机在两不同时刻的成像，O1，O2分别为两时刻的相机光心，可被两帧共同观测的路标点P在两帧中的成像分别为p1，p2， 求相机在两时刻间的运动R，t。

图3 两相邻帧间的运动求解

在第一帧坐标系下，设路标点P的空间坐标为P=[X,Y,Z]T， 根据针孔相机模型，两像素点坐标为p1，p2， 则在其次坐标系下有

p1=KP,p2=K(RP+t)

(1)

x1=K-1p1,x2=K-1p2

(2)

其中，K为相机内参矩阵，x1，x2分别为像素点p1，p2的相机归一化平面坐标。将式(2)带入式(1)可得

x2=Rx1+t

(3)

(4)

将式(2)带入式(4)可得

(5)

将式(4)和式(5)的中间部分分别记为基础矩阵F和本质矩阵E， 可以得到

(6)

至此，求相对运动问题被转换为解方程问题。只要在两个相邻帧间找到8对匹配点即可用八点法求解矩阵E； 当E已知时，R，t可以直接通过方程解出。

2 动态鲁棒视觉SLAM

2.1 系统结构

本文所构建的动态鲁棒视觉SLAM系统结构如图4所示。首先，图像采集模块以30帧每秒(frame per second，FPS)的速率将图像序列送入系统进行目标检测和特征点提取：一方面，目标检测模块根据事先训练好的网络识别图像中的指定物体，并得到目标框的像素坐标，另一方面，特征提取模块计算并存储图像的ORB特征；其次，根据ORB特征点的像素坐标和目标框坐标确定每个ORB特征点的动态属性，将落入动、静态目标框的特征点分别划分为动、静态特征点，对于同时落入动态框和静态框的特征点需结合图像中动态点的数量占比确定属性；最后，将静态的ORB特征点送入ORB-SLAM2进行相机位姿的估算，并将计算结果与真实位姿进行对比，使用开源位姿评估工具evo得到定量的轨迹误差。

图4 动态鲁棒视觉SLAM系统框架

2.2 实时目标检测

图像序列中的动态、静态目标检测是通过YOLOv4目标检测算法实现的。YOLOv4是一阶段(one-stage)目标检测算法，输入图像直接返回其类别，具有高精度、低耗时等优点。YOLOv4在上一版的基础上进行了优化和改进，选择了CSPDarkent53作为主干网络，相较前一版本的Darkent53取得了更高的分类准确率；设计了强大而高效的检测模型，对模型训练的计算需求大大降低，在运算能力较低的设备上也能很好地运行。最终，YOLOv4在多个数据集上相较前一版本获得了更高的检测精度和更快的速度，是当前最优秀的实时目标检测算法之一。

图5 目标检测技术路线

2.3 动态特征点提取与判别

本文使用的特征为ORB-SLAM2视觉前端提取的ORB[10]特征。ORB是一种高效的特征点提取与描述算法，包括oFAST角点和rBRIEF描述子两部分。oFAST角点改进于FAST算法：对于像素点p，依次比较以其为圆心、3为半径的圆周上16个像素点与其自身的灰度值，若圆周上有连续n个像素灰度值比p点大或小，则认为p是FAST角点，n一般取12。oFASTA在此基础上计算以特征点为圆心、r为半径的图像块质心，定义特征点到质心的向量为角点方向，以此实现旋转不变性。BRIEF描述子是一个长为n的二进制串b[n]，以当前特征点为中心划定一个邻域，选择n对像素点，对于每一对像素点比较其灰度值I，若I(pi)>I(qi)， 则b[i]=1， 否则b[i]=0，n一般取256；选取n对像素的方法有多种，采用一种能让n对像素间相关性最小的统计学习方法可以使描述子拥有优秀的旋转不变性，这就是rBRIEF。ORB特征兼顾效率与速度，能满足实时性的需求，因此常用于SLAM等实时系统。

动态点的剔除需要兼顾两点，一是为了最大限度地降低动态目标干扰，需要“应剔尽剔”；二是需要保证相邻帧间匹配点的数量应足够支持SLAM的持续运行。室外道路环境中的动态目标主要为车辆和行人，相机的周围环境在大部分时间有如图6所示的两种可能情景。

图6 动、静态目标框重叠带来歧义

情景一，当目标处于前方且与自身之间有一定距离时，目标在当前视野中近似为矩形，此时可以认为框内全部为目标，如图6中的③、④号框分别为车辆和道路标志，没有歧义；情景二，当目标处于侧方且距离较近时，由于相机的射影变换，目标在视野内近似呈梯形，此时框中内容大面积为非目标内容，如图6中的①、②号框标注为车辆，但均与内容为道路标志的④号框产生重叠；此时重叠部分提取的特征将同时被标注为动态和静态，歧义于是产生。

为了解决这类歧义，本文采取如下方法。首先，由于在大多数时间内目标与相机距离较远，上述情景一为主流，此时“应剔尽剔”为主要需求，可将检测框向四周分别扩张K个像素以降低漏剔率；其次，当特征同时处于两种检测框内时，计算当前图像内动态特征点的数量，若其在总特征点中占比超过一定阈值或剩余静态特征无法支持SLAM的继续运行，则将其归为静态点，反之为动态点。记阈值为T， 动态特征点数量为N1， 特征点总数为N， 则特征点的属性判断见表1。K和T为经验值，本文中分别选取K=10，T=0.3。

表1 动态特征点判别标准

3 实验与结果分析

3.1 数据采集

视觉SLAM常用数据集为TUM[11]、EuRoC[12]和KITTI[13]，其中TUM和EuRoC录制于室内，KITTI虽是室外数据集，但一方面帧率较低，另一方面其图像中动态目标较少，主要为欧洲小镇与公路街景，且录制环境与国内城市场景具有较大不同，无法满足评估需求。为此，本文搭建了一个视觉SLAM数据采集平台，在武汉市内录制了多段图像序列作为本文的数据集。该数据集有多个图像序列，包含普通公路、隧道、大桥、城市快速路、停车场、校园等多个场景，动态目标丰富、环境多变复杂，较好地满足了本文的研究需求。数据采集平台包括车辆、RGB相机、GNSS-IMU定位设备、存储器、便携电脑等，所有传感器已事先校正并准确安装，车辆平台与各传感器相对位置及坐标系设定如图7所示，设备清单见表2。

图7 传感器设定

表2 数据采集平台设备清单

车辆运行期间，ZED2相机同步录制30 FPS，1280*720像素双目RGB图像和100 Hz六轴IMU数据，同时GNSS-IMU设备以10 Hz速率记录有IMU辅助的RTK差分GNSS数据作为相机位姿的真实值，即groundtruth，所有信息经由计算机处理后存入移动硬盘。本文所有实验使用的图像均由ZED2左镜头采集。本文所采集的数据已部分制作为数据集，全部处理完成后将面向社区开源。

3.2 车辆识别

3.3 动态鲁棒视觉SLAM自定位

3.3.1 动态鲁棒视觉SLAM降低误差的定性分析

根据视觉里程计的实现原理，视野中的动态目标对相

表3 YOLOv4与YOLOv4-tiny训练结果

机自定位产生干扰的主要原因是相对运动让相机产生了错误的认知。例如，当相机静止时，前方目标的前进、倒退、向左移动、向右移动有可能让相机分别产生自身正在倒退、前进、向右移动和向左移动的误判。实验中的一些局部结果也很好印证了这个事实，下面结合实验结果对3类场景进行分析。

场景1：相机与前方运动物体较长时间保持近似同速同向运行，使相机对自身运动速度的估计远低于事实，甚至认为自己接近静止。图8中从左上到右下的4张图按时间顺序拍摄于某行车场景，可以从视野中左右两侧较远处的大楼看出相机已实际产生了较大位移，而相机正前方近处的车辆却基本上与相机保持同样的距离，并且在视野中面积占比较高，导致动态特征点占比也较高，进而产生如图9所示的结果(图9、图11、图13横纵坐标分别为经纬度)。

图8 场景1：相机静止误判

图9 算法改进前后场景1中相机自定位效果对比

图9中的线条表示相机真实运动轨迹，星号*和圆圈o分别表示用原ORB-SLAM2和本文所采用的增加了动态特征去除的改进方法计算得到的相机位置，分别记为orb和orb-sta(orb-static的缩写，意为静态)，所有结果投影到以经纬度为坐标的真实世界坐标系中。可以看出，在图9矩形框处，由原方法计算得到的相机位置在较短区间内大量聚集，说明原方法计算的相机速度较低、接近静止，而改进方法可以正确判断出相机在以一定的速度移动；最终结果是原方法计算得到的相机位移远低于实际值、改进方法的结果更接近真实位移。

场景2：相机静止或前行时前方运动物体加速前进，使相机对自身运动速度的估计低于实际值，甚至产生倒退的误判。这种情况多出现于交通信号灯前的起步阶段，此时相机与前车间距较小，动态特征点占比较大，容易产生误判。如图10所示，从右侧建筑物可以判断相机正在前向行驶，但是前方的3辆汽车正在远离，于是产生如图11所示的结果。

图10 场景2：相机倒退误判

图11 算法改进前后场景2中相机自定位效果对比

根据图11可以看出，原方法在车辆起步阶段对相机的运动产生了向左后方偏移的误判，导致后续整段数据明显偏离真实值，产生了极大的误差；而改进后的方法避免了起步时的误判，整体误差明显更小。

场景3：运动物体从相机的某一侧边缘以与其它目标不同的速度进出视野，使相机产生旋转的误判。这种情况多出现于超车与被超车时。图12中，前方左侧车辆逐渐被相机从右侧超越，从车辆上提取的特征点以与其它特征点相比更大的速度持续向相机左侧偏移，这使原算法认为相机正在右转，产生如图13所示的结果。

图12 场景3：相机旋转误判

图13 算法改进前后场景3中相机自定位效果对比

根据图13可以看出，原方法在相机超车时产生了多次右转误判，且由此带来的误差是可以累积的。这导致计算得出的相机轨迹与真实值相比产生极大误差，而改进后的方法显著降低了这种误差。

3.3.2 动态鲁棒视觉SLAM降低误差的定量分析

3.3.1节中定性分析了ORB-SLAM2算法在相机自定位中产生误判的原因、本文的改进点以及改进之后的效果，本节将在采集的多个图像序列中对改进前后的方法进行全面评估，并给出定量结果。实验所用的评估工具为开源工具evo，将实验用到的7个图像序列按序编号为01-07，其详细信息见表4。表中的序列01-03对应3.3.1节中的3段数据。

在上述7段图像序列上分别运行原SLAM和改进后的动态鲁棒视觉SLAM，得到的关键帧位姿结果分别记为orb和orb-sta，用开源轨迹评估模块evo分别比较两者与对应的groundtruth之间的误差，采用绝对位姿误差(absolute pose error，APE)和相对位姿误差(relative pose error，RPE)两个指标，得到表5所示的结果。

表5 算法改进前后在7段数据集上的定位误差对比

表5显示，在APE指标下，改进后的orb-sta在全部7段图像序列上的表现均优于改进前，定位误差大幅降低；在RPE指标下，orb-sta在其中5段上取得了更好的结果。综合以上定性、定量分析可以得出结论，相较于原SLAM系统，本文所提出的改进方法可以有效避免多种运动目标带来的干扰，在城市道路的复杂动态环境中可以显著降低定位误差，取得更准确的结果。

4 结束语

本文针对传统视觉SLAM算法在动态场景中存在的问题，采用YOLOv4目标检测模块改善视觉里程计，优化动态特征点剔除策略，在ORB-SLAM2基础上构建了动态鲁棒的相机自定位系统。该系统可以在动态环境中自行识别并标注运动目标，将对自身定位造成不利影响的动态特征点剔除以降低误差。同时，为全面准确评估该方法的有效性，本文构建了一个武汉城市道路数据集，该数据集有多个图像序列，包含普通公路、隧道、大桥、城市快速路、停车场、校园等多个环境复杂的高动态场景。实验结果表明，该方法可以有效抑制动态目标给相机自定位带来的多种不利影响和误判，与改进前方法相比误差显著降低，有效提升了相机在动态环境中的定位精度。