0 引言

当今导航定位领域，使用惯性导航系统(Inertial Navigation System，INS)+里程计(Odometry，OD)+高程计的组合定位方式进行定位是主流的自主定位手段，但是该系统本身存在的误差会随着时间不断发散，需要靠其他传感器来进行辅助定位。三维点云激光雷达定位作为一个新兴的手段，具有不受光线影响、分辨率高、测量距离远的优点。激光雷达和惯性导航进行组合的定位方式也是当今实现无人驾驶的主流技术途径之一。为了满足该系统的定位精度，传感器之间的参数标定至关重要，参数标定精度直接影响融合定位结果。

最初，激光雷达主要利用外部的经纬仪和测距仪直接进行轮廓测量^[1]，以此得到标定参数，但是这种方法过于繁琐且精度较低。目前已经提出了多种针对激光雷达位姿的标定方法。程金龙^[2]采用三面靶标的激光雷达外参数标定的方法，使用随机采样一致性算法完成了平面分割和同名向量的提取，最后解出标定参数；韩正勇^[3]提出了一种可以在采样帧数比较少的情况下获得较高精度的参数矩阵的方法，该方法采用棋盘面对应性的性质，将坐标系标定问题转换为三维空间中旋转和缩放矩阵的求解问题；韩栋斌^[4]提出了一种在非理想参数初值的条件下依然可以获得较好标定结果的方法，该方法采用多对点云同时匹配迭代生成外参数来进行参数解算。

最小二乘法作为处理空间坐标组合转换的经典方法之一，被广泛应用于多种传感器的系统坐标标定^[5-8]。针对最小二乘法标定的改进也在不断进行^[9]。赵立峰^[10]将整体最小二乘法引入了坐标转换中，提出了一种迭代算法，降低低精度点的影响；杨仕平^[11]提出在标志点数目在4个及以上，且在两套坐标系下均存在随机误差的情况下，采用多元整体最小二乘法进行解算，提高算法精度。

随着即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)技术的发展，基于3D点云匹配技术的点云拼接方法日趋成熟，3D点云匹配的思路可以运用到坐标系标定的问题中，通过点云匹配方法求解两个坐标系的坐标转换矩阵。常用的3D点云匹配方法有ICP、正态分布变换(Normal Distributions Transform，NDT)等算法。点云配准方法有很多种，目前比较普遍的处理方式是基于点-点匹配的迭代最近点方法(ICP)、进一步提取特征进行特征匹配的改进ICP算法以及采用概率模型描述点云正态分布的NDT算法^[12]。原始的ICP算法由Besl等提出^[13]，核心思想为计算使得匹配点对欧氏距离和最小的坐标变换矩阵。针对常规ICP算法的缺陷，国内外的学者也提出了大量的改进算法，如MINGUEZ J^[14]提出了一种新的距离尺度函数，同时考虑到平移和选转，解决了旋转误差的问题。NDT算法是BIBER P^[15]提出的，这种算法完全基于概率模型进行匹配。

本文中将提出一种基于点云匹配思想的车载激光雷达/IMU联合标定的方法，并与基于最小二乘法的标定结果进行对比。

1 基本原理

激光雷达与IMU之间存在安装误差角和位置误差，因此两个传感器测量得到的同一组标志点的三维坐标不同，可以通过对应坐标点的关系来计算得到坐标系之间的转换矩阵，完成激光雷达/IMU坐标系的联合标定。

两坐标系下三维坐标的关系模型如图1所示。

(oX₁Y₁Z₁)为坐标系M，(OX₂Y₂Z₂)为坐标系N，标志点在两个坐标系之间的坐标分别为(x₁，y₁，z₁)、(x₂，y₂，z₂)，两者之间的坐标变换矩阵为T_3D为4×4矩阵，由旋转矩阵R和平移矩阵T组成。

坐标系N相对坐标系M的欧拉角为俯仰角θ、横滚角γ、方位角Ψ，相对于轴向的平移量为t_x、t_y、t_z，则：

2 标定方案

2.1 总体方案

