1 引言

三维激光雷达凭借其高精度、高分辨率及高可靠性等特性,成为了无人车障碍物检测与跟踪、三维地图构建等领域中重要的传感器。同样作为无人车感知的重要部分,全球定位系统(GPS)/惯性导航系统(INS)组合导航系统提供车辆位姿信息,回答了无人车“在哪”的问题。两者相互补充,实现无人车的全局定位与局部感知。两者融合使用的前提是需要对两者安装的相对位姿进行标定。

三维激光雷达坐标系与GPS/INS组合导航系统坐标系之间准确的相对位置和转角关系难以通过简单目测及手工测量直接获得。所以,需要通过算法实现两者之间的精确标定。在同时定位与建图应用方面,良好的标定结果为后续点云配准过程提供了更好的初始位置,能够减少建图配准过程的计算量,可以提高建图精度和效率。

针对三维激光雷达外参数标定问题,Zhu等^[1]提出了一种无监督三维激光雷达外参数标定方法,对6个外参数进行解耦求解,但仅对三维激光雷达相对于地面坐标系进行了研究;Underwood等^[2]提出了通过对特定标定物的匹配来求解三维激光雷达外参数的方法,但其依赖特殊标定物;罗志锋等^[3]提出了一种基于小车直线行驶、原地旋转轨迹特征的标定方法,对于无人车而言,原地转弯难以实现,并且该方法只能求解二维外参数。Levinson等^[4]提出了一种采集车辆转弯数据、利用64线激光雷达中64个单激光束连续多帧激光点云叠加的三维环境进行匹配求解激光雷达外参数的方法;该方法同时适用于三维激光雷达内、外参数的标定,但其计算量大、耗时长、效率低,并且对初值比较敏感,容易陷入局部最优解,若仅用于解决外参数标定问题,就会稍显复杂。Nouira等^[5]基于文献[ 4]提出了一种基于平面特征的无监督外参数优化方法,优化了迭代过程并提出了标定参数评价指标,但不具备通用性。将文献[ 5]的方法与文献[ 4]所提方法进行比较发现,前者的标定效果和速度均有明显改善,但算法复杂,实现难度较大。据此,本文提出在三维激光雷达内部参数标定准确、GPS/INS组合导航系统工作正常的前提下,采集车辆直线往返行驶时激光雷达点云和GPS/INS组合导航系统的位姿数据,选取位置相近、方向相反的点云组成点云对,同时匹配多对点云,通过多步数参数区域迭代生成三维激光雷达外参数的标定方法。

2 标定方法

所搭建实验平台如图1所示。该平台顶部安装GPS天线,后轴中心安装高精度光纤惯性/卫星组合导航系统,实时提供高精度的位姿信息。安装的Velodyne-64HDL(美国Velodyne公司)激光雷达,通过旋转扫描得到0°~360°范围内的环境信息。设置GPS/INS组合导航系统数据频率为激光雷达扫描频率的10倍,可以满足时间同步要求。

2.1 坐标系建立

三维激光雷达坐标系原点O_l建立在三维激光雷达上某点。理论上三维激光雷达坐标系的原点应该在激光雷达安装底座的中心点,但三维激光雷达内部安装配置等固有属性导致坐标系原点并非严格位于此位置。三维激光雷达自转的旋转角0°方向为Y_l轴的正方向,自转轴向上方向为Z_l轴的正方向,根据右手定则可确定X_l轴的正方向,得到O_l-X_lY_lZ_l坐标系,如图1所示。

GPS/INS组合导航系统通过多传感器融合输出车辆位姿数据。由于GPS天线及INS均固定连接在车体上,同属一个刚体,姿态角相同。所以,组合导航系统坐标系中以GPS天线上经纬度数据表示的位置为原点O_g,INS设备上“箭头”所指方向为Y_g轴的正方向,垂直于INS平面向上为Z_g轴的正方向,根据右手定则,得到X_g轴的正方向,得到O_g-X_gY_gZ_g坐标系,如图1所示。

图 1. 实验平台

Fig. 1. Experimental platform

下载图片 查看所有图片

