即时定位与地图构建(simultaneous localization and mapping，SLAM)在无人驾驶、增强现实技术等领域都发挥着重要作用.相对于传统传感器，视觉传感器采集的图像可以提供更多的环境信息，得到了研究者的广泛关注，提出了许多基于视觉传感器的SLAM算法，如ORB-SLAM2[1]，LSD-SLAM[2]、DSO[3]等，然而上述SLAM算法均假设相机所处环境是静态的，当场景中存在动态物体时其定位性能将明显下降.
实际上，在视觉SLAM应用中总会遇到运动的物体，如路上行驶的汽车、走路的行人等.这些动态物体会给视觉SLAM带来误差，并得到错误的位姿估计，最终会导致定位和建图精度的严重下降.尽管ORB-SLAM2采用RANSAC算法能去除小的运动物体引入的外点[4]，从而提高动态环境下的鲁棒性，然而当动态物体在图像中占比较大时，该算法就无法解决此场景下的视觉SLAM准确定位问题.
目前，关于动态环境下的视觉SLAM准确定位问题已经得到了一些研究成果，主要分为直接法[5-7]和多传感器融合方法[8-9].在直接法中，主要通过稠密的场景流、光流检测稀疏特征和计算图像的强度差来检测动态物体；在多传感器融合方法中，主要通过惯性数据补偿特征点运动，并进行滤波和最小化能量函数来消除动态物体的影响.然而在上述方法中，直接法需要额外的深度信息，多传感器融合法需要额外的传感器，在仅使用RGB图像作为唯一输入的情况下，上述两种方法均不能解决有效点较少的动态复杂场景下视觉SLAM准确定位问题.
鉴于以上分析，本文考虑了仅使用单目视觉传感器，在有效点较少的动态复杂场景下，提出了一种基于动态特征点剔除的单目视觉SLAM算法，主要工作有：1)针对有效点较少的情形，提出了ARANSAC算法，从而准确地估计图像间的透视变换矩阵，并用其补偿相机的运动；2)提出了一种基于光流法识别动态特征点的算法，在有效点较少的动态场景中，该算法可以准确地检测出动态特征点；3)在TUM数据集上进行了仿真，并与ORB-SLAM2算法进行对比，结果证明了基于动态特征点剔除的单目视觉SLAM算法的有效性.
1 算法流程
本文提出的算法利用单目视觉传感器捕获原始RGB图像后，在ORB-SLAM2算法中提取特征点并进行动态特征点的检测与剔除，从而仅利用场景中的静态特征点进行SLAM，即场景中的动态物体不再影响相机的定位，整体流程如图1所示.
图1 基于动态特征点剔除的单目视觉SLAM算法框架
Fig.1 Framework of monocular vision SLAM algorithm based on dynamic feature point elimination
2 基于动态特征点剔除的单目视觉SLAM算法
2.1 动态特征点检测算法
本文提出的动态特征点检测算法由两个模块组成，可分为基于ARANSAC算法的自我运动估计和基于光流法的检测.
2.1.1 基于ARANSAC算法的自我运动估计
自我运动估计模块是以上一帧和当前帧的连续两帧图像作为输入，通过提取上一帧ORB特征点并采用Lucas Kanade方法[10]寻找其在当前帧的匹配点.如果匹配对中心3×3图像块的像素差太大则会放弃该匹配，以此减少误匹配的影响.考虑环境中有效点较少的情况(场景中静态特征点的数量少于动态特征点的数量)，提出ARANSAC算法估计前后两帧图像间的透视变换矩阵，并用透视变换矩阵扭曲上一帧获得一个估计图像，目的是使上一帧中的点转换到当前帧的坐标系下.ARANSAC算法的具体步骤如下：
1) 生成最小样本.将输入图像平均分成8×8的区域，然后随机选取4个区域，分别在所选区域内取一个特征点，构建最小模型.
2) 构建模型评价.给定一个评价模型，计算该模型的透视变换矩阵，并对所有匹配特征点对进行投影，从而判断该特征点是有效点还是无效点.评价模型为
(1)
(2)
(3)
式中：i为有效区域；εi为一个区域中有效点的比例；xi为有效区域的中心位置；A为输入图像的面积；C为有效点的协方差矩阵；s为最终的模型评价值.
3) 设定终止条件.当最佳模型无效点的概率满足式(4)条件时，立刻停止迭代，即
(4)
式中：σi为最佳模型对应区域有效点占比；pi=1/N，N为有效区域的个数；m为常数4；KS为迭代次数；η为设定的常数.
2.1.2 基于光流法的检测
自我运动估计后，估计图像中的点与当前帧中的点会被统一在相同的坐标系下，利用Lucas Kanade计算特征点在当前帧和估计图像之间的光流值，并设定阈值τ来判断特征点是静态的还是动态的，其表达式为
(5)
式中：
为特征点fi的2范数；Fdynamic和Fstatic分别为动态和静态特征点集.
2.2 剔除动态特征点的单目视觉SLAM算法
文中单目视觉SLAM算法是在ORB-SLAM2基础上，增加了动态特征点检测并剔除的功能，以此减少环境中的动态物体对视觉SLAM系统定位精度的影响.每输入一帧图片后，先进行自我运动估计和动态特征点检测，然后剔除ORB-SLAM2中的动态特征点并计算剩余特征点的描述子，用静态特征点进行SLAM.
3 实验结果与分析
为验证本文算法的有效性，本文用TUM数据集中的freiburg1序列和freiburg3序列进行算法验证，实验计算机硬件为联想笔记本电脑，CPU为Intel i5，4GB内存，软件平台为Ubuntu16.04系统.freiburg3_walking序列属于高动态环境，一些动态物体的占比会超过50%以上(即有效点较少的情形).本文利用以下四个高动态图像序列进行实验分析，相机运动类型如下：
1) freiburg3_walking_static(fr3/w/static)：相机维持微小运动.
2) freiburg3_walking_xyz(fr3/w/xyz)：相机沿着x-y-z方向移动.
3) freiburg3_walking_rpy(fr3/w/rpy)：相机沿着r-p-y三个方向旋转.
4) freiburg3_walking_halfsphere(fr3/w/half)：相机沿半球形轨迹移动.
3.1 动态特征点检测实验
本文提出的基于ARANSAC动态特征点检测算法的目标是从所有提取的特征点中正确区分出场景中的动态特征点.为了验证该算法的有效性，分别从TUM数据集中的fr3/w/rpy、fr3/w/static、fr3/w/xyz和fr3/w/half序列选取连续两帧图像(图像中的两个人正在桌子前行走，位置不断变化)进行实验，并与基于RANSAC的检测算法进行对比，结果如图2所示.
图2中，对于每一列，最上面是原始的RGB图像，中间为基于RANSAC算法的检测结果，最下面是基于文中ARANSAC算法的检测结果.彩色点表示从当前图像中提取的特征点，红点是动态的，绿点是静态的.从图2中可以看出，对于fr3/w/xyz序列，图像中运动物体占比较少，此时基于RANSAC算法和基于ARANSAC算法的检测结果基本一致，都能正确检测出场景中的动态物体.但是当运动物体在图像中占比较大时，提取的动态特征点增多，如fr3/w/rpy序列动态特征点占比在71.17%左右，fr3/w/static占比在68.04%左右，fr3/w/half占比在55.17%左右，此时利用RANSAC算法将会错误地估计透视变换矩阵，从而不能准确地补偿相机运动，最终错误地识别了正在运动的人身上提取的特征点为静态点，而场景中静态物体(如放置的椅子和电脑)反而被识别为动态的.
图2 动态特征点检测算法实验对比结果
Fig.2 Experimental comparison results for dynamic feature point detection algorithms
利用本文提出的ARANSAC动态特征点检测算法，可以准确地估计透视变换矩阵，进而准确地补偿相机运动，因此能够正确区分场景中的动态特征点和静态特征点，这对视觉SLAM后续稳定工作起到至关重要的作用.实验表明，即使在有效点较少的情况下，本文提出的基于ARANSAC检测算法依然能够正确区分场景中的动态特征点.
3.2 与ORB-SLAM2算法对比
为了体现本文算法在动态环境下的定位性能，实验中，采用绝对轨迹误差(ATE)和相对位姿误差(RPE)作为评价指标，表1～3分别为本文SLAM算法和ORB-SLAM2算法的对比结果.
表1 绝对轨迹误差结果
Tab.1 Results of absolute trajectory error
序列ORB-SLAM2/mRMSES.D本文算法/mRMSES.D变化百分比/%RMSES.Dfr3/w/half0.48770.23070.03360.015693.1193.24fr3/w/rpy0.92610.40310.02770.015797.0196.11fr3/w/xyz0.84960.47680.08520.076489.9784.00fr3/w/static0.41830.07630.00840.004197.9994.63fr3/s/xyz0.01130.00580.00930.004917.7015.52fr1/desk0.01600.00990.01630.0092-1.877.07
注：RMSE为均方根误差；S.D为标准偏差.
表2 旋转漂移结果
Tab.2 Results of rotating drift
序列ORB-SLAM2/(°)RMSES.D本文算法/(°)RMSES.D变化百分比/%RMSES.Dfr3/w/half6.81765.55340.75560.361288.9293.50fr3/w/rpy7.58844.82810.82570.442689.1290.83fr3/w/xyz6.66014.49781.62421.387875.6169.14fr3/w/static3.86343.48680.27470.124292.8996.44fr3/s/xyz0.48970.30000.48030.26001.9213.33fr1/desk1.66671.02891.40090.640715.9537.73
表3 平移漂移结果
Tab.3 Results of translation drift
序列ORB-SLAM2/mRMSES.D本文算法/mRMSES.D变化百分比/%RMSES.Dfr3/w/half0.33220.27140.03150.016390.5294.00fr3/w/rpy0.39180.25120.04090.024989.5690.08fr3/w/xyz0.35880.22920.07880.070378.0469.33fr3/w/static0.21690.01540.01110.005894.8862.34fr3/s/xyz0.01340.00790.01190.006411.1918.99fr1/desk0.02340.01330.02310.01161.2812.78
由表1～3结果可知，在六个运动序列实验中，fr1/desk序列属于静态序列，对比ATE和RPE指标可知，本文所提算法的均方根误差和标准偏差均变化不大，可以看出本文算法和ORB-SLAM2算法的估计结果非常相似，保持在同一个精度水平.fr3/s/xyz序列属于小范围动态环境，对比ATE和RPE指标可知，本文所提算法的均方根误差和标准偏差均略有下降，可以看出本文算法的定位精度略优于ORB-SLAM2算法.
实验中，fr3/w四个运动序列属于高动态环境，与ORB-SLAM2算法相比，本文算法在增加了动态特征点检测与剔除功能后，相机位姿估计值与真实值之间的误差大大降低.对比ATE指标可知，本文所提算法的均方根误差和标准偏差均明显下降，最佳效果分别降低97.99%和96.11%；对比RPE指标可知，旋转漂移的均方根误差和标准偏差最佳效果分别降低92.89%和96.44%，平移漂移的均方根误差和标准偏差最佳效果分别降低94.88%和94.00%.结果表明，在有效点较少的动态环境下，本文算法对四个运动序列估计的轨迹与原始轨迹之间的误差大大降低，提出的单目视觉算法能够显著提高视觉SLAM的定位精度.
为了进一步说明本文提出的单目视觉SLAM算法的定位效果，可视化对比了序列估计值与真实值之间的误差，分别给出了fr3/w/half和fr3/w/xyz序列在ORB-SLAM2和本文算法上运行的ATE和RPE结果，如图3～6所示.
图3 两种算法在fr3/w/half序列上的ATE图
Fig.3 ATE graphs of two algorithms on fr3/w/half sequence
图4 两种算法在fr3/w/half序列上的RPE图
Fig.4 RPE graphs of two algorithms on fr3/w/half sequence
图5 两种算法在fr3/w/xyz序列上的ATE图
Fig.5 ATE graphs of two algorithms on fr3/w/xyz sequence
图6 两种算法在fr3/w/xyz序列上的RPE图
Fig.6 RPE graphs of two algorithms on fr3/w/xyz sequence
图3、4分别给出了fr3/w/half序列在两种算法下的相机轨迹，可以看出，ORB-SLAM2算法估计的相机轨迹在fr3/w/half序列下发生了较大偏移，最大误差可达1.50 m左右，而本文算法估计的相机轨迹与原始轨迹基本吻合，误差在0.12 m左右；图5、6分别给出了fr3/w/xyz序列在两种算法下的相机轨迹，可以看出，ORB-SLAM2算法估计的相机轨迹在fr3/w/xyz序列下也发生了较大偏移，最大误差可达2.60 m左右，同样本文算法估计的相机轨迹与原始轨迹基本吻合，误差在0.10 m左右.上述结果表明，在两种动态场景下，本文方法明显减少了相机位姿的估计误差.因此，在有效点较少的动态环境下，基于ARANSAC动态特征点剔除的单目视觉SLAM算法，能够有效消除动态物体对视觉定位性能的影响，提高定位精度.
为了说明本文所提算法的实时性，表4给出了本文算法和ORB-SLAM2算法在各个模块中的运行时间.
表4 算法耗时比较
Tab.4 Time consuming comparison of algorithms ms
模块ORB-SLAM2本文算法跟踪线程32.5577.32动态特征点检测045.02
从表4中跟踪每帧图像的平均耗时可以看出，本文算法比ORB-SLAM2算法运行时间略长，这主要是因为本文算法不仅要跟踪每帧图像，还要在跟踪线程中加入动态特征点检测及剔除功能，减少动态物体对算法定位性能的影响.虽然本文算法运行时间有一定延长，但是在很小时间代价下可以明显地提高单目视觉SLAM算法在动态复杂场景下的定位精度，实时性也能够满足一般机器人系统的要求.
4 结 论
本文研究了有效点较少的动态复杂场景下视觉SLAM准确定位问题，提出了一种基于ARANSAC动态特征点剔除的单目视觉SLAM算法.考虑动态场景下有效点较少的情况，提出了ARANSAC算法估计图像间的透视变换矩阵，并用透视变换矩阵扭曲上一帧获得一个估计图像，使得上一帧中的点被统一到当前帧的坐标系下.通过光流法计算特征点在当前帧和估计图像之间的光流值，进而对动态特征点和静态特征点进行判断，然后用获得的静态特征点进行SLAM运算.通过仿真结果对比分析，在有效点较少的动态复杂场景下，本文算法显著减少了位姿估计误差，有效提高了单目视觉SLAM的定位精度.
[1] Mur-Artal R，Tardós J D.ORB-SLAM2：an open-source SLAM system for monocular，stereo，and RGB-D cameras [J].IEEE Transactions on Robotics，2017，33(5)：1255-1262.
[2] Engel J，Schöps T，Cremers D.LSD-SLAM：large-scale direct monocular SLAM [M].Munich：Springer International Publishing，2014.
[3] Engel J，Koltun V，Cremers D.Direct sparse odometry [J].IEEE Transactions on Pattern Analysis and Machine Intelligence，2018，40(3)：611-625.
[4] 房立金，刘博，万应才.基于深度学习的动态场景语义SLAM [J].华中科技大学学报(自然科学版)，2020，48(1)：121-126.
(FANG Li-jin，LIU Bo，WAN Ying-cai.Semantic SLAM based on deep learning in dynamic scenes [J].Journal of Huazhong University of Science and Technology (Natural Science Edition)，2020，48 (1)：121-126.)
[5] Alcantarilla P F，Yebes J J，Almazan J，et al.On combining visual SLAM and dense scene flow to increase the robustness of localization and mapping in dynamic environments [C]//Proceedings of IEEE International Conference on Robotics & Automation.Piscataway，USA，2012：1290-1297.
[6] Wang Y，Huang S.Motion segmentation based robust RGB-D SLAM [C]//Proceedings of Intelligent Control & Automation.Piscataway，USA，2015：3122-3127.
[7] Sun Y，Liu M.Improving RGB-D SLAM in dynamic environments：a motion removal approach [J].Robo-tics and Autonomous Systems，2017，89：110-122.
[8] Kim D H，Han S B，Kim J H.Visual odometry algorithm using an RGB-D sensor and IMU in a highly dynamic environment [J].Robot Intelligence Technology and Applications，2015，345：11-26.
[9] Usenko V，Engel J，Stückler J，et al.Direct visua-linertial odometry with stereo cameras [C]//2016 IEEE International Conference on Robotics and Automation (ICRA).Stockholm，Sweden，2016：1885-1892.
[10] Simon B，Iain M.Lucas-Kanade 20 years on：a unifying framework [J].International Journal of Compu-ter Vision，2004，56(3)：221-255.