点线面联合约束的外参数的标定方法流程如图 1所示。首先，通过相机和激光雷达获取二维图像和三维点云；其次，在三维激光雷达点云中提取激光雷达坐标系下的三维点、线、面信息；然后，用二维图像恢复相机坐标系下的三维点、线、面信息；最后，建立相机和激光雷达坐标系下特征之间的约束，求解两种传感器之间的外参数。在本文中，相机坐标系用C系表示，激光雷达坐标系用L系表示，共视物体的世界坐标系用W表示，相机相对于L系的旋转和平移分别表示为${\mathit{R}}_L^C$和${\mathit{t}}_L^C$。

图  1  激光雷达和相机外参数标定流程图

Figure 1.  Flowchart of Extrinsic Parameters Calibration for LiDAR and Camera

为了恢复C系下点、线、面的三维信息，本文使用贴有ArUco码的标定板作为标定目标。ArUco码是一种携带二进制编码信息的靶标，常用于提供辅助的特征信息。本文相机的内参数和畸变参数已经通过标定方法^[29]获得，通过识别靶标，用PnP（perspective-n-point）方法^[30]获得ArUco坐标系（W系）相对于C‍系的转换参数${\mathit{T}}_W^C=\left[\begin{array}{cc}{\mathit{R}}_W^C& {\mathit{t}}_W^C\end{array}\right]$。在已知标定板尺寸的情况下，根据W系和C系之间的转换参数，可以将标定板角点在图像中的二维坐标反投影到C系下，获得C系下角点特征的三维坐标：

求得C系下角点坐标后，用角点之间的两点式方程求出标定板边缘直线的方向向量${\mathit{k}}^C$，作为C系下的直线特征。在W系下，已知标定板的平面法向量为${\mathit{n}}^W={\left[\begin{array}{ccc}\mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right]}^{\mathrm{T}}$，根据姿态外参数${\mathit{R}}_W^C$求出C系下的平面法向量${\mathit{n}}^C={\mathit{R}}_W^C\cdot {\mathit{n}}^W$，获得C系下的三维平面法向量。

三维激光雷达能直接提供点云的三维信息，因此，先提取标定板上的三维点云，再拟合计算平面、直线和角点参数。首先，提取标定板的点云，即对原始的点云进行深度滤波后，可获得标定板平面${\psi }_i$拟合的候选点${\tilde{P}}^{{\psi }_i}=\left\{{\mathit{p}}_{\mathrm{1}}^{{\psi }_i},{\mathit{p}}_{\mathrm{2}}^{{\psi }_i}\cdots {\mathit{p}}_N^{{\psi }_i}\right\}$；然后，用随机一致性检验（random sample consensus，RANSAC）方法^[31]拟合平面，获得第i次扫描的最优拟合结果，包括平面的k个拟合点$P^{{\psi }_i}=\left\{{\mathit{p}}_{\mathrm{1}}^{{\psi }_i},{\mathit{p}}_{\mathrm{2}}^{{\psi }_i}\cdots {\mathit{p}}_k^{{\psi }_i}\right\}$，以及平面参数$\left({\mathit{n}}^L,{\mathit{d}}^L\right)$，其中，${\mathit{n}}^L$表示L系下的平面法向量。

拟合平面后，需要进一步获取线特征，计算直线参数。如图 2所示，由于Velodyne16线激光雷达的点云稀疏，为了使拟合结果更加准确，本文采取了多帧点云叠加拟合边缘线的方法。首先，用深度滤波和强度滤波获取第i次观测中第j条边缘线${\lambda }_{i,j}$拟合的候选点${\tilde{P}}^{{\lambda }_{i,j}}=\left\{{\mathit{p}}_{\mathrm{1}}^{{\lambda }_{i,j}},{\mathit{p}}_{\mathrm{2}}^{{\lambda }_{i,j}}\cdots {\mathit{p}}_M^{{\lambda }_{i,j}}\right\}$；然后，用RANSAC方法拟合直线，由于激光雷达点云中标定板边缘的噪声大，而中心的噪声在有限的范围内^[28]，为了减弱边缘线噪声的影响，将边缘点${\tilde{P}}^{{\lambda }_{i,j}}$投影到平面上，再用RANSAC方法计算直线参数。

图  2  激光雷达点云边缘线提取结果

Figure 2.  Edge Line Extraction Results of LiDAR Point Cloud

