基于几何约束和目标检测的室内动态SLAM

高逸，王庆，杨高朝，刘鹏飞

(东南大学 仪器科学与工程学院,南京 210096)

0 引言

即时定位与地图构建(SLAM)指系统在未知的环境中同时估计自身位置和构建周围地图的过程[1].它是实现移动机器人、自动驾驶、增强现实和无人机的核心技术[2].近几年，以相机为传感器的视觉SLAM受到研究者的重视，产生了一些显著的成果[3-6].

传统SLAM 通常假设周围环境是静止的，但真实环境的运动物体会给视觉里程计(VO)带来错误的数据关联，从而降低系统的精度和鲁棒性，导致后续建立的地图也无法使用[7].为了提高SLAM 系统在动态环境的效果，需要估计运动物体的位置，根据原理的不同可分为基于几何约束和深度学习两种方法[8].

几何约束方法利用系统的静态模型剔除离群的动态点，如SUN 等[9]通过随机抽样一致性(RANSAC)方法和相邻图片像素可以有效剔除低动态环境中孤立的动态点，但当动态点占据多数时，该方法难以将运动物体完全剔除.张慧娟等[10]利用匹配的特征点估计初始位姿，根据静态线特征计算相机的真正运动，提高了系统在动态环境的定位精度.

深度学习方法通过神经网络检测并剔除运动物体上的动态点，如SHENG 等[11]通过Mask R-CNN(Regional CNN，CNN 最早应用于检测和分割问题的方法)[12]识别环境中的动态区域，当某个像素及其邻域都不属于动态区域时才判定为静态点，从而有效剔除了环境中的潜在动态点.YU 等[13]通过SegNet[14]语义分割网络和运动一致性移除环境中的动态点，该方法在数据集上取得了不错的结果，但却不能在真实环境中实时运行.针对分割网络不能满足SLAM 系统实时性的问题，一些学者利用目标检测网络来获取环境中的语义信息，从而提高了系统运行的效率.YANG 等[15]利用目标检测网络YOLOv3[16]移除环境中的运动物体，然后根据位姿进一步判断特征点的真实状态.ZHONG 等[17]通过检测结果和运动概率模型剔除环境中的动态点，从而有效减少了运动物体的影响，但该方法不能剔除人为移动桌椅所导致的定位精度下降的影响.

几何约束方法在低动态环境下可以取得不错的效果，但当面对快速运动的物体时易出现错误.深度学习方法可以有效处理大多数动态环境问题，但当先验的静止物体运动时会保留大量动态点，而且许多基于分割的SLAM 系统通常无法在真实环境中实时运行.

针对上述问题，本文提出一种基于几何约束和目标检测的室内动态SLAM 方法.使用目标检测网络YOLOv5 获取语义信息，提出运动物体漏检的方法；根据先验知识，提出准确识别动态区域的信息判定方法；结合几何约束和深度学习方法剔除动态点，利用静态点估计相机位姿；根据存储信息构建可闭环的静态地图.

1 室内动态SLAM 方法

本文的系统框架如图1 所示.目标检测模块使用YOLOv5 获取语义信息，根据上一帧的检测结果添加漏检的运动物体；信息判定模块根据检测结果和先验知识准确识别动态区域；动态点剔除模块首先通过深度学习初步估计相机运动，然后利用几何约束进一步剔除动态点，最终根据保留的静态点计算真正的位姿；静态地图模块根据存储信息构建可闭环的静态地图.实线框为新增模块，虚线框为已有模块.

图1 系统框架

1.1 轻量级网络及运动物体漏检方法

针对基于分割的SLAM 系统不能实时运行的问题，本文采用YOLOv5 网络来获取语义信息.与其他目标检测算法不同，它利用一个端到端的网络实现更快速的检测任务.YOLOv5 作为YOLO 系列的最新版本，体积仅为原来的1/9，在V100 GPU 检测速度达到400 FPS/s，完全满足系统的实时性要求.

当运动物体移动到图像边缘时易出现漏检，从而导致系统保留大量的动态点.针对上述问题，本文提出运动物体的漏检方法，如果i时刻的运动物体数量和i1时刻数量不相等，根据下式判断i时刻运动物体是否出现漏检：