外参数标定即为获得O_l-X_lY_lZ_l坐标系相对于O_g-X_gY_gZ_g坐标系的旋转参数R和平移参数T。三维激光雷达坐标系下点云坐标通过 xgygzg=Rxlylzl+T即可转换到GPS/INS组合导航系统坐标系下。

2.2 数据预处理

选择在差分信号良好的条件下,采集无人车行驶时的三维激光雷达和GPS/INS组合导航系统数据,记录每帧三维激光雷达点云采集初始时刻相应的GPS/INS组合导航系统位姿数据作为位姿标签,实现两类传感器之间的时间同步。

采集得到的三维激光雷达的原始数据是以距离的形式存储的,为实现车辆周围环境的三维重建,需要根据三维激光雷达每个激光束的角度偏移量、位置偏移量等内参数,及相应时刻的雷达转角等信息将激光雷达距离数据解析成三维坐标的形式;由于车辆运动直接解析得到的三维坐标存在一定的偏差,故需要在解析得到单帧激光雷达点云坐标之后,根据无人车运动状态参数对每帧点云中点的坐标进行位置修正,减小车辆运动对三维重建造成的畸变影响。

GPS/INS组合导航系统主要用于反映车辆的实时位姿,配合三维激光雷达实现无人车重建三维环境。车辆的位置数据是通过获取差分GPS得到的经纬度数据。为便于计算,根据经纬度转换平面坐标的关系^[6-7]将每帧点云对应的车辆的经纬度转化到平面坐标系,得到无人车在该平面坐标系下的坐标。

2.3 目标函数的建立

理论上,在GPS差分状态良好的条件下,将修正好的激光雷达点云依据当前帧点云对应的位姿数据投影到三维空间,可以实现三维环境的重建。实际上由于车辆采集的位姿数据并非三维激光雷达的位姿,而是GPS/INS组合导航系统的位姿数据,所以,依据车辆直接采集的车辆的位姿数据不能实现三维环境的重建,并且偏差主要为三维激光雷达坐标系与GPS/INS组合导航系统坐标系之间的偏差。基于上述思想,可以通过调整预设的外参数值,使得点云实现较好的匹配,得到最优外参数。本研究提出一种基于同时匹配多对方向相反、距离相近的点云迭代生成三维激光雷达外参数的方法。

根据多对三维激光雷达点云和GPS/INS组合导航系统坐标系的相对位姿,以R和T为优化参数,建立反映三维环境重建效果的匹配度目标函数J(R,T),其中R和T分别为雷达坐标系相对于导航系统坐标系的旋转矩阵和平移向量。在计算坐标变换时,R表现出了其计算方便的优势。假设车辆姿态欧拉角即航向角θ_yaw、俯仰角θ_pitch和横滚角θ_roll对应的角度分别为α、β和γ,则其对应的3×3旋转矩阵可以通过下式计算得到:

R(α,β,γ)=RzαRyβRx(γ)=cosαcosβcosαsinβsinγ-sinαcosγcosαsinβcosγ+sinαsinγsinαsinβsinαsinβsinγ+cosαcosγsinαsinβcosγ-cosαsinγ-sinβcosβsinγcosβsinγ,(1)

式中R_z(α)= cosα-sinα0sinαcosα0001,R_y(β)= cosβ0sinβ010-sinβ0cosβ,R_x(α)= 1000cosγ-sinγ0sinγcosγ。

此处θ_yaw、θ_pitch和θ_roll的顺序不可以改变,如果顺序发生改变,则矩阵相乘的顺序也需做出相应的改变,最终所得旋转矩阵的结果也会不同。T=[x y z]。与迭代最近点算法类似,不同于通常的点到点的距离,本研究采用点到面的距离[(2)式]作为距离判定依据,建立优化的目标函数:

J(R,T)=1M∑i=1M1N∑j=1N[wi,jdi,j2(R,T)],(2)

