LiDAR标签和栅格占有图结合的LiDAR/IMU空间标定方法

钱 闯,张红娟,李文卓,刘 晖,李必军

1. 武汉理工大学智能交通系统研究中心,湖北 武汉 430063; 2. 武汉大学测绘遥感信息工程国家重点实验室,湖北 武汉 430079; 3. 武汉大学时空数据智能获取技术与应用教育部工程研究中心,湖北 武汉 430079; 4. 武汉大学卫星导航定位技术研究中心,湖北 武汉 430079

随着自动驾驶技术的发展,LiDAR和惯性测量单元(inertial measurement unit,IMU)已经成为自动驾驶汽车必不可少的两类传感器。利用LiDAR和IMU数据融合可实现车辆精准定位[1-2]、高精度地图构建[3-4]、目标识别与分类[5-6]等应用,但是前提是需要知道准确的LiDAR和IMU的空间转换关系,也称为LiDAR/IMU空间标定参数[7-8]。传感器坐标系一般位于传感器测量元件中心位置,难以给出标定参数精确的手工测量值,因此一般是通过标定算法对LiDAR和IMU的空间相对关系进行离线或在线标定。

LiDAR和IMU空间标定方法主要可分3类:手眼标定、在线标定方法和标签标定法。手眼标定使用LiDAR帧间匹配(scan-to-scan matching)技术得到LiDAR两个连续时刻间相对位置姿态。同时,IMU机械编排可计算IMU两个连续时刻间相对位置姿态[9]。通过两个传感器的外参参数可构造两者的相对运动约束,构建观测方程,非线性求解外参参数。为了使法方程满秩,需要3个以上不同方向上的相对运动。文献[10]将LiDAR/IMU外参数标定转化为手眼标定问题实现系统标定,在此基础上,提出了一种基于因子图优化的LiDAR/IMU紧组合定位方法。文献[11]提出了一种由粗到精的自适应能力强的LiDAR/IMU外参数标定算法,采用手眼标定算法对LiDAR/IMU外参数进行初步求解,结合使用IMU运动估计,计算点云重投影误差,构建联合优化函数,迭代计算得到精确的外参数。文献[12]利用高精度地图对LiDAR/IMU进行手眼标定,LiDAR点云和高精度地图实时匹配获得运行轨迹,通过标定参数将其与IMU的轨迹进行坐标转换,构建约束方程并求解标定参数。文献[13]提出了一种通用的基于传感器间相互运动约束的手眼标定框架,适用于LiDAR/相机/IMU传感器之间的相互标定。手眼标定算法不依赖于传感器外参数初始值精度,但是IMU机械编排受器件零偏误差的影响,LiDAR帧间匹配精度有限,导致标定精度不高。同时,手眼标定算法需要传感器有充分的机动才能保证标定参数的可观性。

LiDAR/IMU在线标定通常在LiDAR/IMU融合里程计或SLAM系统中将待标定的外参参数加入到状态估计方程进行在线估计。文献[14]为了提高LiDAR/IMU在线标定的实时性,提取LiDAR点云中的平面特征并对前后帧进行数据关联,利用多状态约束Kalman滤波器将LiDAR/IMU外参数引入到状态量进行实时解算。文献[15—16]提出了一种基于滑动窗口滤波器的在线空间校准方法,为了提高LiDAR点云中平面特征提取和跟踪的效率与可靠性,提取低曲率平面点并在滑动窗口中进行跟踪,利用异常值探测实现高质量数据关联。文献[17]利用IMU预积分补偿高速环境下LiDAR点云的运动畸变,提高基于平面特征的LIDAR/IMU外参标定精度。文献[18]提出一种用于地下空间恶劣环境的LiDAR/IMU导航器,为满足野外环境下传感器随装随用的性能,该导航器还采用滑动窗口滤波器对LiDAR/IMU外参进行在线估计,但LiDAR点云特征提取采用的是点特征而不是平面特征。文献[19]提出了一种基于多特征(点/球、线/柱、平面特征)的在线LiDAR/IMU标定方法,并同时在线标定LiDAR内参、IMU内参和LiDAR/IMU外参。在线标定方法有效解决了传感器安装位置发生变化时需要重新标定的问题,具有实时性和自主调整能力,但是标定效率依赖于算法的收敛速度,且将标定参数加入待估参数会降低系统的可靠性,当载体的机动不足导致标定参数的可观性降低时,可能会出现错误的估计结果。同时,在线标定方法需要有一个较高精度的标定参数初值才能较快地收敛,这也对离线标定提出了更高的要求。

