激光感知算法--多激光雷达的静态标定

0 前言

本文主要记录多激光雷达与车辆的静态联合标定，使用的方法为最小方差法和点云配准，需要做的准备工作有：1）安装coudcompare软件；2）寻找一个合适的场景（地面和平面平整，推荐地下车库）；3）测距仪器；4）标定板或者具有明显特征的物体（方便寻找匹配的点对）；5）车技熟练的老司机（准确停放车辆）。如有不对的地方，欢迎各位指出~。

1 摘要

目的：实现6个激光雷达与车辆坐标系的联合标定。方法：首先采用最小方差标定法标定128线激光雷达，完成128线激光雷达与自车坐标系的标定工作；其次，根据128线激光雷达的标定结果，将该激光雷达的点云数据转换至自车坐标系，作为其余激光雷达标定的参考点云；最后，通过点云配准的方法实现目标激光点云与参考点云的标定，得到目标点云（待标定激光雷达）与参考点云（自车坐标系）的变换矩阵H。结论：可成功实现多激光雷达与车辆坐标系的标定要求，但标定精度比较依赖最小方差法主激光雷达与车辆坐标系标定的精度。

2 方法介绍

2.1 主激光雷达与自车坐标系标定 – 最小方差法

本项目中主激光雷达采用的是禾塞128线旋转式激光雷达，安装与车辆的正上方。最小方差法的核心在于寻找一个合适的场景，利用RANSAC，GPF等方法提取出地面点和墙面点，通过调整roll,pitch,yaw等参数使得对应的地面（墙面）的点云方差最小，此时对应的roll,pitch,yaw就是当前主激光雷达（128线）与车辆坐标系的误差角。为了准确的进行自车与128线激光雷达的标定，首先需要寻找一个合适的场景。将车辆面朝墙面，车身与墙面保持垂直关系，所选场景的墙面与地面尽可能平整。本次实验选取的场景为地下车库，通过调整车辆位置，使车辆两侧的固定位置到墙面的距离一致，距离的确定可使用激光测距仪，卷尺等测量工具，以及一个车技熟练的老司机。

（1）地面与墙面点提取

本次实验采用地面平面拟合方法（Ground Plane Fitting, GPF）对场景进行地面以及墙面点云进行提取。为了提高提取的精度，可根据场景点云的分布手动对点云进行分割，选取合适的ROI以筛选候选点云（点云数据预处理，手动去噪点）。
本文会对GPF算法做一个简要的介绍，关于地面平面拟合方法的具体原理可参考该博客：基于地面平面拟合的激光雷达地面分割方法

简单来说，GPF算法步骤主要分为以下几步：

1.种子点集选取：对点云数据按高度值（z）进行排序，选出高度最低的NLPR个点<NLPR:选取点云的个数>，并对这些点的高度值求平均值，该平均值称为最低点代表（Lowest Point Representative,LPR），设定种子点高度阈值Thseed，如果点云的高度值(z)小于LPR+Thseed，那么该点记为种子点。种子点集主要用于建立描述地面的初始平面模型；

2.平面模型：设置线性$ax+by+cz+d=0$用于平面模型估计，对上一步选取好的种子点集进行均值和协方差矩阵计算（这一步可使用PCL中的pcl::computeMeanAndCovarianceMatrix()函数实现），接着进行奇异值分解（SVD）,具有最小奇异值的奇异向量即为我们所要获取的平面的法向量n。将法向量代入线性模型$ax+by+cz+d=0$中即可求解参数d，到这一步，平面模型的所有参数已知，设定点到平面的距离阈值为Thdist - d，当点云到我们拟合平面的正交投影距离小于该阈值时，认为该点属于地面点，反之属于非地面点。

3.优化平面主循环：设置迭代次数Niter，得到初始地面模型后，计算每一个点到该地面模型的正交投影距离，并将该距离和设定的平面距离阈值Thdist - d对比，属于地面点的点云会被挑选为下一次迭代的种子点集，进行迭代优化。

需要注意的是，本文中需要提取地面点和墙面点，方法均为GPF，只是在选取种子点集和计算的时候，对于地面点，是根据z值进行排序，挑选z值最小的N个点；对于墙面点则是根据x值排序，挑选x值最大的N个点（坐标系正方向为前x，左y，上z）。

（2）最小方差标定法

通过地面以及墙面点云的提取，可以分别计算点云的误差。其中，激光雷达的pitch角和roll角（俯仰和横滚）会影响高度值，因此可以通过计算地面点云的高度方差，通过角度补偿的方式找到一个合适的pitch和roll角使得地面上的点云方差最小。当误差值接近为0，此时该pitch和roll即为当前激光雷达相对车身坐标系的误差角。同理可得：当墙面点云纵向距离方差的值接近为0时，对应的yaw角即为当前激光雷达相对车身坐标系的误差角。

