面向低光弱纹理环境的SLAM系统实验教学平台设计

叶 涛， 赵云龙， 汪寿安， 李允旺， 严翔明

（中国矿业大学（北京）机械与电气工程学院，北京 100083）

0 引 言

同步定位与建图（Simultaneous Localization and Mapping，SLAM）是机器人领域中的一项关键技术，应用该技术的机器人能在未知环境中同时实现自身定位和构建地图。对于机器人专业学生而言，SLAM 课程具有重要的学术意义和实践价值。由于历史原因以及教学和实验条件的限制，SLAM 课程主要侧重于理论教学，缺乏实际应用，导致学生对该课程作用的认识不足，教学效果并不理想。学生普遍反映，该系列课程所涉及的技术过于抽象和枯燥，侧重点不够突出，影响了对该课程的学习兴趣和积极性。

为解决上述问题，本文提出一种改进视觉点线特征提取方法并搭建面向低光弱纹理环境的SLAM实验教学平台，旨在为学生提供探索低光弱纹理环境下机器人导航与定位的实践机会。通过在模拟场景中进行实验操作，学生能深入理解SLAM 算法在复杂环境下的应用和挑战，提高培养解决实际问题的能力。该教学平台有助于拓展学生的学术视野，激发学生对该领域的研究兴趣。

1 案例工程背景

低光弱纹理空间常见于地下停车库、暗室等日常场景以及地铁隧道、矿井巷道等工业场景。这些场景光线照度低、缺乏明显纹理特征、全球定位系统（Global Positioning System，GPS）［1］信号缺失等特点，机器人在上述复杂场景中难以实现精准、鲁棒的地图定位。随着SLAM技术的兴起，机器人使用的各种传感器可实现4 自由度（Four Degrees of Freedom，4DOF）的位姿估计。单目视觉方法具有结构简单、成本低、无距离限制和实时性良好等优点。由于其缺乏尺度信息、视场角有限以及受弱纹理结构影响较大，仅依靠纯视觉传感器难以达到所需效果。融合惯性测量单元（Inertial Measurement Unit，IMU）传感器是解决上述问题的有效手段。相比于基于单目视觉的方法，惯性导航系统能在短时间内提供相对准确的角加速度和线加速度，数据更新率更高且不依赖于任何外部信息。由于IMU传感器的噪声和偏置，定位误差随时间累积导致定位不稳定且不准确。相比之下，采用视觉和惯性融合的方法［3-4］能克服这些缺点并提高自主定位的精度和状态估计的可靠性。

文献［5-7］中提出松耦合VIO 算法传感器融合（sensor fusion），该算法充分融合了视觉和惯性传感器的优点，实现了更为精确和鲁棒的定位和导航。Falquez等［8］提出基于直接视觉里程计法取得了更好的效果，一些基于滤波方式的VIO 也被提出［9-10］。优化的全向视觉同时定位地图构建（Omnidirection Visual SLAM，ORB-SLAM）等算法［11-13］与滤波算法相比能在保证实时性的同时有效提高检测精度。Qin 等［14］提出单目视觉惯性系统（VINS-Mono），能在执行初始化过程中为最小化视觉和惯性误差的非线性优化过程提供初始值，通过设计重定位和姿态图优化模块实现在复杂环境下实现高精度的定位和导航。尽管上述单目视觉惯性算法取得了不错的性能，但地铁隧道、矿井巷道、工业管道等空间弱纹理、低照度、空间狭窄等实际复杂场景为视觉惯性SLAM 提出了新的挑战：在单目相机定位过程中，通常使用人工定义的角点特征识别环境中的几何特征，这种角点特征在弱纹理环境下难以提取，严重影响了SLAM的整体性能，也限制了机器人在这种环境的自主导航能力。上述环境中仍然包含大量可用于提取的线特征，基于这些线特征构建的环境地图具有更为丰富的几何信息，能有效提升算法的定位精度和鲁棒性。文献［15］中提出一种从三维直线运动矩阵中提取常用运动矩阵的方法，并对通用矩阵进行校正，使其能准确地表示运动。文献［16］中使用线特征研究视觉惯性里程计的可观测性，解决了基于惯性测量和线的视觉观测估计车辆在三维运动状态的问题。He等［17］构造了一种结合点线特征的紧耦合单目VIO 系统。该算法实现局部位姿估计，没有考虑全局的定位误差，在低光照弱纹理环境中的检测鲁棒难以得到保证。

