面向动态环境的双目/惯性SLAM系统

张乐添，赵冬青，贾晓雪，杨朝永，赖路广，郭文卓

面向动态环境的双目/惯性SLAM系统

张乐添，赵冬青，贾晓雪，杨朝永，赖路广，郭文卓

（信息工程大学地理空间信息学院，郑州 450001）

基于静态场景假设的即时定位与建图(SLAM)系统仍存在诸多问题：传统视觉SLAM中的关键帧并不包含语义信息，在实际场景中总包含大量动态点，不仅会影响系统精度，甚至可能出现跟踪失败。针对以上问题，提出一种语义SLAM思路。首先，提出利用掩膜区域卷积神经网络（Mask R-CNN）给出的先验语义信息进行筛选，摒弃动态点的干扰，将这项功能作为新的线程加入尺度不变特征变换即时定位与建图（ORB-SLAM3）。其次，结合ORB-SLAM3的双目/惯性里程计部分，基于惯导预积分协方差设置自适应阈值，判断先验动态点的真实动态性。最后，利用实测数据对算法进行验证。实验结果表明，3个数据集序列绝对轨迹误差分别降低56.3%、71.5%、19.4%。因此，该系统能够有效判别特征点的真实动态性，屏蔽动态点的干扰，从而提高定位精度。

动态环境；即时定位与建图；预积分；语义分割；神经网络

0 引言

近年来，视觉即时定位与建图（simultaneous localization and mapping, SLAM）的整体框架已趋近成熟。当前成熟的开源方案[1-4]一般都基于静态环境的假设，然而真实环境往往难以满足条件[5]。由于系统估计的是载体与物体之间的相对运动，此类环境下运行会分不清载体与物体的运动，而最终导致绝对定位信息不准确、环境地图构建发生严重的偏差等问题[6]。

随着深度学习算法的迅速发展，基于卷积神经网络的目标检测算法与视觉SLAM相结合，取得了良好的性能。文献[7]基于尺度不变特征变换即时定位与建图（oriented fast and rotated brief simultaneous localization and mapping，ORB-SLAM2），结合目标检测（you only look once，YOLO）[8]算法对检测到的关键帧进行检测，剔除位于矩形框内的特征点。然而，仅给出矩形框难免会影响筛选的精度，但是语义分割方法可达到像素级别从而识别环境中动态特征点的先验信息。文献[9]增加了一个基于语义分割网络（segmentation network，SegNet）[10]的语义分割线程，移除先验目标，利用随机抽样一致算法（random sample consensus, RANSAC）估计位姿，随后利用极线约束做运动一致性判断。文献[11]同样基于ORB-SLAM2，考虑了3种相机模型，利用Mask R-CNN[12]分割出潜在的运动目标后，利用剩余的图像区域进行后续的定位与建图，针对RGB-D相机能采集深度信息，添加了一个基于多视图几何的运动一致性检测，用于判断分割区域的真实动态性。文献[13]能够实现运动分割、动态目标跟踪、摄像机位姿估计以及静态和动态结构估计，输出动态物体的轨迹以及它们随时间化的位姿估计值，并提取速度信息。文献[14]在ORB-SLAM3上融合了SegNet和Mask R-CNN，提出适用3种相机的系统，利用先验的语义信息更新地图点的移动概率信息用来判断动态性，同时为了解决分割的实时性问题提出了一种非阻塞关键帧模型。

上述研究中，大多仅从理论说明单目、双目的可用性，并没有进行实验验证，对于物体真实动态性的判断依赖位姿的估计，其本身缺乏准确性。并且纯视觉SLAM在动态物体占幅大时，仅用静态点跟踪容易造成跟踪丢失。本文在ORB-SLAM3的基础上，通过建立单独的语义线程，利用Mask R-CNN对双目图像进行分割，计算IMU预积分的点位中误差，对于分割出的目标进行真实动态性检测，并利用实测数据对系统性能进行分析和验证。

图1 系统结构

1 基于语义的双目视觉/惯性系统

RANSAC、IMU和多地图机制，使得ORB-SLAM3在一定程度上可以抵抗动态点的干扰。然而，在实际场景中，例如道路街景，动态目标在图像中占比较大，且持续出现，难以保证系统的精度。

1.1 离群点剔除

RANSAC对暴力匹配产生的误匹配进行筛选，一定程度上能将动态物体上的特征点识别为离群值，将其剔除；但动态物体占比较大时，特征点可能大量分布在动态物体上。