式中：x为i1 时刻的运动物体；y为i时刻的运动物体；y0为i时刻的所有运动物体的数量；Di1,x为i1 时刻运动物体的位置；Di,yj0＜y＜y0为i时刻所有运动物体的位置；dx为i1时刻运动物体和i时刻所有运动物体的最小距离；Xi1,x为i1 时刻运动物体的位移.

1.2 基于先验知识的信息判定方法

信息判定方法根据检测结果和先验知识准确识别动态区域.室内物体根据先验知识分为三类：运动物体、相对物体和静止物体，如表1 所示.运动物体是具有主动运动状态的物体，如人和宠物.相对物体是具有被动运动状态的物体，如被人移动的椅子.静止物体是具有绝对静止状态的物体，如不可移动的柜子和冰箱，拿起的书籍和杯子等具有被动运动状态的小物体在几何约束中当作离群值剔除，也视为静止物体.

表1 室内物体

传统方法仅剔除人所在的动态区域，在面对极端情况时易出现如下问题：

1) 当运动物体占据大部分区域时，提取的静态特征点数量过少导致视觉里程计跟踪失败；

2) 当人移动椅子时，系统误将其当作静止物体，从而保留大量的动态特征点.

针对上述问题，本文提出准确识别动态区域的信息判定方法：

1) 当运动物体和静止物体重叠时，两者重叠的区域视为静态，避免极端情况下静态特征点数量不足的情况；

2) 当运动物体和相对物体重叠时，当满足下式时，剔除相对物体的潜在动态点

式中：S重合表示运动物体和相对物体的重合面积；S相对表示相对物体的面积.

1.3 动态点剔除原理

动态点剔除原理如表2 所示.根据深度学习结果初步估计相机位姿，此时系统中仍存在少量动态点，需要结合几何约束进一步剔除动态点，从而计算相机真正的运动.

表2 动态点剔除

极线约束原理如图2 所示.空间点P在相机1 和相机2 的像素点分别为p1和p2，光心O1和光心O2的连线与像平面的交点分别为极点e1和极点e2，投影点和极点的连线分别为极线l1和极线l2.

图2 极线约束

根据针孔相机模型，像素点p1和p2满足：

式中：s1表示空间点P在相机1 的深度；s2表示空间点P在相机2 的深度；R和t表示相机1 转换到相机2 的旋转和平移.

像素点坐标转换为归一化坐标：

式中：K表示相机内参；x1表示空间点P在相机1 的归一化坐标；x2表示空间点P在相机2 的归一化坐标.

经过一系列变换得到极线约束：

式中：F表示相机1 转换到相机2 的基础矩阵.

计算像素点到极线的距离，若距离大于阈值 ε，剔除像素点的匹配关系：

式中：l2表示像素点p1在相机2 的极线；a、b、c分别表示极线l2的三维坐标；d表示像素点p2到极线l2的距离.

根据静态特征点构建重投影误差函数，计算相机的真实位姿为

式中：T表示相机的位姿；u表示像素坐标.

1.4 构建静态地图

传统方法构建的地图仅利用空间的几何信息，不能有效剔除环境中的运动物体，导致地图中出现运动物体的重影.本文方法根据目标检测获取的语义信息构建不包含运动物体的静态地图，当检测到闭环时根据存储的信息重新构建地图，提高了系统在动态环境下的地图效果，具体如表3 所示.

表3 静态地图

2 实验与分析

为了验证本文方法的效果，在TUM 数据集[18]和室内真实环境进行实验.TUM 数据集包含两种动态序列：在walking 高动态序列中，两个人围绕办公桌来回走动并不时移动一些物体；在sitting 低动态序列中，两个人坐在办公桌旁一边交谈一边打手势.室内真实环境通过已标定的D435i 深度相机采集宿舍生活场景：一个人坐在椅子上，随后起身，一边交谈一边四处移动.

2.1 TUM 数据集

2.1.1 动态点剔除

本文方法和ORB-SLAM2 的动态点剔除结果，如图3 所示.图3(a)中，ORB-SLAM2 保留了人和移动椅子上的动态点，从而产生了大量的误匹配，降低了系统的精度和鲁棒性.图3(b)中，本文方法利用几何约束和深度学习有效剔除了行走的人和移动椅子上的动态点，仅使用静态特征点估计位姿，从而实现了更好地定位效果.