式中J(R,T)表示多对点云对的点对距离平方均值的均值,表征点云匹配的程度,其值越小,说明匹配程度越高;i为点云对的编号;M为点云对的数量;j为点云对的对应点对编号;N为对应点对的数量;w_i,j为点对距离的权重,若点对之间的距离大于设定的距离阈值,则w_i,j=0,否则,w_i,j=1;d_i,j(R,T)的定义为

di,j(R,T)=ni,j[pi,j(R,T)-mi,j(R,T)],(3)pi,j(R,T)=Rnav(p'i,j)(Rp'i,j+T)+Tnav(p'i,j)mi,j(R,T)=Rnav(m'i,j)(Rm'i,j+T)+Tnav(m'i,j),(4)

式中n_i,j为点的切平面的法向量;p_i,j和m_i,j分别为第i对匹配源点云和目标点云的第j个对应点对在全局坐标系下的坐标;p'_i,j和m'_i,j分别为第i对匹配源点云和目标点云的第j个对应点对在各自雷达坐标系下的坐标;R_nav和T_nav分别为GPS/INS组合导航系统坐标系相对于平面坐标系的旋转矩阵和平移向量,其值几乎每帧都不相同,随着车辆的位姿变化而变化。

由于每帧点云包含十多万个点,数据量较大,直接搜索会导致算法运行时间很长,本研究采用k-dimensional(KD)树^[8]快速搜索最近点的方法对上述算法进行优化,以缩短标定时间。采用主成分分析法求解点云中点的法向量,根据每个点距离最近的20个点列出协方差矩阵的最小特征值对应的特征向量得到该点的法向量^[9]。分别根据不同的外参数组合R和T求解以上目标函数的值,得到不同外参相应的匹配程度,比较目标函数值并选取最优的外参数。改变外参数初值组合,继续迭代得出最优外参数。

2.4 迭代优化方法

采用参数多步数迭代方法,分别设定每个参数的迭代中心为a₀、步长为d、步数为n,得到每个参数的2n+1个参数值。目标函数中待优化参数R通过θ_pitch、θ_yaw和θ_roll等3个欧拉角计算得到,T由x、y、z等3个参数表示,共6个参数。然后,根据不同参数组合计算其相应的目标函数值,将目标函数值最小时对应的参数组合作为下步各自的迭代中心。

通常,通过目测或简单测量预估a₀的值。根据预估的参数真值,选用合适的初始步长。标定步数的设定原则是保证预估标定参数真值在初始步长和步数组合包含的范围内。3个参数确定后,参数的取值为a=a₀+id,{i|-n≤i≤n,i∈Z},假设n=5,则参数的可能取值有11个,两个位置参数的组合为121种组合,依次类推。

采用旋转参数与平移参数交替迭代的方法优化外参数。同一步长条件下,在迭代旋转参数时,平移参数保持其当前最优状态不变,遍历当前旋转参数中心限定区域内参数所有可能的取值组合,寻找目标函数最小值对应的参数并更新为下一次迭代的中心,直到迭代至最优解不发生变化时,将该最优解确定为该步长下的最优解。得到最优旋转参数后,保持旋转最优参数不变,平移参数步长减半,迭代平移参数,寻找最优平移参数,循环交替迭代,直到满足终止条件。标定流程图如图2所示。

图 2. 程序流程图

Fig. 2. Program flow diagram

下载图片 查看所有图片

3 实验及结果

3.1 数据采集

采集车辆在同一直路上匀速(5 km/h)往返行驶的三维激光雷达和GPS/INS组合导航系统数据作为标定数据。低速行驶是为了降低两类传感器时间不完全同步对标定的影响。直线往返行驶时车辆的θ_yaw不同,满足姿态角改变的求解条件。同时,避免了转弯时点云的转弯修正环节,减少了其误差对外参数结果的影响。

图3为车辆采集数据轨迹。蓝色矩形框所选段位为标定选用的数据段,右侧蓝框为左侧蓝框的放大图,图中20~24和46~50为点云编号,每个红色箭头表示位置间隔5 m的点云对应的GPS/INS组合导航系统输出的相对位置及航向。绿色矩形框所选段位为位置相近、方向相反的点云组成的点云对,例如编号20和50为一对点云对。本研究选择在差分GPS信号较好、有明显地物特征的环境下采集数据进行标定。实验采集了两个不同场景进行标定,如图4所示,分别命名为1号标定场景和2号标定场景。

