1 引言

近几年,随着人工智能浪潮的再次兴起,智能驾驶技术迎来了井喷式的发展。三维激光雷达具有精度高、测距范围大,以及不受光线影响等特点^[1],已被广泛应用于智能驾驶车辆的障碍物检测^[2]、即时定位与地图构建^[3]、车道线检测^[4]等环境感知领域^[5-8]。随着三维激光雷达量产规模的不断扩大以及固态激光雷达技术的不断发展,其成本高的缺点正在被逐步克服,这使得它的使用价值有望进一步提高。

三维激光雷达一般安装于智能驾驶车辆的顶部,而车体坐标系的原点一般设置为激光雷达沿铅垂方向投影到地面的点,这使得激光雷达坐标系与车体坐标系之间存在纵向位移Δz;此外由于实际安装误差或特殊使用要求,激光雷达未水平安装于车辆顶部,这使得上述两个坐标系在俯仰、横滚和航向方向上存在角度偏差α、β和γ。外参数标定的任务是求解由俯仰角α、横滚角β、航向角γ和纵向位移Δz这4个不方便直接测量的物理量构成的旋转平移变换矩阵R。常见的标定方法主要有传统人工标定和多传感器联合标定。传统人工标定一般先建立地面坐标系,然后在地面坐标系的不同位置多次放置同一个纸箱,进而采集同一纸箱的一个角在地面坐标系和激光雷达坐标系下的多组位置,构成超定方程组,进而求解两个坐标系之间的变换矩阵^[9]。该方法需要人工多次测量诸多物理量,标定效率较低,而且车辆纵轴与地面坐标系的纵轴很难完全重合,从而导致产生额外的测量误差。多传感器联合标定需要摄像头、全球定位系统(GPS)、惯性导航等其他传感器的参与^[10-12],标定算法一般比较复杂,标定精度也会受到其他传感器测量精度的影响,并且对标定场的要求也更苛刻,例如标定场必须具有良好的光线和GPS信号等。

针对以上问题,本文提出了一种车载三维激光雷达外参数分步自动标定算法,将标定过程分解为2个步骤:第1步标定算法以地面为参照,完成对激光雷达俯仰角、横滚角和纵向位移的标定;第2步标定算法以第1步标定的完成为基础,在车辆沿直线行驶过程中,以标定杆为参照,完成对激光雷达航向角的标定。整个标定过程由程序自动执行,无须人工测量任何物理量,标定算法不涉及除激光雷达之外的其他传感器。

2 算法原理

激光雷达坐标系x'y'z'和车体坐标系xyz在空间中的相对位置关系如图1所示,α、β、γ分别为两个坐标系沿x、y、z轴方向的旋转角度偏差,Δz是2个坐标系沿z轴方向的纵向位移。

激光雷达坐标系和车体坐标系之间的转换关系为

xyz1=Rx'y'z'1,(1)R=cosβcosγcosβsinγ-sinβ0sinαsinβcosγ-cosαsinγsinαsinβsinγ+cosαcosγsinαcosβ0cosαsinβcosγ+sinαsinγcosαsinβsinγ-sinαcosγcosαcosβΔz0001。(2)

式中R为两个坐标系之间的旋转平移变换矩阵。

由此可知,求解出α、β、γ和Δz后便可求出变换矩阵R,从而实现激光雷达坐标系向车体坐标系的转化,完成对三维激光雷达外参数的标定。

2.1 第1步标定算法原理

第1步标定算法忽略航向角γ的影响,以地面为参照求解激光雷达坐标系相对于车体坐标系的俯仰角α、横滚角β和纵向位移Δz。α和β的存在使得水平地面点云在激光雷达坐标系内并不是水平的,如图2所示。

图 1. 激光雷达坐标系与车体坐标系的相对位置

Fig. 1. Relative position of lidar coordinate system and vehicle coordinate system

下载图片 查看所有图片

图 2. 未标定的激光雷达坐标系

Fig. 2. Uncalibrated lidar coordinate system

下载图片 查看所有图片

地面在激光雷达坐标系下的平面方程为

z'=Ax'+By'+C,(3)

式中A,B,C均为参数。当A=B=0时,(3)式表示的平面平行于相应坐标系的x'o'y'平面,即地面在激光雷达坐标系下呈绝对水平状态,这说明激光雷达坐标系和车体坐标系在俯仰和横滚方向的角度偏差均为0。据此构造地面在激光雷达坐标系下的水平度函数f:

f=A+B。(4)

f值越小,说明地面在激光雷达坐标系下的水平度越好,激光雷达坐标系和车体坐标系在俯仰和横滚方向上的角度偏差也就越小。因此第1步标定算法的目标是先假设γ、Δz为0,求出一组最优的α值和β值,并将其代入(1)式对地面点云进行坐标变换。此时求解出的f值应最小且接近0,而相应的(3)式中的参数C即为Δz。

采用最小二乘法拟合地面点云在激光雷达坐标系下的平面方程^[13-14]。求取地面水平度函数f的最小值为非线性优化问题,因此采用粒子群优化(PSO)算法^[15],以f值最小为优化目标,自动求取α和β的最优解。

第1步标定算法整体流程如下:

1) 输入原始地面点云数据。

2) 初始化参数γ=Δz=0,α和β为随机值。

3) PSO算法以地面水平度函数f值最小为优化目标,自动更新一组α值和β值。

4) 根据更新的α值和β值,将原始地面点云数据通过(1)式进行三维坐标变换。

5) 根据(3)式,通过最小二乘法拟合更新地面点云在激光雷达坐标系下的平面方程。

6) 根据(4)式,更新地面水平度函数f。

7) 判断f值是否小于阈值E₀或者迭代次数是否达到预设值N;如果满足其一,则终止迭代,输出α和β以及此时(3)式中的参数C;如果都不满足,则循环执行步骤3)~6)。

2.2 第2步标定算法原理

第1步标定完成后,在激光雷达坐标系的二维平面内进行第2步标定,即不考虑所有激光点云的高度,计算激光雷达坐标系与车体坐标系之间的航向角γ。

在车辆沿直线行驶过程中,采集多帧含有同一标定杆的激光点云,通过高程滤波等方式从激光点云中提取标定杆,再采用K-means聚类算法^[16],得到第i帧标定杆在激光雷达坐标系二维平面内的聚类中心(x'_i,y'_i),如图3(a)所示,如果以激光雷达坐标系为参照,则多帧同一标定杆的聚类中心如图3(b)所示。

图 3. 第2步标定算法原理。(a)标定杆聚类中心;(b)以激光雷达坐标系为参照的多帧同一标定杆聚类中心

Fig. 3. The second step calibration algorithm principle. (a) Clustering center of calibration rod; (b) multi-frame same calibration rod clustering center with reference to the lidar coordinate system

下载图片 查看所有图片

采用最小二乘法对图3(b)中的多帧同一标定杆的聚类中心进行直线拟合,得到拟合直线在激光雷达坐标系下的方程:

y'=kfx'+b,(5)

式中k_f为拟合直线的斜率,b为拟合直线的截距。k_f与车辆沿直线行驶的轨迹斜率相等,该斜率的反正切值所对应的角度即为所求航向角γ,即γ=arctan k_f。

3 误差分析及算法仿真验证

提出的车载三维激光雷达外参数分步自动标定算法所标定的系统误差主要有激光雷达本身的测距误差和最小二乘法拟合数据的误差。此外,第2步标定算法中车辆行驶轨迹的直线度误差会影响多帧同一标定杆聚类中心进行直线拟合的精确度,进而影响航向角γ的标定精度,因此提高车辆横向控制精度可以减小第2步标定算法的误差;而第1步标定算法PSO参数的选择对第1步标定结果的精度影响很大,因此在实车实验前,对第1步标定算法进行仿真,通过调整相关PSO参数来减小第1步的标定误差。经过多次仿真调整,兼顾标定精度和算法耗时,最终选取的PSO参数如表1所示。

仿真中用于测试第1步标定算法的地面点云由人工通过程序在已知倾斜角度(目标标定值)的平面内设定若干点构成,计算过程中PSO算法每次迭代更新的α、β与目标标定值之间的偏差曲线,以及地面水平度函数f值的变化曲线,如图4和图5所示。由图4和图5可知,随着PSO算法迭代次数增加,两条偏差曲线以及f值曲线均呈向0收敛的趋势,验证了算法的可行性。

图 4. 标定误差

Fig. 4. Calibration error

下载图片 查看所有图片

图 5. 地面水平度函数f值

Fig. 5. Ground levelness function f value

下载图片 查看所有图片

选取多组目标标定值进行仿真,结果如表2所示。由表2可知,选取的PSO参数在迭代次数小于100时可使标定精度达到10^-2数量级,足以满足实际工程的使用需求。