图3 本文方法和ORB-SLAM2 的动态点剔除

2.1.2 定位精度

系统在室内动态环境的定位精度使用绝对轨迹误差(ATE)作为评价指标.为了降低不确定性的影响，最终结果取15 次运行的中间值.

本文方法和ORB-SLAM2 的定位精度，如表4所示.本文方法在高动态序列的定位精度比ORBSLAM2 提升了一个数量级，在低动态序列的提升则相对较小.原因在于：在高动态序列中人和椅子发生了明显移动，本文通过深度学习和几何约束有效剔除了动态点，而ORB-SLAM2 误将其视为静态区域从而产生大量的误匹配.在低动态序列中人基本处于静坐状态，只有局部肢体发生运动，ORB-SLAM2 通过剔除少量离群点实现了准确定位，本文方法改进效果有限.

表4 本文方法和ORB-SLAM2 的定位精度

本文方法和ORB-SLAM2 在fr3/walking_xyz下的运行轨迹，分别如图4～5 所示.图4 中，ORB-SLAM2受运动物体影响导致估计轨迹和真实轨迹产生了较大的偏差.图5 中，本文方法通过剔除动态点，估计轨迹和真实轨迹基本重合.

图4 ORB-SLAM2 的运行轨迹

图5 本文方法的运行轨迹

本文方法和DS-SLAM 的性能，如表5 所示.本文方法的运行时间远小于面向动态环境的语义视觉SLAM(DS-SLAM)，原因在于：DS-SLAM 对每一幅图片进行像素级别地处理，而本文方法通过YOLOv5缩短了获取语义信息的时间，提高了系统运行的效率.本文和DS-SLAM 的定位精度互有优劣，但在walking_xyz和waling_rpy 中却明显优于DS-SLAM，原因在于：DS-SLAM 仅将人所在的区域视为动态而未考虑移动椅子产生的大量动态点，同时SegNet 分割网络对运动物体的局部轮廓分割效果较差.本文通过深度学习剔除了人和移动椅子上的大部分动态点，利用几何约束剔除了移动书籍和端起杯子的少量动态点，从而实现了高精度的定位结果.

表5 本文方法和DS-SLAM 的性能

2.1.3 静态地图

本文方法和ORB-SLAM2 在fr3/walking_xyz序列下的静态地图结果，如图6 所示.图6(a)中，ORBSLAM2 构建的静态地图因含有许多重影从而不能正常使用.图6(b)中，本文方法根据相机位姿和语义信息剔除了环境中的运动物体，从而准确构建了周围环境的静态地图.

图6 本文方法和ORB-SLAM2 的静态地图

2.2 室内真实环境

2.2.1 动态点剔除

室内真实环境的动态点剔除结果，如图7 所示.图7(a)中，ORB-SLAM2 未能剔除人身上的大量动态点，从而降低了系统的定位精度，导致构建的静态地图也出现错误.图7(b)中，本文方法有效剔除了人身上的动态点，根据保留的静态特征点准确估计相机位姿.

图7 室内真实环境的动态点剔除

2.2.2 静态地图

室内真实环境的静态地图结果，如图8 所示.图8(a)中，ORB-SLAM2 构建的地图不仅含有人的重影，错误的定位结果也导致地图的其他结构出现错误.图8(b)中，本文方法构建的静态地图不仅有效剔除了环境中的运动物体，而且完整再现了室内的空间结构.

图8 室内真实环境的静态地图

3 结束语

针对室内动态环境下定位精度低和地图效果差的问题，提出一种基于几何约束和目标检测的室内动态SLAM 方法.使用YOLOv5 获取语义信息，提出运动物体的漏检方法；根据先验知识，提出准确识别动态区域的信息判定方法；结合几何约束和深度学习剔除动态点，利用静态点估计相机位姿；根据存储信息构建可闭环的静态地图.在TUM 数据集和室内真实环境进行实验，有效提高了室内动态SLAM 的定位精度和地图效果，但仍然存在一些缺陷，当环境特征缺失和相机运动过快时会出现跟踪失败的问题，后续可以通过添加线面特征和惯性测量单元(IMU)来提高系统的鲁棒性.