图 3. 车辆轨迹图

Fig. 3. Vehicle trajectory diagram

下载图片 查看所有图片

图 4. 标定前两个标定场景的点云叠加图。(a) 1号标定场景;(b) 2号标定场景

Fig. 4. Two scene point cloud overlay maps before calibration. (a) Calibration scene 1; (b) calibration scene 2

下载图片 查看所有图片

3.2 标定结果分析

3.2.1 较优初始迭代中心标定实验

通过目测预估各参数初值y=100 cm,x=0 cm,θ_yaw=θ_pitch=θ_roll=0°,其中位置参数的真实值在预估值±30 cm附近、角度参数的真实值在预估值±3°附近范围内。分别设定不同步长和步数,利用1号标定场景中的数据进行标定。终止条件为位置步长小于0.01 cm,同时角度步长小于0.001°。

表1为不同初始步长和步数组合的标定结果。相同初始迭代中心条件下,分别采用初始位置步长与初始角度步长组合为20 cm & 2°、15 cm & 1.5°、10 cm & 1°、5 cm & 0.5°、2 cm & 0.2°,步数为1和其他步数进行标定实验。文献[ 4]中的迭代方法是所提方法步数为1时的特例。其他标定步数的设定原则为保证预估标定参数真值在初始步长和步数组合设定的范围内。

步数为1的5组实验中,标定时间均在4 min左右,相对于文献[ 4]中提到的较差标定初值条件下单步数迭代标定出结果的时间约1 h,所提方法标定时间明显缩短,体现了所提标定数据处理方法和目标函数的优势。比较5组标定结果,15 cm & 1.5°初值组合相应的J明显大于其他4组,并且θ_yaw的结果与其他4组有较大偏差,说明此组标定可能得到比较差的局部最优解。2 cm & 0.2°初值组合对应的J值最小,说明其得到的标定结果较其他初值更为可靠。

在初始迭代中心和初始步长相同的条件下,多步数标定后目标函数值均小于各自对应的单步数标定目标函数,说明多步数迭代比单步数迭代更容易得到更优的标定结果。但是,多步数迭代标定消耗的时间较单步数更长。标定消耗时间随着步数的增加而快速增加。比如,当步数为15时,标定时间将在几百分钟以上,这显然不是很好的选择。实际标定中,在对时间要求不高的条件下,可以考虑采用多步数迭代进行标定,通常选择步数为2~5。

3.2.2 较差初始迭代中心的标定实验

为验证算法的可靠性,采用2号标定场景中的数据,设定较差初始迭代中心。实验结果进一步说明在初始迭代中心和初始步长相同的条件下,多步数迭代较单步数迭代更容易得到更优外参数。设定较差初始迭代中心y=x=0 cm,θ_yaw=θ_pitch=θ_roll=0°,初始角度步长为5°,初始位置步长为20 cm。分别设定步数为1和5进行标定实验,迭代过程中各参数的变化过程如图5所示。

图5为2号标定场景中x、y、θ_yaw以及J随迭代次数的变化情况。该平台上三维激光雷达安装的θ_pitch和θ_roll均很小,其迭代结果未反映在图5中。当步数为5时,x和y很快到达最优位置附近,逐渐细化,直到最优;θ_yaw刚开始偏离初始位置较大,通过迭代偏差逐渐减小,并逐渐收敛到最优位置。当步数为1时,x逐步渐进式到达最优位置;y和θ_yaw在初始位置附近陷入了局部最优,未能到达正确位置。图5(d)中步数为5时的J值总体小于步数为1时的J值,说明点云对经过多步数迭代得到位姿变化后,点云匹配程度更高、结果更准确。参照图6所示的目标函数等高线图,形象地展示了外参数最优位置处目标函数最小。

图 5. 2号标定场景中几个主要外参数及目标函数随迭代次数的变化情况。(a) x;(b) y;(c) θyaw;(d) J

