82、单目相机与平面激光雷达的统一时空校准

单目相机与平面激光雷达的统一时空校准

1. 引言

在机器人任务执行中，多传感器及多种传感模式的结合使用十分常见，其目的是让机器人的任务执行更加稳健和可靠。传感器通常会被选择为互补关系，这样既能发挥组合的优势，又能减轻单个传感器的局限性和缺点。

单目相机和平面激光雷达就是一对互补的传感器。单目相机能提供低漂移的六自由度（6 - DOF）运动估计，但存在尺度模糊的问题；而平面激光雷达虽然只能在其单一扫描平面内提供准确的距离信息，但能提供已知尺度的精确距离数据。二者结合后，可作为低成本的3D激光雷达扫描仪，以较低的成本生成密集且配准的3D点云。

不过，多传感器融合虽有诸多好处，但也有一个前提条件：在任何多传感器系统中，校准都是确保数据正确融合的必要步骤。一方面，需要精确掌握传感器之间的空间变换，才能将各个传感器的测量值正确整合到同一个参考框架中；另一方面，时间校准也至关重要，即要对齐传感器时钟，并确定是否存在固有的处理延迟。如果仅根据原始传感器时间戳简单地融合数据，可能会导致严重的估计误差。

我们致力于开发一种统一的相机 - 激光雷达时空校准程序，该程序需满足两个条件：一是能同时求解空间和时间校准参数；二是能在任意环境中工作，无需特定的校准目标或已知的场景结构。我们的技术基于前人的研究方法，在此基础上有了一些新的贡献，包括统一空间和时间校准的问题公式化、使用理论上可靠的方法对不同更新频率的传感器进行位姿插值、引入加速校准过程的快速贝叶斯优化程序，以及通过实验证明在任意配置和未知结构环境中都能对相机和激光雷达传感器进行时空校准。

2. 技术方法及相关工作

传统的相机 - 激光雷达空间校准方法通常会对校准环境和校准过程本身施加一些不理想的限制。例如，常常需要确保传感器有重叠的视野，或者每个传感器观察的场景具有已知结构，又或者需要有已知的校准对象。还有一些更通用的校准方法，但可能要求每个传感器都能测量自身的运动，而对于在三维空间中移动的平面激光雷达扫描仪来说，这是无法实现的。此外，现有的大部分校准文献要么忽略了时间校准，要么假定时间偏移（延迟）已经通过其他方式确定。

我们提出的校准方法的关键思想是：当激光雷达扫描数据进行配准（给定一组相应的相机位姿）时，校准参数会影响最终激光雷达点云的熵。校准不准确会导致点云出现“模糊”效果，不同的激光雷达扫描受到的影响也不同，最终会使点云的熵增大。每个校准参数对点云熵的影响方式不同，其中旋转和轨迹尺度参数的影响最大。大量实验表明，真实的校准参数能使点云的整体熵最小化。因此，我们通过最小化基于Rényi二次熵（RQE）的成本函数来求解最优参数。在我们的方法中，单目相机仅作为运动传感器，该方法也可扩展到使用其他传感器（如立体相机、惯性测量单元或GPS - INS单元）替代单目相机的平台。

2.1 点云生成

我们的目标是估计一组定义从激光雷达坐标系 (F_{L}) 到相机坐标系 (F_{C}) 变换的（常量）参数：
[
\Xi =
\begin{bmatrix}
x_{L} & y_{L} & z_{L} & \varphi_{L} & \theta_{L} & \psi_{L} & s & t_{d}
\end{bmatrix}^{T}
]
其中，六个空间参数（平移参数 (x_{L})、(y_{L})、(z_{L}) 和旋转参数 (\varphi_{L})、(\theta_{L})、(\psi_{L})）用于构建从激光雷达坐标系到相机坐标系的 (4\times4) 刚体变换矩阵 (T_{C,L})。尺度因子 (s) 应用于相机位姿的平移分量。在实践中，我们发现像某些文献那样加入额外的距离偏差参数，对校准结果的改善并不明显，有时甚至会产生负面影响。