综上所述，机器人在低光弱纹理场景下如何维持SLAM系统的估算精度、实时性、累计误差之间的均衡仍是VIO所面临的实际应用难题。本文提出一种以相机和惯性导航系统作为传感器输入的面向低光弱纹理环境的视觉惯性融合的鲁棒SLAM 算法实验平台，实现机器人在此类复杂环境下的定位与导航的高精度和鲁棒性。通过该实验平台与授课相结合的方式可巩固学生所学的理论知识，提高学生的创新实践能力和解决工程问题的能力。

2 案例设计

本文所提面向低光弱纹理环境的视觉惯性融合的鲁棒SLAM系统框架如图1 所示。该系统输入为相机和IMU数据，其算法处理框架由初始化和前端与后端优化两部分组成。前端对传感器输出的数据进行预处理，提取点特征和线特征，并进行视觉初始化以建立视觉观测模型。同时，构建IMU 预积分观测模型，并将这两种观测模型结合，构建视觉惯性里程计。将先验信息、IMU残差和点特征残差输入紧耦合滑动窗口，通过回环检测判断当前位置是否需要位姿图优化，以获得关键帧。系统对输出经优化后的位姿进行状态估计。

图1 视觉惯性融合SLAM系统框架

2.1 点线特征的提取和模型观测

针对低光照弱纹理环境下角点特征提取困难，线特征计算成本过高，难以保证算法的实时性，本文对LSD算法进行优化：通过研究隐藏参数的调整简化线特征的检测，针对原有N层高斯金字塔进行多尺度表达的创建调整固定采样比例和高斯模糊次数，改善线特征提取图像梯度的一致性；通过长度拒绝策略过滤掉短线特征，根据调节图像缩放系数和最小密度阈值设计长度拒绝系数；采用LBD（Line Band Descriptor）描述子［22］对旋转和尺度缩放具有较高的鲁棒性，可有效提高匹配效率和计算速度。通过上述优化提高了算法在低光照和弱纹理环境下线段特征的提取和跟踪性能，为SLAM系统提供更可靠和高效的视觉特征，提升定位和导航的精度和鲁棒性。

在完成图像线特征的关联后，对世界坐标系下的相机位姿、路标观测以及线段的重投影误差进行建模。设世界系下相机第j帧的位姿为，在该帧观测到的第k个特征点为，其在图像上的投影为，第l条线段的首尾点可表示为，在图像平面对应的齐次坐标点为，其点特征的重投影误差和线特征的重投影误差分别为：

式中：dpj，l为到、的投影线段的距离；lj，l为相机光心与线段端点构成平面的法线，线特征重投影误差对线段的雅可比矩阵为：

式中：Rjw为世界系下相机第j帧的位姿为中的平移分量；fx、fy分别为相机的焦距；opx、opy、opz和oqx、oqy、oqz分别为点在相机坐标系x y z坐标轴上的分量。线特征重投影如图2 所示。

图2 线特征重投影

2.2 IMU预积分观测模型构建

对于IMU 系统，主要考虑旋转分量R、平移分量p、角速度ω、线速度v和加速度a。由旋转运动学以及欧拉积分的离散形式解析，通过指数映射将李代数中的旋转向量对应到李群中的旋转矩阵，并将受到噪声与重力影响的陀螺仪测量的角速度和加速度计测量的线加速度进行解析替换，通过对采样频率进行固定，每个离散时刻由k=0，1，…表示，可得关于旋转R、速度v和平移p的3 个离散运动方程：

式中：ΔRij为预积分旋转量；Δvij为预积分速度量；Δpij为预积分平移量。对预积分旋转量的测量值和理论值进行解析，把噪声项分离：

式中：Δ为旋转量预积分测量值，它由陀螺仪测量值和对陀螺bias的估计得到；δ→φij为测量噪声。同理，对线速度和位姿的理论值和测量值进行解析，分离噪声项：

由于旋转量的测量值、一阶雅克比、Δt采样时间这些都是已知量，为零均值高斯噪声，（一阶近似）也为零均值高斯噪声。同理可得速度量和平移量的噪声项都具有高斯分布的形式：

当bias变化较小时，假设预积分观测随零偏呈线性变化，在此基础上进行线性化并保留一阶项，对原始观测值进行修正，可得：

在图优化系统中，对IMU的残差计算：

2.3 紧耦合滑动窗口和位姿图优化

在包含了点、线和IMU测量信息的紧耦合滑动窗口中定义所有状态向量：

