SensorX2car：在道路场景下的完成传感器到车体坐标系标定

文章：SensorX2car: Sensors-to-car calibration for autonomous driving in road scenarios

作者：Guohang Yan, Zhaotong Luo, Zhuochun Liu and Yikang Li

编辑：点云PCL

代码：https://github.com/OpenCalib/SensorX2car

作者单位：上海人工智能实验室

摘要

自动驾驶系统中传感器的性能基本上取决于传感器校准的质量。必须将传感器准确地标定到车体坐标系下，才能够提供有价值的定位和环境感知结果。然而，虽然目前已经提出了许多在线校准的方法来实现传感器之间的外参校准，但很少有研究集中在传感器与车体坐标系之间的校准。为此，作者提出了SensorX2car，一个用于在线校准道路场景中传感器与车体坐标系之间关系的校准工具箱。它包含四种常用的传感器：IMU（惯性测量单元）、GNSS（全球导航卫星系统）、LiDAR（激光雷达）、相机和毫米波雷达。我们分别为每个传感器设计了一种方法，并主要校准了其相对于车体的旋转。在实际路测和模拟实验中证明了所提出方法的准确性和泛化能力。同时，相关代码已经开源以造福社区。据我们所知，SensorX2car是第一个开源的传感器与车体校准工具箱。

代码已开源：https://github.com/OpenCalib/SensorX2car。

主要贡献

目前有一些与传感器校准相关的开源项目，但大多数都是用于两个或多个传感器之间的校准。对于传感器与车之间的校准方法仍然非常少。因此，基于之前的OpenCalib[1]校准研究和项目经验，作者提出了一种基于道路场景的多传感器在线校准方法SensorX2car。本文的贡献如下：

• 提出了一种基于道路几何特征的相机与车体坐标系之间的校准方法，可以用单张图像进行实时校准。
• 提出了一种基于LiDAR的在线校准方法，可用于任意驾驶路线并实时运行。该方法可以准确估计旋转和z轴位移。
• 通过选择最可能直接在车辆前方的静止物体和使用基于道路场景中静态物体的最小二乘优化拟合的新型校准优化方法，引入了一种Radar的在线校准方法。
• 提出了一种从姿态传感器到车辆的道路场景在线校准方法，可在直线或非直线行驶条件下实施。
• 所提出的校准方法在我们的模拟和真实数据集上表现出良好的性能，同时，相关代码已经开源以惠及社区。

主要内容

A. 坐标系定义

本文中的参考坐标系都是以图1中定义的车辆和其他传感器参考坐标系，使用(φ; θ; ψ)来表示横滚角、俯仰角和偏航角。

图1. 车辆和传感器的参考坐标系：z轴向上，x轴向前，y轴向左。

B. 相机标定

相机标定的总体过程如图2所示，首先通过深度学习网络从单张图像中估计消失点和水平线的角度，假设相机的内参已知，可以计算出相机和路面之间的三个旋转角度，当车辆直行时，车辆的方向可以视为路面的方向。因此，可以得到了相机和车辆之间的旋转，需要注意的是，车辆不需要一直直行，我们在算法中使用消失点位置的稳定性作为判断条件来提取有用的图像片段。

图2，相机标定的流程

1）检测网络：网络概述如图3所示，对于输入图像估计消失点位置和水平线角度，使用Transformer作为基本结构，并将提取的线特征馈送到解码器层，使用堆叠的Hourglass网络作为骨干网络来生成特征图，因为它可以在扩大感知范围的同时保留局部细节，在原始Hourglass网络之前和之后添加MaxPool层，以降低特征图的分辨率，以适应Transformer编码器输入。对于输入图像，来自骨干网络的高维特征图（W; H; W）被展平（W; H×W），并馈送到编码器层，然后特征被编码并传递到解码器层。

图3，消失点和地平线检测网络综述

设置了两个可学习的查询对象作为解码器层的输入，以估计消失点的位置和水平线的角度。使用反正切投影将线和点表示在高斯球上。图像帧中的点p =（u; v）可以在高斯球上表示为：

此外，通过LSD提取线特征，并在卷积网络之后将其输入解码器，线可以表示为两个端点的叉积：

在解码器的输出端为每条线形成一个分类任务，以区分其是否通过消失点。通过这种方式，网络被迫更加关注朝向消失点的线条，我们设置一个阈值来标记距离消失点的线条是否为正样本或负样本。

其中d_i是线段l_i和消失点p之间的距离。

最终的损失函数包含三个部分：消失点位置的余弦相似度、地平线角度的绝对值误差和线分类的二元交叉熵损失(BCE loss)。

其中，c_i 是第 i条线的标签，s_i是第i条线的 SoftMax 网络的输出。