1）用深度和强度滤波获取边缘线候选点${\tilde{P}}^{{\lambda }_{i,j}}=\left\{{\mathit{p}}_{\mathrm{1}}^{{\lambda }_{i,j}},{\mathit{p}}_{\mathrm{2}}^{{\lambda }_{i,j}}\cdots {\mathit{p}}_M^{{\lambda }_{i,j}}\right\}$。定义强度I为：$I=\mathrm{m}\mathrm{a}\mathrm{x}\left(r_{ij-\mathrm{1}}-r_{ij},r_{ij+\mathrm{1}}-r_{ij}\right)$，$r_{ij}=x_{{}_{ij}}^{\mathrm{2}}+y_{{}_{ij}}^{\mathrm{2}}+z_{{}_{ij}}^{\mathrm{2}}$。

2）将${\tilde{P}}^{{\lambda }_{i,j}}$中的点投影到平面上，把投影后的点集$P^{{\lambda }_{i,j}}$放入点云集合$P^{{\lambda }_j}$，组成i帧点云的集合，$P^{{\lambda }_j}=\left\{P^{{\lambda }_{\mathrm{1},j}},P^{{\lambda }_{\mathrm{2},j}}\cdots P^{{\lambda }_{i,j}}\right\}$。

3）在$P^{{\lambda }_j}$中随机选取两个点$Q_j=\left\{{\mathit{p}}_a^{{\lambda }_{m,j}},{\mathit{p}}_b^{{\lambda }_{n,j}}\right\}$，用这两个点计算直线方程。

4）根据直线方程，计算$P^{{\lambda }_j}$中的内点率，判断内点的依据是三维点到直线的距离是否小于阈值$\tau$，实验中$\tau =\mathrm{0.001}$。

5）重复步骤1）~步骤4）N（N=100）次，选取内点率最高的且大于阈值的直线拟合模型。

求得直线参数后，计算两条直线的交点即可获得三维角点的坐标。但在实验中，两条三维空间直线可能并不相交，因此，本文以两条直线间的最短线段的中点作为角点。得到角点后，分析多帧的角点位置可验证其噪声的高斯分布特性，因此，将多帧拟合的角点求均值可以减弱激光雷达点云噪声对角点的影响。

对于角点${\mathit{p}}_{ij}$、直线向量${\mathit{k}}_{ij}$和平面法向量${\mathit{n}}_{ij}$，其在L系和C系下的约束关系为：

式中，${\mathit{n}}_{ij}^L$和${\mathit{n}}_{ij}^C$分别表示平面${\psi }_i$在L系和C系下的平面法向量；${\mathit{k}}_{ij}^L$和${\mathit{k}}_{ij}^C$分别表示边缘线${\lambda }_{i,j}$在L系和C系下的方向向量；${\mathit{p}}_{ij}^L$和${\mathit{p}}_{ij}^C$分别表示L系和C系下的三维点坐标。利用上述约束建立如下的目标函数：

式中，3个部分分别表示面-面约束、线-线约束和三维角点约束；i表示观测数量；j表示单次观测中每种约束的数量。其中，每个标定板提供1个面约束、2个线约束与4个角点约束。

根据Kabsch方法^[32]，可以求出旋转矩阵${\mathit{R}}_L^C$的闭式解，代入约束方程求平移量${\mathit{t}}_L^C$。首先，计算${\mathit{R}}_L^C$的闭式解，可将式（3）中关于${\mathit{R}}_L^C$的每一种约束分别用Kabsch方法分解；然后，把3种约束一起联合求解。对于角点约束，其目标方程为：

把角点去中心化，令${\overline{\mathit{p}}}^L=\frac{\mathrm{1}}{n}{\displaystyle \sum _{j=\mathrm{1}}^n}{\mathit{p}}_{ij}^L$，${\overline{\mathit{p}}}^C=\frac{\mathrm{1}}{n}{\displaystyle \sum _{j=\mathrm{1}}^n}{\mathit{p}}_{ij}^C$，可得待分解矩阵${\mathit{M}}_{p_i}$：

$\begin{array}{l} {M_{{p_i}}} = (p_i^L - {{\bar p}^L}) \cdot {(p_i^C - {{\bar p}^C})^{\rm{T}}} = \\ \;\;\;\;M_{{p_i}}^L \cdot {(M_{{p_i}}^C)^{\rm{T}}} = {(U\Lambda {V^{\rm{T}}})_{{p_i}}} \end{array}$