标签标定法利用特定的人工目标或特定设施,通过第三方精密仪器(通常是全站仪)测量标签的精确全局位置,建立标签在全局坐标系、LiDAR坐标系和IMU坐标系下的位姿传递方程,求解标定参数。文献[20]设计了一种使用角反射器作标志物的标定方法,通过区域分割、地面滤除和标志点提取的预处理方法来提取标志点,使用迭代最近邻点算法求得标志点在LiDAR下的坐标,对IMU和LiDAR坐标系下的标志点数据进行数据拟合,求得标定参数。文献[21]通过提取人工制作的锥形圆柱筒的几何特征来建立LiDAR和IMU的坐标转换关系,利用扩展卡尔曼滤波对标定参数进行优化。文献[22]采用在建筑物面布设人工标签,通过全站仪获得标签坐标,并从点云中提取标签坐标,利用最小二乘求解标定参数。标签标定法简便且可靠,但是其标签的布设、测量、维护与提取过程较为复杂,影响标定的效率。

综上所述,标签标定法可以通过标签布设保证标定参数的可观性,从而实现标定参数的可靠解算,也可为在线标定等方法提供高精度的标定参数初值。因此,本文设计一种基于LiDAR标签的LiDAR/IMU空间标定方法,利用组合导航获取IMU的高精度位置和姿态信息,采用标定参数初始值和IMU姿态约束构建LiDAR栅格占有地图,基于地图匹配方法获得LiDAR标签在图中的位置,最终通过LiDAR标签的已知高精度位置实现标定参数的最优估计。

1 方 法

LiDAR/IMU外参标定流程如图1所示。首先通过车载GNSS和IMU进行组合导航计算所有时刻的IMU在“东-北-天”导航系下的位置和姿态。然后利用标定参数初始值和IMU组合导航数据进行姿态约束的LiDAR里程计解算,生成近似导航系下的栅格占有地图。最后将LiDAR标签获取的点云与栅格地图进行匹配,基于LiDAR标签的已知位置,利用非线性优化算法求解LiDAR/IMU外参数。本文构建LiDAR栅格占有图而后将标签点云与地图匹配,而非采用文献[23—24]中某一时刻的点云帧与标签点云匹配,一方面是为了提高匹配精度,另一方面后者需要标签布设在待标定的LiDAR附近,极大限制了标签的空间分布, 加大了布设难度。本文采用的LiDAR的采样频率为20 Hz,IMU的采样频率为600 Hz,为实现数据同步,以LiDAR时间点为基准,将IMU组合导航结果线性插值到LiDAR时刻。

图1 LiDAR/IMU外参标定流程Fig.1 Flowchart of LiDAR/IMU calibration

本文涉及的坐标系如下。

LiDAR坐标系(l系):LiDAR坐标系原点位于雷达内部,定义为l系,Y轴正方向指向雷达自转的旋转角0°方向,Z轴指向自转轴向上,X轴垂直于Y与Z轴,构成右手直角坐标系。

IMU坐标系(b系):IMU坐标系原点通常位于IMU中心,定义为b系,是加速度计和陀螺仪测量输出值定义的坐标系,X、Y和Z轴通常采用“前右下”或“右前上”,构成右手直角坐标系。

导航坐标系(n系):导航系以第一帧的IMU坐标为坐标系原点,Z轴与椭球法线重合,向上为正(天向),Y轴与椭球短半轴重合(北向),X轴与地球椭球的长半轴重合(东向)所构成的直角坐标系。

各坐标系的转换关系如下。

(1)b系到n系的空间转换如下

(1)

(2)l系到n系的空间转换如下

(2)

1.1 GNSS/IMU组合导航

GNSS和IMU组合可利用两种传感器提供的互补信息来提高导航系统的精度和可靠度。本文采用基于扩展卡尔曼滤波(extended Kalman filter,EKF)的GNSS/IMU松组合模型对GNSS定位观测和IMU观测进行融合。首先基于IMU机械编排构建状态误差方程如下

(3)

(4)

式中,Hk为状态量系数阵;I为单位阵;vk为观测噪声;O为零矩阵。进一步构造EKF状态更新方程如下

(5)

(6)

(7)

1.2 基于组合导航结果的LiDAR点云拼接

(8)

然后,将t时刻的激光扫描帧St中的激光点x∈St从l系转换到n系,公式如下

(9)

但是,式(8)和式(9)假设了LiDAR/IMU标定参数不存在误差。当把标定参数的初始值代入式(8)时

(10)

当考虑标定参数误差时,式(8)和式(10)可改写为

(11)

(12)

(13)

(14)

1.3 姿态约束的LiDAR栅格占有图

