对车辆地平面法线的估计方法

Towards Accurate Ground Plane Normal Estimation from Ego-Motion

摘要

本文提出了一种新颖的轮式车辆地平面法线估计方法。在实际应用中，由于制动和不稳定的路面，地平面会动态变化，因此估计地平面法线对提高自动驾驶任务的鲁棒性具有重要意义。本文的方法仅使用里程计作为输入，并实时估计准确的地平面法线向量。通过多次实验验证了其可用性和显著的鲁棒性改进，例如在KITTI数据集上，达到了最先进的准确性。

引言

准确地估计地平面法线对自动驾驶应用的感知、导航和规划至关重要，这是因为车辆坐标系中的地平面由于制动和不稳定的路面而动态变化。传统方法通常通过估计单应性变换然后分解为地平面法线和自身运动来实现。然而，这些方法在实时环境中的准确性和鲁棒性不足。本文提出的方法利用自车位姿里程计与地平面之间的关系，通过不变扩展卡尔曼滤波器（IEKF）实时估计地平面法线，具有高效且准确的特点。

 

图1. 展示了前置摄像头在移动车辆上的典型动态运动。尽管车辆直线行驶，路面看起来足够平坦，但俯仰角（绕x轴旋转）实际上在大约1度的振幅内波动。当车辆遇到不完美的路面和减速带时，这种俯仰角的振荡会被放大。

方法

地平面法线估计

本文认为在车辆移动时，车辆参考系统中的地平面法线向量是振荡的。通过在KITTI数据集上选取一个剪辑进行验证，发现大多数实际道路边缘未与地面真实值正确对齐，超出校准1度以上。这表明地平面法线向量不是恒定的，而是在车辆移动时动态变化。基于这一观察，本文提出一种实时估计地平面法线的方法，充分利用自车位姿里程计信息，并通过不变扩展卡尔曼滤波器进行动态估计。

IEKF模型

IEKF模型利用自车位姿与地平面法线的基本关系，建模车辆自运动的动态并实时估计地平面法线。该方法适用于多种传感器提供的自我运动信息，如单目相机和惯性测量单元（IMU）。在实验中，通过对比使用静态法线向量和动态法线向量的IPM图像，验证了本文提出方法的有效性。实验结果表明，使用动态法线向量后，IPM图像中的道路边缘更加平行和一致，图像质量显著提高。

 

在上图中，我们观察到在具有固定外部参数的相邻帧之间进行IPM后（即Inverse Perspective Mapping，逆透视变换），道路边缘未能正确对齐。为了进一步研究这一现象，我们使用了数据集的LiDAR点云来计算地面真值（GT，Ground Truth）的地平面法线。在此基础上，我们以红色虚线标记了GT道路边缘，以便进行对比。

通过这些标记，我们清楚地发现大多数实际道路边缘未能与GT正确对齐，超出校准1度以上。为了更全面地了解这种动态不对齐现象的统计信息，我们对帧数进行了计数，根据它们在横滚和俯仰方向上相对于GT的变化数量。最终的统计结果如下图所示。

 可以观察到俯仰和横滚角的平均变化约为1.2度和3.5度，分别。换句话说，地平面法线向量不是恒定的，而是在车辆移动时动态变化。类似地，下表呈现了所有KITTI里程计序列上横滚和俯仰动态的平均值。我们可以得出相同的结论：当车辆移动时，地平面法线不是恒定的（大约为1度）。这种不稳定性可能进一步影响自动驾驶任务的性能。

使用我们提出的方法之前和之后的透视变换图像比较。上图中的（a）来自KITTI里程计数据集的原始图像。上图中的（b）使用从相机到地面的固定外参的透视映变换图像。上图中的（c）使用我们提出的方法计算的动态外参的透视变换图像。可以清楚地观察到（c）中的图像更为准确。

这里详细介绍了提出的地平面法线估计方法。下图展示了整个流程。简而言之，基于IEKF构建了从里程计（来自图像或IMU）到地平面法线的关系。为了更好地描述，以下描述以装在轮式车辆上的前置单目摄像头为例。

 

提出的IEKF可以处理来自各种传感器的自我运动，例如IMU、来自单目图像的视觉里程计，以及可以提供实时里程计信息的SLAM系统。最终的地平面法线向量N是基于IEKF残差旋转和先前校准的静态外参的组合实时预测的。

卡尔曼滤波器基础

卡尔曼滤波器是一种广泛应用于动态系统状态估计的算法。其基本思想是利用系统的动态模型和测量数据，通过递归更新状态估计和误差协方差矩阵，逐步逼近系统的真实状态。在本文中，IEKF（不变扩展卡尔曼滤波器）被用于地平面法线的估计。IEKF是一种扩展卡尔曼滤波器的改进版本，特别适用于非线性系统的状态估计。

IEKF的应用

IEKF在本文中的应用主要包括以下几个步骤：

状态初始化：初始状态向量包含车辆的位姿和地平面法线向量。初始值可以通过静态条件下的简单测量或估计获得。

预测步骤：根据车辆的运动模型，预测下一时刻的状态向量和误差协方差矩阵。这一步需要考虑车辆的运动学约束和地平面法线的动态变化。

更新步骤：利用新到达的测量数据（如里程计信息），更新状态向量和误差协方差矩阵。IEKF在更新过程中考虑了测量数据的非线性特性，提高了估计的准确性。

地平面法线的动态建模

地平面法线的动态建模是本文方法的核心。为了准确反映车辆运动过程中地平面法线的变化，本文利用车辆的运动学模型和传感器数据，建立了地平面法线的动态模型。该模型假设在小范围内地平面可以近似为平坦，并通过里程计信息实时调整法线向量。

单目相机和IMU融合