标定步骤：

1. 采集标定场景数据；
2. 播放录制的bag数据包（亦可在线实时标定）;
3. 启动激光雷达驱动，获取激光雷达点云数据；
4. 启动最小方差标定程序节点（对应的代码后续会单独说明）;
5. 观察误差结果，根据误差结果手动输入参数值，参数值的调整为原参数加上当前误差结果，直至误差均接近为0;
6. 打开rviz校验点云：可通过拉直rviz的网格，使其与某个坐标轴平行，以此来检验标定的准确性。

2.2 多激光雷达联合标定

128线激光雷达经过最小方差标定法后得到相对自车的pitch,roll,yaw误差角,根据标定外参参数x,y,z即可将128线激光雷达点云转换至车身坐标系，其点云作为多激光雷达标定的参考点云。
本次多激光雷达的标定采用CloudCompare软件，实现点云由粗到精的配准，以获取待标定激光雷达到参考点云（车身坐标系）的变换关系。

2.2.1 点云粗配准

使用CloudCompare软件中的Tool->Registration->Align(point pairs picking)工具手动选取匹配点对，该步骤目的在于为后续点云精配准提供一个准确的初值。该工具允许用户通过在两个点云中选取至少三对等效点对来对齐两个点云。该方法对于精确对齐点云非常有用，尤其对于两个点云差异程度很大的情况（ICP,NDT等配准无法正常工作）。对齐的精度取决于配准点对选取的精度。为了获取准确的配准点对，数据采集过程中使用标定板作为激光雷达的共同特征，使待标定激光雷达和参考激光雷达同时完整扫描到标定板。
步骤如下：
（1） 使用CloudCompare打开参考点云和待标定点云的PCD文件;
（2） 点击Tool->Registration->Align(point pairs picking)，确定待标定点云和参考点云；
（3） 手动选取匹配的点对，如图2所示，选取共同的特征点，此处选取的匹配点对为标定板的3个角点；
（4） 点击align进行点云对齐，结果如图3所示，得到两个点云的变换矩阵H1及对齐后的点云。若结果符合预期，则点击ok，完成粗配准，得到旋转后的点云；反之，点击reset，重新选取匹配点对。
（5） 验证时，CC将显示一份报告（控制台中会显示相同的信息，单控制台显示的数字精度更高）。

图中，A1，A2，A3和R1，R2，R3为对应的点对。

2.2.2 点云精配准

精匹配使用的方法为cloudcompare软件内置的ICP算法。ICP算法的主要参数如下：

$$ICP参数设置$$

• Number of iteration: 迭代次数；
• RMS difference: 相邻迭代误差值；
• Final overlap: 设置待配准点云之间的重叠度；
• Adjust scale: 若待配准的两个点云尺寸不一致，可勾选此项;
• Random sampling limit: 随机子采样点的最大数量，为了提高计算速度；若不考虑时间，为了提高配准精度，只需将该值设置大于待配准的点云数即可;
• Rotation: 增加指定轴(X、Y或Z)旋转约束；
• Enable farthest point removal: 提出距离较远的点，对噪声点具有一定的抑制性；
• Use displayed model/data scalar field as wights: 此选项允许用户使用标量值作为权重（主要用于点云与模型的配准，不建议用于点云间配准）。
  ICP是一个迭代过程，在此过程中，配准误差（缓慢）减少。通过设置最大迭代次数或RMS difference为ICP算法的结束条件。
  步骤：
  （1）选取待配准的两个点云，利用CloudCompare工具对点云进行点云分割，选取具有明显特征的点云子集；

$$参考点云$$

$$待配准点云$$
（2）点云分割后，分别得到两个待配准点云的点云子集，对点云子集进行ICP配准，得到配准结果；选取点云子集的目的：1）降低配准点云数量，提高配准速度；2）降低噪声点，选取特征明显的点云进行配准，利于提高配准的准确性。

（3）完成配准后，Cloudcompare将会显示一份报告，此报告的结果待配准点云子集到参考点云子集的变换矩阵（粗配准后点云到参考点云的结果），控制台的结果为原始待配准点云到参考点云的变换矩阵（最终结果）。

$$多激光雷达点云数据标定前效果示意图$$

$$多激光雷达点云数据标定后效果示意图$$

3 总结

基于上述的最小方差法和使用cloudcompare软件点云配准方法可以有效的完成多激光雷达与车身坐标系的静态标定，但是精度受限于参考激光雷达和车身坐标系的标定精度（最小方差法的弊端），因为很难保证车辆与侧边的墙面平行，同时地面和墙面也并非完全的平整。后续会继续探索动态标定的相关方法。