动态场景中的单目视觉里程计

傅 彬，金 棋

（绍兴职业技术学院，浙江 绍兴 312000）

0 引 言

随着城市现代化的发展，交通变得愈加拥堵，能源浪费严重，因而自动驾驶汽车在人们的生活中就显得尤为重要。自动驾驶至关重要的任务是自动驾驶汽车对三维环境的感知和定位。自动驾驶汽车感知环境时用到多种传感器，如激光雷达、毫米波雷达等。摄像头相比于激光雷达等距离传感器有价格低廉、重量轻、耗能少等优点，因此基于摄像头的定位和三维环境感知成为研究的热点。车载环境是复杂的动态环境，环境中不仅有正在运动的行人、车辆等目标，还有房屋和树木等静止的背景，因此，自动驾驶技术需要在三维环境中获得自身的位置信息、静态障碍物位置估计以及三维运动目标的位置和角度估计，来保证行车安全。

视觉同步定位与建图（Simultaneous Localization and Mapping，SLAM）[1]算法能够实现静态环境中的定位和地图构建，但是在动态环境如车载环境中，视觉SLAM精确度会变差。因为视觉SLAM采用静态特征点计算相机运动，如果不能区分图像中静止和运动的特征点，计算得到的相机位姿将会出错。视觉SLAM中常常采用RANSAC[2]算法剔除异常特征点，如果图像中仅有少量运动特征点，RANSAC根据大部分符合模型的静态特征点剔除运动特征点使得估计相机运动较为准确。RANSAC这种方法在低动态场景中比较适用，但是在车载环境中，运动车辆占图像大部分面积或被前车大量遮挡，将会降低SLAM的鲁棒性。现有的multibody SFM[3]和multibody SLAM[4]提出基于运动分割SFM和SLAM用于动态环境中相机定位，但是需要事先得到运动目标和静止背景的先验信息或者从运动物体占大面积的图像中得到几何约束的缺点，所以不能很好地解决未知的车载环境下视觉SLAM定位问题。

现有的单目视觉的测距方法中，任工昌等人[5]提出一种由变焦镜头获得焦距与像距的双脉冲数，利用神经网络计算物距的单目视觉测距。朱丙丽等人[6]将裁剪后的图像输入多通道卷积神经网络，得到多个深度图候选对象，并将其映射合并为一个深度映射候选对象，最后通过傅里叶反变换生成最终估计深度图。STEIN等人[7]根据投影几何关系估计地面上物体深度和垂直物体高度。基于深度神经网络的单目深度估计虽然能够得到相对深度但是无法得到绝对深度，而几何测距方法存在需要事先标定相机安装外参参数的缺点。

目前，基于深度卷积网络的目标检测和分割已经达到了很高的精度，目标检测算法、语义分割算法及实例分割算法都能达到高检测精度。虽然二维图像目标检测能够精确地在图像中检测目标的位置，但是不能直接得到目标物体的三维信息。因此有人提出单目3D目标检测算法。现有的单目3D目标检测算法通过带有目标3D信息的标签有监督训练端到端的神经网络，直接通过神经网络输出目标6自由度姿态和尺寸，但是3D标签标注获取困难，会对网络训练样本数量带来影响，并且这类方法只能得到可能运动的物体，但是不能确定物体目前是否在运动。

本文针对以上不足，提出使用深度学习对图像实例分割，结合惯性传感器单元（Inertial Measurement Unit，IMU）预测得到的运动信息在分割结果上分离得到静止目标、运动目标，利用静止特征点进行视觉定位和三维重建。

1 相关工作

在动态环境中实现相机定位、静态目标定位以及动态目标深度和角度估计，需要分割图像中的运动目标，静态特征点用于视觉定位，估计运动目标深度和角度。

