激光雷达外参标定算法详解

图 3-2 给出了 SSAC 算法的标定场景。该算法需要一个垂直标定杆作为外部参考，并依赖于道路水平的先验信息。SSAC 算法在总体上分为两个阶段：在第一阶段，通过地面点云进行平面拟合，得到水平地面在激光雷达坐标系下的方程，并构造水平度函数，然后利用粒子群优化算法求得水平度最小时对应激光雷达相对车体的俯仰角 pitch、横滚角 roll 和高度 Δz；

在第二阶段，在车辆直线行驶过程中分析同一个标定杆对应的点云特征，具体则是在每一帧中通过聚类获取标定杆的中心点，而后将我们从多帧中获取的标定杆中心点连成直线，由该直线与车辆前进方向的夹角计算求解出激光雷达相对车体的偏航角 yaw。SSAC 算法通过上述步骤即可完成对激光雷达 4 个外参的标定，x 轴和 y 轴方向的位移偏差则可以通过测量得到。

3.2.1 SSAC 第一阶段

1. 基本思路

在该阶段的标定中，SSAC 算法根据地面点云求解俯仰角 pitch、横滚角 roll 和纵向位移（即高度）Δz。如图 3-3 所示，我们注意到，当 LiDAR 坐标系和车体坐标系有角度偏差时，例如在 pitch 角度分量有顺时针方向的俯仰角 γ，则水平地面在 LiDAR 坐标系下将呈现逆时针方向的倾角 γ。

图 3-3 水平地面在不同坐标系下的表现形式

该算法需要我们首先从原始点云中分割出地面点云 P'（地面分割的具体方法详见第 5 章）。而后，基于待求解的激光雷达外参，得到车体坐标系下的地面点云 P。具体而言，我们可以将激光雷达坐标系下的坐标点 (x', y', z') 经式（3-1）转换得到车辆坐标系下的坐标点 (X, Y, Z)：

$$ \begin{bmatrix} x \ y \ z \end{bmatrix} = R_l^v \begin{bmatrix} x' \ y' \ z' \end{bmatrix} + d_v^l \tag{3-1} $$

其中： $d_v^l = [\Delta x, \Delta y, \Delta z]^T, R_l^v = R_x R_y R_z$

因此，在车体坐标系下，我们基于地面点云 P 可以拟合得到形如式（3-2）的地平面方程：

$$ z = Ax + By + C \tag{3-2} $$

其中，A、B、C 为平面方程系数。

根据简单的几何知识可知，当 A = B = 0 时，对应的平面将平行于 OXY 平面，因此陈贵宾等提出构建式（3-3）中的水平度函数：

$$ f = |A| + |B| \tag{3-3} $$

如果外参标定正确，则基于地面点云 P 得到的地面方程应为水平面，对应的水平度 f 趋近于 0。因此，可通过迭代搜索使得水平度 f 最小，得到对应俯仰角和横滚角的标定值，并可结合此时的 z 值计算得到激光雷达的安装高度。

2. 粒子群优化的基本概念

SSAC 采用粒子群优化算法对水平度函数 f 进行最小化。粒子群优化（Particle Swarm Optimization，PSO）算法是一种基于种群智能的启发式直接搜索算法，其在多个领域得到了广泛应用。PSO 最初是由 Kennedy 和 Eberhart [11] 通过对鸟群的觅食行为进行观察，提出的一种全局优化搜索方法。粒子群优化算法把优化问题的设计域抽象为'飞行空间'，把优化问题的解抽象为在空间中飞行的'粒子'。在算法搜索过程中，每个粒子通过自身的飞行记录以及整个粒子群体的飞行经验来确定下一步的飞行状态（包括粒子的速度和位置）。标准的粒子群优化算法一般用于解决单目标连续优化问题 $\arg\min f(x)$。假设在迭代过程中粒子 i 自身搜索到的最优点为 $Pbest_i$，整个种群搜索到的最优点为 Gbest，则该粒子的速度和位置更新公式为

$$ \begin{array}{l} v_i^{k+1} = w \times v_i^k + r_1 \times \operatorname{rand}_1 \times (Pbest_i - x_i^k) + r_2 \times \operatorname{rand}_2 \times (Gbest - x_i^k) \ x_i^{k+1} = x_i^k + v_i^{k+1} \tag{3-4} \end{array} $$

其中，i 是粒子的索引值，k 表示迭代次数，w 是惯性权重，r1 和 r2 被称为学习因子或加速常数，$v_i^k$ 和 $v_i^{k+1}$ 分别是第 i 个粒子在第 k 次迭代和第 k+1 次迭代中的飞行速度，$x_i^k$ 和 $x_i^{k+1}$ 是第 i 个粒子在第 k 次迭代和第 k+1 次迭代中的位置（即变量值）。

3. 算法流程

SSAC 第一阶段的流程如下。

