一种基于三维特征点集的激光雷达与相机配准方法

俞德崎，李广云，王 力，李帅鑫，宗文鹏

(信息工程大学，河南 郑州 450001)

当前汽车制造产业正朝着电动化、智能化和互联网化方向不断发展。无人驾驶汽车是通过车载的相机、激光雷达和IMU(inertial measurement unit)等传感器采集周围环境信息，利用数据融合技术实现障碍物认知、环境认知和车辆认知等并实现三维环境重构，进而自动规划行车路线并控制车辆到达预定目标[1]。在无人驾驶汽车研究领域，环境感知是该领域最基础的技术之一。没有对周围环境的感知，车辆就无法实现自主决策和控制[2]。随着传感器技术及无人驾驶技术的不断发展，激光雷达与相机已经成为了一组不可或缺的传感器组合。

激光雷达通过向周围环境发射激光束，然后接收从目标反射的回波，作适当处理后，可获得目标的距离、方位、姿态和形状等结构信息，但无法获得周围目标的色彩信息。相机可以获得丰富的色彩和特征信息，可用于对行人、树木等特征目标的检测，但易受环境因素的干扰。因此，激光雷达与相机在功能上具有良好的互补性。在无人驾驶领域，将激光雷达与相机获得的点云和图像数据进行融合处理，实现对周围环境目标的感知，使汽车自主避开障碍，防止事故的发生。而在数据融合过程中，激光雷达与相机的配准是影响数据融合效果的关键环节。

目前，在激光雷达和相机配准研究领域，国内外都已经取得了一定的进展，目前的配准方法主要有以下两类：一是基于棋盘格、3D图形等标志的点、线或形状特征集，通过建立点云与图像特征集间的对应关系，求解配准参数[3-6]；二是基于点云和图像的一些共有属性，通过互信息值计算损失函数，再利用优化算法实现最佳配准[2，7-9]，但这类方法只适用于传感器间位移和旋转较小的情况。文献[10]提出了一种新的运动恢复结构(structure from motion,SFM)算法用于配准参数解算，文献[11]利用IMU提供的运动信息辅助配准，这两种配准方式对硬件条件有较高的要求。

本文提出一种基于激光雷达与相机数据中对应三维特征点集，适用于低密度激光雷达与相机的配准方法。该方法利用ArUco标签[12]辅助得到纸板角点在相机坐标系下的坐标；利用随机抽样一致算法(RANSAC)提取激光雷达点云中的纸板边缘，以边缘交点为角点，从而建立两组角点的对应关系；最后利用Kabsch算法求解最优配准参数。

1 传感器与标志

本文使用Velodyne VLP16型激光雷达和KinectV2 RGB-D相机作为试验的传感器组合对提出的配准方法进行研究。Velodyne VLP16型共有16条扫描线，水平视场360°，垂直视场30°，测量距离可以达到100 m，每秒可接受30万个点云数据。KinectV2 RGB-D相机有彩色和红外两个镜头，红外镜头可以取得目标的深度信息，水平视场70°，垂直视场60°，距离检测距离0.5～4.5 m，每秒可传输30帧图像。在对激光雷达与相机进行配准前必须先对相机内参数进行标定。

1.1 相机标定

RGB-D相机标定主要包括3部分：彩色镜头内参数标定、红外镜头内参数标定，以及彩色与红外镜头安置参数标定。相机镜头的内参数标定可采用张正友标定法[13]。棋盘格采用尺寸为7×9的网格，网格边长40 mm，从不同视角对棋盘板拍摄50次，拍摄原则是以不同距离不同视角拍摄，保证靶标在图像的各个区域。已知镜头内参数后，可按照双目相机标定方法解算两镜头外参数。表1为相机内标定结果，(fu,fv,u0,v0)为内方位元素，k1、k2为径向畸变系数，p1、p2为切向畸变系数。表2为彩色镜头和红外镜头外标定结果。

表1 相机内参数标定结果

表2 两镜头外参数标定结果

1.2 标志设置

测量标志的选取原则：保证标志存在相机与激光雷达都能简单识别和检测到的特征。矩形纸板是一种简单有效的标志，并且可以裁成任意尺寸，重量轻，易于悬挂。由于本文使用的低密度Velodyne VLP16激光雷达只有16根扫描线，如果纸板尺寸太小或扫描距离太远会导致只有2～3根扫描线打在纸板，那么每个边缘只有2～3个点，这样在点云数据中很难拟合出纸板的边缘。因此，纸板尺寸及扫描距离必须合适。经过前期多次测试，确定了较合适的纸板尺寸和扫描距离。文中使用的是边长为50 cm的正方形纸板，扫描距离为2 m，保证有足够的扫描线打在纸板上。

