基于三维标定板的相机-激光雷达联合标定方法

王 庆，谭镕轩，冯悠扬，严 超，孙 杨

（东南大学 仪器科学与工程学院，南京 210096）

相机和激光雷达是同时定位与建图（Simultaneous Localization and Mapping,SLAM）领域最具代表性的两类传感器。基于相机的视觉SLAM 能够利用图像中丰富的纹理信息，但是容易受到光线的干扰[1]；基于激光雷达的激光SLAM 能够直接获取测量点的深度，但是在走廊、隧道等缺少形态信息的场景易出现退化和失效的问题[2]。相机与激光雷达联合感知可以有效降低成本，同时弥补各自的不足，而实现相机与激光雷达数据融合的前提是标定出相机的内参和相机与激光雷达的外参。

针对相机与激光雷达的联合标定算法有很多，但大多只关注相机与激光雷达的外参标定，相机内参需要提前标定好。主流的相机标定算法[3]通过平面标志物进行标定，陈文艺等[4]使用双神经网络获取棋盘格角点从世界坐标系到相机坐标系的映射关系，得到比传统方法更高的标定精度。王靖等[5]考虑到圆形阵列板存在的边缘模糊和偏心问题，改进Zernike 矩和偏心误差修正获取更精确的圆心坐标，提高了标定精度。类似的研究还有很多，但大多算法采用的都是平面标志物，在标定过程中失去了一个维度的约束，容易出现过拟合问题[6]。相机与激光雷达外参标定方法主要通过在图像和点云中提取对应特征，构建约束条件求解外参。Zhou 等[7]通过检测棋盘格在图像和点云中的边缘，利用直线与平面对应的方式求解外参。Wang等[8]根据棋盘格的点云反射强度提取特征点，采用未校准透视n 点(Uncalibrated Perspective-n-Point,UPnP)算法获取外参。Pusztai 等[9]使用三面体进行标定，通过检测标定板顶点建立点到点的约束获得高精度外参，但该算法需要人工剔除背景点云并人工标注图像角点。Toth 等[10]通过从点云和图像中计算球体标志物中心，并用球体轮廓进行匹配得到外参。

除此之外，还有很多热门的相机与激光雷达开源标定工具。lidar_camera_calibration[11]将ArUco 标记粘贴到两个平面矩形板上，能够自动标定相机与激光雷达的外参，但需要人为地筛选出标定板边缘点。Autoware[12,13]使用平面棋盘格标定板能够同时标定相机内参和相机与激光雷达外参，但需要人为地选定点云特征，得到的结果稳定性差。velo2cam_calibration[14]设计了一种包含四个ArUco 图案和四个圆孔的平面标定板，通过提取圆心坐标构建点和点对应的约束求解外参。

上述外参标定算法需要在点云中提取点和线特征，而主流的16 线激光雷达分辨率低，提取的点和线特征精度较低，因此这些算法对低线束激光雷达的标定效果较差。且大部分联合标定算法只关注相机与激光雷达外参标定，相机的内参需要提前标定好，标定过程中存在大量人工操作，标定步骤较为复杂。

本文针对现有标定算法存在的问题，设计一种由三个棋盘格制作的三维标定板，能够实现相机内参和相机与激光雷达外参的联合标定。相比平面标定方法，三维标定方法在内参标定过程中增加一个维度的约束，可有效提高相机内参标定的精度和稳定性。使用图像和点云中的平面特征构建约束方程，在低线束激光雷达中也能提取出准确的平面特征，只需在算法初始化阶段从激光点云中选取三个种子点，即可自动获取高精度的相机内参和相机与激光雷达外参。

1 标定原理

相机与激光雷达的联合标定就是估计相机内参矩阵K和相机与激光雷达之间的刚体变换矩阵Tcl的过程。主流相机内参标定方法通过平面棋盘格的角点=(Xi,Yi,Zi,1)T和相机像素坐标系下的对应点=(ui,vi,1)T建立一一对应关系：

其中，fx和fy是相机焦距，cx和cy为相机主点坐标，Tcw为相机在世界坐标系下的位姿矩阵，Rcw为位姿Tcw中的旋转矩阵，tcw为位姿Tcw中的平移矩阵，s为点在相机坐标系下的深度。