式中，${\mathit{p}}_i^L=\left\{{\mathit{p}}_{\mathit{i},\mathrm{1}}^{\mathit{L}},{\mathit{p}}_{\mathit{i},\mathrm{2}}^{\mathit{L}}\cdots {\mathit{p}}_{\mathit{i},\mathrm{8}}^{\mathit{L}}\right\}$，${\mathit{p}}_i^C=\left\{{\mathit{p}}_{\mathit{i},\mathrm{1}}^{\mathit{C}},{\mathit{p}}_{\mathit{i},\mathrm{2}}^{\mathit{C}}\cdots {\mathit{p}}_{\mathit{i},\mathrm{8}}^{\mathit{C}}\right\}$，分别表示第i次观测时L系和C系下的8个三维角点坐标；${\mathit{M}}_{p_i}^L$和${\mathit{M}}_{p_i}^C$分别表示L系和C系下三维角点去中心化的坐标矩阵；$\mathit{\Lambda }$为奇异值矩阵。可得角点约束下旋转矩阵${\widehat{\mathit{R}}}_L^C=\mathit{V}{\mathit{U}}^{\mathrm{T}}$。同理，对线-线约束和面-面约束也可求得待分解矩阵${\mathit{M}}_{{\lambda }_i}$和${\mathit{M}}_{{\psi }_i}$：

式中，${\mathit{M}}_{{\psi }_i}^L=\left\{{\mathit{n}}_{i,\mathrm{1}}^L,{\mathit{n}}_{i,\mathrm{2}}^L\right\}$为第i次观测时L系下的平面法向量组成的矩阵；${\mathit{M}}_{{\psi }_i}^C=\left\{{\mathit{n}}_{i,\mathrm{1}}^C,{\mathit{n}}_{i,\mathrm{2}}^C\right\}$为第i次观测时C系下的平面法向量组成的矩阵；${\mathit{M}}_{{\lambda }_i}^L=\left\{{\mathit{k}}_{i,\mathrm{1}}^L,{\mathit{k}}_{i,\mathrm{2}}^L,{\mathit{k}}_{i,\mathrm{3}}^L,{\mathit{k}}_{i,\mathrm{4}}^L\right\}$，${\mathit{M}}_{{\lambda }_i}^C=$$\left\{{\mathit{k}}_{i,\mathrm{1}}^C,{\mathit{k}}_{i,\mathrm{2}}^C,\right.$$\left.{\mathit{k}}_{i,\mathrm{3}}^C,{\mathit{k}}_{i,\mathrm{4}}^C\right\}$，分别为两个坐标系下标定板边缘线的三维方向向量组成的矩阵。3种约束联合求解的公式为：

式中，${\mathit{M}}^L$和${\mathit{M}}^C$分别为：

根据${\mathit{M}}_i$的奇异值分解结果，即可求得旋转矩阵${\mathit{R}}_L^C$$=\mathit{V}{\mathit{U}}^{\mathrm{T}}$，代入式（4），获得平移参数的方程：

为了验证本文方法的标定效果，分别进行了不同组实验，并对每组数据结果进行比较分析，验证本文方法在收敛性、稳定性和精度方面的性能提升。相机和激光雷达标定在有稳定光源的室内实验室进行，标定场景如图 3（a）所示。标定采用两块贴有50 cm×42 cm ArUco二维码的标定板，使其在相机和激光雷达的可视范围内。实验使用的激光雷达为Velodyne VLP-16，相机为ZED双目相机，数据为原始彩色影像，激光雷达和相机的坐标系定义如图 3（b）所示。操作平台为Ubuntu 16.04下的机器人操作系统。

图  3  标定场环境及坐标系定义

Figure 3.  Calibration Field Environment and Coordinate Systems Definition

相机提取点线面的结果如图 4所示，红色边框是ArUco二维码的边框，其显示的白色数字是二维码对应的编号，二维码角点间的距离与尺子测量值的均方根误差小于1 cm。黄色点是提取的标定板角点特征，绿色线段是标定板的边线特征，黄色数组是两个标定板的法向量。可以看到，ArUco二维码识别准确，角点和边缘线也与标定板拟合，标定板法向量接近于平行，说明图像提取的点线面特征准确，满足后续联合求解的需求。

图  4  相机提取点线面特征

Figure 4.  Camera Extracts Point, Line and Plane Features

对于每组数据，采用本文方法和Dhall标定方法分别计算外参数。为了比较二者结果的收敛性和稳定性，每种方法均用同样初值迭代计算300次，得到每组数据的外参数标定结果，包括L系相对于C系的平移量x、y、z和旋转角r（横滚角）、p（俯仰角）、y（航向角）。

