Zheng, Zengrui, Shifeng Lin, and Chenguang Yang. “RLD-SLAM: A Robust Lightweight VI-SLAM for Dynamic Environments Leveraging Semantics and Motion Information.” IEEE Transactions on Industrial Electronics, 2024, 1–11. https://doi.org/10.1109/TIE.2024.3363744.
Abstract
现存的主流动态SLAM 可分为两个方向：基于语义分割和基于物体检测，两者均存在明显的优劣势，为此，作者提出了一种robust lightweight dynamic SLAM (RLD-SLAM)，利用语义、运动信息在动态环境中实现鲁棒、轻量的视觉-惯性SLAM系统。
1 Introduction
本算法结合物体检测和贝叶斯滤波器来准确识别出环境中的动态特征点，不依赖于深度信息，因此可适用于各种输入设备。此外，RLD-SLAM 可使用IMU观测预积分来辅助高动态场景中动态物体的跟踪，如Fig.1所示。
本文的贡献：
- 结合物体检测和贝叶斯滤波器来快速获取动态环境中的静态特征点；
- 使用IMU 信息来跟踪动态物体，以增强SLAM 在高动态场景中的鲁棒性；
- 设计了RLD-SLAM，可适应各种输入传感器和动态场景，实验证明了算法的有效性。
3 System Introduction
3.1 Overall Framework
RLD-SLAM 包含以下四个线程：
- 物体检测
- 特征跟踪
- 局部制图
- 回环检测
本系统支持单目/双目/RGB-D 相机作为输入。通过IMU 预积分来获取位姿转换矩阵 $T_I$ ，可作为一个先验位姿，其作用在于：
- 用于矫正图片以提高物体检测性能；
- 实现更好的3D 物体跟踪，特别是在超过半数特征均是语义动态的场景下。
为了增强系统的容错性能，RLD-SLAM 会在IMU 失效时继续工作：图片矫正矩阵通过后端优化得到的参考关键帧来获取，且只实现2D 物体跟踪。
3.2 Image Rectification and Object Detection
本系统使用YOLOv5 进行物体检测，物体检测算法在相机旋转情况下会面临精度下降的问题，为解决该问题，作者利用IMU 数据来获取一个矫正矩阵：
首先，计算重力加速度于图片y 轴的夹角：
然后，将该图片旋转一定的角度 $\theta$ ：
当IMU 不可用时，利用后端优化的参考关键帧位姿来提供矫正矩阵。
图片矫正过程如Fig.3所示，虽然步骤简单，但是在相机大角度旋转情况下可大幅提高动静态特征点的识别精度。
3.3 Multiview Probability Estimation and Weighted Optimization
物体检测bbox 内会包含大量的静态背景，针对这个情况，作者设计一种基于贝叶斯滤波器的多视图概率估计算法来区分bbox 内的动静态特征点，该过程如Fig.4所示，本算法利用与当前帧具有最高共视率的N 个关键帧来计算特定特征点的动态概率。
语义概率的更新可表示为下述的贝叶斯滤波问题：
其中，$\eta$ 为归一化常数。当前观测 $z_i$ 仅依赖于当前状态 $x_i$ ，因此概率分布 $p(z_i | x_i)$ 可从物体检测的置信度获取。
假设服从一阶Markov 过程，则有：
根据关键帧的选取策略可知，关键帧之间的时间间隔是不确定的；且物体在动态环境中的运动状态会随着时间改变，基于此，可以自然作出假设：同一物体在不同关键帧之间的运动状态相关性会随着时间增长而下降。因此，特征点的动态概率估计中可增加一个衰减因子：
作者提出的该方法与其他使用帧间动态概率传递方法的不同之处在于：有效利用了相关的多个历史关键帧。贝叶斯滤波器中引入的衰减因子使得该算法可以更侧重临近观测，这使得系统可以快速识别出从动态转为静态的特征点，然后作为优化过程中的地标特征点。包含了静态概率权重的RLD-SLAM 后端优化方程表示如下：
其中，$\mathcal{Z}^j$ 为所有观测到的 3D 地标；$\mathbf{r}_{ij}$ 表示重投影误差；$\Omega$ 表示将地表点投影至像素坐标系；$\Lambda_{ij}$ 表示信息矩阵；$P_{staticij}$ 表示第j 个特征点在第i 次观测后的静态概率。
3.4 Dynamic Object Tracking With IMU Measurement
仅利用语义信息无法判断物体的真实运动状态，部分算法使用物体跟踪来解决该问题，但同时会带来大量的算力负担。本算法融合IMU 观测和视觉里程计来进行动态物体的3D 跟踪，并实时更新速度与运动状态；根据物体的运动状态，系统可自动决定是否将这些特征点包含进优化过程。该过程如Fig.5所示：
当一个动态物体进入相机视野中时，系统利用卡尔曼滤波器来对观测进行处理：
首先，假设观测到的动态物体（汽车）服从匀加速模型，初始状态表示为：
然后，预测该动态物体的下一状态：
当获取了观测信息后，更新状态：
该算法的关键在于获取动态环境中的观测信息。当算法可获取充足的静态特征点时，VO 效果较好，此时可计算出动态物体的世界坐标：
当物体检测算法判断当前视野中大部分特征点属于先验动态物体时，难以获取相机自身位姿 $T_{WC}^i$ ，也就无法实现对动态物体速度等状态信息的计算。在此情况下，本算法利用IMU 来增强系统的鲁棒性：对关键帧之间的IMU 观测进行短期预积分，假设(i-1) 和i 关键帧分别对应 j 和 n IMU 观测数据，则有
陀螺仪和加速度计的偏差可从后端的近期优化中获取，噪声影响可通过信息矩阵来表示，考虑到关键帧之间的时间间隔较短（一般不超过1s），可认为IMU 预积分误差可忽略，由此可计算汽车位姿：
利用上述观测信息来实现滤波器更新，以此计算物体的运动状态。对于静态物体，其特征点可为VO 提供更多的地标，由此可增强SLAM 的精度与鲁棒性；在此基础上，可将VO 与IMU 进行紧耦合优化来消除IMU 漂移：
总的来讲，本算法利用短期 IMU 预积分来恢复高动态场景下的 VO，然后进一步对VO 和 IMU 进行联合优化，解决IMU 的长期漂移问题。
4 Experimental Results
作者在KITTI 数据集进行测试：在01-04 序列的郊区环境中，相机载体与动态物体速度较慢，本算法的提升效果较小；序列13和19 是城市场景，有各种动态物体，如自行车、行人、汽车等，作者认为对于非常规形状或较小形状的物体而言，没必要从中提取地标；序列08和20是高速路场景，移动物体数量较多、速度较快，该场景中RLD-SLAM 算法优势明显，这是使用历史视觉信息和IMU 运动先验带来的优势；此外，使用卡尔曼滤波器对动态物体进行松耦合状态估计也会大幅减小运算负担。