为了考虑传感器间的测量时间延迟，我们采用传感器平台轨迹的连续时间表示：
[
T_{G,L}(t) = T_{G,C}(t + t_{d}) T_{C,L}
]
在时间步 (t = t_{1}, t_{2}, \cdots, t_{N}) 对 (T_{G,L}(t)) 进行评估，以获得轨迹上所有 (N) 次激光雷达扫描对应的激光雷达位姿。通过在（不同步的）相机位姿之间进行插值来得到 (T_{G,C}(t + t_{d}))。如果延迟值 (t_{d}) 为正，则相机时钟领先于激光雷达时钟；反之亦然。插值使用STEAM模型进行，该模型允许对插值位姿的不确定性进行原则性估计。相机位姿的不确定性用于在熵计算中对单个激光雷达扫描的贡献进行加权。

在时间 (t_{n})，激光雷达产生一次扫描，包含 (K) 个测量值 ((x_{L_{n}}^{(k)}, y_{L_{n}}^{(k)}))，(k = 1, \cdots, K)，每个测量值代表扫描平面中一个测量点的坐标。我们使用当前估计的激光雷达位姿将这些点转换到全局坐标系中：
[
\hat{p} {G {n}}^{(k)} = h^{-1}(p_{L_{n}}^{(k)} | T_{G,C_{n}}, \Xi) = T_{G,L}(t_{n})
\begin{bmatrix}
x_{L_{n}}^{(k)} & y_{L_{n}}^{(k)} & 0 & 1
\end{bmatrix}^{T}
]
为简化符号，在后续推导中省略齐次分量，即 (\hat{x} {G {n}}^{(k)} \leftarrow \hat{p} {G {n}}^{(k)})：
[
\hat{x} {G {n}}^{(k)} =
\begin{bmatrix}
x_{G_{n}}^{(k)} & y_{G_{n}}^{(k)} & z_{G_{n}}^{(k)}
\end{bmatrix}^{T}
\leftarrow
\begin{bmatrix}
x_{G_{n}}^{(k)} & y_{G_{n}}^{(k)} & z_{G_{n}}^{(k)} & 1
\end{bmatrix}^{T}
= \hat{p} {G {n}}^{(k)}
]
我们使用模型的雅可比矩阵和相机位姿协方差，根据以下公式获得全局坐标系中每个激光雷达点的位置协方差估计：
[
\Sigma_{n}^{(k)} = J_{n}^{(k)} Q_{n} J_{n}^{(k)T}
]
[
J_{n}^{(k)} = \frac{\partial h^{-1}(x_{L_{n}}^{(k)}|T_{G,C_{n}}, \Xi)}{\partial T_{G,C_{n}}}
]
其中，(T_{G,C_{n}}) 是插值后的6 - DOF相机位姿，即 (T_{G,C}(t_{n} + t_{d}))，(Q_{n}) 是相关的相机位姿协方差矩阵。需要注意的是，相机位姿的平移不确定性必须进行尺度缩放，以匹配轨迹的（度量）尺度，并且 (Q_{n}) 是插值后相机位姿的协方差矩阵，而非任何测量相机位姿的协方差矩阵。

2.2 Rényi二次熵计算

将所有激光雷达扫描组合在一起，我们将整个激光雷达点集 (\hat{X}) 建模为高斯混合模型。总共有 (NK = M) 个点 (\hat{x}^{(m)})，(m = 1, \cdots, M)，每个点都有对应的协方差矩阵 (\Sigma^{(m)})，(i = 1, \cdots, M)。所有点的坐标都在全局坐标系 (F_{G}) 中表示。点云的RQE可以通过以下公式计算：
[
H[\hat{X}] = -\log
\left(
\frac{1}{M^{2}}
\sum_{i = 1}^{M}
\sum_{j = 1}^{M}
\mathcal{N}
\left(
\hat{x}^{(i)} - \hat{x}^{(j)},
\Sigma^{(i)} + \Sigma^{(j)} + 2\sigma^{2}I_{3}
\right)
\right)
]
其中，(\sigma^{2}I_{3})（(I) 是 (3\times3) 单位矩阵）用于表示激光雷达距离测量的不确定性。由于对数是单调函数，且 (\frac{1}{M^{2}}) 是常数，我们可以通过求解以下公式来最小化RQE并找到最优校准参数 (\Xi^{ })：
[
\Xi^{ } = \arg\min_{\Xi}
-
\sum_{i = 1}^{M}
\sum_{j = 1}^{M}
\mathcal{N}
\left(
\hat{x}^{(i)} - \hat{x}^{(j)},
\Sigma^{(i)} + \Sigma^{(j)} + 2\sigma^{2}I_{3}
\right)
]
RQE值以一种对异常值部分鲁棒的方式量化了3D环境的“结构化程度”。包含许多聚集在表面上的点的点云，其RQE往往比那些点分散在空间中的点云低。因此，使用正确的相机 - 激光雷达校准生成的点云应该具有较低的RQE，即使在缺乏大平面表面（如校准目标）的环境中也是如此。