式中：xk为第k帧中IMU在世界坐标系下的信息；pbwk、Rbwk、vbwk分别为位置量、旋转量和速度量；ba、bg分别为加速度偏差和陀螺仪偏差；nk、np和nl分别为滑动窗口中关键帧、空间点和线的总数；λ 为点特征与其第1个观察到的关键帧的反距离；o为3D线的4 参数正交表示。定义滑动窗口中的目标函数

式中：Iprior为先验信息，它是在滑动窗口［23］中边缘化旧帧时获得的；eimu、epoint分别为点重投影残差和IMU 残差；eloop为闭环约束；eline为线重投影残差。在滑动窗口后，构建一个具备全局信息的因子图优化框架，如图3所示。该框架考虑了先验信息、IMU里程计因子、IMU预积分因子以及线特征路标和点特征路标对相机关键帧位姿的影响。

图3 因子图概览

3 实验平台开发

本文对所提视觉惯性紧耦合SLAM算法进行了一系列实验，并与其他先进的视觉惯性SLAM 算法进行了比较。这些实验在开源的M2DGR 数据集上进行，该数据集为室内外地面机器人收集了长期具有挑战性的序列，并配备完整的传感器套件。实验中，本文使用了分辨率为640 × 480、频率为15 Hz 的Inivation DVXplorer事件相机和9 轴IMU（频率为100 Hz）传感器。实验计算机采用AMD R5 2600 CPU，主频为3.4 GHz，六核心和32 GB 运行内存。所有算法均采用C++实现，并在Ubuntu 18.04 系统上使用ROS 的Melodic版本执行。

为评估所提方法的准确度和误差状态，在M2DGR数据集hall_04 和street_03 序列上进行定量分析。所有算法均在开启回环的情况下进行测试。图4 ～6 展示了低光照弱纹理的实验环境。其中，图4、5 显示了夜晚黑暗室外环境，图6 显示了无光照条件的室内环境。

图4 低光照弱纹理的实验环境-1

图5 低光照弱纹理的实验环境-2

图6 低光照弱纹理的实验环境-3

图7 、8 展示了不同实验序列绝对轨迹误差（ATE）和轨迹。图7 展现了hall_04 序列绝对轨迹误差SE（3）分析和轨迹图，图8 展示了street_03 序列绝对轨迹误差SE（3）分析和轨迹图。图7（a）、（b）和图8（a）、（b）展示的灰色直线为APE（绝对位姿误差），蓝色直线为均方根误差，绿色线为误差中位数，红色线为误差平均值，紫色线为标准差所在范围区间，可见，本文所提方法的纵坐标RMSE 等值具有更小的误差值。在图7（c）、（d）和图8（c）、（d）展示的轨迹误差图中，右侧的蓝色部分为较小的误差值，而红色部分为较大的误差值。可见，本文所提方法在右侧标记的轨迹误差值较小，蓝色部分线段的分布更为密集。

图7 hall_04 序列的绝对轨迹误差SE（3）分析和轨迹图

图8 street_03 序列的绝对轨迹误差SE（3）分析和轨迹图

图9 、10 分别展示了vins_mono 方法和ours 方法的轨迹xyz_view和rpy_view 对比，表1、2 分别列出了hall_04 和street_03 序列不同算法和真值之间绝对轨迹误差统计。

表1 hall_04 序列不同算法和真值之间绝对轨迹误差统计

表2 street_03 序列不同算法和真值之间绝对轨迹误差统计

图9 vins_mono和ours的轨迹xyz_view对比

图10 vins_mono和ours的轨迹rpy_view对比

综合对表1、2 和图7、10 的数据分析，本文研究的算法在低光照、弱纹理场景的定位精度与vins_mono方法比有着明显的改善，取得了更小的绝对轨迹误差，在rpy_view的变化上也更加稳定。

4 结 语

本文所提改进视觉点线特征提取的方法，旨在解决弱纹理环境下基于视觉点特征数据误关联问题，能有效提升算法在弱纹理场景的定位精度，并在此基础上设计一种面向低光弱纹理环境的视觉惯性融合鲁棒SLAM软件实验平台。通过引入该实验教学平台，学生将有机会深入了解SLAM 技术的实际应用，并掌握在低光弱纹理环境中解决导航与定位问题的关键技术。通过让学生团队合作完成巡检机器人SLAM实验的方式能有效培养学生的个人和协作能力，提高机器人专业本科生的专业能力和竞争力，为其未来职业发展奠定坚实基础。