智能驾驶之激光雷达算法详解02

第3章 激光雷达-车体的外参标定

3.1 引言

在本章，我们将介绍如何获取激光雷达相对车体的位置和姿态，即如何对激光雷达进行外参标定。在获取激光雷达外参后，我们才能将激光雷达感知的目标转换至车体坐标系下，以供后续模块使用。根据标定过程中自车是否运动，我们可将LiDAR-车体的外参标定分为静态标定和动态标定两类。静态标定一般需要专业的标定设备和场地，结合四轮定位台架（或摆正器）、标定板、激光测距仪及全站仪等设备进行。图3-1展示了华为智能车BU建立的极狐感知系统传感器静态标定间。

图3-1 感知系统传感器静态标定间示例

（注：图片来自ADS高阶智能驾驶官方公众号）

目前在汽车工业中，整车厂主要通过静态标定间对LiDAR、相机、雷达等进行标定。静态标定的原理相对简单，其标定精度主要取决于标定设备的精度和传感器数据的质量，而高精度的标定件通常需要几十万到几百万人民币不等（根据配置和精度不同，其价格有较大浮动）。此外，当车辆交付用户使用以后，由于长期的振动甚至行驶中的剐蹭，也可能使得传感器外参发生变化，进而影响后续辅助驾驶系统的感知或定位功能。因此，近年来一些研究机构和整车厂正在探索通过车辆在特定路段或正常道路中的行驶，动态地标定得到传感器的外参或监测传感器的位姿状态。

在本章，我们主要介绍LiDAR的动态外参标定。基于对国内外研究现状的调研，我们将其细分为下述三个方向。

1）基于道路、标定物的外参标定

这类方法通常假设地面绝对水平，然后对地面进行拟合，并反过来推算 LiDAR 相对车体、地面的垂向高度（ zzz 值）、俯仰角和翻滚角。对于偏航角的标定，参考文献 [1] 中的算法依赖于路边的垂直杆状物，如路灯、标定桩等。参考文献 [2] 在标定偏航角时指出，当 LiDAR 安装倾角较大时，其照射到地面的波形是双曲线，可通过双曲线中心朝向推算出偏航角。上述标定

算法均需要地面水平或垂直的参考物体的先验信息，这在特定的标定道路内可以在一定程度上得到满足，但在用户实际驾驶过程中很难得到保证。

2）基于手眼模型的外参标定

手眼标定早期被应用于机械手上摄像头的标定，并可扩展至多种具有定位功能的传感器标定。该方法一般通过求解形如 AX=XBAX = XBAX=XB 的等式约束方程，得到外参矩阵 XXX ， AAA 和 BBB 为由不同传感器得到的运动变化量。具体到单激光雷达的外参标定，百度Apollo基于手眼标定方法实现了LiDAR和IMU之间偏航角的标定。同样，参考文献[3]也采用了手眼模型来实现3D激光雷达和IMU之间的外参标定。英伟达的DriveWork在进行LiDAR的外参标定时，选择了基于手眼标定算法来求解其偏航角和俯仰角，并根据地面拟合标定俯仰角和翻滚角。当两种方法标定的俯仰角误差小于设定的阈值时，就认为得到了俯仰角的一致性结果，并且当迭代满足设定的终止条件时，输出最终的标定结果。

从手眼标定的求解公式 AX=XB[4]AX = XB^{[4]}AX=XB[4] 可以看出该方法存在的一些问题。首先，为了使方程容易求解，需要使得位姿变换矩阵 A\mathbf{A}A 和 B\mathbf{B}B 不能接近单位阵，对应的物理含义就是要求车辆在各自自由度上有尽可能大的运动。由于车辆几乎总是平行于地面运动，车辆在侧倾方向上一般不会有太大的变化；因此，该方法通常用于激光雷达偏航角或俯仰角的标定，并要求在特定的场地中按照“8”字形或圆形轨迹行驶[5]。其次，LiDAR里程计、SLAM和车辆位姿模块的误差最终会被引入外参标定结果，因此这类方法一般需要高精度的定位模块。

3）基于点云特征优化的外参标定

这类方法通常需要建立点云地图，通过对地图质量特征进行分析，并结合非线性优化来获取外参标定结果。Levinson 和 Thrun[6] 基于 Velodyne 64 线束 LiDAR 提出了一种无监督的外参标定算法：通过已知的车辆运动获取累积的点云地图，针对累积点云提出一种能量方程，以表征每个线束中激光点与相邻线束邻域激光点构成平面之间的距离，并通过对能量方程进行优化搜索，获取 LiDAR 的外参标定参数。参考文献 [7] 基于低分辨率的 AesinGCH MStar 8000 LiDAR，提出通过优化 Reny Quadratic Entropy（RQE）函数来获取外参标定矩阵。

参考文献 [8] 认为点云中局部邻域的点是共面的，并基于此提出了一种表示不同帧点云之间距离的能量方程，但该方法对于树木、草丛较多、大平面建筑较少的场景精度不高。参考文献 [8] 还对移动建图系统进行了激光雷达的外参标定，分析了点云的 7 种几何特征并构建了不同的损失函数。在参考文献 [9] 中，作者针对传感器获取的点云结合定位系统数据构建出点云地图，在对点云地图进行体素滤波后，进行上述特征的计算，并代入构建的目标函数，通过对目标函数进行优化，最终求得传感器外参。参考文献 [10] 则认为 LiDAR 外参可以通过对累积 3D 点云的局部散度进行量化来获得，该方法通过对点云地图中的每个激光点及其邻域点集进行主成分分析，提出了对应的量化目标函数，并通过对其进行优化来获取最优的外参估计值。