本文采用具有多面反射、反射强度高、高度可调等优点的角反射器作为标志物，通过移动角反射器可获得在雷达坐标系下标志物在不同位置的坐标，并同时采用差分GPS获得标志物在当前位置的地理坐标系下的坐标。因标定场地不水平，通过车载IMU测得车辆相对地理坐标系的姿态角及IMU处的地理坐标系坐标，通过坐标变换将标志物在地理坐标系下的坐标转换到IMU坐标系下。坐标系描述如下：地理坐标系为(OX₁Y₁Z₁)，IMU坐标系为(OX₂Y₂Z₂)，激光雷达坐标系为(oX₃Y₃Z₃)。标定系统总体方案示意图如图2所示。

IMU和激光雷达坐标系的参数标定流程如图3所示，标定步骤如下：

(1)采集地理坐标系标志点的原始数据和激光雷达坐标系下的点云数据。

(2)因为IMU和差分全球导航卫星系统(Global Navigation Satellite Systems，GNSS)的原点基本重合，可使用IMU传感器测量得到的姿态角计算IMU与地理坐标系之间的转换矩阵，求解IMU坐标系下标志点的坐标数据。

(3)对激光雷达测量的原始点云数据进行预处理，找到标志点对应的点云坐标。

(4)对IMU和雷达坐标系下的标志点数据进行数据拟合，求得坐标系转换矩阵。

(5)将步骤(2)得到的IMU坐标系下的标志点坐标经过步骤(4)中求解得到的转换矩阵计算，转移到雷达坐标系中，与步骤(3)中得到的测量数据进行对比，进行误差分析。

至此标定方法转变为通过数据拟合、优化的方法求解IMU坐标系与雷达坐标系之间坐标转换矩阵。传统的方法是采用最小二乘法求解。随着SLAM技术的发展，基于3D点云匹配技术的点云拼接方法日趋成熟。本文借鉴3D点云匹配思路，将这一标定问题转换为两帧点云拼接的问题，进而通过点云匹配方法求解两个坐标系的坐标转换矩阵。常用的3D点云匹配方法有ICP、NDT等算法，本文选择ICP算法来求解坐标转换矩阵，并与最小二乘法求解结果进行对比。

2.2 最小二乘法基本原理

使用最小二乘法进行系统坐标标定，求解的坐标转换公式如下：

由此方法能够得到使三维坐标误差最小的坐标系转换矩阵。

2.3 ICP点云匹配算法原理

ICP点云匹配算法(迭代最近点算法)是最常用的三维点云匹配算法之一。ICP算法最早由Besl和Mckay二人提出，主要思想为计算两个点云中的最近点对进行匹配，然后根据配准点对来求得两个点云之间的坐标转换矩阵。ICP算法通常应用于不同时刻两组点云之间的匹配，从而求得载体的位姿变化。那么，可以将在IMU坐标系及雷达坐标系下采集到的标志点点云数据作为不同时刻的点云，通过3D点云匹配算法求其位姿变化，即为两坐标系的坐标转换关系。

激光雷达探测到的点云包含大量无关点云，需进行预处理来从中提取标志点。

预处理分为3个步骤：

(1)区域切割。根据先验知识只选定距离在一定范围之内的点云进行分析。

(2)地面滤除。将点云数据栅格化，本文设置栅格大小为0.3×0.3并将每个栅格内的点云按照高度排序，将每个栅格中高度最低的点视为地面点，将与最低点距离小于阈值的点也当作地面点并一同滤掉，便于后续处理。

(3)标志点提取。地面点云滤除后，点云信息中只包含标志物，取最高点当作标志点。

由上述步骤，可以得到测量的标志点在雷达坐标系下的坐标。

点云预处理标志点提取算法的主要流程如下：

最终得到的T_3D即转换矩阵。

3 参数标定实例及误差分析

3.1 标定实例

使用差分GPS测得标志点及车载IMU处的地理位置坐标，根据车载IMU给出的姿态角度，可以得出车载IMU坐标系下各标志点的三维坐标。

根据上述的最小二乘法，可得到车载惯性坐标系转换到车载激光雷达坐标系的转换矩阵，将计算得出的结果与激光雷达获得的点云坐标进行比对，其中x、y、z的坐标值误差如表1所示。