基于深度神经网络的图像目标检测和分割算法，能够精准识别图像中物体并用框或者像素级掩模标记目标在图像中的位置。深度卷积网络能够检测和分割车载环境下拍摄得到的目标，可以从预测标签中用人类先验知识判断目标是否会运动，比如树木和建筑物标签的物体是不会运动的，但是根据标签无法得知车辆、行人等可能会运动的目标是否在运动。在固定摄像头的情况下，JAIN等人[8]提出帧间差分法，该方法运算速度快，但一般不能完全检测出运动物体的所有像素点并且常常在检测到的运动物体内部出现“空洞”现象。针对帧间差分法的不足，人们提出基于统计的背景建模，WREN等人[9]提出单高斯模型的背景建模，STAUFFER等人[10]提出高斯混合模型背景建模方法，能够更好地适应复杂场景。基于非概率的背景建模方法中，LONG等人[11]提出基于样本一致性（SACON）的背景建模方法，BARNICH等人[12]提出ViBe算法，不需要假定任何的概率模型，计算速度快。静止的摄像机在监控环境下应用广泛，但是在车载环境中，摄像机在运动，图像中物体的运动来源于物体独立运动和相机运动的叠加，因此固定摄像头运动目标分割的方法在运动摄像头下大部分都失效。在运动摄像头情况下，基于动态物体先验知识的运动分割方法中，WANG等人[13]将运动物体上的SURF描述子构成一个集合，在SURF特征点检测时通过与集合中的描述比较来确定运动物体。虽然基于运动物体先验知识方法对退化运动比较有效，但是需要事先得到运动目标和静止背景的信息。基于几何约束的方法中，JUNG等人[14]采用随机投票的方式选出满足同一对极几何约束的特征点。基于几何约束的方法虽然不需要先验知识，通过跟踪以前帧中静止特征点计算运动，但是在运动物体占大面积的图像中静止特征点将会跟踪失败，因此在运动物体占大面积的图像中将失效。

目前，单目视觉SLAM根据特征点是否需要匹配可以分为特征点法和直接法两类。PTAM算法提出并实现了跟踪与建图并行处理，是第一个后端非线性优化的算法。ORB-SLAM算法继承了PTAM算法多线程的优点，采用ORB特征并使用三个线程（tracking，local mapping和loop closing）完 成SLAM。基于直接法的SLAM被提出用于克服基于特征点优化的SLAM在特征点提取匹配上比较耗时、特征点比较稀疏等缺点。ENGEL等人[15]提出的LSD_SLAM描述了像素梯度与直接法的关系以及像素梯度与极线方向在半稠密重建中的角度关系。直接法容易受到光照、曝光时间等影响，导致算法鲁棒性降低。ENGEl等人[16]提出了光度标定，认为对相机的曝光时间、暗角、伽马响应等参数进行标定后，能够提高鲁棒性。但在实际车载环境中，实际行车路线和数据集采集过程中采集车的行车路线有着不同之处，在实际行车过程中很少形成闭环，如果闭关检测错误，将引入很大的误差。

本文通过深度神经网络实例分割得到目标类型的先验知识，区分出静止的背景和可能会运动的目标；采用IMU信息估计相机运动，在可能会运动的物体中分类正在运动的目标和静止目标，利用静止特征点同时定位和地图重建，并且估计道路平面方程，得到可行驶区域。

2 理论基础

2.1 符号与定义

在本文中，定义(·)w为世界坐标系，z轴方向与重力对齐。为采样第kth帧图像时摄像机坐标系，为采样第kth帧图像时的IMU坐标系，如图1所示。

图1 坐标系

本文使用旋转矩阵R表示旋转，用下标表示旋转方向，如Rcw表示世界坐标系到摄像机坐标系的旋转。位移用P来表示，Pcw表示在世界坐标系下摄像机的位置。重力用gw=[0 0g]T表示，g=-9.81 m·s-2为重力加速度大小。用Spc表示摄像机坐标系下三维点坐标，Spw表示世界坐标系下三维点坐标。表示第kth帧相机运动状态，。本文用表示测量值。记李代数到李群的指数映射和李群到李代数的对数映射：

式 中：∅∈R3，⋅∧为反对称矩阵运算,。

2.2 IMU运动状态

惯性传感器单元（Inertial Measurement Unit，IMU）包含三轴加速度计和三轴陀螺仪，分别测量得到IMU坐标系下加速度和三维角速度。

此处忽略地球旋转的影响，认为地球为惯性参考系。

根据IMU运动学模型，假设在∆t时间内和是常数且IMU采样与摄像机采样同步，由于IMU采样速率比摄像机快，因此需要计算两帧之间IMU采样值的计算。第k帧和第k+1帧之间IMU采样j-i次，IMU运动方程如下：

2.3 摄像机投影模型

投影三维点到图像平面，首先要将世界坐标系中的点转换到摄像机坐标系中，如式（8）所示。