根据前人的研究，我们可以通过忽略对熵贡献可忽略不计的点来加速原本复杂度为 (O(M^{2})) 的RQE计算。引入一个调整参数 (k)，可以进行如下近似：
[
\mathcal{N}
\left(
\hat{x}^{(i)} - \hat{x}^{(j)},
\Sigma^{(i)} + \Sigma^{(j)} + 2\sigma^{2}I
\right)
\approx 0
\quad
\text{如果}
\quad
\left\lVert
\hat{x}^{(i)} - \hat{x}^{(j)}
\right\rVert
\geq 2k
\sqrt{
\max
\left(
\lambda_{1}(\Sigma^{(i)}),
\lambda_{1}(\Sigma^{(j)})
\right)
+ \sigma^{2}
}
]
其中，(\lambda_{1}(\Sigma)) 是矩阵 (\Sigma) 的最大特征值。(k) 值越大，计算时间越短，但精度会降低。可以使用k - d树来有效地确定哪些点对RQE计算有显著影响。

2.3 贝叶斯优化

虽然上述简化使算法的运行时间变得可行，但也带来了一个额外的难题。由于在优化的每次迭代中选择的点集不同，成本函数不再具有连续的解析导数，因此无法使用牛顿法轻松进行优化。尽管成本函数在微观尺度上表现出“锯齿状”，但在宏观尺度上仍然（几乎）是平滑的。此外，在真实校准值的邻域内，该函数（几乎）呈凸性。

考虑到成本函数的性质（即评估成本高且导数不连续），我们采用贝叶斯优化作为替代的最小化方法。该方法通过对参数空间进行采样，构建成本函数的高斯过程代理模型。然后，优化器会在参数空间中有效地查询那些能提供最多信息（或产生最大潜在改进）的点。我们使用BayesOpt库来求解校准参数，与前人的研究相比，计算速度提高了八倍或更多。

3. 实验结果

为了验证所提出的校准程序，我们进行了一系列模拟研究，并处理了多个真实世界的校准数据集。我们的目标是评估校准算法在各种条件下的性能，确定可靠校准所需的数据量，以及评估传感器平台所需的运动类型。

3.1 模拟研究

我们在五种不同的环境中进行了多次模拟，这些环境包含不同数量的平面、圆柱和球形表面，具体如下：
1. 简单房间 ：由六个平面组成的长方体。
2. 圆形房间 ：由一个圆柱形空间和两个作为地板和天花板的平面组成。
3. 平面城市 ：由11个平面（包括地板和天花板）组成的长方体，内部平面呈不规则角度排列。
4. 停车场 ：与简单房间环境类似，但包含六个圆柱形“柱子”和两个球形目标。
5. 森林 ：最具挑战性的环境，由“波浪状”地面和一系列带有球形“树叶”的圆柱形“树木”组成，树木和树叶随机分布。

在每次模拟试验中，虚拟相机 - 激光雷达对沿着正弦轨迹（包括位置和方向）移动，使激光雷达能够从多个不同的有利位置反复扫描环境中的大部分表面。我们通过向每个模拟相机位姿添加标准差为5 mm（位置）和0.5°（方向）的不相关零均值高斯噪声，来评估相机位姿估计噪声的影响。通过对每个相机位姿的平移分量进行均匀缩放来模拟尺度不确定性。两个传感器的数据生成频率均为40 Hz，但更新时间有所偏移，以模拟传感器时钟之间的实际延迟。每个模拟数据集的长度为90 s，激光雷达通常扫描两到四百万个点。

