首个将车位显式融合到前后端的紧耦合VIO框架

首个将车位显式融合到前后端的紧耦合VIO框架

如何解决车库地面反光？

传统SLAM方法从相机图像中获取特征点，结合其他传感器如惯性测量单元（IMU）和轮速传感器（WSS）来构建SLAM模块。如图1所示，在自动泊车辅助（APA）和自动代客泊车（AVP）场景，传统的视觉方法受到不稳定特征跟踪的影响，往往是由光线反射和频繁移动的物体引起的。这些方法通常无法在纹理稀疏和光线动态变化的停车环境中提取足够的有效特征点。

附赠自动驾驶最全的学习资料和量产经验：链接

实际上在复杂的停车环境中，不足和不稳定的特征点会导致累积漂移，甚至在极端情况下导致失败。尽管有一些方法可以减轻累积漂移如闭环检测，但在如此复杂的场景中设计具有高精度和稳定性的定位方法仍然是一个问题。

本文介绍的VIPS-Odom[1]是一种基于语义视觉惯性优化的里程计系统，通过动态结合检测到的停车位角点作为SLAM前端的额外稳健特征点，同时在SLAM后端利用停车位观察作为语义约束，减少累积定位误差。与以往的工作不同，在多目标跟踪框架下稳健地跟踪和维护所有停车位的状态。我们的方法在自动泊车方面比其他基准方法实现了更高的定位精度。主要技术贡献如下：

• 利用停车位作为额外的高级语义观察来辅助SLAM。VIPS-Odom是第一个通过显式方式将停车位观察融合到SLAM前端和后端的整体解决方案。

• 为一辆电动汽车配备了相关传感器作为实验平台，并在不同地下停车场的长距离和短距离轨迹上进行了广泛的现实世界实验，这比以往的自动泊车相关工作更为丰富。

• 实验基准方法多：VIPS-Odom相对于其他基准方法的优越性和稳定性，包括基于惯性的EKF、传统的视觉惯性优化VINS、语义视觉惯性优化VISSLAM和基于LiDAR的A-LOAM

具体方法

传感器设备与标定

多模态传感器数据在 VIPS-Odom 中包括从角点检测中得到的视觉特征点、惯性测量单元（IMU）和轮速传感器（WSS）之间的测量数据以及从鸟瞰图（BEV）图像中得到的停车位（PS）观察数据。

传感器配置

系统（如图2所示）包括一个安装在车辆中心的惯性测量单元（IMU）、一个安装在车辆右后轮附近的轮速传感器（WSS）和四个安装在前、后、左、右侧的环视鱼眼摄像头。图像以1920×1080像素的分辨率记录。分辨率为576×576像素的BEV图像覆盖11.32米×11.32米的区域。车辆顶部还安装了一个360°激光雷达（LiDAR），以生成点云数据来支持A-LOAM的LiDAR-SLAM评估。

传感器标定

由于VIPS-Odom中的各种传感器测量数据是紧密耦合的，我们仔细标定了相关参数。利用一个开源工具箱来优化四个鱼眼摄像头的内参[2]。对于外参数标定，我们参考相关方法来优化摄像头-IMU和IMU-WSS的转换[3]。

鸟瞰图（BEV）图像生成

采用逆透视映射 (IPM)构建由四个环视鱼眼摄像头生成的鸟瞰图（BEV）图像。车体坐标系、图像坐标系和BEV坐标系之间的关系可以表示为：

其中，⁻¹π⁻¹(·) 是鱼眼投影模型的逆函数，��� 是摄像头相对于车体坐标系的外参矩阵，��� 表示从BEV坐标系到车体坐标系的变换。 [��,��]�、[��,��]� 和 [��,��]� 分别代表在车体、鱼眼摄像头和BEV坐标系中的位置。

优化

总体来说，VIPS-Odom 是一个紧耦合的基于优化的 SLAM 系统，它集成了来自多模态传感器的测量数据。VIPS-Odom 的总体框架如图 3 所示。展示了 VIPS-Odom 的优化框架。