使用至少四组数据即可估计相机内参K，相机标定通过重投影误差计算标定精度，计算公式如下：

其中，|||·||2是向量的2 范数。

常用的平面标定方法通过计算与拟合目标(平面标志物)的重投影误差来证明其方法的有效性，但对于整个场景的拟合效果较差，存在对标志物过拟合的现象，而三维标志物能够有效克服过拟合问题，因此本文通过三维标定板进行相机内参标定。

在已知相机内参K的前提下可以通过透视n点(Perspective-n-Point,PnP)算法获取外参。根据Geiger等[15]的工作，外参标定性能测试可通过线性平移误差et和角度误差er进行评估：

其中，Rgt和tgt分别为旋转矩阵和平移矩阵的真值，tr(·)为矩阵的迹。

由于低线束激光雷达的点线特征提取精度差，导致现有算法在低线束激光雷达的标定效果较差。本文采用三维标定板，通过提取平面特征，构建平面对应的约束方程计算外参，从而使得相机与低线束激光雷达也能获取精确的外参。

2 基于三维标定板的联合标定算法

本文设计的三维标定板如图1 所示，标定板三个面相互垂直，每个面放置一个棋盘格图案，事先使用全站仪测量得到棋盘格角点的三维坐标。

图1 三维标定板Fig.1 3D calibration plate

2.1 相机内参标定

令P=KTcw，式(1)可以转换为：

通过式(6)能构造如下方程：

对式(7)使用奇异值分解（Singular Value Decomposition,SVD）得到投影矩阵P的解。

投影矩阵可以分解为P=，对矩阵H进行正交三角分解（Orthogonal Triangular Decomposition）即可得到矩阵K和矩阵Rcw，内参矩阵K最后一个元素值为1，令K=K/K33进行归一化，最后计算平移矩阵tcw=K-1h。

上述方法得到的解没有考虑到相机畸变的影响，只能作为初值，通过列文伯格-马夸尔特(Levenberg-Marquardt,LM)算法同时优化相机每一帧的位姿Tcw以及内参矩阵K和畸变系数k1、k2，构造代价函数如下：

2.2 相机-激光外参标定

2.2.1 图像平面提取

假设平面在三维直角坐标系中的方程为nxx+nyy+nzz+d=0，定义平面模型由平面的单位法向量n=(nx,ny,nz)T和平面到坐标系原点的距离d组成，使用最小二乘法计算标定板的三个平面在世界坐标系下的平面模型，利用式(8)计算得到的位姿Rcw和tcw可以构造的关系如下：

2.2.2 激光点云平面提取

激光平面提取算法中常用点的几何曲率和法线方向作为判断依据，但由于机械式激光雷达点云在垂直和水平方向上的分布是不均匀的，单帧点云估计出来的曲率和法线精度很低，导致平面提取结果较差。

由于激光雷达的水平分辨率高，在单帧点云中能够获取较高精度线曲率，很多激光SLAM 算法[16]通过线曲率提取点云特征。本文通过计算线曲率的方式代替几何曲率，在第一帧激光点云中，标注每个标定板平面内的中心作为种子点，通过区域生长算法扩散到平面边缘得到待提取的平面。后续每一帧点云通过正态分布变换(Normal Distributions Transform,NDT)算法将第一帧中的种子点变换到后续帧的点云中，取距离变换后的与种子点最近的点作为后续帧的种子点，因此只需在算法初始化的阶段标注三个种子点，即可自动实现点云平面提取。点云平面提取步骤描述如下：

(1)如果是第一帧，按照固定顺序在标定板每个平面选择一个种子点。如果是后续帧，根据当前帧的相对位姿，将第一帧的种子点转换到当前帧中，取最近的一个点作为当前帧的种子点；

(2)将点云按线束划分为Lk(k=1,2...n)，对每个线束Lk的点云按照角度进行排序；

(3)从种子点X(k,seed)开始计算当前线束Lk中点的曲率c，计算公式如下：