1.2节已经证明,在车辆近似直线运动下,点云转换后的坐标系轴向是一致的,但是点云帧之间的位置变化不能直接使用IMU的组合导航位姿结果。因此,为了解决LiDAR/IMU标定参数误差导致的点云拼接地图中的重影问题,本文提出了一种姿态约束的LiDAR SLAM方法构建三维栅格占有图,充分利用IMU姿态的约束信息,提高地图的相对精度[25],同时解决地图拼接的重影问题。三维栅格占有图是将三维空间按照2 cm的空间分辨率划分为三维格网,每个栅格的初始占有概率设为0.01。输入每一帧的LiDAR点云时,基于最大似然估计方法(maximum likelihood estimation,MLE)统计每个激光脚点落在某个栅格内的概率。随着点云的输入,不断对栅格的占有概率进行更新,最终生成全局的栅格占有图。MLE是一种scan-to-map的点云匹配方法,可以消除帧间匹配带来的累计误差。基于贝叶斯理论,假设点云之间相互独立,t时刻的激光扫描帧St与上一时刻的栅格占有图Mt-1之间的似然值如式(15)所示

(15)

式中,P(x|Mt-1)为激光点x∈St落在Mt-1中的概率。由于LiDAR一直在运动,对应的激光雷达坐标系l系也随时间变化,为了实现式(15),需要将St与Mt-1统一到同一个坐标框架下,同时保证P(St|Mt-1)最大,如式(16)所示

R*=argmax(P(R∝St|Mt-1))

(16)

式中,R∝St为经过可能的坐标转换R后的扫描帧。从式(16)可看出,MLE方法涉及了两个关键步骤:①从t=1时刻到t-1时刻的栅格占有图Mt-1;②获取t时刻的LiDAR与Mt-1的坐标转换关系R*。

(17)

对式(17)进行一阶泰勒展开,将其代入到最小二乘问题中并关于ΔR求导,令导数为0可得到高斯-牛顿方程

[1-P(R0∝x|Mt-1)]

(18)

式中,H为二阶海森矩阵;S为扫描点的距离测量值。不断迭代上面的增量方程求解ΔR,若ΔR足够小,则停止;否则令Rt+1=Rt+ΔRt,返回式(18)。

(19)

1.4 基于栅格地图匹配的标定参数估计

(20)

(21)

2 试验验证

试验选择在武汉某园区内进行,图2(a)为测试环境,可以看出环境中主要特征为建筑物的立柱,且特征存在重复性。图2(b)为本文采用的1台移动的LiDAR雷达标签,采用GNSS RTK静态测量获得15个已知点的厘米级三维绝对坐标。图2(c)为待标定的车载LiDAR/IMU设备,其中车载GNSS设备为天宝BD982双频板卡,IMU为霍尼韦尔4930战术级模块,均内置于结构外壳中,车载LiDAR和LiDAR标签均为Velodyne-16线激光雷达。表1和表2显示了IMU和LiDAR的性能参数。

表1 IMU性能参数

表2 LiDAR性能参数

图2 试验环境与激光雷达标签Fig.2 Experimental environment and LiDAR labels

在标定过程中,LiDAR会在15个已知点之间移动,车辆大致沿直线路线行驶,行驶长度约为100 m。图3显示了行驶过程中车辆的航向角、俯仰角和速度。从图3中可看出,航向角并不是严格保持不变,而是在86°～90°范围内波动,本文方法不需要严格约束车辆直线行驶。俯仰角反映了道路坡度,从图2(a)可看出,道路较为平坦,因此俯仰角波动也较小。为了体现本文方法对车辆运行速度不敏感,车辆在行驶期间维持变化的速度。

图3 车辆行驶中航向角、俯仰角和速度的变化Fig.3 Changes of yaw angle, pitch angle and speed of vehicle during the trip

图4显示了基于LiDAR/IMU标定参数初始值和IMU组合导航结果拼接的n系下的LiDAR点云图。从图4中可看出,虽然标定参数有误差,建筑物轮廓和道路轮廓基本保持为直线,证明了2.2节描述的标定参数误差不会造成地图扭转的结论。但是图4中的点云看起来非常杂乱,点云重影严重,使地图非常不清晰,同时也证明了1.2节描述的标定参数误差下利用组合导航位姿结果拼接造成的点云重影问题。

图4 基于标定参数初始值和组合导航结果的LiDAR点云拼接结果Fig.4 The mosaicked LiDAR point cloud using initial calibration parameters and combined navigation results