（1）输入原始的地面点云数据。 （2）初始化参数。令 $\gamma = \Delta z = 0$，$\Delta x$ 和 $\Delta y$ 可通过测量得到；并随机初始化粒子种群 ${(pitch_i, roll_i)|i = 1,2,\dots ,N_p}$，其中 $N_p$ 为种群个数。 （3）对于每个粒子参数，进行下述操作。 （3.1）结合式（3-1）得到车体坐标系下的地面点云 P。 （3.2）通过最小二乘法得到车体坐标系下的平面方程，如式（3-2）所示。 （3.3）结合式（3-3）计算水平度值 $f_i$。 （3.4）结合每个粒子搜索到的水平度值 $f_i$，更新粒子搜索到的最优点 $Pbest_i$ 和种群搜索到的最优点 Gbest。 （4）结合 PSO 的更新公式［见式（3-4)]，得到新的粒子种群。 （5）重复步骤（3）和步骤（4），直至达到最大迭代次数 N 或者搜索到的水平度小于设定的阈值 $E_0$，输出 Gbest 对应的粒子，即为标定得到的 pitch 和 roll。 （6）在标定 pitch 和 roll 后，结合对应平面方程中的参数值 C 和车辆坐标系下的高度进一步计算得到 $\Delta z$。

3.2.2 SSAC 第二阶段

在该阶段的标定中，SSAC 算法仅考虑激光雷达相对车体偏航角的标定。具体过程如下：假设车辆沿着世界坐标系的 x 轴直线行驶，记录车辆行驶过程中包含同一个标定杆的激光点云，通过高程滤波等方式提取出标定杆，再采用 K 均值算法得到标定杆的点云聚类中心，在世界坐标系的 OXY 二维平面上，将每帧标定杆中心经最小二乘法拟合，得到一条直线，如式（3-5）所示。

$$ y = k_f x + b \tag{3-5} $$

这条直线的斜率 $k_f$ 反映了激光雷达相对车体的偏航角，即

$$ \text{yaw} = \arctan(k_f) \tag{3-6} $$

SSAC 第二阶段的算法流程如图 3-4 所示。

3.3 基于手眼模型的 LiDAR 外参标定

3.3.1 手眼模型简述

观察图 3-5，我们首先对手眼模型的基本原理进行分析。假设在 T 时刻，车辆在世界坐标系下的位置为 E，对应激光雷达的位置为 F。令车辆行驶的运动量为 $RV_k$，在 K 时刻对应的位置为 H，此时激光雷达的位置为 G，它由 F 到 G 的运动量为 $RL_k$。从图 3-5 可以看出，从 F 点出发到达 H 点有两条路径，一条路径是从 F 点出发经由 G 点到达 H 点，另一条路径是从 F 点出发经由 E 点到达 H 点。令激光雷达到车体的外参矩阵为 $T_v^l$，则有

$$ R_V^k T_v^l = T_v^l R_L^k \tag{3-7} $$

在车辆的连续运动中，根据运动数据可以构建出多个形如式（3-7）的等式方程，然后可以采用 Tasi 算法 [12]、Navy 算法 [13] 或 Nguyen 算法 [14] 进行求解。上述算法一般采用两步法将旋转变量和平移变量的求解过程分离，并将欧氏空间中的旋转运动分别转换成旋转向量、李群以及四元数的表示形式，以简化求解过程 [15]。下面我们选取基于李群的 Navy 算法进行分析。

图 3-5 基于手眼模型进行激光雷达外参标定的示意图

3.3.2 使用 Navy 算法求解手眼模型

Navy 算法是由 Frank C. Park 和 Bryan J. Martin 等于 1994 年提出的，其针对我们在机器人传感器标定中遇到的 AX = XB 等式求解问题，引入了李群、李代数理论，并结合非线性最小二乘法，推导出了手眼模型的封闭解。

在等式 AX = XB 中，位姿变换矩阵 A、B、X 均属于欧氏群 SE(3)，具体如式（3-8）所示。

$$ \operatorname{SE}(3) = \left{\boldsymbol{T} = \begin{bmatrix}\boldsymbol{\Theta} & \boldsymbol{b} \ \boldsymbol{0} & \boldsymbol{I}\end{bmatrix} \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{\Theta} \in \operatorname{SO}(3), \quad \boldsymbol{b} \in \mathbb{R}^3 \right} \tag{3-8} $$

将 A、B、X 记为

$$ \boldsymbol{A} = (\boldsymbol{\Theta}_A, \boldsymbol{b}_A), \quad \boldsymbol{B} = (\boldsymbol{\Theta}_B, \boldsymbol{b}_B), \quad \boldsymbol{X} = (\boldsymbol{\Theta}_X, \boldsymbol{b}_X) \tag{3-9} $$

其中，$\Theta$ 表示三维空间内的旋转矩阵，$b$ 表示三维空间内的平移矢量，并有

$$ \mathrm{SO}(3) = \left{\boldsymbol{\Theta} \in \mathbb{R}^{3 \times 3} \mid \boldsymbol{\Theta}\boldsymbol{\Theta}^\mathrm{T} = \boldsymbol{I}, \det(\boldsymbol{\Theta}) = 1 \right} \tag{3-10} $$

SE(3) 和 SO(3) 均是李群，并且由上述定义可以看出，SE(3) 和 SO(3) 均只对乘法封闭，即有

$$ T_1 T_2 \in \mathrm{SE}(3), \quad \theta_1 \theta_2 \in \mathrm{SO}(3) \tag{3-11} $$