下面我们将分别针对上述三个方向，选取具有代表性的算法进行分析和讨论。

3.2 基于道路、标定物特征的LiDAR动态外参标定

本节我们选取吉林大学的陈贵宾等[1]于2017年提出的车载3D激光雷达分步自动标定

（Step-by-Step Automatic Calibration，SSAC）算法进行分析。

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

图3-2SSAC算法的标定场景示意图

（注：图片来自参考文献[1]）

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

3.2.1 SSAC第一阶段

1. 基本思路

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

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

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

[xyz]=Rlν[x′y′z′]+dνl(3-1) \left[ \begin{array}{l} x \\ y \\ z \end{array} \right] = R _ {l} ^ {\nu} \left[ \begin{array}{l} x ^ {\prime} \\ y ^ {\prime} \\ z ^ {\prime} \end{array} \right] + d _ {\nu} ^ {l} \tag {3-1} ​xyz​​=Rlν​​x′y′z′​​+dνl​(3-1)

其中：

dvl=[Δx,Δy,Δz]′,Rlv=RxRyRz \boldsymbol {d} _ {v} ^ {l} = \left[ \Delta x, \Delta y, \Delta z \right] ^ {\prime}, \boldsymbol {R} _ {l} ^ {v} = \boldsymbol {R} _ {x} \boldsymbol {R} _ {y} \boldsymbol {R} _ {z} dvl​=[Δx,Δy,Δz]′,Rlv​=Rx​Ry​Rz​

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

z=Ax+By+C(3-2) z = A x + B y + C \tag {3-2} z=Ax+By+C(3-2)

其中， AAA 、 BBB 、 CCC 为平面方程系数。

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

f=∣A∣+∣B∣(3-3) f = | A | + | B | \tag {3-3} f=∣A∣+∣B∣(3-3)

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

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

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

vik+1=w×vik+r1×rand⁡1×(Pbest⁡i−xik)+r2×rand⁡2×(Gbest⁡−xik)xik+1=xik+vik+1(3-4) \begin{array}{l} v _ {i} ^ {k + 1} = w \times v _ {i} ^ {k} + r _ {1} \times \operatorname {r a n d} _ {1} \times \left(\operatorname {P b e s t} _ {i} - x _ {i} ^ {k}\right) + r _ {2} \times \operatorname {r a n d} _ {2} \times \left(\operatorname {G b e s t} - x _ {i} ^ {k}\right) \\ x _ {i} ^ {k + 1} = x _ {i} ^ {k} + v _ {i} ^ {k + 1} \tag {3-4} \\ \end{array} vik+1​=w×vik​+r1​×rand1​×(Pbesti​−xik​)+r2​×rand2​×(Gbest−xik​)xik+1​=xik​+vik+1​​(3-4)

其中， iii 是粒子的索引值， kkk 表示迭代次数， www 是惯性权重， r1r_1r1​ 和 r2r_2r2​ 被称为学习因子或加速常数， νik\nu_i^kνik​ 和 νik+1\nu_i^{k+1}νik+1​ 分别是第 iii 个粒子在第 kkk 次迭代和第 k+1k+1k+1 次迭代中的飞行速度， xikx_i^kxik​ 和 xik+1x_i^{k+1}xik+1​ 是第 iii 个粒子在第 kkk 次迭代和第 k+1k+1k+1 次迭代中的位置（即变量值）。

3. 算法流程

SSAC第一阶段的流程如下。

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

3.2.2 SSAC 第二阶段

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

y=kfx+b(3-5) y = k _ {f} x + b \tag {3-5} y=kf​x+b(3-5)

这条直线的斜率 kfk_{f}kf​ 反映了激光雷达相对车体的偏航角, 即

yaw=arctan⁡(kf)(3-6) \mathrm {y a w} = \arctan \left(k _ {f}\right) \tag {3-6} yaw=arctan(kf​)(3-6)

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

图3-4SSAC第二阶段的算法流程

（注：图片根据参考文献[1]制作而成）

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

3.3.1 手眼模型简述

观察图3-5，我们首先对手眼模型的基本原理进行分析。假设在 TTT 时刻，车辆在世界坐标系下的位置为 EEE ，对应激光雷达的位置为 FFF 。令车辆行驶的运动量为 RVkRV_{k}RVk​ ，在 KKK 时刻对应的位置为 HHH ，此时激光雷达的位置为 GGG ，它由 FFF 到 GGG 的运动量为 RLkRL_{k}RLk​ 。从图3-5可以看出，从 FFF 点出发到达 HHH 点有两条路径，一条路径是从 FFF 点出发经由 GGG 点到达 HHH 点，另一条路径是从 FFF 点出发经由 EEE 点到达 HHH 点。令激光雷达到车体的外参矩阵为 TvlT_{v}^{l}Tvl​ ，则有