本文采用针孔摄像机模型，摄像机内参矩阵K事先标定得到式（9）。

3 系统框架

本文实现的视觉定位、三维重建与运动目标定位系统，总共由运动分割模块和视觉惯性里程计模块两个部分组成，如图2所示。

图2 系统框图

场景中有静止的背景如房子、道路，交通灯等，也有可能会运动的物体，如车辆和行人。运动分割模块的作用是将运动物体和静止物体分离出来，分离得到静止的背景、静止的物体以及运动的物体，从而将图像中的特征点分为运动物体上的特征点和静止物体上的特征点。运动分割模块为视觉惯性里程计提供静止的特征点，帮助视觉惯性里程计更具有鲁棒性。深度神经网络对图像进行实例分割，得到物体的分割掩模，没有被分割的部分为道路、房子等背景，被分割出来的物体为交通牌、行人和车辆等目标，通过先验知识可以判断物体能不能运动，但是实例分割不能判断当前帧物体是不是在运动。本文利用IMU估计运动和对极几何约束区分当前时刻正在运动的物体和静止物体。静止物体和背景上的特征点作为视觉惯性里程计的输入。

基于特征点的视觉里程计估计相机运动、特征点三维点位置以及IMU偏置。视觉里程计接收静止特征点作为输入，减少RANSAC迭代次数。每来一帧新图像，先用IMU预测相机运动Sck，用于运动分割。优化后得到的相机运动输出作为运动目标深度估计模块的输入。

4 运动分割

车载环境下，摄像机采集的图像有着三维环境场景大、摄像机运动速度快、图像中有大块快速运动物体等特点。视觉SLAM算法通过多幅图像中静止的特征点根据多视角几何关系完成同时定位与三维重建的任务，但是两帧图像中的特征点可能是误匹配，也可能是运动物体，视觉SLAM常常采用RANSAC算法排除这些异常值。

RANSAC算法是一种从带有异常值的数据中估计模型参数的算法。视觉SLAM利用RANSAC算法从图像匹配好的特征点对中估计相机旋转R和位移t，其中异常值为误匹配的特征点对和位于运动物体上的特征点。RANSAC算法从观测的特征点对中随机采样最小点对集合，通过最小点对集合计算模型假设，通过其他点对验证模型假设，反复采样N次之后，选择最符合其他点对的模型假设作为最佳模型。采样次数N是RANSAC算法成功与否的非常重要的参数，如式（10）所示：

式中：s为随机采样最小点对集合的大小，ε为异常值占所有数据的比例，p为置信度。在置信度为p的情况下，N次采样中，至少有一次采样得到的最小点对集合中的点对全部为正常值。以八点法为例，s=8，异常值比例ε=60%，要达到p=99%的置信度，需要迭代至少7 025次。当静止特征点在图像中占主要部分时，RANSAC算法能发挥用处，但是在车载环境中，摄像机前有大量动态物体或者被运动物体遮挡大部分画面时，RANSAC算法将会失效，因此，找出图像中正在运动的物体显得尤为重要。

4.1 基于几何约束的运动目标分割

图像上二维点的运动是二维点对应的三维点在空间中的独立运动和相机运动造成的。因此，想要找出独立运动的物体，需要排除相机运动的干扰。对极几何约束是两个视角之间的投影几何关系，如图3所示，对于静止的三维点P在两帧的投影x1、x1´满足x1´TFx1=0。其中F矩阵为基本矩阵（fundamental matrix），，对极线l2=Fx1。对于非退化的运动，若P点运动到P´点，P´在第二帧上的投影x2´将不满足对极几何约束。

图3 对极几何约束

本文采用基于卷积神经网络的物体分割方法，用Mask R-CNN对图像中的物体进行实例分割，确定图像中的物体，如图4所示。通过Mask R-CNN输出的物体标签，将物体分为不会运动的物体（如交通灯）和会运动的物体（如汽车），但是还不能确定这辆汽车是停靠在路边的汽车还是开动的汽车。

图4 语义分割和运动分割的对比图

在不知道两帧运动之前，视觉SLAM算法采用八点法或五点法，用RANSAC的方法从特征点集中计算基本矩阵F并分解得到相机运动，同时分辨出异常值。但是这在高动态环境中是不适用的，在不知道相机运动的情况下无法得到基本矩阵F，在没有基本矩阵F的情况下不能得到正常值计算相机运动。本文利用惯性传感器信息，在视觉惯性里程计优化之后用IMU测量得到的加速度和角速度估计相机运动，得到下一帧图像的旋转R和位移t，用于计算基本矩阵F。