1.2 语义关键帧筛选策略

关键帧机制可以有效避免跟踪和优化过程中不必要的冗余。每个关键帧存储相机姿态、特征点和共视关系。大多语义分割方法的分割速度难以满足每个图像帧的即时需求，因此通常的做法是仅仅获取关键帧的语义标签。然而，仍有一部分，例如Mask R-CNN，无法为系统提供最新的语义标签。因此，本文采用了文献[14]中的非阻塞模型，在一个滑窗内同时分割最旧的帧和最新的帧。从而，尽量为语义线程提供最新的语义标签。

1.3 语义分割方法

Mask R-CNN可实现像素级别的图像实例分割，并行处理物体检测和目标分割。Mask R-CNN是在快速区域卷积神经网络（faster region convolutional neural network, Faster R-CNN）[15]基础上发展而来，在其基础上增加兴趣区域对齐，将分类预测和掩膜预测拆分为网络的2个分支，分类预测分支与Faster R-CNN相同，对兴趣区域给出预测，产生类别标签以及矩形框坐标输出，而掩码预测分支产生的每个二值掩码依赖分类预测结果，基于此刻分割出物体，对每个类别均独立地预测一个二值掩码，避开类间的竞争。图2为Mask R-CNN分割结果。

图2 分割结果

1.4 真实动态性检测

在真实的高动态场景中，并不是所有的先验目标均为动态。如果仅使用静态背景上的点进行跟踪，难以保证跟踪点的质量，进而影响系统的精度，尤其是动态目标占比较高时，甚至会造成跟踪丢失。为了避免使用系统估计出的刚体姿态变换，而是通过IMU预积分量对投影点位置进行预测。通过预积分的协方差传递，得到点位误差，判断匹配点位是否在限差内。

2 基于预积分点位误差阈值设置

2.1 基于IMU预积分量的像素点位预测

为了获得预测点位与匹配点位的距离，需要利用IMU预积分量，对上一关键帧中的先验语义点进行投影，得到下一关键帧中的预测像素坐标。

式中：为零矩阵；为单位矩阵。

已知上一关键帧载体系在世界坐标系下的位姿，可得当前关键帧在世界坐标系下的位姿为

本文的标定结果，相机内参以及相机系和载体系的转换，均是利用校准（Kalibr）功能包[17]标定得到。

2.2 基于协方差的点位误差阈值设定

在ORB-SLAM3中，预积分协方差不断地传递。相邻二帧之间的IMU预积分量协方差为

由此可以得到像素坐标的中误差分别为

则该点的点位误差为

依据正态分布，按照95.45%的置信概率，将阈值设置为

3 实验与结果分析

3.1 实验配置

图3为本文采集数据所用的车载实验平台。平台上传感器指标如表1所示。表1中，PPI（pixels per inch）为每英寸的像素数量，1英寸等于 2.54 cm。

图3 车载多源试验平台

表1 传感器指标

相机和IMU的标定使用Kalibr工具箱，传感器的空间位置采用事先标定的方式，所有传感器统一至GPS时（GPS time, GPST）。实验采用PPP/INS紧组合算法结果作为对比方法，NovAtel IE（NovAtel inertial exploer）软件解算的实时动态差分定位（real-time kinematic positioning, RTK）/战术级惯导事后双向紧组合平滑解作为参考值。

3.2 IMU辅助动态性判断实验验证

本文截取了3段数据集，用来证明不同环境下的系统性能。Car_01数据集处于城市停车场周边，目标动态、静态参半；Car_02数据集处于城市街道，先验语义目标大多为动态；Car_03数据集处于乡村街道，车辆在路边停靠，先验语义目标大多为静态。本文使用绝对轨迹误差（absolute trajectory error, ATE）的均方根误差（root mean square error, RMSE）作为评价算法精度的指标。

3.2.1 Car_01数据验证

图4所示为利用IMU辅助动态性判断前后图像帧中特征点的情况。路边植被和路灯上的深色点为静态点，车辆上较浅色的点为动态点。

图4 利用IMU修正前后特征点对比

从图4不难看出，仅利用语义分割（左）时，系统默认右侧停车场内静态车辆上所提取特征点为动态点，然而这些点可以提供高质量的观测信息。如果贸然剔除，不仅会影响精度，并且在面对图片质量不高、特征信息缺少的场景时，很有可能造成跟踪丢失。右图为进行利用IMU进行判断动态性后，修补误剔除特征点后的情况。