等式 AX = XB 可以被改写为下述形式：

$$ \begin{bmatrix}\boldsymbol{\Theta}_A & \boldsymbol{b}_A \ \boldsymbol{0} & \boldsymbol{1}\end{bmatrix} \begin{bmatrix}\boldsymbol{\Theta}_X & \boldsymbol{b}_X \ \boldsymbol{0} & \boldsymbol{1}\end{bmatrix} = \begin{bmatrix}\boldsymbol{\Theta}_X & \boldsymbol{b}_X \ \boldsymbol{0} & \boldsymbol{1}\end{bmatrix} \begin{bmatrix}\boldsymbol{\Theta}_B & \boldsymbol{b}_B \ \boldsymbol{0} & \boldsymbol{1}\end{bmatrix} \tag{3-12} $$

展开后可以得到：

$$ \boldsymbol{\Theta}_A \boldsymbol{\Theta}_X = \boldsymbol{\Theta}_X \boldsymbol{\Theta}_B \tag{3-13} $$

$$ \boldsymbol{\Theta}_A \boldsymbol{b}_X + \boldsymbol{b}_A = \boldsymbol{\Theta}_X \boldsymbol{b}_B + \boldsymbol{b}_X \tag{3-14} $$

1. 理想解推导

式（3-13）实际上是 SO(3) 上形如 AX = XB 的等式方程，Park 和 Martin 指出在理想条件下求解 $\theta_X$ 至少需要两个等式约束。假设现有两组无噪声的旋转运动量观测数据 $(\Theta_{A_1}, \Theta_{B_1})$ 和 $(\Theta_{A_2}, \Theta_{B_2})$，则方程组可表示为