其中，X(k,i)表示线束Lk中要计算的当前点，X(k,j)表示X(k,i)的近邻点，S表示线束Lk中点X(k,i)前后5 个点组成的点集，kS表示点集S的数量。

如果当前点的曲率c小于阈值，则认为是平面点，继续计算周围点的曲率；如果当前点的曲率c大于阈值，则认为已经扩散到边缘。当前线束提取完毕后，搜索线束Lk-1和Lk+1中距离种子点X(k,seed)最近的点,如果与种子点X(k,seed)的距离小于阈值，则认为该线束中存在平面点，将最近点作为起始点计算线曲率，通过这种方式判断每个线束是否有平面点，得到完整的平面提取结果。最后对提取的平面点云采用随机抽样一致性(Random Sample Consensus,RANSAC)算法剔除离群点，得到精确的平面模型。

重复步骤(1)-(3)直到处理完所有数据，得到每一帧的平面提取结果。

2.2.3 外参估计

将式(12)中的误差项展开：

式(13)中第一项不含旋转矩阵Rcl，第二项中=I，实际待优化的误差项为：

根据式(14)可以定义矩阵W为：

对矩阵W进行SVD 分解：

其中，U和V为矩阵W分解得到的酉矩阵，∑为矩阵W分解得到的对角矩阵。

旋转矩阵的最优估计值为Rcl=UVT。对于平移矩阵tcl构建如下方程：

对式(17)构成的超定方程组进行SVD 分解即可得到平移矩阵的最小二乘解，将计算得到的外参初值再带入到LM 算法中进行优化得到高精度的外参。

3 实验结果与分析

为了测试标定方法的稳定性与精度，用小觅相机和VLP-16 激光雷达搭建实验平台，基于ROS 操作系统在Linux 平台上实行，进行相机与激光雷达联合标定实验。小觅相机的分辨率为752×480，相机基线距离为12 cm，内参值为fx=368.03像素，fy=368.12像素，cx=377.36像素，cy=247.23像素。VLP-16 激光雷达的测距范围为50 m，测距精度为2 cm，棋盘格尺寸设置为420 mm×297 mm，棋盘格内角点的世界坐标通过NTS-340R6A 型号全站仪精确测量，全站仪水平角、垂直角置零后，将全站仪坐标系作为世界坐标系，传感器的坐标系建立如图2 所示。

图2 联合标定实验平台Fig.2 Joint calibration experimental platform

3.1 相机内参标定实验

实验过程中，在三维标定板内随机取一半的特征点采用三维标定方法进行内参标定，参与标定的特征点计算内符合重投影误差，量化标定方法的拟合精度，未参与标定的点计算外符合重投影误差，量化标定方法对整个场景的拟合精度。随机选定三维标定板中的一个棋盘格图案，采用平面标定方法进行内参标定，并计算内符合重投影误差，未参与标定的棋盘格角点计算外符合重投影误差，每组数据采集20 张图像。10组实测数据标定结果的平均值与真值进行对比，计算内参标定误差，表1 统计了10 组实测数据计算得到的平均重投影误差和相机内参误差。

表1 内参标定实验误差Tab.1 Error of intrinsic calibration experiments

由表1 可知，相机焦距fx、fy和相机主点坐标cx、cy的三维标定方法精度要优于平面标定方法。

从重投影误差来看，三维标定方法的内符合误差大于平面标定方法，但外符合重投影误差要远小于平面标定方法。三维标定方法的内符合误差和外符合误差在同一水平，说明三维标定方法能够有效克服过拟合问题。而平面标定方法的外符合误差和内符合误差差异较大，说明平面标定方法会对标定数据过拟合，从而导致对整个场景的拟合效果较差，因此采用三维标定方法能得到更精确的相机内参。图3 为采用平面标定方法与三维标定方法校正后的图像，其中平面标定方法校正后的图像中门框存在弯曲的部分，而三维标定方法的畸变校正结果明显优于平面标定方法的校正结果。

图3 相机校准结果Fig.3 Camera calibration results

3.2 相机-激光雷达外参标定实验

为了验证点云平面提取算法的性能，对区域生长算法和本文的方法进行测试，并通过Polyworks 工具人工提取平面点云进行对比。图4 为两种点云平面提取方法以及人工提取的结果，平面提取结果通过不同颜色进行标注。