初始模拟结果总结在表1中。在大多数情况下，我们能够得到平移误差小于5 mm、旋转误差小于0.1°、时间延迟误差小于0.15 ms的校准估计。不过，在极具挑战性的“森林”环境中是个例外，由于该环境本身比较稀疏，激光雷达扫描的点数要少得多。我们认为，通过修改传感器平台的轨迹，使激光雷达主要扫描地面和模拟的“树木”，而不是空旷空间，可以显著提高该环境下的校准精度。

我们还通过生成几个长度为180 s的新模拟数据集，测试了更长序列对校准精度的影响。结果表明，精度与之前的模拟结果相当，这表明在整体轨迹长度方面存在收益递减的情况。准确的校准需要激光雷达从多个不同的视角扫描场景，以有效地约束校准参数。但在某个阶段，大多数表面都会被反复扫描（导致点密度较高），此时添加更多数据可能不会显著改善校准结果。

环境	x [mm]	y [mm]	z [mm]	φ [°]	θ [°]	ψ [°]	s ( \times 10^{-3} )	td [ms]
简单房间	1.07	1.68	2.21	0.012	0.025	0.012	0.013	0.020
圆形房间	0.96	3.12	3.54	0.026	0.050	0.008	0.055	0.135
平面城市	2.45	3.37	2.80	0.010	0.038	0.009	0.126	0.029
停车场	2.45	4.33	3.69	0.028	0.064	0.014	0.178	0.081
森林	12.9	11.4	13.9	1.06	0.548	0.235	35.8	4.76

下面是模拟研究的流程：

graph TD;
    A[选择模拟环境] --> B[设置传感器运动轨迹];
    B --> C[添加噪声和模拟尺度不确定性];
    C --> D[生成模拟数据];
    D --> E[进行校准计算];
    E --> F[评估校准结果];

3.2 真实世界实验

在真实世界中，我们使用Hokuyo UTM - 30LX激光雷达和PointGrey Flea3单目相机（刚性连接，且视野不重叠）在杂乱的实验室环境中进行了多次测试。激光雷达的采样频率为40 Hz，相机的工作频率较高，以便我们对相机位姿进行插值。我们收集了一个大型数据集，然后将其分为三个独立的、不重叠的子集，每个子集长度为100 s，每个子集中大约有三百万个激光雷达扫描点。我们使用ORB - SLAM2确定相机的轨迹（直到未知尺度因子）。

校准结果如表2所示。大多数校准试验的优化参数是一致的，但也有一些例外，例如数据集2中的z平移。我们认为这些差异是由特定的平台轨迹造成的，这也凸显了激发所有运动自由度以充分约束整个校准参数集的必要性。

数据集	x [mm]	y [mm]	z [mm]	φ [°]	θ [°]	ψ [°]	尺度	td [s]
数据集1	50.5	0.2	-211.0	181.7	-1.1	-88.8	0.216	0.016
数据集2	55.1	7.7	-234.2	186.1	1.0	-89.4	0.212	0.018
数据集3	47.8	-0.4	-224.2	184.2	-1.5	-89.7	0.214	0.018

真实世界实验的流程如下：

graph TD;
    A[准备实验设备] --> B[收集实验数据];
    B --> C[划分数据集];
    C --> D[确定相机轨迹];
    D --> E[进行校准计算];
    E --> F[分析校准结果];

4. 讨论

通过使用基于RQE的时空校准技术，我们获得了一些有用的见解。我们发现，获得准确校准的关键是使用大规模的传感器轨迹。在模拟研究中，我们观察到，当传感器平台在模拟环境中移动的范围更大时，校准精度有了显著提高；在真实世界的实验中也有同样的情况。我们认为，这种改善的一个原因是，更大/更长的轨迹有效地降低了相机位姿估计误差的影响（相对于整体轨迹而言，位姿误差的相对大小减小了）。虽然更长的轨迹意味着激光雷达扫描在空间和时间上的间隔更大，但从广泛不同的视角扫描表面已被证明能更好地约束校准参数。当相邻扫描在更远的距离采集时，能够将激光雷达点收集到共同表面上的有效校准变换就会更少（无论涉及的表面的确切形状如何）。