$$ \left{ \begin{array}{l}\boldsymbol{\Theta}_{A_1} \boldsymbol{\Theta}X = \boldsymbol{\Theta}X \boldsymbol{\Theta}{B_1} \ \boldsymbol{\Theta}{A_2} \boldsymbol{\Theta}_X = \boldsymbol{\Theta}X \boldsymbol{\Theta}{B_2}\end{array}\right. \tag{3-15} $$

当满足 $\operatorname{tr}(\Theta_{A_i}) \neq -1$、$\operatorname{tr}(\Theta_{B_i}) \neq -1$ 且 $\left| \log \Theta_{A_i} \right| = \left| \log \Theta_{B_i} \right| (i = 1,2)$ 时，该等式方程组的解为

$$ \boldsymbol{\Theta}_X = \boldsymbol{A} \boldsymbol{B}^{-1} \tag{3-16} $$

其中，A 和 B 均为 3×3 的矩阵，分别由列向量 ${\alpha_1, \alpha_2, \alpha_1 \times \alpha_2}$ 和 ${\beta_1, \beta_2, \beta_1 \times \beta_2}$ 构成，要求

$$ \boldsymbol{\alpha}i = \log \boldsymbol{\Theta}{A_i}, \quad \boldsymbol{\beta}i = \log \boldsymbol{\Theta}{B_i}, \quad i = 1, 2 \tag{3-17} $$

且有

$$ \log \boldsymbol{\Theta} = \frac {\phi}{2 \sin \phi} \left(\boldsymbol{\Theta} - \boldsymbol{\Theta}^\mathrm{T}\right) \tag{3-18} $$

其中，$1 + 2\cos \phi = \mathrm{tr}(\Theta), |\phi | < \pi ,| \log \Theta |^2 = \phi^2$

在得到旋转变量后，进一步求解平移变量 $b_X$。在无噪声的旋转、平移数据 $(\boldsymbol{\Theta}{A_1}, \boldsymbol{\Theta}{B_1}, \boldsymbol{b}{A_1}, \boldsymbol{b}{B_1})$ 和 $(\boldsymbol{\Theta}{A_2}, \boldsymbol{\Theta}{B_2}, \boldsymbol{b}{A_2}, \boldsymbol{b}{B_2})$ 的基础上，可以得到：

$$ \left{ \begin{array}{l}\left(\boldsymbol{\Theta}{A_1} - \boldsymbol{I}\right) \boldsymbol{b}X = \boldsymbol{\Theta}X \boldsymbol{b}{B_1} - \boldsymbol{b}{A_1} \ \left(\boldsymbol{\Theta}{A_2} - \boldsymbol{I}\right) \boldsymbol{b}X = \boldsymbol{\Theta}X \boldsymbol{b}{B_2} - \boldsymbol{b}{A_2}\end{array}\right. \tag{3-19} $$

进一步整理后可以得到：

$$ \begin{bmatrix}\left(\boldsymbol{\Theta}{A_1} - \boldsymbol{I}\right) \ \left(\boldsymbol{\Theta}{A_2} - \boldsymbol{I}\right)\end{bmatrix} \boldsymbol{b}X = \begin{bmatrix}\boldsymbol{\Theta}X \boldsymbol{b}{B_1} - \boldsymbol{b}{A_1} \ \boldsymbol{\Theta}X \boldsymbol{b}{B_2} - \boldsymbol{b}_{A_2}\end{bmatrix} \tag{3-20} $$

将式（3-16）求解得到的 $\theta_X$ 代入，即可求解得到 $b_X$。

2. 最小二乘解推导

在实践中，我们获取的运动量观测值通常包含了计算或测量噪声。假设我们获取的一组运动量观测值为 ${(A_1,B_1),\dots ,(A_k,B_k)}$，基于最小二乘法的思想，我们可以通过最小化式（3-21）得到 $\Theta_X$ 的最优解，具体如式（3-22）所示。

$$ \eta_1 = \sum_{i = 1}^k \left| \boldsymbol{\Theta}_X \boldsymbol{\beta}_i - \boldsymbol{\alpha}_i \right|^2 \tag{3-21} $$

$\boldsymbol{\alpha}_i$ 和 $\boldsymbol{\beta}_i$ 的定义见式（3-17）。

$$ \boldsymbol{\Theta}_X = \left(\boldsymbol{M}^\mathrm{T} \boldsymbol{M}\right)^{-1 / 2} \boldsymbol{M}^\mathrm{T} \tag{3-22} $$

其中：

$$ \boldsymbol{M} = \sum_{i = 1}^k \boldsymbol{\beta}_i \boldsymbol{\alpha}_i^\mathrm{T} \tag{3-23} $$

在求得旋转向量 $\Theta_X$ 后，同样基于最小二乘法的思想，构建式（3-24），并对 $\eta_2$ 进行最小化。

$$ \eta_2 = \sum_{i = 1}^k \left| (\boldsymbol{\Theta}_{A_i} - \boldsymbol{I}) \boldsymbol{b}X - \boldsymbol{\Theta}X \boldsymbol{b}{B_i} + \boldsymbol{b}{A_i} \right|^2 \tag{3-24} $$

该最小二乘优化的解为

$$ \boldsymbol{b}_X = \left(\boldsymbol{C}^\mathrm{T} \boldsymbol{C}\right)^{-1} \boldsymbol{C}^\mathrm{T} \boldsymbol{d} \tag{3-25} $$

其中：

$$ \boldsymbol{C} = \begin{bmatrix}\boldsymbol{I} - \boldsymbol{\Theta}{A_1} \ \vdots \ \boldsymbol{I} - \boldsymbol{\Theta}{A_k}\end{bmatrix}, \quad \boldsymbol{d} = \begin{bmatrix}\boldsymbol{b}{A_1} - \boldsymbol{\Theta}X \boldsymbol{b}{B_1} \ \vdots \ \boldsymbol{b}{A_k} - \boldsymbol{\Theta}X \boldsymbol{b}{B_k}\end{bmatrix} $$

3.3.3 DriveWorks 中激光雷达外参的标定

经过前面的介绍，读者应该对手眼标定的基本原理和求解方法有了一定的了解，本节将结合英伟达 DriveWorks 中的 LiDAR Self-Calibration (LSC) 标定方法，进一步介绍手眼模型在激光雷达外参标定中的应用。LSC 主要标定激光雷达相对车体的翻滚角 roll、俯仰角 pitch、偏航角 yaw 以及高度差 height。在硬件方面，LSC 需要 360° FOV 的多线束激光雷达，例如 Velodyne HDL-32e 和 Velodyne HDL-64e 以及 GPS 和 IMU 等。在标定时，LSC 要求待标定角度的初值与真值的直接误差不超过 10°，并且要求高度的初值与真值之间的误差不超过 10cm，此外还要求车辆的行驶速度不低于 5km/h。当激光雷达的扫描频率为 1Hz 时，标定总时长不超过 10min。

与 SSAC 算法类似，LSC 通过进行地面拟合实现了激光雷达外参中 roll、pitch 和 height 的标定。具体来说，LSC 主要通过对拟合地平面的法向量进行分析来求解这三个参数。

假设在激光雷达坐标系下得到地平面的单位法向量为 $\boldsymbol{n}$，水平地面在世界坐标系下的单位法向量为 $e = [0,0,1]$，则垂直的单位法向量 $e$ 经过 roll 和 pitch 角度的旋转便可得到 $\boldsymbol{n}$，旋转轴为

$$ t = e \times n \tag{3-26} $$

e 和 n 之间总的角度为

$$ \theta = \arccos (e \cdot n) \tag{3-27} $$

如前面的图 3-3 所示，激光雷达的旋转方向与拟合得到的地平面的旋转方向相反，角度相等。由此，我们得到仅考虑 roll 和 pitch 时，激光雷达偏转的轴角/旋转向量表示形式 $(\theta, t)$。根据罗德里格斯旋转公式，可由轴角转换得到旋转矩阵，故有

$$ \boldsymbol{R} = \cos (\theta) \boldsymbol{I} + (1 - \cos (\theta)) (-t) (-t)^\mathrm{T} + \sin (\theta) (-t)^\wedge \tag{3-28} $$

在得到旋转矩阵 R 后，再通过旋转矩阵和欧拉角之间的变换关系，即可求解出 pitch 和 roll。相关的旋转变换关系均封装在 Eigen 库中，在进行代码实现时，读者可以利用 Eigen 库快速实现轴角、旋转矩阵、欧拉角和四元数之间的变换。

$$ \boldsymbol{R} = \boldsymbol{R}_x \boldsymbol{R}_y \tag{3-29} $$

其中：

$$ \boldsymbol{R}_x = \begin{bmatrix}1 & 0 & 0 \ 0 & \cos (\text{roll}) & \sin (\text{roll}) \ 0 & -\sin (\text{roll}) & \cos (\text{roll})\end{bmatrix}, \quad \boldsymbol{R}_y = \begin{bmatrix}\cos (\text{pitch}) & 0 & -\sin (\text{pitch}) \ 0 & 1 & 0 \ \sin (\text{pitch}) & 0 & \cos (\text{pitch})\end{bmatrix} \tag{3-30} $$

此外，LSC 采用手眼模型并基于激光雷达和车体的运动变化量进行 yaw 和 pitch 的标定。前面已经提到过，在采用手眼模型时，需要车辆在待标定的角度方向有一定的运动变化，因此在利用手眼模型标定 yaw 和 pitch 时，需要车辆走圆形或'8'字形轨迹，并且要有一定的上、下坡。

车辆的运动变化通常可以通过 IMU 或 GNSS/IMU 组合导航获取，因此需要车辆定位模块首先进行精确的外参标定；而激光雷达的运动变化，通常可以通过 LiDAR ego-motion 模块获取，比如通过两帧间点云的 ICP 匹配、LOAM 算法等，具体将在第 11 章展开介绍。

随着车辆行驶，LSC 将得到多组标定结果，可根据所标定数据的分布，计算出标定最优值。如果基于地面拟合和手眼标定得到的 pitch 角度差在设定的阈值内，我们就认为得到了一致的 pitch 标定结果。最终当 yaw、pitch、roll 的标定最优值波动小于 0.3°、高度值变化小于 3cm 或者标定时长达到 10min 时，算法将输出最终的标定结果，如图 3-6 所示。

3.4 基于累积点云特征优化的 LiDAR 外参标定

近年来，许多学者基于累积点云特征优化实现了 LiDAR 外参标定。这类方法一般不依赖于标定物或外界先验信息，其基本思想如下：当 LiDAR 外参标定正确时，世界坐标系下一个静止物体的表面，在累积点云地图中应表现出清晰的边界特征；而当 LiDAR 外参没有得到正确标定时，在点云地图中，一个物体的表面会出现模糊、错层等，如图 3-7 所示。

图 3-7 LiDAR 外参对世界坐标系下点云地图的影响示意图

3.4.1 AESC-MMS 算法

本节我们选取比较有代表性的 AESC-MMS（Automatic Extrinsic Self-Calibration of Mobile Mapping Systems）算法进行分析和讨论，该算法是由 Hillemann 等人 [9] 于 2019 年基于移动建图系统提出的，用于激光雷达与位姿估计传感器的外参标定。其中位姿传感器可以是 IMU、GPS，也可以是运行 SLAM 算法的摄像头等。代码的开源地址为 GitHub 上的 markushillemann/Feat-Calibr。

1. 算法流程

AESC-MMS 算法的基本流程如图 3-8 所示。首先，结合 LiDAR 的初始外参和获取的多帧激光点云、位姿信息，构建世界坐标系下的累积点云地图。

图 3-8 AESC-MMS 算法的基本流程

假设外参的矢量表达为 $cal = [\mathrm{roll},\mathrm{pitch},\mathrm{yaw},\Delta x,\Delta y,\Delta z]$，则齐次变换矩阵 $T_y^l$ 可表示为

$$ \boldsymbol{T}_l^v = \begin{bmatrix}\boldsymbol{R}_l^v & \boldsymbol{d}_l^v \ \boldsymbol{0} & 1\end{bmatrix}, \quad \boldsymbol{d}_v^l = [\Delta x, \Delta y, \Delta z]^T, \quad \boldsymbol{R}_v^l = \boldsymbol{R}_x \boldsymbol{R}_y \boldsymbol{R}_z \tag{3-31} $$

将激光雷达坐标系下第 t 帧激光点云 ${}^l PC_t$ 结合车辆位姿和外参转换到世界坐标系 w 的过程可以用式（3-32）来表示。

$$ {}^w \boldsymbol{PC}_t = \boldsymbol{R}_v^w . * (\boldsymbol{R}_l^v . * {}^l \boldsymbol{PC}_t + \boldsymbol{T}_v^l) + \boldsymbol{d}_w^v \tag{3-32} $$

其中的'.*'表示对点云中的每个激光点进行运算。

由第 t 帧到第 k 帧累积得到的点云地图 $Map_{t,k}$ 可由式（3-33）得到：

$$ Map_{t,k} = \bigcup_{r=t}^{k} {}^w PC_r \tag{3-33} $$

然后，根据当前栅格尺寸参数，对累积点云用体素栅格滤波进行降采样，以减少点云地图中点的个数，从而降低后续优化过程的计算开销。令降采样后世界坐标系下的点云地图为

$$ Map'{t,k} = \text{VoxelFilter}(Map{t,k}) \tag{3-34} $$

在降采样后的点云地图中，对每个激光点及其最近的 k 个邻域点进行几何特征分析，并利用损失函数量化点云地图的质量，通过对损失函数进行最小化得到外参的估计值。

此外，AESC-MMS 算法会在最外层循环中，递归地减小体素栅格的尺寸，从而在一定程度上避免内层优化陷入局部极值，提升最终外参标定的精度。

2. 点云地图几何特征分析及损失函数构建

在分析点云地图的 3D 结构特征时，AESC-MMS 算法将首先基于点云地图 $Map'{t,k}$ 中的任意激光点 $p_i \in \mathbb{R}^3$ 及其最近的 M 个邻域点的集合 $\mathcal{N}i:{p{i1},\dots ,p{iM}}$ 计算 3D 协方差矩阵，具体如式（3-35）所示。

$$ C_i = (p_{i0} - \bar{p}i, \dots , p{iM} - \bar{p}i)(p{i0} - \bar{p}i, \dots , p{iM} - \bar{p}_i)^\mathrm{T} \tag{3-35} $$

$$ \bar{p}i = \frac {1}{M + 1} \sum{j = 0}^M p_{ij}, \quad p_{i0} = p_i \tag{3-36} $$

然后对 3D 协方差矩阵进行主成分分析（PCA），得到三个实数特征值，将它们按下述规则编号：

$$ \lambda_{1,i} \geqslant \lambda_{2,i} \geqslant \lambda_{3,i} \geqslant 0 \tag{3-37} $$

根据特征值，我们可以初步分析 $p_i$ 及其邻域 $\mathcal{N}_i$ 呈现出的 3D 几何特征，具体如下。

（a）当 $\lambda_{1,i} \gg \lambda_{2,i}, \lambda_{3,i}$ 时，表示 $p_i$ 及其邻域 $\mathcal{N}i$ 沿着 $\lambda{1,i}$ 对应的主轴呈线性排列。 （b）当 $\lambda_{1,i}, \lambda_{2,i} \gg \lambda_{3,i}$ 时，表示 $p_i$ 及其邻域 $\mathcal{N}i$ 近似地呈 2D 平面分布，平面法向量近似为 $\lambda{3,i}$ 对应的主轴方向。 （c）当 $\lambda_{1,i} \approx \lambda_{2,i} \approx \lambda_{3,i}$ 时，表示 $p_i$ 及其邻域 $\mathcal{N}_i$ 在 3 个主轴方向呈现出相对平均的分布状态。

在基于 PCA 对点云中局部的 3D 结构特征进行分析后，AESC-MMS 算法进一步引入了参考文献 [16] 和 [17] 中的 7 种度量公式，以描述点云地图中 $p_i$ 及其邻域 $\mathcal{N}_i$ 的几何特征，具体如下。

线性度（L）： $g_{L,i}(p_i,\mathcal{N}i) = 1 - \frac{\lambda{1,i} - \lambda_{2,i}}{\lambda_{1,i}}$ (3-38)

平面度 (P)： $g_{P,i}(p_i,\mathcal{N}i) = 1 - \frac{\lambda{2,i} - \lambda_{3,i}}{\lambda_{1,i}}$ (3-39)

球形度（S）： $g_{S,i}(p_i,\mathcal{N}i) = \frac{\lambda{3,i}}{\lambda_{1,i}}$ (3-40)

各向同性度（O）： $g_{O,i}(p_i,\mathcal{N}i) = \sqrt[3]{\lambda{1,i}\lambda_{2,i}\lambda_{3,i}}$ (3-41)

各向异性度（A）： $g_{A,i}(p_i,\mathcal{N}i) = 1 - \frac{\lambda{1,i} - \lambda_{3,i}}{\lambda_{1,i}}$ (3-42)

特征值熵（E）： $g_{E,i}(p_i,\mathcal{N}i) = -\sum{j = 1}^{3}\lambda_{j,i}\ln(\lambda_{j,i})$ (3-43)

曲率变化量（C）： $g_{C,i}(p_i,\mathcal{N}i) = \frac{\lambda{3,i}}{\lambda_{1,i} + \lambda_{2,i} + \lambda_{3,i}}$ (3-44)

理论上，当 $\lambda_{1,i} = \lambda_{2,i} = \lambda_{3,i} = 0$ 时，上面的一些指标会出现未定义的情况，此时 $p_i$ 和 $\mathcal{N}i$ 中的点坐标相等。但是由于激光雷达本身存在的测量噪声以及前述体素栅格的滤波操作，在进行实际标定时不会出现三个特征值均为 0 的情况。将上述特征描述公式统一记为 $g{F,i}(p_i,\mathcal{N}_i) \in [0,1], F \in {L,P,S,O,A,E,C}$。

Hillemann 等提出通过最小化下述损失函数来表征点云地图的质量：

$$ \mathcal{R} = \sum_{i = 1}^{N'} \left(g_{F,i}(p_i,\mathcal{N}i)\right)^2, \quad p_i \in Map'{t,k} \tag{3-45} $$

其中 $N'$ 为下采样后点云地图 $Map'{t,k}$ 中点的个数，为了进一步确保参与优化的激光点个数固定的同时舍弃一部分噪点，AESC-MMS 算法会对 $Map'{t,k}$ 中的每个点 $p_i$ 计算其 $g_{F,i}(p_i,\mathcal{N}i)$ 指标并排序，取其中 $g{F,i}(p_i,\mathcal{N}_i)$ 最小的 $\xi%$ 部分点云子集用于地图质量优化。

此外，为了降低某些点处几何特征值异常对整个目标函数或优化搜索的影响，Hillemann 等人引入了 M 估计理论，并最终将式（3-45）中的损失函数改写为下述形式：

$$ \mathcal{R} = \sum_{i = 1}^{L} \rho_k(g_{F,i}p_i,\mathcal{N}i), \quad p_i \in Map'{t,k} \tag{3-46} $$

其中 L 为点云子集中激光点的个数，$\rho_k$ 为 Huber 估计，并有

$$ \rho_k (x) = \left{ \begin{array}{c c} 0.5 x^2 & , x < k \ k (| x | - 0.5 k) & , x \geqslant k \end{array}\right. \tag{3-47} $$

因此，激光雷达的外参标定值可通过求解下述优化问题而获得：

$$ [\text{yaw},\text{pitch},\text{roll},\Delta x,\Delta y,\Delta z]_{\text{final}} = \underset{\text{cal}}{\operatorname{argmin}} \mathcal{R} \tag{3-48} $$

Hillemann 等基于室内、室外等多场景的数据进行了算法测试，结果表明使用指标得到的外参通常能够使得累积点云地图具有更优的点云质量。图 3-9 给出了基于 KITTI 数据集中的一个场景对激光雷达外参标定后得到的点云地图，以及使用 KITTI 数据集参考外参得到的点云地图。我们可以看出，二者在建图效果上几乎一致，这也表明了 AESC-MMS 算法的有效性。

（a）使用 KITTI 数据集参考外参得到的累积点云俯视图

（b）基于 $g_\sigma$ 标定后的累积点云俯视图

图 3-9 基于 KITTI 数据集标定后的点云地图对比

3.4.2 DyLESC 算法

随着当前激光雷达硬件技术的不断成熟，其线数也由早期的 16 线、32 线发展到现在业界普遍使用的等效 125 线、128 线等。一方面，高线束激光雷达的点云能够更清晰地反映周围环境、物体（如地面、房屋、护栏、树木、草丛）等表面的细微特征，供标定算法使用；另一方面，基于全部点云进行质量优化所需的计算开销较大，这对标定算法的计算耗时提出了更高要求。

在此背景下，我们提出了 DyLESC 动态标定算法 [20]。该算法抽取了点云中比较有代表性的平面点，并结合其邻域点构建微观上的面元，而后基于面元附近的几何特征和累积点云的点数指标构建了表征累积点云质量的模糊度函数，最后通过对该模糊度函数进行非线性优化实现了激光雷达外参的标定。

1. 算法整体流程

具体在标定过程中，DyLESC 算法通过构建表征点云质量的非线性优化问题来进行激光雷达外参中 yaw 角度和 pitch 角度的优化，而 roll 角度和高度 z 则采用类似于 DriveWorks 根据地面方程反算的方式来求解。激光雷达相对车体的 x 轴和 y 轴位置可通过手动测量的方式获取。DyLESC 算法对 yaw 角度和 pitch 角度进行标定的整体流程如图 3-10 所示。算法的固定输入为原始激光点云和车辆的运动信息。考虑当车辆行驶在颠簸路段时，采集到的点云抖动明显，且位姿数据误差也明显增大，我们决定在运动过滤步骤中，通过 IMU 和轮速计给出的车辆运动信息来进行标定数据的筛选。具体来说，当车辆运动状态满足 z 方向的加速度小于 0.3m/s²且速度被限制为 V > 3km/h 时，其对应时刻的点云才会被选择用于激光雷达外参的标定。为了减少标定过程中目标运动对累积点云质量造成的不利影响，DyLESC 算法还可以接入目标检测模块的结果，去除行人、车辆等物体对应的激光点，提取纯静态的激光点云用于外参标定。

考虑到定位模块提供长时间的绝对定位相对困难，但是可以在短时间内提供较高精度的车辆相对位姿变化，DyLESC 算法选择 r+1 个连续点云帧（如第 l 帧点云至第 l+r 帧点云，默认 r 取 10）来构建子地图 $Map_{l,l+r}^m$ 并进行地图特征优化，以缓解长距离点云地图中定位误差对外参标定的影响。此外，为了保证子地图分布的均匀性，我们可根据行驶距离等条件进行关键点云帧 l 的选择。而后，基于子地图 $Map_{l,l+r}^m$ 对提出的模糊度函数进行最小化，得到第 m 次标定结果。最后，使用 RANSAC 算法去除由多个子地图（m=1,…,K）得到的标定结果中的异常值，并得到最终的外参标定结果。

图 3-10 DyLESC 算法的整体流程

2. 点云运动畸变矫正

激光雷达在采集数据时，其激光发射器在水平视场角（horizontal field of view，HFOV）内做旋转运动。此时，若激光雷达本身也在运动，激光点云数据就会受到激光雷达运动的影响，我们称这种现象为点云的运动畸变。图 3-11 示意性地展示了点云运动畸变的原理，其中图 3-11(a) 给出的场景如下：当车辆静止时，激光雷达在 $[t_0,t_1]$ 时刻内完成对前方墙体的一帧点云扫描，得到无偏的点云 $P = {p_0,\dots,p_n}$；而在图 3-11(b) 中，车辆在 $[t_0,t_1]$ 时刻内沿 x 轴向前行驶了 $\Delta x$ 的距离，在这种状态下得到的激光点 $p_n'$ 的 x 值相比无偏激光点 $p_n$ 的 x 值小 $\Delta x$。

（a）车辆静止

（b）车辆向前行驶

图 3-11 点云运动畸变原理示意图

为了消除自车/激光雷达运动对外参标定的影响，我们首先需要去除点云的运动畸变。假设激光雷达/车辆在每帧之间是匀速运动的，由此便可采用线性插值的方式将第 k 帧内任意时间戳对应的激光点云投影到帧首或帧尾时刻。以将点云矫正到帧首时刻 $t_k$ 为例，令 $T_k^l(t)$ 为 t 时刻由高频 IMU 数据和当前外参矩阵得到的 $[t_k,t]$ 时刻内激光雷达的位姿变换矢量，有 $T_k^l(t) = [\theta_k^l(t),\tau_k^l(t)]^T$，其中 $\tau_k^l(t) = [t_x,t_y,t_z]^T$ 为位移矢量，$\theta_k^l(t) = [\theta_x,\theta_y,\theta_z]^T$ 为旋转角矢量。根据罗德里格斯旋转公式，可由旋转角矢量求解对应的旋转矩阵：

$$ \boldsymbol{R}_k^l(t) = \mathrm{e}^{\hat{\theta}_k^l(t)} = \boldsymbol{I} + \frac {\hat{\boldsymbol{\theta}}_k^l(t)}{\left| \boldsymbol{\theta}_k^l(t) \right|} \sin \left| \boldsymbol{\theta}_k^l(t) \right| + \left(\frac {\hat{\boldsymbol{\theta}}_k^l(t)}{\left| \boldsymbol{\theta}_k^l(t) \right|}\right)^2 \left(1 - \left| \cos \boldsymbol{\theta}_k^l(t) \right|\right) \tag{3-49} $$

其中 $\hat{\boldsymbol{\theta}}_k^l(t)$ 为旋转矢量 $\boldsymbol{\theta}_k^l(t)$ 的反对称阵。

假定对于激光点 $p_{(k,i)} \in P_k$，对应的时间戳为 $t_{(k,i)}$，$T_{(k,i)}^l = [\theta_{(k,i)}^l(t),\tau_{(k,i)}^l(t)]^T$ 为 $[t_k,t_{(k,i)}]$ 时刻内对应的位姿变换矢量，则 $T_{(k,i)}^l$ 可以基于 $T_k^l(t)$ 通过插值求得，如式（3-50）所示。

$$ \boldsymbol{T}{(k,i)}^l = \frac {t{(k,i)} - t_k}{t - t_k} \boldsymbol{T}_k^l(t) \tag{3-50} $$

因此，通过下式可实现将激光点投影到帧首时刻，获取去除运动畸变后的激光点。

$$ \tilde{\boldsymbol{p}}{(k,i)} = \boldsymbol{R}{(k,i)}^l \boldsymbol{p}{(k,i)} + \boldsymbol{\tau}{(k,i)}^l \tag{3-51} $$

其中，$R_{(k,i)}^l$ 可由旋转矢量 $\theta_{(k,i)}^l(t)$ 经式（3-49）求解得到。

3. 平面点提取和面元构建

为了减少计算开销，避免对点云中所有的点进行特征优化，DyLESC 算法选择抽取点云中有代表性的平面特征点进行分析。如何在点云中抽取平面特征点是 LiDAR Slam 领域比较经典的问题，相应地也有多种成熟的方法可以使用。例如，LOAM 算法 [18] 使用广义曲率来识别角点和平面特征点；TC-LVIO 算法 [19] 则首先计算局部点集的 Hessian 矩阵，然后对其特征值进行分析以确定平面特征点；DyLESC 算法则选取了 LOAM 算法中计算广义曲率的方式，具体的计算方式为

$$ c_{(l,i)} = \frac {1}{|S| | \boldsymbol{p}{(l,i)} |} \left| \sum{j \in S, j \neq i} \left(\boldsymbol{p}{l,i} - \boldsymbol{p}{l,j}\right) \right| \tag{3-52} $$

其中，$c_{(l,i)}$ 为第 l 帧点云中第 i 个激光点 $p_{(l,i)}$ 的广义曲率，S 为 $p_{(l,i)}$ 的邻域点集，$p_{(l,j)}$ 为邻域点集 S 中的激光点。

DyLESC 算法会对第 l 帧点云中的每个激光点进行广义曲率的计算，并将它们按照大小排序，选择其中广义曲率最大的 $N_p$ 个点作为平面特征点。而后在第 l 帧点云中，以每个平面点 $P_{(l,q)}|q = 1,\dots,N_p$ 为中心，在其附近选取最近的 N 个激光点 $a_{(l,q)}|q = 1,\dots,N$，对得到的局部点集进行特征值分解，并根据点法式构建面元 $MP_{(l,q)}$。