图5、图6为Car_01序列分别在3种方法下运行的轨迹和ATE曲线。Car_01序列所处场景为动、静态目标参半，理论上来说此场景下本文方法更为适用。不难看出，本文方法所得到的轨迹，与真实IE解算的轨迹更为近似。表2为计算的不同方法ATE的RMSE（10次取平均）。可以看出，本文方法的轨迹误差明显降低，性能提升达56.3%，因此可以证明本文方法在该场景下是更为有效的。

图5 Car_01序列轨迹对比

图6 Car_01序列ATE

表2 Car_01序列ATE对比

3.2.2 Car_02数据验证

Car_02数据集所处场景中先验目标均为动态。为了证明本文方法的普适性，需要证明在此场景下，系统精度并不会比仅加入语义分割的方法差。

图7、图8为Car_02序列轨迹对比以及ATE曲线，表3为Car_02序列ATE的RMSE（10次取平均）。结合图7、图8和表3，可以看出加入IMU修正前后性能提升分别为70.2%、71.5%，其轨迹也十分相似。2种方法在此场景下的精度相当。可以证明，本文的方法在动态目标占比大的场景也可以保证稳定性，具有普适性。

图7 Car_02序列轨迹对比

图8 Car_02序列ATE

表3 Car_02序列ATE对比

3.2.3 Car_03数据验证

Car_03数据集所处场景中先验目标大多数为静态，更符合ORB-SLAM3的原本场景假设。同样，为了证明本文方法的普适性，需要证明在此场景下，系统精度并不会比原本ORB-SLAM3的方法变差。图9、图10为CAR_03序列的不同方法轨迹对比图和ATE曲线图，表4为对应的ATE的RMSE（10次取平均）。

从图9、图10中可以看出，因为场景中的先验目标大多数为静态，仅加入语义分割的方法精度最低，主要原因是对所有静态的先验目标上的特征点进行剔除，只利用树叶、光滑的马路等之类的特征点进行跟踪，特征信息质量不够高。同时，由于只有一辆路中行驶的车辆，这是导致本文方法精度略高于原ORB-SLAM3的原因，但是其精度基本是近似的。因此，可以认为本文方法即使在原ORB-SLAM3的静态场景的假设下，同样具备适用性。

图9 Car_03序列轨迹对比

图10 Car_03序列ATE

表4 Car_03序列ATE对比

4 结束语

本文基于ORB-SLAM3提出了一种结合语义分割和IMU判断真实动态性的动态VI-SLAM。首先，通过在ORB-SLAM3中加入一个独立的线程，利用Mask R-CNN进行语义分割，从而得到先验目标的物体掩膜，并将其中的特征点设置为待定点。其次，基于IMU预积分协方差递推得到点位中误差，并据此设定阈值，对待定点判断其真实动态性。最后，基于车载多源平台所采的数据集对本文方法进行验证：Car_01序列绝对轨迹误差均方根降低了56.3%；Car_02序列绝对轨迹误差均方根降低了71.5%；Car_03序列绝对轨迹误差均方根降低了19.4%。通过利用3个不同场景的数据集对3种方法进行对比，验证了本文方法的有效性和普适性。

[1] MUR-ARTAL R, MONTIEL J M M, TARDOS J D. ORB-SLAM: a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163.

[2] MUR-ARTAL R, TARDOS 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.

[3] CAMPOS C, ELVIRA R, JJG RODRIGUEZ, et al. ORB-SLAM3: an accurate open-source library for visual, visual-inertial and multi-map SLAM[J]. IEEE Transactions on Robotics, 2020, 37(6): 1874-1890.

[4] QIN T, LI P L, SHEN S J, et al. VINS-Mono:a robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.

[5] 王柯赛, 姚锡凡, 黄宇, 等. 动态环境下的视觉SLAM研究评述[J]. 机器人, 2021, 43(6): 18.

[6] 高兴波, 史旭华, 葛群峰, 等. 面向动态物体场景的视觉SLAM综述[J]. 机器人, 2021, 43(6): 18.

[7] ZHANG L, WEI L, SHEN P, et al. Semantic slam based on object detection and improved octomap[J]. IEEE Access, 2018, 6: 1-1.