本文采用图像上特征点到对极线的距离来评价3D点运动与否，如式（11）所示。

式中：Fx1(i)表示Fx1向量第i个元素。图像经过Mask R-CNN实例分割的多个刚体，对于同一物体上的特征点满足相同基本矩阵F，因此采用RANSAC算法去除同一个物体上错误的特征点匹配。第k个物体上的特征点到对极线的平均距离如式（12）所示。

若同一个物体上特征点到对极线的平均距离EK>threshold则判定物体为独立运动物体。判决阈值threshold的选取如图5所示，选取运动目标与静止目标最开始相交的点作为判决阈值。

图5 对极几何约束误差直方图

4.2 摄像机运动估计和地面建模

由于车载视频摄像机运动较大，直接法容易陷入局部最优解，并且直接法容易受到室外光照变化的影响，因此本文选择较为鲁棒的基于特征点的视觉SLAM。由于应用场景不同，与室内场景和AR应用不同，在车辆行驶过程中很少有闭合回路的情况出现，在高速公路上更不可能闭合回路，因此回环检测线程对于车载环境来说是没有必要的。本文去除回环检测线程，保留前端跟踪线程和后端优化线程。

视觉里程计前端跟踪模块用于计算两帧之间的相机运动，包括旋转和位移。前端跟踪模块将地图中的三维点投影到当前帧并与当前帧特征点匹配，通过优化当前帧j上所有匹配上的特征点的重投影误差得到相机运动。

后端优化模块的作用是减少误差累积。后端优化是对连续n个关键帧状态和h个关键帧能观测到的三维点进行优化。当有新的一帧被确认为关键帧时，开启后端local BA优化地图中n个关键帧能够观测到的三维点和相机状态。

式中：f为待优化帧的集合，g为待优化三维点的集合。

假设图像中下方1/3、中间1/5的位置是地面，且地面是一个平面。视觉SLAM根据静态背景的特征点已经估计出两帧之间相机相对位姿{R,p}。图像中位于地面上的特征点在三维中处于同一个平面，根据平面单应性质，第i帧同一个平面特征点映射到第j帧xj´=Hxi，其中H为单应矩阵，{n,d}为平面方程。第j帧与第i帧匹配特征点得到观测量xk。为了得到路面方程，以重投影误差建立目标函数，优化目标函数得到地平面方程{n,d}。

式中：eHk=xj´-xj。估计得到路面的平面方程y=nTx+d。距离地面10 cm以上的三维点被认定为静态障 碍物。

4.3 基于运动分割的单目视觉定位

本文构建的系统在KITTI数据集[17]上测试。KITTI是无人驾驶数据库，包含长达将近50 km不同场景下的行车图像以及激光雷达和IMU信息。这个历程及序列中，相机旋转和位移速度较快，还包含部分运动目标，对于单目视觉具有一定挑战性。序列00-10提供每帧相机位置和姿态，通过相似变换让每帧轨迹与真实轨迹对齐，测量绝对路径误差。所有实验在Intel Core i7-4810MQ（2.8 GHz，4 cores）的CPU和8 GB内存的笔记本电脑上运行，可以达到实时的要求。

表1显示了每帧估计的轨迹均方根误差（Root Mean Square Error，RMSE）以及真实地图的大小。本文分别比较了不做任何处理的ORB-SLAM算法和运动分割的ORB-SLAM算法的轨迹RMSE。由表1可知，有运动分割比没有运动分割的ORBSLAM算法在序列02-06、08-10上的RMSE好1.91%～23.75%，序列00、07分别变差6.78%、13.2%。运动分割有助于车载环境下提高单目SLAM的鲁棒性和精确度。图6为显示相机运动轨迹对比图。

图6 相机运动轨迹

表1 KITTI视觉里程计测试

5 结 语

本文提出的深度神经网络实例分割结合IMU可以有效实现运动对象、静止对象及静止背景的分离，克服图像中运动物体占图像大部分面积时无法运动分割的缺点。在KITTI车载数据库上的实验表明，本文提出的方法可以精确地估计自身运动，重建静态三维场景。