� 表示前视鱼眼相机捕获的视觉特征点，� 表示世界坐标系中停车位中心的观察，� 和 分别表示测量数据和轮速传感器（）测量数据。目标是通过多传感器观测来更精确地估计�4分别表示���测量数据和轮速传感器（���）测量数据。目标是通过多传感器观测来更精确地估计T。使用贝叶斯定理重新定义。使用贝叶斯定理重新定义p$，如下所示：

进一步通过将 � 从其他模态分离出来对 � 进行分解，因为视觉关键点 � 和 �� 观测 � 是从两个独立的传感器测量中获得的：

其中，第一个括号与视觉特征、IMU/WSS 运动数据有关，其余部分表示与 PS 相关的项。基于上述分析，我们可以通过联合最小化视觉重投影误差、惯性运动误差和 PS 误差来进行优化。VIPS-Odom 同时处理低级别的几何/运动数据以及高级语义信息，有助于实现停车场景中更稳定和更精确的定位。

停车位（PS）检测与管理

PS 检测

不同于 AVP-SLAM[4] 中的特征线检测和 VISSLAM 中基于 Matlab 的模型，我们利用卷积神经网络（CNN）进行停车位检测。基于 YOLOv5开发了轻量级的停车位检测器，在鸟瞰图（BEV）图像中检测停车位并预测其状态（即被占用或空闲，见图 3）。检测到的停车位状态将帮助自动泊车规划选择空闲停车位。

PS 管理

利用经典的多目标跟踪框架 SORT来管理停车位状态。

• 停车位是在 BEV 图像帧中检测到的。然后检测到的停车位角点被转换到自车的车体坐标系（见公式 1）。根据停车位在 BEV 图像中被检测到的时间戳，可以从滑动窗口中的关键帧中获取最近的车辆位姿。

• 停车位的位姿可以从车体坐标系转换到世界坐标系。世界坐标系在轨迹开始时初始化。每个停车位可以视为在世界坐标系中不同位置的不同矩形。它们还绘制在构建的地图和 BEV 图像上（见图 7），这有助于自动泊车的下游任务。

• 为了将检测分配给现有停车位，需要及时更新跟踪器状态。通过卡尔曼滤波器预测每个目标的边界框几何形状，并计算检测与现有目标的所有预测边界框之间的交并比（IoU）距离的分配成本矩阵。通过匈牙利算法解决分配问题。此外，设置了最小 IoU 以拒绝检测与目标重叠少于 0.3 的分配。匹配的停车位检测与现有停车位将共享相同的 id。

• 还实现并评估了 VISSLAM 中使用的硬匹配关联方法，该方法为以前的停车位关联和新停车位创建预定义了阈值。

前端与停车位（PS）融合

对于前端视觉里程计，使用 Shi-Tomasi 算法检测视觉特征点，并在每张新图像中维护大约一百个特征点。通过 KLT 稀疏光流跟踪视觉特征。需要注意的是，由于停车场通常缺乏纹理和光线动态变化，检测到的特征点数量较少，因此传统的 SLAM 方法如 ORB-SLAM 在停车场景中的表现往往不令人满意。

如图 4 所示，引入检测到的停车位（PS）角点作为额外的稳健特征点，以缓解上述问题。通过 IPM将多个视角的鱼眼摄像头图像集成到一个鸟瞰图（BEV）图像中。BEV 图像中的每个点都与原始鱼眼摄像头上的点一一对应，因此通过变换矩阵，每个 BEV 空间中的 PS 特征点与前视鱼眼摄像头中的像素相配对。

通过 PS 检测器预测后，这些 BEV 图像帧中的 PS 特征点可以根据 BEV 坐标与鱼眼摄像头坐标之间的变换矩阵反向投影到摄像头帧中，作为额外的特征点。为了保持更高质量和更稳健的特征点，将这些额外的 PS 特征点动态合并到原始检测到的 Shi-Tomasi 视觉特征点序列中。这些额外的特征点有助于在停车场景中实现更稳健的定位。2D 特征点首先进行去畸变，然后在通过离群值拒绝后投影到单位球体上。在 SLAM 前端期间进行关键帧选择，遵循 VINS中常用的两个标准，主要依据当前帧与前几帧之间的视差。

停车位（PS）后端优化

视觉惯性项