2）旋转推导： 这里推导从VP位置和水平线角度到相机坐标系和路面坐标系之间的旋转过程，当车辆直行时，我们可以得到相机到车辆的旋转矩阵， 对于针孔相机，从三维空间P（X，Y，Z）到图像平面p（u，v）的投影可以描述为：

由于消失点是在无穷远处的平行线的交点，因此Z_vp=1。这里我们用p1表示图像平面上的消失点，用R3表示旋转矩阵R的第三列,通过使用方程式9的齐次性，我们可以得到：

可以从 R3 推导出偏航角和俯仰角：

横滚角就是水平线的角度。

3）手动标定：如上所述作者还设计了一种手动工具，通过使用消失点、水平线和旋转角度之间的关系来进行相机标定，在这个工具中用户可以手动逐步改变角度。图像将通过您调整的新外参数进行重投影：

这里的T_WC表示从世界坐标系到相机坐标系的转换，T_M是我们手动添加的转换矩阵，一旦T_M = T_WC的逆，消失点就变成了：

其中，cx和cy是K中的内参，因此，调整目标是使你自己估计的消失点与光心重合，并使水平线看起来更加水平。这个工具的精度取决于你估计的消失点和水平线的质量。

C. LiDAR 标定

对于 LiDAR，需要校准它的旋转和高度（离地高度），具体而言，首先通过提取地平面来校准俯仰、横滚和高度，然后通过分析相应时间戳处轨迹方向和速度方向的差异来估计偏航角。

1）俯仰和横滚角估计：为了估计俯仰、横滚和 LiDAR 高度，基本上通过 RANSAC 算法提取地平面实现。该算法重复多次，使用随机的初始点，并对结果进行平均，以获得最终平面参数。然而，如果地面非常不平坦，RANSAC 可能无法找到足够的点来拟合平面。然后，这里作者采用 LPR [42] 作为替代方案，使用确定性分配的种子点迭代地提取地面表面。它首先按 z 轴值对点进行排序，并选择若干个具有最小 z 值的点来计算初始平面。然后它区分内点并进行迭代拟合平面。提取的平面可以由其法向量 np 和截距 dp 表示：

然后可以得到旋转向量和旋转角度：

激光雷达的旋转R和高度z导出为：

其中Z^垂直水平面的法向量.

2）偏航角估计：在获取了俯仰角和横滚角之后，我们进行点云旋转，使得点云的 z 轴垂直于地面平面，通过SLAM 算法可以在每个时间戳获得 6 自由度位姿，并且 LiDAR 在地面上的 2D 轨迹可以通过位置 (xei，yei) 获得。接下来分析轨迹方向和速度方向之间的差异来估计偏航角，具体来说，使用一个长度为 3 的滑动窗口，在每个时间戳上计算当前和前两个时间戳的位置差分和速度差分，然后使用这些差异来计算速度方向和轨迹方向的夹角，这就是 LiDAR 的偏航角。

在实验中，我们选择FAST-LOAM [43]作为算法，因为它能够同时实现实时性能和良好的准确性。然后使用B样条插值来根据t x(t)，y(t) 拟合x、y曲线，并获取它的导数。

n是B样条的次数。方向速度可以计算为：

其中v_i可以看作时间戳i时车辆的前进方向，e_i是时间戳i时LiDAR的方向。因此这两个角度之间的差异就是我们需要校准的偏航角偏差。此外根据两个条件过滤掉一些会产生较差估计的数据点，因为我们使用导数来计算速度，当车辆速度较低甚至静止时，计算出的方向容易产生显著的误差。因此需要设置一个最小值来删除移动速度过慢的数据点，由于算法的性能高度依赖于曲线拟合的效果，则需要删除曲率大的数据点，这意味着车辆有很大的加速度或者进行了急转弯。虽然我们的算法通常可以在曲线路径上正常工作，但建议使用直行驾驶数据以获得更准确的结果。

最终的偏航角是通过对每个有效时间戳的 vi 和 ei 的差异进行平均计算得出的。

D. 毫米波雷达校准

毫米波雷达校准一般只需要校准雷达的偏航角，这个过程分为三步，包括粗略校准、静止物体识别和曲线拟合。毫米波雷达测量包含物体在雷达参考系下的相对速度、角度和距离，在这里还需要其他传感器的帮助，比如GNSS或者轮速计来获取车辆的速度，如果物体静止，则表示为：

其中theta是雷达坐标系到车辆坐标系的偏航角，是需要进行求解的值，这个公式在图4中有解释。

图4，物体静止时的速度关系

• 偏航角校准：在偏航角校准阶段，首先将速度接近当前车速的目标点作为静止物体，通过分析具有最多静止物体的方向得到初始的偏航角度估计。
• 静止物体识别：使用当前的偏航角度估计可以找出满足式子（24）的静止物体。
• 曲线拟合：找到所有帧的静止物体后，可以拟合cos函数曲线来更新偏航角度估计，步骤2和3以迭代的方式进行，以优化校准结果。