由上述计算结果可知，误差在0.3 m范围内，距离误差的平均值为0.167 m，误差的标准差为0.068 8 m。最小二乘法计算结果与实际测量坐标对比如图4所示。

根据三维点云拟合的ICP算法，得到的x、y、z点云坐标与测量坐标的误差如表2所示。

由上述计算结果，误差在0.4 m范围内，距离误差的平均值为0.197 m，误差的标准差为0.092 m，拟合效果较好。点云匹配方法计算结果与实际测量坐标对比如图5所示。

3.2 实验结论

使用3D点云匹配的方式能够较好地估算两个坐标系之间的坐标转换矩阵，与最小二乘法标定得出的结果基本一致，说明点云匹配的标定方法是有效的，这能够为激光雷达/IMU的参数联合标定提供一种新的思路。

4 结论

本文设计利用角反射器作为标志物的车载激光雷达与IMU坐标系的标定方法，通过区域分割、地面滤除、标志点提取的方法提取标志点，借鉴3D点云匹配算法的思路，将激光雷达与IMU坐标系标定问题转换为点云匹配问题，通过ICP算法迭代求解两者之间坐标变换矩阵，并与最小二乘法结果对比，结果基本一致，说明基于点云匹配思想的标定方法是有效的。

参考文献

[1] KRABILL W B，WRIGHT C W，SWIFT R N.Airborne laser mapping of assateague national seashore beach[J].Photogrammetic Engineering and Remote Sensing，2000，66(1)：65-71.

[2] 程金龙，冯莹，曹毓，等.车载激光雷达外参数的标定方法[J].光电工程，2013，40(12)：89-94.

[3] 韩正勇，卜春光，刘宸.一种针孔相机与三维激光雷达外参标定方法[J].传感器与微系统，2018，37(4)：9-12，16.

[4] 韩栋斌，徐友春，王任栋，等.基于多对点云匹配的三维激光雷达外参数标定[J].激光与光电子学进展，2018，55(2)：455-462.

[5] 熊烁，叶伯生，蒋明.机器人工具坐标系标定算法研究[J].机械与电子，2012(6)：60-63.

[6] 余和青，刘文波，邓钊波，等.最小二乘法视觉系统标定研究[J].日用电器，2014(12)：61-64.

[7] 段建民，王昶人，任璐，等.基于多层激光雷达的可行驶区域信息提取算法[J].电子技术应用，2017，43(10)：78-82.

[8] 王建中，杨璐.基于GPS定向测姿系统的研究[J].电子技术应用，2016，42(9)：14-18.

[9] 蒋从元，杨杰.基于分布式多跳误差估计目标位置感知算法[J].电子技术应用，2017，43(11)：95-98.

[10] 赵立峰.整体最小二乘法及其在坐标转换中的应用[J].林业科技情报，2011，43(4)：114-115.

[11] 杨仕平，范东明，龙玉春.基于整体最小二乘法的任意旋转角度三维坐标转换[J].大地测量与地球动力学，2013，33(2)：114-119.

[12] 韩明瑞.基于激光LiDAR的室外移动机器人三维定位与建图[D].南京：东南大学，2016.

[13] ZHANG Z.Iterative point matching for registration of free-form curves and surfaces[J].International Journal of Computer Vision，1994，13(2)：119-152.

[14] MINGUEZ J，MONTESANO L，LAMIRAUX F.Metric-based iterative closest point scan matching for sensor displacement estimation[J].IEEE Transactions on Robotics，2006，22(5)：1048-1054.

[15] BIBER P，STRASSER W. The normal distributions transform：a new approach to laser scan matching[C].Proceedings of 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems，2003(IROS 2003)，2003，3：2743-2748.

[16] 沈海平，达飞鹏，雷家勇.基于最小二乘法的点云数据拼接研究[J].中国图象图形学报，2005(9)：1112-1116.

[17] 郑忠阳.基于车载激光雷达的点云配准算法及应用技术研究[D].长沙：国防科学技术大学，2015.

作者信息:

吴昱晗，王蕴宝，薛庆全，郄晓斌，李志强

(北京航天发射技术研究所，北京100076)