平移量标定的结果如图 5所示，图 5（a）和5（b）是实验A标定结果的平移和旋转，图 5（c）和5（d）是实验B标定结果的平移和旋转。图 5（a）表示前300帧迭代计算的x、y、z均值结果，蓝色线表示本文方法的结果，绿色线表示Dhall标定方法的结果，红色线表示尺子测量的平移量参考值。与Dhall标定方法相比，本文方法使用点线面约束联合求解，在X轴方向的精度提升明显。图5（b）是旋转参数r、p、y的均值结果，L系旋转（90°，-‍90°，0°）后，与C系的夹角近似为‍0°，两种方法所得的旋转角均为微小量。与Dhall标定方法相比，本文使用多帧点云叠加拟合直线，并对拟合角点求均值，提高了稳定性和收敛性。从稳定性分析，Dhall标定方法的平移量和旋转角波动较大，而本文方法波动较小，结果更稳定；从收敛性来看，Dhall标定方法在150帧之后收敛，本文方法在20帧左右就能收敛到比较可靠的结果，证明了本文方法的收敛性优于Dhall标定方法。

图  5  实验A和实验B的外参数标定结果

Figure 5.  Extrinsic Parameters Calibration Results of Experiments A and B

为了定性检验外参数的准确性，将标定板上的激光雷达点云用外参数投影到二维图像上，图 6中红色方框表示用初始位姿计算的投影区域，蓝色方框表示用最终外参数计算的投影区域，绿色方框表示图像提取的标定板区域，黄色点表示雷达点云投影到图像上的结果。

图  6  激光雷达点云投影结果

Figure 6.  Projection Results of LiDAR Point Cloud

由图 6可以看出，初始姿态的投影区域均不准确。图 6（a）中点云经投影变换后基本分布于标定板上，能与标定板很好地对齐，说明本文方法的外参数准确，修正了初始外参数的误差；而图 6（b）‍中激光雷达点云与标定板存在明显的偏差，说明Dhall标定方法的结果存在较大误差。

角点重投影误差能定量反映外参数结果的准确性。实验中，重投影误差定义为：L系下的角点投影到C系下，减去C系中的角点坐标后，取其模长的均值，计算公式如下：

本文方法与Dhall标定方法的重投影误差比较如表 1所示。从表‍1中重投影误差比较可知，Dhall标定方法的重投影误差在角点位置提取结果较好时为3.40 cm，在角点噪声较大时达到了6.52 cm，重投影误差明显增大；而本文方法的重投影误差虽然也在缓慢增大，但由于添加了线面约束，5组实验中的重投影误差均维持在1~2 cm，且每组实验的重投影误差均小于Dhall标定方法，验证了本文方法比Dhall标定方法具有更高的精度和鲁棒性。

本文方法与Dhall标定方法的收敛时间比较如表 1所示，其处理设备为华硕RX310U笔记本电脑，每组实验所用的图像和激光雷达数据的频率均为10 Hz。从表‍1可见，在5组实验中，与Dhall标定方法相比，本文方法由于使用了多帧点云叠加拟合直线，并对拟合角点求均值，能在更少的观测帧数时收敛，极大地减少了收敛时间。

对于每一组双目图像数据，计算获得激光雷达和左、右相机的之间的外参数，便可以推算ZED左、右相机的外参数。而ZED双目相机的默认参数可作为真实外参数，因此，可以比较推算外参数和真实外参数的差异，结果比较见表 2。

由表 2可见，Dhall标定方法的误差在x轴方向上比较大，在实验1中为5.96 cm，而本文方法使用点线面约束联合求解，较好地改善了x轴方向的误差，误差减小到0.55 cm。由于本文方法的约束在z轴不如x和y方向强，因此，z方向的误差增大了0.08 cm，但也在较小范围内。在实验2中，本文方法的误差在x、y和z方向上均更小。

本文提出了一种点线面联合约束的激光雷达和相机标定方法，一方面，采用多帧点云叠加，利用更多的激光点云拟合直线来减小误差，同时用多帧角点求均值来削弱噪声的影响，提高收敛性和稳定性；另一方面，加入线面特征，用点线面约束联合求解来提高精度和稳定性。实验结果验证，与Dhall标定方法相比，本文方法能够在20帧内收敛，且重投影误差小于2 cm，这对于自动驾驶和机器人传感器快速标定非常重要。但本方法仍有值得改进的地方，如仍需要标定物体，需要给定初值等，如果能实现无标定物体且无初值的准确标定，会使标定更智能。