如图4(a)所示，设置计算法向量和曲率的近邻点数为kn=10时，由于点云在空间上分布不均匀，区域生长算法计算的曲率和法线精度较低，区域生长算法在单帧激光点云中无法提取正确的平面点云。设置kn=50时，如图4(b)所示，区域生长算法能够得到平面提取结果，但三个标定板的连接处的点云没有被提取。由图4(d)可以看出，设置kn=10时，除了少数边缘点和离群点没有提取，本文方法能够获取稳定的提取结果。将图4(c)中Polyworks 获取的标定板点云作为真值，区域生长算法(kn=50)的召回率为87.97%，但kn设置越大，参与计算的点云数量越多，算法会更耗时，而本文方法在kn=10的条件下即可将召回率提升为98.77%，能够获取准确的标定板平面点云，且不需要在所有平面分割结果中搜索和匹配对应平面特征，耗时更短。

图4 点云平面提取效果Fig.4 Point cloud plane extraction effect

由于真实世界无法准确地获取相机与激光雷达的外参，本文在GAZEBO 仿真平台进行对比测试，基于实测数据对本文的方法进行验证。仿真平台搭载Bumblebee xb3 相机和VLP-16 激光雷达，两者的噪声符合高斯模型ε～，相机的像素强度误差为=0.007m，激光雷达的测距误差为=0.008m，相机与激光雷达之间的平移量设置为tx=0m，ty=-0.29m，tz=-0.15m，旋转角设置为θroll=0 rad，θpitch=0.1rad，θyaw=0 rad。

采集了10组仿真数据进行相机与激光雷达外参标定的精度测试。表2 统计了本文方法与Velo2cam_calibration算法的平均平移误差et和平均角度误差er。在仿真实验中，本文方法的平均平移误差et和平均角度误差er均小于Velo2cam_calibration，说明本文的标定方法对平移矩阵和旋转矩阵的估计精度要优于Velo2cam_calibration。

表2 仿真测试结果Tab.2 Simulation test results

在实际测试中，根据黄强等[17]的工作，引入相机左目到右目的外参作为真值，分别计算相机左目与激光雷达的外参以及相机右目与激光雷达的外参，从而得到相机右目到左目的外参估计值：

将式(18)得到的结果与双目相机外参真值进行对比来评估标定算法在实测数据中的性能。通过10 组数据计算得到的平移误差et和角度误差er如表3 所示。

表3 实测数据结果Tab.3 Measured data results

由表3 可知，在实测数据中，本文方法的平均标定误差为et=1.63cm 和er=0.022rad,相比于Velo2cam_calibration 算法，外参标定的平均平移误差减小了39.4%，平均旋转误差减小了37.1%。

为了验证在现实世界中的有效性，将激光雷达点云投影到图像上，并通过激光点云与图像数据融合进行三维重建。图5 绘制了停车场环境下点云投影图，并对局部区域进行了特写处理，图6 绘制了运用多帧点云和图像纹理融合得到的彩色点云图。

图5 点云投影效果Fig.5 Point cloud projection effect

图6 点云重建效果Fig.6 Point cloud reconstruction effect

从图5 和图6 可以看出，激光点云与图像中的物体边缘能够对齐，重建的彩色点云图色彩纹理与点云的形状吻合，本文提出的方法具备实用价值。

4 结论

本文针对平面标定方法存在的过拟合问题，通过三维标定板校准相机内参。针对单帧激光点云提取效果差的问题，设计了一种基于线曲率的平面提取方法；最后通过图像与激光点云中对应的平面特征一一对应的方式计算外参，提高相机与低线束激光雷达的标定精度。实验结果表明：基于三维标定板的相机内参标定相比平面标定板更精确。同时，通过仿真和实测数据进行实验，本文采用的外参标定方法可有效提高相机与低线束激光雷达的标定精度，与velo2cam_calibration 算法相比，外参标定的平均平移误差减小了39.4%，平均旋转误差减小了37.1%。但目前本文方法的外参标定精度受限于图像的平面特征提取精度，后续还要对图像的平面特征提取方法进行优化。