E. 位姿-传感器校准

在位姿传感器校准中，估计IMU和车辆的偏航角以校正前向方向的传感器的安装误差，有两个分支：

1）直行：如果已知车辆直行，使用RANSAC算法将GNSS轨迹拟合成一条直线，并将其视为车辆的方向，表示为v。然后使用所有时间戳处的IMU测量Ii，并计算v和Ii之间的差异：

其中S_l是直线路径的集合。

2）非直行驾驶：该过程类似于LiDAR校准方法，计算车辆行驶方向与IMU测量的偏差角度，车辆行驶路线由GNSS测量得到，在局部参考框架中表示为（xiG; yiG; ziG）,然后采用B样条方法平滑行驶路线，并在每个时间戳获取方向，表示为vi,IMU测量中的偏航角度表示为iI,通过删除行驶方向快速变化的数据，例如急转弯，可以获得实时真实偏航角的精确近,校准公式可以描述为：

由于 GNSS 轨迹不是非常精确和可靠，因此建议直接记录直线行驶数据进行校准。

实验

A. 相机标定

为了训练和评估我们的检测网络，首先标注了一个数据集，其中包含消失点和地平线的注释。据我们所知，很少有开源数据集包含消失点和水平线标注，适用于自动驾驶场景。在基于KITTI [44]构建了自己的数据集。每帧的地平线通过IMU的绝对姿态和KITTI提供的IMU与相机之间的相对姿态自动生成。对于消失点，使用半自动方式，我们首先使用Neurvps估计一个大致的消失点。该算法在许多情况下可能失败，例如弯曲的道路或光照变化等。因此，需要人工手动检查并纠正它们。最终数据集包含43195帧，包括城市、住宅区、高速公路、校园、山区等不同场景。图5展示了一些示例，该数据集大致分为70％，15％和15％用于训练，验证和测试。

图5，不同场景下标注的KITTI数据集示例

将我们的方法分别与消失点检测网络和水平线估计网络进行了比较，对于消失点，将我们的方法与Neurvps [21]进行比较，对于地平线，我们将其与TCHL 进行比较。结果如表1所示。实验结果表明，我们的方法具有竞争性能和相对较高的推理速度。

B. LiDAR校准

在几个现实场景和模拟数据集上测试了算法。

1.定性结果: 为了定性地证明我们算法的有效性，可视化了车辆方向vi从轨迹中推导出来的，以及LiDAR方向vi在所有时间戳上的情况。如果这两条曲线具有相同的趋势，并且只有固定的偏移量不同，则意味着拟合效果是可靠的，如图6所示。

图6，车辆偏航角与激光雷达偏航角的比较

图7，虚拟环境中的2D激光雷达路线1

2. 定量结果：对于模拟数据，我们有旋转角的真实值，表2展示了估计结果与真实值之间的差异，虚拟世界中车辆的路径如图7所示，对于真实世界的数据可以使用不同帧结果的一致性作为评估指标。

此外还有一个粗略的初始校准结果，该结果是在校准房间中获得的，可以用作参考，结果如表3所示，在真实世界的数据集中，路径是任意的，包含曲线和直线。

C. 毫米波雷达校准

1.定性结果：为了定性评估，可视化了公式 24 中的余弦曲线拟合，如图 8 所示，大多数数据点都很好地拟合了余弦曲线，证明了该方法的有效性。

图8，毫米波雷达标定中的余弦拟合

2.定量结果：对于定量评估，使用数据一致性和拟合优度指标，一个真实世界的雷达数据的实验结果如表4所示。

D.姿态传感器校准

1.定性结果：我们在图9中可视化了B样条曲线拟合的效果，GNSS轨迹的原始偏航角度波动很大，使用B样条插值后，曲线得到了平滑，可以发现，尽管存在固定的偏移量，但两个曲线具有相似的趋势。

图9，毫米波雷达定标中的余弦拟合

2.定量结果：使用RANSAC拟合的得分和直线条件下的速度投影验证来评估结果，RANSAC得分是通过内点数与总数据点数的比例计算得出的，速度投影是使用校准后的偏航角计算得到的径向速度分量，较小的速度分量表示更准确的偏航角估计，至于自由行驶条件，使用所有帧估计的数据一致性来衡量结果。

总结

本文提出了SensorX2car，这是第一个开源的实现传感器车体坐标系系标定工具箱，其中包含了四种常用传感器的标定方法：相机、激光雷达、毫米波雷达和姿态传感器(IMU+GNSS)，这些方法是无需目标的，可以在常见的路况下工作，通过在模拟和真实场景数据集上验证了这些方法的实用性和有效性，我们的工作仍有一些改进的空间，未来将进一步优化这些方法，并进行更详细的实验，以系统地评估它们。