2 特征点提取

为了求解相机坐标系与激光雷达坐标系间的配准参数，需要两组对应的3D特征点集：一组在相机坐标系下，另一组在激光雷达坐标系下。一旦找到了这两组对应的三维特征点，就能利用算法求解两传感器间的配准参数。试验数据采集场景如图1所示。

图1 试验设置

2.1 基于相机的特征点提取

本文采用一种增强现实(AR)标签，ArUco是一种特殊的编码模式，有助于标签本身的检测和纠错，将标签用A4纸打印并粘贴在硬纸板上作为标志。在纸板尺寸和标签位置已知的情况下，纸板角点的位置可以通过计算得到。相机可以识别ArUco标签，并得到标签中心到相机坐标系的转换参数，如图2所示，具体使用方法可参考文献[14]，在标签尺寸位置已知的情况下，利用转换参数可以将角点从标签坐标系转换到相机坐标系下，从而得到角点在相机坐标系下的三维坐标。

图2 相机识别ArUco标签

2.2 基于激光雷达的特征点提取

激光雷达扫描线是与地面平行的水平线，因此如果矩形纸板以边缘平行地面方式放置，那么激光雷达只能扫到与地面垂直的2个边缘。为了避免这种情况，将纸板以边缘与地面大约呈45°的方式悬挂，这样可以扫到4个边缘。利用RANSAC算法[15]拟合点云中的边缘线。

RANSAC算法基本假设是，数据集中包含局内点，也包含局外点，采用不断迭代的方法，寻找最优模型，并剔除不符合最优模型的局外点，构建一个仅由局内点组成的数据子集。其基本步骤如下[16]：

(3) 计算点云P中其余点pi到Lt的距离di及与nt的夹角σi是否满足条件

di<ε且σi<α

(1)

式中，ε为阈值距离；α夹角阈值。将满足条件的点的个数，作为Lt的分数SLt。

(4) 重复执行步骤(1)～步骤(3)T次，选择出得分最高的平面Lt*。其中T由下式决定

(2)

式中，τ为Lt*局外点所占比例的估计值；Φ为T次采样后最优平面被选中的概率。

(5) 记录Lt*并在P中移除计入SLt*的点。

(6) 重复步骤(1)～步骤(5)，直至无法从剩余的点中选择出一个得分至少为阈值S0的平面。

(7) 将每个平面L拟合的点投影到L上，并将区域的边界点作为边缘线的候选点集Q。

(9) 计算Q中其余点qj到Im的距离dj，检查是否满足条件

dj<ξ

(3)

式中，ξ为点到直线距离阈值。将满足条件的点的个数作为Im的分数SIm。

(10) 重复步骤(8)～步骤(9)M次，选择出得分最高的直线Im*，其中M由下式确定

(4)

(11) 记录Im*并在Q中移除计入SIm*的点。

在拟合出纸板4条边缘线之后，可以解得4条边缘的线方程，以它们的交点作为纸板角点。若求出的边缘线方程无交点，则把两边缘线距离最短处连线的中点近似为角点。

3 配准参数求解

在得到两组对应的三维特征点之后，相机坐标系与激光雷达坐标系间的转换参数可以利用Kabsch算法求解得到。同时为验证本文提出的配准方法的准确性，用本文的方法对相机与相机配准参数进行求解。

3.1 基于Kabsch算法求解配准参数

Kabsch算法用于计算两个待配准点集最优旋转矩阵，利用该方法求解两点集的最优矩阵对点集旋转后可以使均方根偏差(RMSD)最小，在旋转已知的情况下，两坐标系的平移参数也可以确定。设P和Q是两个待配准点云，算法过程如下

(5)

(6)

(7)

(8)

(9)

将式(9)代入目标函数式(5)得

(10)

(11)

由矩阵迹的性质，式(11)试可简化为

tr(X′TX′)+tr(YTY)-2tr(YTX)

(12)

2tr(YTX′)

(13)

消去不包含R的项，得到新的目标函数

(14)

由X′=RX和迹的性质

tr(YTX′)=tr(YTRX)=tr(XYTR)