视觉惯性项可以通过 VINS 的公式构建，进一步利用轮速传感器（WSS）来丰富复杂环境中的惯性测量。在每个滑动窗口中，VIPS-Odom 维护一组 � 个关键帧 �0,…,��−1 以执行后端优化。每个关键帧 �� 与相关的时间戳 �� 和相机姿态 ��=[��,��]∈��(3) 相关联。连续两个关键帧之间的运动（与方向、位置和速度的变化相关）可以通过预积分的 IMU/WSS 数据或视觉里程计来确定。通过预积分合成 IMU 和 WSS 的测量数据。总体视觉惯性优化函数公式如下：

其中，�(�) 表示视觉惯性后端损失函数，� 表示观察到的特征点（或地标）的索引，� 表示地标 � 出现在的图像的索引。� 是滑动窗口中的图像数量。��,�� 是BA的重投影残差，��� 表示惯性残差，�� 表示边缘化残差。�� 是所有重投影项的统一信息矩阵，Σ�,�+1 表示协方差的前向传播。向量 � 包含关键帧的运动状态，每个地标的逆深度可以表示为：

其中，�−�ℎ 关键帧的状态为 ，��，λ0,…,λ�−1 表示相机帧中每个地标的逆深度，���, ���, ��� 表示 k-th 帧的姿态、速度和方向，对应于 ��，���, ��ω 表示与 k-th 图像相关的 IMU 加速度计和陀螺仪的偏置。优化过程的其他细节基本上与 VINS 相同。

PS 项

在后端中明确地引入 PS。在每个滑动窗口中，由于每个具有相同 id 的 PS 与多个观测关联，它自然形成了每个观察到的 PS 与其在世界坐标系中维护的状态之间的配准约束。每个维护的状态和 PS 观测通过 PS 管理模块提供的 PS id 关联。PS 项如图 5 所示。

因此，关键帧 � 处观察到的停车位 � 的配准误差可以定义为：

其中，�� 表示世界坐标系中当前维护的 PS（id=i）位置状态，�� 表示关键帧 � 的车辆姿态（由公式 7 中的位置和方向计算），��,� 表示车体坐标系中的 PS（id=i）观测。PS 误差项的公式为：

其中，� 是该滑动窗口中看到的 PS 集合，ρ(·) 表示稳健核函数。α�,� 是基于关键帧 � 对某个停车位（id=i）的观测的重新加权项。如果 k-th 关键帧中没有观察到 i-th PS，则 α�,� 将为 0。添加此重新加权项的原因是由于透视变形和镜头失真，生成的 BEV 图像的外部区域通常会被拉伸或压缩，这会给 PS 检测引入更多误差（见图 5）。对于某个 k-th 关键帧，首先计算观测到的 i-th PS 在该关键帧中的中心点与 BEV 图像中心点之间的像素距离 ��,�：

综上所述，�(�) 包含了视觉惯性约束项，而 �(�) 提供了语义配准约束项。

实验效果

总结一下

VIPS-Odom是一种新颖的紧耦合 SLAM 系统，融合了惯性测量单元、轮速传感器和四个环视鱼眼摄像头的测量数据，实现了高精度且稳定的定位。其实时检测 BEV 图像中的停车位，可靠地维护观察到的停车位状态，并同时将停车位观察结果作为前端视觉特征和后端优化因子，提升定位精度。该方法还开发了一个相关传感器套件的实验平台，用于评估 VIPS-Odom 的性能。覆盖短距离和长距离停车场景的大量现实世界实验反映了相对于其他基准方法的有效性和稳定性。

改进点：

• 尚未将闭环检测模块纳入系统。虽然闭环检测对于所有短距离 APA 场景和部分长距离 AVP 场景来说不是必需的，但可以扩展闭环检测模块，进一步提升长距离场景的性能

• 除了停车位，还可以将更多的语义对象纳入系统

参考

[1] VIPS-Odom: Visual-Inertial Odometry Tightly-coupled with Parking Slots for Autonomous Parking

[2] A toolbox for easily calibrating omnidirectional cameras

[3] Improved wide-angle, fisheye and omni- directional camera calibration

[4] Avp-slam:Semanticvisualmappingandlocalization for autonomous vehicles in the parking lot