仿真结果表明,所提出的车载三维激光雷达外参数自动标定算法中的第1步标定算法在输入地面点云后,标定过程由程序自动执行,该算法可行,而且标定精度高,时间复杂度低。

4 实车实验

4.1 第1步标定算法的实车实验

实车实验中使用C++语言编写算法程序。由于C++语言的执行效率远高于仿真中M函数的执行效率,因此在使用真实激光点云数据之前,利用点云库(PCL)生成已知倾斜角度的模拟地面点云来调整仿真得到的PSO参数,以进一步提高标定精度。最终调整的PSO参数如表3所示,其他PSO参数与表1中的相同。经过测试,标定精度提高到了10^-5数量级,而算法耗时从仿真的20 s缩短到0.5 s左右。

实验平台是基于某品牌纯电动车改造的试验车,该车搭载美国Velodyne公司的HDL-32E型激光雷达,实验车行驶到地面平整的开阔场地后,采集包含地面点云的点云数据(PCD)文件。相继使用PCL中的loadPCDFile函数以及Viewer类相关函数,读取离线PCD文件中的原始点云数据并将其可视化示,然后进行滤波处理^[17],得到地面激光点云,如图6所示。

将滤波得到的地面点云数据输入到第1步标定算法中,标定前后的地面点云以及使用最小二乘法拟合的平面如图7所示。由标定结果可知,提出的车载三维激光雷达外参数分步自动标定算法中的第1步标定算法在实车实验中同样可以准确地计算出两个坐标系之间的α、β和Δz。

图 6. 地面点云的提取

Fig. 6. Extraction of ground point cloud

下载图片 查看所有图片

图 7. 第1步标定结果

Fig. 7. Results of the first step calibration

下载图片 查看所有图片

4.2 第2步标定算法的实车实验

将第1步标定算法计算出的α、β和Δz代入(1)式,对PCD文件中的所有点云进行坐标变换,从而完成第1步标定。在此基础上进行第2步标定,即计算γ。

第2步标定算法实车实验所需要的标定杆以及实际标定场景如图8所示。使用的实验车已经具备一定程度的自动驾驶功能,为了减小车辆在沿直线行驶过程中的直线度误差,在车辆沿直线行驶时规划一条直线路网,然后开启试验车的自动驾驶功能,使车辆沿规划的直线路网行驶,并在该行驶路段放置一个标定杆,采集多帧包含同一标定杆点云的PCD文件。

对离线PCD文件中的点云数据进行滤波及K-means聚类,以获取多帧同一标定杆的聚类中心,如图9所示,然后使用最小二乘法在激光雷达坐标系二维平面内对多帧聚类中心进行直线拟合,最后将拟合直线的斜率代入(6)式计算γ。

图 8. 第2步标定场景图

Fig. 8. Scene of the second step calibration

下载图片 查看所有图片

图 9. 标定杆提取及聚类

Fig. 9. Calibration pole extraction and clustering

下载图片 查看所有图片

将计算出的γ代入(1)式,再对离线PCD文件中的所有点云进行坐标变换,完成第2步标定。为了验证标定效果,重新提取标定杆进行聚类处理,并对聚类中心进行直线拟合,第2步标定前后同一点云数据在激光雷达坐标系xoy平面内的显示如图10所示。由图10可知,根据标定前后2条拟合直线的斜率,第2步标定算法能在很大程度上消除两个坐标系之间的航向偏差。

图 10. 第2步标定结果

Fig. 10. Results of the second step calibration

下载图片 查看所有图片

5 结论

针对车载三维激光雷达外参数标定问题,提出了一种分步自动标定算法。第1步在提取地面方程后构造水平度函数,通过PSO算法优化水平度函数来求解a、β和Δz;第2步以第1步标定的完成为基础,在车辆沿直线行驶过程中采集多帧含有同一标定杆的激光点云,通过滤波及聚类得到了多帧同一标定杆聚类中心,然后对其进行直线拟和,根据拟合直线的斜率求解γ。通过实车实验验证了本算法的标定精度可以达到10^-5数量级,能满足实际工程的使用需求,并且算法不使用其他传感器数据,无须人工测量任何物理量,进入相应的标定场景后可由程序自动完成标定工作。对地面点云和标定杆提取的准确性和有效性,是影响本文标定算法精度和效率的重要因素,也是本课题组后续研究的重要内容。