(15)

利用奇异值分解有XYT=UDVT，式(15)等于

(16)

令M=VTRU则有

(17)

M是正交矩阵的乘积，也是正交矩阵，det(M)=+/-1，M中每个列向量的长度等于1，向量的每个分量小于或等于1。令Mii=1，其他分量为0，则有

M=I⟹VTRU=I⟹R=VUT

(18)

R是一个旋转矩阵，R∈SO(3)，需要确保det(R)=+1。若由式(18)得到的R，其det(R)=-1，那需要找到一个R，能使tr(YTX′)取得第二大值。

tr(YTX′)=d1M11+d2M22+d3M33

(19)

式中，d1≥d2≥d3且|Mii|≤1。

当式(19)有第二大值时M11=M22=1，M33=-1，综上则最优矩阵为

R=UCVT

(20)

式中，C为校正矩阵

(21)

3.2 相机与相机的配准

本文的主要目的是获得激光雷达与相机准确的配准参数，但由于旋转参数在实际试验过程中测量不便，为了验证本文方法解算的配准参数的准确性，利用文中的配准方法，求解两台RGB-D相机C1和C2配准参数,配准过程如下：

(1) 执行以上配准步骤，求解C1与激光雷达(LiDAR)的配准参数TLiDAR-to-C1。

(2) 求解C2与激光雷达的配准参数TLiDAR-to-C2。

(3) 利用二者间的关系，可以得到C1到C2的配准参数

(22)

(4) 利用所求配准参数，融合点云。得到相机与相机的配准参数，利用该结果对深度相机的点云进行数据融合。如果配准参数准确，那么点云数据可以很好地对准；如果配准参数不够准确，融合结果将会出现较大偏差。

4 试验与结果分析

利用Velodyne VLP16激光雷达采集的点云数据和KinectV2深度相机获取的点云数据来测试方法的可行性与准确性。相机与激光雷达的点云数据如图3和图4所示。

图3 相机点云

图4 激光雷达点云

首先，验证本文配准方法的可行性，在激光雷达与相机采集数据时，确保激光雷达与相机的位置固定。为减少噪声数据对试验结果的影响，提高试验结果的精度，固定传感器位置采集多组数据求解多组配准参数，以配准参数的平均值作为最终的配准参数结果，即

(23)

改变激光雷达与相机的位置，重复以上试验步骤求解配准参数，结果如图5所示。

图5 平移参数

由图5可以看出，平移参数的的变化率随着采集次数的增加而不断减小，最终达到一个稳定的值。当采集次数小于15次时，平移参数的变化率减小幅度较大。当采集次数大于15次时，平移参数仍有波动，但幅度很小。当采集次数大于20次时，平移参数处于一个稳定的值，将该值作为平移参数的准确值，作为最终结果。配准参数最终结果见表3和表4。

表3 位置1配准参数结果

表4 位置2配准参数结果

为了检验方法的可行性，利用卷尺人工测量激光雷达与相机的平移参数作为概略值；由于旋转参数很难人工测量，本文未测量旋转参数概略值。将试验结果与人工测量结果进行比较。由表3、表4可知，试验结果与人工测量结果的误差在2 cm以内，由此可知该方法是可行的；由于人工测量的参数值只是概略值，因此2 cm并不是试验结果的最终精度。

为了进一步验证该方法所求配准参数的准确性，用本文方法求解相机与相机的配准参数，将所求配准结果用于相机点云数据融合，点云融合前后分别如图6和图7所示。

图6 点云数据融合前

图7 点云数据融合后

由图6、图7可知，点云数据融合前，两组点云数据存在明显的偏差；利用所求配准参数对两组点云进行数据融合之后，点云数据得到很好的对准，几乎没有偏差。这说明本文方法所求的配准参数具有很好的准确性。

5 结 语

本文对无人驾驶技术中的激光雷达与相机的配准问题进行了研究，提出了基于3D特征点集的配准方法。在完成相机内标定和镜头外标定之后，利用ArUco标签提供的坐标系转换关系和RANSAC算法拟合标志边缘提取角点，获得角点在两传感器坐标系下的坐标，从而建立两组对应特征角点的关系，最后基于Kabsch算法求解最优配准参数。试验中，将试验结果与人工测量结果进行对比证明了方法的可行性；通过点云数据融合的质量证明了方法的准确性。结果表明本文方法可以很好地实现激光雷达与相机的配准。