[8] REDMON J, FARHADI A. YOLO9000: better, faster, stronger[EB/OL].[2022-03-06]. https://openaccess.thecvf. com/ content_cvpr_2017/papers/Redmon_YOLO9000_Better_Faster_CVPR_2017_paper.pdf.

[9] YU C, LIU Z, LIU X, et al. DS-SLAM:a semantic visual SLAM towards dynamic environments[EB/OL].[2022-03-06].https://arxiv.org/ftp/arxiv/papers/1809/1809.08379.pdf.

[10] BADRINARAYANAN V, HANDA A, CIPOLLA R. SegNet: a deep convolutional encoder-decoder architecture for robust semantic pixel-wise labelling[J]. Computer Science, 2017, 39(12): 2481-2495.

[11] BESCOS B, FÁCIL J M, CIVERA J, et al. DynaSLAM: tracking, mapping and inpainting in dynamic scenes[EB/OL].[2022-03-06]. https://arxiv.org/pdf/1806.05620.pdf.

[12] HE K, GKIOXARI G, P DOLLÁR, et al.Mask R-CNN[EB/OL].[2022-03-06]. https://openaccess.thecvf.com/ content_ICCV_2017/papers/He_Mask_R-CNN_ICCV_2017_paper.pdf

[13] ZHANG J, HENEIN M, MAHONY R, et al. VDO-SLAM: a visual dynamic object-aware SLAM system [EB/OL]. [2022-03-06]. https://arxiv. org/abs/2005.11052.

[14] LIU Y, MIURA J. RDS-SLAM: real-time dynamic SLAM using semantic segmentation methods[J]. IEEE Access, 2021, 9: 1-1.

[15] REN S, HE K, GIRSHICK R, et al. Faster R-CNN: towards real-time object detection with region proposal networks[EB/OL].(2016-01-06)[2022-03-06].http://papers.nips.cc/paper/5638-faster-r-cnn-towards-realtime-object- detection-with-region-proposal-networks.pdf.

[16] 高翔, 张涛, 刘毅, 等. 视觉SLAM十四讲: 从理论到实践[M]. 2版.北京:电子工业出版社, 2019: 46-47.

[17] FURGALE P, REHDER J, SIEGWART R. Unified temporal and spatial calibration for multi-sensor systems[EB/OL]. [2022-03-06]. https://furgalep.github.io/bib/furgale_iros13.pdf.

Stereo/Inertial SLAM system for dynamic environment

ZHANG Letian, ZHAO Dongqing, JIA xiaoxue, YANG Chaoyong, LAI Luguang, GUO Wenzhuo

（School of Geodesy and Geomatics, Information Engineering University, Zhengzhou 450001, China）

There are still many problems with the static scenario based on simultaneous localization and mapping (SLAM): the keyframes in traditional visual SLAM do not contain semantic information, but a large number of dynamic points in the actual scenario that not only affect system accuracy, but also easily track failure. This paper proposes a semantic slam system. First, the interference of the dynamic point is removed with prior semantic information given by mask region convolutional neural network (Mask R-CNN), the function join oriented fast and rotated brief simultaneous localization and mapping (ORB-SLAM3) as a new thread .Secondly, combining the stereo/inertial odometry, the adaptive threshold is set based on the inertial measurement unit (IMU) preintegration covariance, and the real dynamics of the transcendental dynamic point is determined. Finally, the experimental results show that the absolute trajectory errors of the three datasets are reduced by 56.3%, 71.5% and 19.4%, respectively. Therefore, the system effectively distinguish the real dynamics of feature points, shield the interference of dynamic points, and improve the positioning accuracy.

dynamic environment; simultaneous localization and mapping; preintegration; semantic segmentation; neural network

P228

A

2095-4999(2022)06-0150-07

张乐添，赵冬青，贾晓雪，等. 面向动态环境的双目/惯性SLAM系统[J]. 导航定位学报, 2022, 10(6): 144-150.（ZHANG Letian, ZHAO Dongqing, JIA xiaoxue, et al. Stereo/Inertial SLAM system for dynamic environment[J]. Journal of Navisgation and Positioning, 2022, 10(6): 144-150.）

10.16547/j.cnki.10-1096.20220619.

2022-07-12

国际自然科学基金项目（41774037，42104033）。

张乐添（1998—），男，河南焦作人，硕士研究生，研究方向为视觉/惯性SLAM。

赵冬青（1976—），男，湖北十堰人，博士，教授，研究方向为导航定位与位置服务。