目前，校准算法假设单个激光雷达扫描中的所有点都是同时采集的。当传感器平台移动缓慢时，这是一个合理的近似，但当平台快速旋转或平移时，就忽略了运动失真的可能性。我们之前的研究表明，不正确的时间延迟估计（约20 ms）可能会导致空间变换参数的校准误差高达一定程度。

为了进一步提高校准的准确性和鲁棒性，我们可以考虑以下几点改进：
1. 考虑运动失真 ：开发一种能够考虑激光雷达扫描过程中运动失真的校准算法。可以通过对激光雷达扫描进行更精细的建模，例如将扫描过程划分为多个子阶段，每个子阶段考虑平台的瞬时运动。
2. 自适应调整参数 ：根据不同的环境和传感器平台运动情况，自适应地调整校准算法中的参数，如RQE计算中的调整参数 (k)。可以通过机器学习方法，根据环境特征和运动特征自动选择最优的参数值。
3. 多传感器融合的深度利用 ：除了单目相机和激光雷达，进一步探索与其他传感器（如IMU、GPS等）的深度融合，以提供更准确的位姿信息和更丰富的环境感知，从而更好地约束校准参数。

5. 总结

本文提出了一种统一的单目相机与平面激光雷达时空校准程序，该程序能够同时求解空间和时间校准参数，并且可以在任意环境中工作，无需特定的校准目标或已知的场景结构。我们的方法基于Rényi二次熵，通过最小化点云的熵来确定最优的校准参数。

通过模拟研究和真实世界实验，我们验证了该校准程序的有效性。在大多数模拟环境中，我们能够实现高精度的校准，平移误差小于5 mm、旋转误差小于0.1°、时间延迟误差小于0.15 ms。在真实世界实验中，校准结果也表现出较好的一致性，但特定的平台轨迹可能会对校准结果产生影响。

我们还发现，大规模的传感器轨迹对于获得准确校准至关重要，因为它可以降低相机位姿估计误差的影响，并更好地约束校准参数。然而，目前的校准算法在处理快速运动时存在一定的局限性，未来可以通过考虑运动失真、自适应调整参数和深度利用多传感器融合等方法进行改进。

以下是整个校准流程的总结表格：
| 步骤 | 操作内容 |
| — | — |
| 1 | 选择模拟环境或准备真实世界实验设备 |
| 2 | 设置传感器运动轨迹（模拟）或收集实验数据（真实世界） |
| 3 | 添加噪声和模拟尺度不确定性（模拟）或划分数据集（真实世界） |
| 4 | 生成模拟数据（模拟）或确定相机轨迹（真实世界） |
| 5 | 进行校准计算，包括点云生成、RQE计算和贝叶斯优化 |
| 6 | 评估校准结果（模拟）或分析校准结果（真实世界） |

整个校准过程的流程图如下：

graph LR;
    classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px;
    classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px;
    A([开始]):::startend --> B{模拟或真实世界}:::process;
    B -->|模拟| C(选择模拟环境):::process;
    B -->|真实世界| D(准备实验设备):::process;
    C --> E(设置传感器运动轨迹):::process;
    D --> F(收集实验数据):::process;
    E --> G(添加噪声和模拟尺度不确定性):::process;
    F --> H(划分数据集):::process;
    G --> I(生成模拟数据):::process;
    H --> J(确定相机轨迹):::process;
    I --> K(进行校准计算):::process;
    J --> K;
    K --> L(评估/分析校准结果):::process;
    L --> M([结束]):::startend;

通过以上的研究和实验，我们为单目相机与平面激光雷达的时空校准提供了一种有效的方法，并且指出了未来可能的改进方向，有望在机器人感知和导航等领域得到更广泛的应用。