单目相机和IMU是本文实验中使用的两种主要传感器。单目相机通过视觉SLAM技术提供车辆的自运动信息，而IMU则通过惯性测量提供运动加速度和角速度数据。本文的方法能够融合这两种传感器的数据，利用视觉信息提高定位精度，利用惯性信息补偿视觉系统的弱点（如光照变化、遮挡等）。

数据处理和滤波

在实际应用中，传感器数据往往会受到噪声的影响，因此需要进行适当的滤波处理。本文采用IEKF对传感器数据进行滤波，消除噪声对地平面法线估计的影响。具体步骤包括对原始数据进行预处理，去除明显的异常值，然后通过IEKF进行状态估计和更新。

实时性和计算效率

本文方法的一个重要特点是能够实时估计地平面法线向量。这对于自动驾驶系统来说至关重要，因为地平面法线的动态变化会直接影响车辆的行驶安全。通过优化算法结构和利用高效的数值计算方法，本文实现了每帧3-50毫秒的计算时间，满足了实时应用的需求。

实验设置与结果

实验选择了两种具有挑战性的传感器设置，分别为单目摄像头和纯IMU里程计，并在KITTI数据集上进行验证。对于单目设置，使用ORB-SLAM2算法从左侧RGB摄像头图像中获取自我运动信息。在纯IMU里程计方面，采用AI-IMU算法提取自我运动信息。通过定量评估，验证了本文方法的精度和鲁棒性。实验结果显示，与现有方法相比，本文方法在精度方面表现出色，估计的向量误差显著降低。

KITTI数据集验证

实验结果表明，本文方法在单目和仅IMU里程计方面均取得了令人满意的精度。例如，在KITTI数据集上，与现有方法相比，本文方法将估计的向量误差从3.02°降低到0.39°。此外，基于单目的方法相较于仅使用IMU的里程计，提供了稍微更佳的结果。本文方法还显著减少了计算时间，每帧在3-50毫秒之间，进一步证明了其实时估计准确地平面法线向量的能力。

表2. 提出的方法与之前研究的定量比较。这里还进行了运行时间的比较，以展示使用我们的方法提高效率的改进。特别是，采用的IEKF每帧的运行时间不到一毫秒

视觉化结果

为了直观展示方法的效果，本文还对实验结果进行了视觉化处理。通过将估计的

地平面法线向量应用于图像中，生成了多张IPM图像（鸟瞰图像）。这些图像显示出道路边缘与地面真实值的对齐情况。实验结果表明，使用本文方法估计的地平面法线向量生成的IPM图像中，道路边缘更加平行和一致，图像质量显著提高。相比之下，传统方法生成的IPM图像在某些情况下存在显著的失真和不对齐现象。

消融实验

为了进一步验证本文方法的有效性，进行了消融实验。主要探讨了IEKF在地平面法线估计中的作用。通过分别使用纯里程计和纯视觉信息进行地平面法线估计，实验发现，纯相对里程计在某些情况下会导致地面法线的不一致估计，而绝对里程计由于误差累积导致估计结果更差。这表明，IEKF在结合两种传感器信息、消除误差方面具有显著优势。

 

如上图所示，仅使用纯相对里程计在某些情况下导致地面法线的不一致估计。这是因为帧之间的相对旋转仅包含车辆姿势的“瞬时信息”，因此无法处理各种道路表面，如小坡道或凸起。对于绝对里程计，由于里程计误差随着时间的推移而累积，结果甚至更糟。定量结果如下表所示。

其他数据集的验证

除了KITTI数据集外，本文还在其他数据集上进行了验证，例如Cityscapes和Waymo数据集。这些数据集具有不同的传感器设置和环境条件，验证了本文方法在多种场景下的通用性和鲁棒性。实验结果显示，本文方法在不同数据集上均取得了优异的性能，进一步证明了其广泛适用性。

局限性与未来工作

局限性

尽管本文方法在地平面法线估计方面表现出色，但仍存在一些局限性。首先，本文方法假设地平面在小范围内可以近似为平坦，因此在极不平整的道路上或车辆急转弯情况下，估计精度可能会降低。其次，本文方法依赖于传感器的精度和数据的稳定性，当传感器数据受到严重干扰或失效时，估计结果可能会受到影响。

未来工作

针对上述局限性，未来工作可以从以下几个方面进行改进和拓展：

增强地平面模型：考虑更复杂的地平面模型，如多段平面模型或曲面模型，以适应更复杂的道路情况。

多传感器融合：结合更多种类的传感器数据，如激光雷达、双目相机等，提高地平面法线估计的鲁棒性和精度。

自适应滤波器：研究和开发自适应滤波器算法，根据不同的驾驶场景和传感器数据动态调整滤波器参数，进一步提高估计精度。

实时优化：优化算法结构和实现，进一步减少计算时间，提高实时性能，以适应更多实际应用场景。

总结

本文提出了一种新颖的轮式车辆地平面法线估计方法，通过结合自车位姿里程计和不变扩展卡尔曼滤波器，实时估计地平面法线向量。实验结果表明，本文方法在单目和仅IMU里程计方面均取得了令人满意的精度，并在KITTI数据集上实现了最先进的准确性。此外，本文方法具有高效的计算性能，能够满足实时应用的需求。

通过消融实验验证了IEKF在地平面法线估计中的有效性，并在多个数据集上进行了广泛的验证，证明了本文方法的通用性和鲁棒性。尽管本文方法在某些极端条件下可能存在局限性，但通过进一步研究和优化，有望在未来取得更广泛的应用。

总之，本文提出的地平面法线估计方法在自动驾驶领域具有重要意义，能够显著提高自动驾驶系统的感知、导航和规划能力，为实现更加安全和高效的自动驾驶提供了有力支持。