RVkTvl=TvlRLk(3-7) \boldsymbol {R} \boldsymbol {V} _ {k} \boldsymbol {T} _ {v} ^ {l} = \boldsymbol {T} _ {v} ^ {l} \boldsymbol {R} \boldsymbol {L} _ {k} \tag {3-7} RVk​Tvl​=Tvl​RLk​(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=XBAX = XBAX=XB 等式求解问题，引入了李群、李代数理论，并结合非线性最小二乘法，推导出了手眼模型的封闭解。

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

SE⁡(3)={T=[Θb0I]∈R4×4∣Θ∈SO⁡(3),b∈R3}(3-8) \operatorname {S E} (3) = \left\{\boldsymbol {T} = \left[ \begin{array}{l l} \boldsymbol {\Theta} & \boldsymbol {b} \\ \boldsymbol {0} & \boldsymbol {I} \end{array} \right] \in \mathbb {R} ^ {4 \times 4} \mid \boldsymbol {\Theta} \in \operatorname {S O} (3), \quad \boldsymbol {b} \in \mathbb {R} ^ {3} \right\} \tag {3-8} SE(3)={T=[Θ0​bI​]∈R4×4∣Θ∈SO(3),b∈R3}(3-8)

将 AAA 、 BBB 、 XXX 记为

A=(ΘA,bA),B=(ΘB,bB),X=(ΘX,bX)(3-9) \boldsymbol {A} = \left(\boldsymbol {\Theta} _ {A}, \boldsymbol {b} _ {A}\right), \quad \boldsymbol {B} = \left(\boldsymbol {\Theta} _ {B}, \boldsymbol {b} _ {B}\right), \quad \boldsymbol {X} = \left(\boldsymbol {\Theta} _ {X}, \boldsymbol {b} _ {X}\right) \tag {3-9} A=(ΘA​,bA​),B=(ΘB​,bB​),X=(ΘX​,bX​)(3-9)

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

SO(3)={Θ∈R3×3∣ΘΘT=I,det⁡(Θ)=1}(3-10) \mathrm {S O} (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} SO(3)={Θ∈R3×3∣ΘΘT=I,det(Θ)=1}(3-10)

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

T1T2∈SE(3),θ1θ2∈SO(3)(3-11) T _ {1} T _ {2} \in \mathrm {S E} (3), \quad \theta_ {1} \theta_ {2} \in \mathrm {S O} (3) \tag {3-11} T1​T2​∈SE(3),θ1​θ2​∈SO(3)(3-11)

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

[ΘAbA01][ΘXbX01]=[ΘXbX01][ΘBbB01](3-12) \left[ \begin{array}{l l} \boldsymbol {\Theta} _ {A} & \boldsymbol {b} _ {A} \\ \boldsymbol {0} & \boldsymbol {1} \end{array} \right] \left[ \begin{array}{l l} \boldsymbol {\Theta} _ {X} & \boldsymbol {b} _ {X} \\ \boldsymbol {0} & \boldsymbol {1} \end{array} \right] = \left[ \begin{array}{l l} \boldsymbol {\Theta} _ {X} & \boldsymbol {b} _ {X} \\ \boldsymbol {0} & \boldsymbol {1} \end{array} \right] \left[ \begin{array}{l l} \boldsymbol {\Theta} _ {B} & \boldsymbol {b} _ {B} \\ \boldsymbol {0} & \boldsymbol {1} \end{array} \right] \tag {3-12} [ΘA​0​bA​1​][ΘX​0​bX​1​]=[ΘX​0​bX​1​][ΘB​0​bB​1​](3-12)

展开后可以得到：

ΘAΘX=ΘXΘB(3-13) \boldsymbol {\Theta} _ {A} \boldsymbol {\Theta} _ {X} = \boldsymbol {\Theta} _ {X} \boldsymbol {\Theta} _ {B} \tag {3-13} ΘA​ΘX​=ΘX​ΘB​(3-13)

ΘAbX+bA=ΘXbB+bX(3-14) \boldsymbol {\Theta} _ {A} \boldsymbol {b} _ {X} + \boldsymbol {b} _ {A} = \boldsymbol {\Theta} _ {X} \boldsymbol {b} _ {B} + \boldsymbol {b} _ {X} \tag {3-14} ΘA​bX​+bA​=ΘX​bB​+bX​(3-14)

1. 理想解推导

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

{ΘA1ΘX=ΘXΘB1ΘA2ΘX=ΘXΘB2(3-15) \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} {ΘA1​​ΘX​=ΘX​ΘB1​​ΘA2​​ΘX​=ΘX​ΘB2​​​(3-15)

当满足 tr⁡(ΘAi)≠−1\operatorname{tr}(\Theta_{A_i}) \neq -1tr(ΘAi​​)=−1 、 tr⁡(ΘBi)≠−1\operatorname{tr}(\Theta_{B_i}) \neq -1tr(ΘBi​​)=−1 且 ∥log⁡ΘAi∥=∥log⁡ΘBi∥(i=1,2)\left\| \log \Theta_{A_i} \right\| = \left\| \log \Theta_{B_i} \right\| (i = 1,2)∥logΘAi​​∥=∥logΘBi​​∥(i=1,2) 时，该等式方程组的解为

ΘX=AB−1(3-16) \boldsymbol {\Theta} _ {X} = \boldsymbol {A} \boldsymbol {B} ^ {- 1} \tag {3-16} ΘX​=AB−1(3-16)

其中， AAA 和 B\pmb{B}B 均为 3×33\times 33×3 的矩阵，分别由列向量 {α1,α2,α1×α2}\{\alpha_{1},\alpha_{2},\alpha_{1}\times \alpha_{2}\}{α1​,α2​,α1​×α2​} 和 {β1,β2,β1×β2}\{\beta_1,\beta_2,\beta_1\times \beta_2\}{β1​,β2​,β1​×β2​} 构成，要求

αi=log⁡ΘAi,βi=log⁡ΘBi,i=1,2(3-17) \boldsymbol {\alpha} _ {i} = \log \boldsymbol {\Theta} _ {A _ {i}}, \quad \boldsymbol {\beta} _ {i} = \log \boldsymbol {\Theta} _ {B _ {i}}, \quad i = 1, 2 \tag {3-17} αi​=logΘAi​​,βi​=logΘBi​​,i=1,2(3-17)

且有

log⁡Θ=ϕ2sin⁡ϕ(Θ−ΘT)(3-18) \log \boldsymbol {\Theta} = \frac {\phi}{2 \sin \phi} \left(\boldsymbol {\Theta} - \boldsymbol {\Theta} ^ {\mathrm {T}}\right) \tag {3-18} logΘ=2sinϕϕ​(Θ−ΘT)(3-18)

其中， 1+2cos⁡ϕ=tr(Θ),∣ϕ∣<π,∥log⁡Θ∥2=ϕ21 + 2\cos \phi = \mathrm{tr}(\Theta),|\phi | < \pi ,\| \log \Theta \| ^2 = \phi^21+2cosϕ=tr(Θ),∣ϕ∣<π,∥logΘ∥2=ϕ2

在得到旋转变量后，进一步求解平移变量 bX\pmb{b}_{X}bX​ 。在无噪声的旋转、平移数据 (ΘA1,ΘB1,bA1,bB1)(\pmb{\Theta}_{A_1},\pmb{\Theta}_{B_1},\pmb{b}_{A_1},\pmb{b}_{B_1})(ΘA1​​,ΘB1​​,bA1​​,bB1​​) 和 (ΘA2,ΘB2,bA2,bB2)(\pmb{\Theta}_{A_2},\pmb{\Theta}_{B_2},\pmb{b}_{A_2},\pmb{b}_{B_2})(ΘA2​​,ΘB2​​,bA2​​,bB2​​) 的基础上，可以得到：

{(ΘA1−I)bX=ΘXbB1−bA1(ΘA2−I)bX=ΘXbB2−bA2(3-19) \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} {(ΘA1​​−I)bX​=ΘX​bB1​​−bA1​​(ΘA2​​−I)bX​=ΘX​bB2​​−bA2​​​(3-19)

进一步整理后可以得到：

[(ΘA1−I)(ΘA2−I)]bX=[ΘXbB1−bA1ΘXbB2−bA2](3-20) \left[ \begin{array}{l} \left(\boldsymbol {\Theta} _ {A _ {1}} - \boldsymbol {I}\right) \\ \left(\boldsymbol {\Theta} _ {A _ {2}} - \boldsymbol {I}\right) \end{array} \right] \boldsymbol {b} _ {X} = \left[ \begin{array}{l} \boldsymbol {\Theta} _ {X} \boldsymbol {b} _ {B _ {1}} - \boldsymbol {b} _ {A _ {1}} \\ \boldsymbol {\Theta} _ {X} \boldsymbol {b} _ {B _ {2}} - \boldsymbol {b} _ {A _ {2}} \end{array} \right] \tag {3-20} [(ΘA1​​−I)(ΘA2​​−I)​]bX​=[ΘX​bB1​​−bA1​​ΘX​bB2​​−bA2​​​](3-20)

将式（3-16）求解得到的 θX\pmb{\theta}_{X}θX​ 代入，即可求解得到 bX\pmb{b}_{X}bX​ 。

2. 最小二乘解推导

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

η1=∑i=1k∥ΘXβi−αi∥2(3-21) \eta_ {1} = \sum_ {i = 1} ^ {k} \left\| \boldsymbol {\Theta} _ {X} \boldsymbol {\beta} _ {i} - \boldsymbol {\alpha} _ {i} \right\| ^ {2} \tag {3-21} η1​=i=1∑k​∥ΘX​βi​−αi​∥2(3-21)

αi\pmb{\alpha}_{i}αi​ 和 βi\pmb{\beta}_{i}βi​ 的定义见式（3-17）。

ΘX=(MTM)−1/2MT(3-22) \boldsymbol {\Theta} _ {X} = \left(\boldsymbol {M} ^ {\mathrm {T}} \boldsymbol {M}\right) ^ {- 1 / 2} \boldsymbol {M} ^ {\mathrm {T}} \tag {3-22} ΘX​=(MTM)−1/2MT(3-22)

其中：

M=∑i=1kβiαiT(3-23) \boldsymbol {M} = \sum_ {i = 1} ^ {k} \boldsymbol {\beta} _ {i} \boldsymbol {\alpha} _ {i} ^ {\mathrm {T}} \tag {3-23} M=i=1∑k​βi​αiT​(3-23)

在求得旋转向量 ΘX\pmb{\Theta}_{X}ΘX​ 后，同样基于最小二乘法的思想，构建式（3-24），并对 η2\eta_{2}η2​ 进行最小化。

η2=∑i=1k∥(ΘAi−I)bX−ΘXbBi+bAi∥2(3-24) \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} η2​=i=1∑k​∥(ΘAi​​−I)bX​−ΘX​bBi​​+bAi​​∥2(3-24)

该最小二乘优化的解为

bX=(CTC)−1CTd(3-25) \boldsymbol {b} _ {X} = \left(\boldsymbol {C} ^ {\mathrm {T}} \boldsymbol {C}\right) ^ {- 1} \boldsymbol {C} ^ {\mathrm {T}} \boldsymbol {d} \tag {3-25} bX​=(CTC)−1CTd(3-25)

其中：

C=[I−ΘA1⋮I−ΘAk],d=[bA1−ΘXbB1⋮bAk−ΘXbBk] \boldsymbol {C} = \left[ \begin{array}{c} \boldsymbol {I} - \boldsymbol {\Theta} _ {A _ {1}} \\ \vdots \\ \boldsymbol {I} - \boldsymbol {\Theta} _ {A _ {k}} \end{array} \right], \quad \boldsymbol {d} = \left[ \begin{array}{c} \boldsymbol {b} _ {A _ {1}} - \boldsymbol {\Theta} _ {X} \boldsymbol {b} _ {B _ {1}} \\ \vdots \\ \boldsymbol {b} _ {A _ {k}} - \boldsymbol {\Theta} _ {X} \boldsymbol {b} _ {B _ {k}} \end{array} \right] C=​I−ΘA1​​⋮I−ΘAk​​​​,d=​bA1​​−ΘX​bB1​​⋮bAk​​−ΘX​bBk​​​​

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

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

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

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

t=e×n(3-26) t = e \times n \tag {3-26} t=e×n(3-26)

eee 和 nnn 之间总的角度为

θ=arccos⁡(e⋅n)(3-27) \theta = \arccos (\boldsymbol {e} \cdot \boldsymbol {n}) \tag {3-27} θ=arccos(e⋅n)(3-27)

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

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

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

R=RxRy(3-29) \boldsymbol {R} = \boldsymbol {R} _ {x} \boldsymbol {R} _ {y} \tag {3-29} R=Rx​Ry​(3-29)

其中：

Rx=[1000cos⁡(r o l l)sin⁡(r o l l)0−sin⁡(r o l l)cos⁡(r o l l)],Ry=[cos⁡(p i t c h)0−sin⁡(p i t c h)010sin⁡(p i t c h)0cos⁡(p i t c h)](3-30) \boldsymbol {R} _ {x} = \left[ \begin{array}{c c c} 1 & 0 & 0 \\ 0 & \cos (\text {r o l l}) & \sin (\text {r o l l}) \\ 0 & - \sin (\text {r o l l}) & \cos (\text {r o l l}) \end{array} \right], \quad \boldsymbol {R} _ {y} = \left[ \begin{array}{c c c} \cos (\text {p i t c h}) & 0 & - \sin (\text {p i t c h}) \\ 0 & 1 & 0 \\ \sin (\text {p i t c h}) & 0 & \cos (\text {p i t c h}) \end{array} \right] \tag {3-30} Rx​=​100​0cos(r o l l)−sin(r o l l)​0sin(r o l l)cos(r o l l)​​,Ry​=​cos(p i t c h)0sin(p i t c h)​010​−sin(p i t c h)0cos(p i t c h)​​(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∘0.3^{\circ}0.3∘ 、高度值变化小于 3cm3\mathrm{cm}3cm 或者标定时长达到10min时，算法将输出最终的标定结果，如图3-6所示。

图3-6 DriveWorks激光雷达外参标定的结果图

（注：图片来自 DriveWorks 官方网站）

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=[roll,pitch,yaw,Δx,Δy,Δz]\mathrm{cal} = [\mathrm{roll},\mathrm{pitch},\mathrm{yaw},\Delta x,\Delta y,\Delta z]cal=[roll,pitch,yaw,Δx,Δy,Δz] ，则齐次变换矩阵 TylT_{y}^{l}Tyl​ 可表示为

Tlv=[Rlvdlv01],dvl=[Δx,Δy,Δz]′,Rvl=RxRyRz(3-31) \boldsymbol {T} _ {l} ^ {v} = \left[ \begin{array}{c c} \boldsymbol {R} _ {l} ^ {v} & \boldsymbol {d} _ {l} ^ {v} \\ \boldsymbol {0} & 1 \end{array} \right], \quad \boldsymbol {d} _ {v} ^ {l} = \left[ \Delta x, \Delta y, \Delta z \right] ^ {\prime}, \quad \boldsymbol {R} _ {v} ^ {l} = \boldsymbol {R} _ {x} \boldsymbol {R} _ {y} \boldsymbol {R} _ {z} \tag {3-31} Tlv​=[Rlv​0​dlv​1​],dvl​=[Δx,Δy,Δz]′,Rvl​=Rx​Ry​Rz​(3-31)

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

wPCt=Rvw.∗(Rlv.∗lPCt+Tvl)+dwv(3-32) { } ^ { w } \boldsymbol { P } \boldsymbol { C } _ { t } = \boldsymbol { R } _ { v } ^ { w } . * ( \boldsymbol { R } _ { l } ^ { v } . * ^ { l } \boldsymbol { P } \boldsymbol { C } _ { t } + \boldsymbol { T } _ { v } ^ { l } ) + \boldsymbol { d } _ { w } ^ { v } \tag {3-32} wPCt​=Rvw​.∗(Rlv​.∗lPCt​+Tvl​)+dwv​(3-32)

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

由第 ttt 帧到第 kkk 帧累积得到的点云地图 Mapt,k\mathrm{Map}_{t,k}Mapt,k​ 可由式（3-33）得到：

KaTeX parse error: Double superscript at position 55: … {r = t} ^ {k} ^̲ {w} P C _ {r} …

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

Map⁡t,k′=V o x e l F i l t e r(Map⁡t,k)(3-34) \operatorname {M a p} _ {t, k} ^ {\prime} = \text {V o x e l F i l t e r} (\operatorname {M a p} _ {t, k}) \tag {3-34} Mapt,k′​=V o x e l F i l t e r(Mapt,k​)(3-34)

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

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

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

在分析点云地图的3D结构特征时，AESC-MMS算法将首先基于点云地图 Mapt,k′\mathrm{Map}_{t,k}^{\prime}Mapt,k′​ 中的任意激光点 pi∈R3p_i\in \mathbb{R}^3pi​∈R3 及其最近的 MMM 个邻域点的集合 Ni:{pi1,…,piM}\mathcal{N}_i:\{p_{i1},\dots ,p_{iM}\}Ni​:{pi1​,…,piM​} 计算3D协方差矩阵，具体如式（3-35）所示。

Ci=(pi0−pˉi,…,piM−pˉi)(pi0−pˉi,…,piM−pˉi)T(3-35) C _ {i} = \left(\boldsymbol {p} _ {i 0} - \bar {\boldsymbol {p}} _ {i}, \dots , \boldsymbol {p} _ {i M} - \bar {\boldsymbol {p}} _ {i}\right) \left(\boldsymbol {p} _ {i 0} - \bar {\boldsymbol {p}} _ {i}, \dots , \boldsymbol {p} _ {i M} - \bar {\boldsymbol {p}} _ {i}\right) ^ {\mathrm {T}} \tag {3-35} Ci​=(pi0​−pˉ​i​,…,piM​−pˉ​i​)(pi0​−pˉ​i​,…,piM​−pˉ​i​)T(3-35)

pˉi=1M+1∑j=0Mpij,pi0=pi(3-36) \bar {\boldsymbol {p}} _ {i} = \frac {1}{M + 1} \sum_ {j = 0} ^ {M} \boldsymbol {p} _ {i j}, \quad \boldsymbol {p} _ {i 0} = \boldsymbol {p} _ {i} \tag {3-36} pˉ​i​=M+11​j=0∑M​pij​,pi0​=pi​(3-36)

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

λ1,i⩾λ2,i⩾λ3,i⩾0(3-37) \lambda_ {1, i} \geqslant \lambda_ {2, i} \geqslant \lambda_ {3, i} \geqslant 0 \tag {3-37} λ1,i​⩾λ2,i​⩾λ3,i​⩾0(3-37)

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

（a）当 λ1,i≫λ2,i,λ3,i\lambda_{1,i} \gg \lambda_{2,i}, \lambda_{3,i}λ1,i​≫λ2,i​,λ3,i​ 时，表示 pip_ipi​ 及其邻域 Ni\mathcal{N}_iNi​ 沿着 λ1,i\lambda_{1,i}λ1,i​ 对应的主轴呈线性排列。
(b) 当 λ1,i,λ2,i≫λ3,i\lambda_{1,i}, \lambda_{2,i} \gg \lambda_{3,i}λ1,i​,λ2,i​≫λ3,i​ 时, 表示 pi\pmb{p}_ipi​ 及其邻域 Ni\mathcal{N}_iNi​ 近似地呈 2D 平面分布, 平面法向量近似为 λ3,i\lambda_{3,i}λ3,i​ 对应的主轴方向。
（c）当 λ1,i≈λ2,i≈λ3,i\lambda_{1,i}\approx \lambda_{2,i}\approx \lambda_{3,i}λ1,i​≈λ2,i​≈λ3,i​ 时，表示 pip_ipi​ 及其邻域 Ni\mathcal{N}_iNi​ 在3个主轴方向呈现出相对平均的分布状态。

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

线性度（L）： gL,i(pi,Ni)=1−λ1,i−λ2,iλ1,ig_{L,i}(\pmb {p}_i,\mathcal{N}_i) = 1 - \frac{\lambda_{1,i} - \lambda_{2,i}}{\lambda_{1,i}}gL,i​(pi​,Ni​)=1−λ1,i​λ1,i​−λ2,i​​ (3-38)

平面度 (P)(P)(P)gP,i(pi,Ni)=1−λ2,i−λ3,iλ1,ig_{P,i}(\pmb {p}_i,\mathcal{N}_i) = 1 - \frac{\lambda_{2,i} - \lambda_{3,i}}{\lambda_{1,i}}gP,i​(pi​,Ni​)=1−λ1,i​λ2,i​−λ3,i​​ (3-39)

球形度（S）： gS,i(pi,Ni)=λ3,iλ1,ig_{S,i}(\pmb {p}_i,\mathcal{N}_i) = \frac{\lambda_{3,i}}{\lambda_{1,i}}gS,i​(pi​,Ni​)=λ1,i​λ3,i​​ (3-40)

各向同性度（O）： gO,i(pi,Ni)=λ1,iλ2,iλ3,i3g_{O,i}(\pmb {p}_i,\mathcal{N}_i) = \sqrt[3]{\lambda_{1,i}\lambda_{2,i}\lambda_{3,i}}gO,i​(pi​,Ni​)=3λ1,i​λ2,i​λ3,i​​ (3-41)

各向异性度（A）： gA,i(pi,Ni)=1−λ1,i−λ3,iλ1,ig_{A,i}(\pmb {p}_i,\mathcal{N}_i) = 1 - \frac{\lambda_{1,i} - \lambda_{3,i}}{\lambda_{1,i}}gA,i​(pi​,Ni​)=1−λ1,i​λ1,i​−λ3,i​​ (3-42)

特征值熵（E）： gE,i(pi,Ni)=−∑j=13λj,iIn(λj,i)g_{E,i}(\pmb {p}_i,\mathcal{N}_i) = -\sum_{j = 1}^{3}\lambda_{j,i}\mathrm{In}(\lambda_{j,i})gE,i​(pi​,Ni​)=−∑j=13​λj,i​In(λj,i​) (3-43)

曲率变化量（C）： gC,i(pi,Ni)=λ3,iλ1,i+λ2,i+λ3,ig_{C,i}(\pmb {p}_i,\mathcal{N}_i) = \frac{\lambda_{3,i}}{\lambda_{1,i} + \lambda_{2,i} + \lambda_{3,i}}gC,i​(pi​,Ni​)=λ1,i​+λ2,i​+λ3,i​λ3,i​​ (3-44)

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

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

R=∑i=1N′(gF,i(pi,Ni))2,pi∈Map⁡t,k′(3-45) \mathcal {R} = \sum_ {i = 1} ^ {N ^ {\prime}} \left(g _ {F, i} \left(\boldsymbol {p} _ {i}, \mathcal {N} _ {i}\right)\right) ^ {2}, \quad \boldsymbol {p} _ {i} \in \operatorname {M a p} _ {t, k} ^ {\prime} \tag {3-45} R=i=1∑N′​(gF,i​(pi​,Ni​))2,pi​∈Mapt,k′​(3-45)

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

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

R=∑i=1Lρk(gF,ipi,Ni),pi∈Map⁡t,k′(3-46) \mathcal {R} = \sum_ {i = 1} ^ {L} \rho_ {k} \left(g _ {F, i} \boldsymbol {p} _ {i}, \mathcal {N} _ {i}\right), \quad \boldsymbol {p} _ {i} \in \operatorname {M a p} _ {t, k} ^ {\prime} \tag {3-46} R=i=1∑L​ρk​(gF,i​pi​,Ni​),pi​∈Mapt,k′​(3-46)

其中 LLL 为点云子集中激光点的个数， ρk\rho_{k}ρk​ 为Huber估计，并有

ρk(x)={0.5x2,x<kk(∣x∣−0.5k),x⩾k(3-47) \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} ρk​(x)={0.5x2k(∣x∣−0.5k)​,x<k,x⩾k​(3-47)

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

[y a w,p i t c h,r o l l,Δx,Δy,Δz]f i n a l=argmin⁡c a lR(3-48) [ \text {y a w}, \text {p i t c h}, \text {r o l l}, \Delta x, \Delta y, \Delta z ] _ {\text {f i n a l}} = \underset {\text {c a l}} {\operatorname {a r g m i n}} \mathcal {R} \tag {3-48} [y a w,p i t c h,r o l l,Δx,Δy,Δz]f i n a l​=c a largmin​R(3-48)

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

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

（b）基于 gσg_{\sigma}gσ​ 标定后的累积点云俯视图
图3-9 基于KITTI数据集标定后的点云地图对比

（注：图片来自参考文献[9]）

3.4.2 DyLESC算法

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

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

1. 算法整体流程

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

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

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

2. 点云运动畸变矫正

激光雷达在采集数据时，其激光发射器在水平视场角（horizontal field of view，HFOV）内做旋转运动。此时，若激光雷达本身也在运动，激光点云数据就会受到激光雷达运动的影响，我们称这种现象为点云的运动畸变。图3-11示意性地展示了点云运动畸变的原理，其中图3-11

(a) 给出的场景如下: 当车辆静止时, 激光雷达在 [t0,t1][t_{0}, t_{1}][t0​,t1​] 时刻内完成对前方墙体的一帧点云扫描, 得到无偏的点云 P={p0,…,pn}P = \{p_{0}, \dots, p_{n}\}P={p0​,…,pn​} ; 而在图 3-11 (b) 中, 车辆在 [t0,t1][t_{0}, t_{1}][t0​,t1​] 时刻内沿 xxx 轴向前行驶了 Δx\Delta xΔx 的距离, 在这种状态下得到的激光点 pn′p_{n}^{\prime}pn′​ 的 xxx 值相比无偏激光点 pnp_{n}pn​ 的 xxx 值小 Δx\Delta xΔx 。

（a）车辆静止

（b）车辆向前行驶
图3-11 点云运动畸变原理示意图

为了消除自车/激光雷达运动对外参标定的影响，我们首先需要去除点云的运动畸变。假设激光雷达/车辆在每帧之间是匀速运动的，由此便可采用线性插值的方式将第 kkk 帧内任意时间戳对应的激光点云投影到帧首或帧尾时刻。以将点云矫正到帧首时刻 tkt_ktk​ 为例，令 Tkl(t)\pmb{T}_k^l (t)Tkl​(t) 为 ttt 时刻由高频IMU数据和当前外参矩阵得到的 [tk,t][t_k,t][tk​,t] 时刻内激光雷达的位姿变换矢量，有 Tkl(t)=[θkl(t),τkl(t)]T\pmb{T}_{k}^{l}(t) = [\pmb{\theta}_{k}^{l}(t),\pmb{\tau}_{k}^{l}(t)]^{\mathrm{T}}Tkl​(t)=[θkl​(t),τkl​(t)]T ，其中 τkl(t)=[tx,ty,tz]T\pmb{\tau}_{k}^{l}(t) = [t_{x},t_{y},t_{z}]^{\mathrm{T}}τkl​(t)=[tx​,ty​,tz​]T 为位移矢量， θkl(t)=[θx,θy,θz]T\pmb{\theta}_{k}^{l}(t) = [\theta_{x},\theta_{y},\theta_{z}]^{\mathrm{T}}θkl​(t)=[θx​,θy​,θz​]T 为旋转角矢量。根据罗德里格斯旋转公式，可由旋转角矢量求解对应的旋转矩阵：

Rkl(t)=eθ^kl(t)=I+θ^kl(t)∥θkl(t)∥sin⁡∥θkl(t)∥+(θ^kl(t)∥θkl(t)∥)2(1−∥cos⁡θkl(t)∥)(3-49) \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} Rkl​(t)=eθ^kl​(t)=I+​θkl​(t)​θ^kl​(t)​sin​θkl​(t)​+​​θkl​(t)​θ^kl​(t)​​2(1−​cosθkl​(t)​)(3-49)

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

假定对于激光点 p(k,i)∈Pkp_{(k,i)} \in P_kp(k,i)​∈Pk​ ，对应的时间戳为 t(k,i)t_{(k,i)}t(k,i)​ ， T(k,i)l=[θ(k,i)l(t),τ(k,i)l(t)]TT_{(k,i)}^l = \left[\theta_{(k,i)}^l(t), \tau_{(k,i)}^l(t)\right]^{\mathrm{T}}T(k,i)l​=[θ(k,i)l​(t),τ(k,i)l​(t)]T 为 [tk,t(k,i)][t_k, t_{(k,i)}][tk​,t(k,i)​] 时刻内对应的位姿变换矢量，则 T(k,i)lT_{(k,i)}^lT(k,i)l​ 可以基于 Tkl(t)T_k^l(t)Tkl​(t) 通过插值求得，如式（3-50）所示。

T(k,i)l=t(k,i)−tkt−tkTkl(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} T(k,i)l​=t−tk​t(k,i)​−tk​​Tkl​(t)(3-50)

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

p~(k,i)=R(k,i)lp(k,i)+τ(k,i)l(3-51) \tilde {\boldsymbol {p}} _ {(k, i)} = \boldsymbol {R} _ {(k, i)} ^ {l} \boldsymbol {p} _ {(k, i)} + \boldsymbol {\tau} _ {(k, i)} ^ {l} \tag {3-51} p~​(k,i)​=R(k,i)l​p(k,i)​+τ(k,i)l​(3-51)

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

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

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

c(l,i)=1∣S∣∥p(l,i)∥∥∑j∈S,j≠i(pl,i−pl,j)∥(3-52) 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)​=∣S∣∥p(l,i)​∥1​​j∈S,j=i∑​(pl,i​−pl,j​)​(3-52)

其中， c(l,i)c_{(l,i)}c(l,i)​ 为第 lll 帧点云中第 iii 个激光点 p(l,i)\pmb{p}_{(l,i)}p(l,i)​ 的广义曲率， SSS 为 p(l,i)\pmb{p}_{(l,i)}p(l,i)​ 的邻域点集， p(l,j)\pmb{p}_{(l,j)}p(l,j)​ 为邻域点集 SSS 中的激光点。

DyLESC 算法会对第 lll 帧点云中的每个激光点进行广义曲率的计算，并将它们按照大小排序，选择其中广义曲率最大的 NpN_{p}Np​ 个点作为平面特征点。而后在第 lll 帧点云中，以每个平面点 P(l,q)∣q=1,…,Np\mathbf{P}_{(l,q)}|q = 1,\dots ,N_pP(l,q)​∣q=1,…,Np​ 为中心，在其附近选取最近的 NNN 个激光点 a(l,q)∣q=1,…,Na_{(l,q)}|q = 1,\dots ,Na(l,q)​∣q=1,…,N ，对得到的局部点集进行特征值分解，并根据点法式构建面元 MP(l,q)\mathrm{MP}_{(l,q)}MP(l,q)​ 。