图5为基于1.3节方法构建的姿态约束的LiDAR栅格占有图。已知点在n系下的坐标预先通过静态GNSS-RTK获得。采用5个雷达标签参与1.4节中的计算估计标定参数,10个雷达标签验证估计参数的精度。从图5中可看出,LiDAR点云经过姿态约束和地图匹配后,构建的地图清晰,点云重影问题得到了极大的缓解,且图中几何特征明显,比如U形的建筑物柱子轮廓,这些几何特征可以提高地图匹配和构图的精度。值得注意的是,虽然本次构建的地图精度高,但是本图是在n′系而不是n系,为了获取标定参数的精确估计值,需要将地图从n′系转到n系,求解n′系与n系的转换参数。

图5 基于姿态约束LiDAR栅格占有图的点云拼接结果Fig.5 The mosaicked LiDAR point cloud using LiDAR occupancy map method with attitude constraints

基于雷达标签的点云和图5中栅格占有图的地图匹配,可以获得雷达标签在n′系中的位置。图6(a)为雷达标签点云与图5中栅格占有图的地图匹配效果图。图6(b)为基于地图匹配的结果拼接的雷达标签点云图。从图6中可看出,基于高斯-牛顿法的地图匹配精度高,比如6(a)中U形的建筑物柱子轮廓的黄色点云与白色点云基本是重合的。一方面是因为本文选取的测试场地几何特征丰富,另一方面说明了高斯-牛顿法的稳健性和可靠性。从图6(b)也可看出,基于高精度匹配结果拼接的雷达标签点云也非常清晰,并且涵盖了场地内大部分的几何特征。

图6 基于栅格占有图地图匹配的雷达标签点云拼接结果Fig.6 The mosaicked LiDAR point cloud of LiDAR labels using the occupancy map

依据式(20)可通过5个安装在已知点的雷达标签在n系下的高精度位置和其在n′系下的地图匹配位置结果来估计n′系与n系的转换参数,最后通过式(21)求解LiDAR/IMU标定参数的估计值。为了验证式(21)求解的LiDAR/IMU标定参数的估计值精度,图7显示了在n系下利用估计的标定参数与IMU组合导航结果拼接的点云图。对比图4中利用标定参数初始值拼接的点云图,地图精度极大提高,点云重影问题也得到了极大的缓解,与图5中姿态约束的LiDAR栅格占有图的精度相当。

为了定量分析标定参数的估计精度,本文采用10个已知点上安装的雷达标签点云与图7中的点云图做地图匹配,基于高斯-牛顿法获得雷达标签在n系中的位置,最后与对应的已知点的位置进行对比。图8显示了利用GNSS-RTK采集的15个已知点在n系下的高精度位置(蓝色三角形),作为已知点的位置真值;显示了参与1.4节中标定参数估计的5个已知点通过与图5中的栅格占有图匹配获得的在n′系下的位置(黑色正方形),注意图中已将n系和n′系的原点重合;显示了用于标定参数精度验证的10个已知点在n′系下的位置(绿色正方形);显示了通过与图7基于标定参数估计值拼接的LiDAR点云图匹配得到的10个已知点在n系下位置(红色圆)。从图8中可看出,利用标定参数初值获得的n′系中的已知点位置与位置真值存在较大的偏差,利用估计的标定参数获得的n系中的已知点位置与位置真值基本重合。表3显示了匹配得到的10个已知点在n系下的位置与真值之间的水平偏差,偏差为厘米级,证明了估计的标定参数的高精度。

图8 15个已知点在不同框架下的位置Fig.8 Positions of the 15 known points in different frames

3 结 论

本文提出了一种不依赖人工布设标志物的LiDAR/IMU空间标定方法,采用放置于高精度已知位置的LiDAR标签提供绝对空间基准,基于GNSS-RTK和IMU组合导航获得IMU的高精度轨迹。从理论上分析了利用IMU的位置和姿态与标定参数初始值拼接的LiDAR点云图的点云重影问题,以及证明了LiDAR点云图方向上的一致性特性。基于该特性,本文提出一种基于IMU姿态约束的LiDAR栅格占有图构建方法,进而利用LiDAR标签点云与地图匹配获得LiDAR标签在地图坐标系中的位置,通过LiDAR已知高精度位置和非线性优化方法,实现地图坐标系与绝对空间基准的空间转换,最终基于该转换关系计算LiDAR/IMU的标定参数。通过试验验证,本文方法只需预先采集LiDAR标签的位置、组合导航结果和LiDAR标签点云,可以极大地简化LiDAR/IMU空间标定流程,实现了空间标定的自动化。利用已知点验证了本文标定方法的精度,标定参数可以实现厘米级的绝对坐标系下制图结果。