Fig. 5. Variation of several main extrinsic parameters and objective function with the number of iterations in calibration scene 2. (a) x; (b) y; (c) θyaw; (d) J

下载图片 查看所有图片

图6(a)为根据初始设定的(-100,100)×(-100,100)范围内,间隔20 cm的x和y的121个组合位置相应的目标函数值生成的等高线图。图中红色圆圈处表示位置组合(0,100)附近目标函数值最小,将其确定为下一步的迭代中心,计算以(0,100)为参数迭代中心时的121个位置参数组合,如图6(b)中黑色圆圈所示。根据目标函数值生成第2个等高线图如图6(c)所示。图6(c)区域内(0,120)处目标函数值最小,将其确定为下一步的迭代中心继续迭代。迭代过程点如图6(b)所示,每个点表示迭代过程中的一个位置组合点,逐渐缩小范围,直至满足终止条件。旋转参数迭代过程与之类似,此处不再赘述。

图 6. 2号标定场景迭代过程图。(a)初始范围;(b)迭代过程点;(c)迭代中心移动后范围

Fig. 6. Iteration process diagrams in calibration scene 2. (a) Initial range; (b) points of the iteration process; (c) range after moving the iteration center

下载图片 查看所有图片

表2为2号标定场景中步数分别为1和5时的标定结果。综合分析图6与表2可得,相同标定环境下,步数为5时迭代生成的外参数最终的目标函数值明显小于步数为1时得到的目标函数值。步长为1时得到的相对位置为(8.154296,-16.82617),参照图5(a),对应于其中红色五角星所示位置,可明显看出其位置并非整个区域的最优位置。可见,对于设定的较差参数初值,步数为5比步数为1更容易得到较优外参数。

图7为两个标定场景外参数标定后三维激光雷达点云叠加效果图。对比图4所示标定前的两个三维场景可以明显看出,通过所提方法的标定,基本实现了三维激光雷达与GPS/INS组合导航系统之间的外参数标定。

图 7. 标定后两个场景的点云叠加图。(a) 1号标定场景;(b) 2号标定场景

Fig. 7. Two scene point cloud overlay maps after calibration. (a) Calibration scene 1; (b) calibration scene 2

下载图片 查看所有图片

3.2.3 不同初始迭代中心标定实验

基于2号标定场景,根据第3.2.1节中预估的参数迭代中心,在该初始中心位置参数的±40 cm、旋转参数的±4°范围内随机生成标定迭代中心,设定步长为20 cm、为3进行标定实验。随机进行20次实验,得到20组标定结果,统计每个参数的均值和均方差,结果如表3所示。

由表3可见,各个参数的均方差均比较小,可知不同标定初始迭代中心得到的标定结果偏差非常小。据此可以说明,多步数迭代对初始迭代中心的依赖性降低,更容易得到较优的外参数值。

4 结论

提出了一种利用车辆直线往返行驶的三维激光雷达和GPS/INS组合导航系统数据,选择距离相近、方向相反的点云组成点云对,基于连续多对点云同时匹配,遍历多步数参数区域迭代获得外参数的方法。该方法无需采集车辆转弯时的数据进行标定(车辆转弯时,雷达点云需要转弯修正,修正误差会影响标定结果),数据处理相对简单,减少了标定前的中间环节;设定了更为简单的目标函数,计算量小,标定时间缩短;迭代采用多步数参数区域,比单步数参数区域更容易得到更优的外参数。但标定结果的影响因素较多,如差分数据的精度及三维激光雷达数据与GPS/INS组合导航系统时间同步的精度等系统误差都会对标定结果产生影响,匹配的精度对环境也具有一定依赖性。所以,为提高标定的准确度,标定时尽量选择结构规整、差分信号稳定的环境进行标定。本研究未能建立通用的评判标定精度的定量指标,建立一种通用的不同标定方法标定精度的评价指标将是后续的研究工作,目前只是通过目标函数值定性反映标定结果,为实际应用提供了一种改进方法。因为车辆运动近似平面运动,标定过程中没有考虑高度参数,下一步将考虑采集往返同一坡道的数据进行包含高度参数的标定。