https://www.zhihu.com/video/1577313722581811200
作者：自动驾驶人 | 原文出处： 公众号【自动驾驶专栏】
激光雷达建图算法
建图算法比里程计算法以更低的频率运行，并且每次sweep仅调用一次。在第次sweep结束的时候，激光雷达里程计生成了去畸变的点云，并且同时计算得到一个位姿变换，它为激光雷达在时间段内的运动。建图算法将点云与世界坐标系{}下地图进行匹配和配准，如下图所示。
为了解释这一过程，定义为地图中的点云，它随着每一次sweep而累积，并且记为第次sweep结束的时候激光雷达相对于地图的位姿。在获取激光雷达里程计算法输出的位姿后，建图算法通过到这段时间内的sweep和来计算，并且将点云投影到世界坐标系{}下，记为。下一步，建图算法通过优化激光雷达位姿来匹配和地图点云。特征点提取方式和里程计算法相同，只是需要使用10倍数量的特征点。为了寻找特征点的匹配关系，使用边长为10米的立方体区域来存储地图中的点云，将与有交集的立方体中的点云提取出来并且存储在3D的KD树中。建图算法在特征点周围的一定区域内寻找中的点，记为找到的周围点集。对于一个边缘特征点，仅需要存储中边缘线上的点；对于一个平面特征点，仅需要存储中平面小块上的点。接着，计算的协方差矩阵，以及的特征值和特征向量，分别记为和。如果分布在一条边缘线上，则的一个特征值远比另外两个特征值大，并且最大特征值在中关联的特征向量表示该边缘线的方向。另一方面，如果分布在一个平面小块上，则包含两个较大的特征值和第三个远小的特征值，并且最小特征值在中关联的特征向量表示该平面小块的法向量。边缘线或者平面小块的位置穿过的几何中心。为了计算每个特征点与其对应关系的距离，选择边缘线上的两个点和平面小块上的三个点，距离的计算公式与激光雷达里程计算法相同。与里程计算法不同的是，中所有点的时间戳相同。非线性优化问题再次通过Levenberg-Marquardt方法进行鲁棒拟合求解，使用求解得到的位姿将更新到地图上。为了使得点云均匀分布，地图点云使用边长为5cm的立方体的体素网格滤波器进行下采样。
位姿变换融合如上图所示。蓝色区域表示激光雷达建图算法输出的位姿变换，它每一个sweep输出一次。橘黄色区域表示激光雷达里程计算法输出的位姿变换，它以10Hz的频率运行。激光雷达相对于地图的位姿是这两种位姿变换的组合，其频率与激光雷达里程计相同。
实验
在本实验中，处理激光雷达数据的算法运行在2.5GHz主频和6Gib内存的电脑上，电脑里的Linux系统安装了机器人操作系统（ROS）。本文提出的方法需要两个核运行，里程计和建图程序分别运行在这两个分开的核上，该软件代码和数据集是公开的。
A.室内和室外实验
本文提出的方法在室内和室外环境下测试。在室内实验中，激光雷达和电池以及笔记本电脑一起放置在一辆手推车上，手推车由一个人推着走。下图中的图(a)和图(c)展示了两个有代表性的室内环境（狭窄长走廊和大厅）建立的地图，图(b)和图(d)分别展示了相同场景所拍摄的两张照片。在室外实验中，激光雷达被安装在地面车辆的前方。图(e)和图(g)展示了植被道路和旁边有两排树的果园环境下建立的地图，图(f)和图(h)分别展示了相同场景所拍摄的照片。在所有的测试中，激光雷达以0.5m/s的速度移动。
为了评估地图的局部精度，在相同环境下采集第二组激光雷达点云。激光雷达在数据采集的过程中保持固定并且放置在每个环境中的一些不同位置处。将之前采集的点云数据和这第二组点云数据使用点到平面的ICP方法进行匹配和比较。在匹配完成以后，前一组点云和第二组点云对应平面小块的距离被认为是匹配误差。下图展示了误差分布密度，它表明室内环境下的匹配误差要小于室外环境下的匹配误差。这个结果是合理的，因为自然环境下的特征匹配不如人造场景下的特征匹配那么精确。
除此之外，本文对运动估计的累积漂移进行测试，测试环境选择包含闭环的走廊作为室内测试环境，这使得实验能够在相同位置开始和结束，起始和结束位置之间由运动估计产生的偏差代表了运动估计漂移的大小。对于室外环境，本文选择果园作为测试环境，携带激光雷达的地面车辆上安装了一个高精度的GPS/INS装置用于获取运动轨迹的真值。测量得到的真值和估计值之间的偏差相对于行驶距离作为相对精度衡量，如下表所示。一般而言，室内测试的相对精度大约为1%，室外测试的相对精度大约为2.5%。
B.IMU辅助
本实验给激光雷达安装了Xsens MTi-10 IMU来处理较大速度改变的情况。与之前不同的是，点云在发送给本文提出的方法之前，先进行以下两步预处理：1）有了IMU(陀螺仪)提供的旋转之后，在一次sweep中接收到的点云被旋转对齐到这次sweep的初始旋转姿态；2）有了IMU（加速度计）提供的加速度测量后，点云的运动畸变可以被部分消除，就好像激光雷达在这次sweep中恒速运动那样。经过这两步点云预处理后，将点云发送给激光雷达里程计和建图程序进行处理。上述第一步预处理过程中，IMU旋转是用卡尔曼滤波器融合陀螺仪的角速度以及加速度计的读数来获得的。下图图(a)展示了一个实验结果，该实验中由人手持激光雷达并且在楼梯上移动。当计算图中红色轨迹曲线的时候，使用IMU提供的旋转，本文提出的方法只用来估计平移量，在5分钟的数据采集过程中角度漂移超过25°。下图中绿色曲线仅依赖于本文提出的方法，不依赖于IMU。下图中蓝色曲线通过IMU数据进行预处理，然后再用本文提出的方法估计运动，通过实验发现绿色和蓝色曲线存在微小差别。下图图(b)展示了蓝色曲线对应的地图，图(c)比较了图(b)中黄色矩形框内的两个地图的近视图。图(c)中上下两幅图分别对应于蓝色和绿色曲线，仔细比较后发现上面那幅图中构建的轮廓更清晰（说明运动估计精度高）。
下表比较了使用和没有使用IMU进行运动估计的相对误差。激光雷达由一个步速为0.5m/s的人手持移动，并且激光雷达上下移动的幅度大约为0.5m，该实验中真值通过卷尺手动测量。在所有4个实验中，本文提出的方法加上IMU辅助，获得了最高的精度，然而只使用IMU提供的旋转则精度最低。实验结果表明IMU在消除非线性运动方面非常有效，本文提出的方法可以处理线性运动（因此两者结合精度非常高）。
C.KITTI数据集测试
本研究也使用KITTI里程计数据集来验证所提出的方法，该数据集由一辆在结构化道路行驶的乘用车（下图图(a)）上安装的传感器来采集。该车辆装载了一个360°的Velodyne激光雷达，彩色/单色双目相机和用于获取真值的高精度GPS/INS。激光雷达帧率为10Hz，本文提出的方法使用它来估计里程计。由于篇幅限制，这里就不展示结果了，然而鼓励读者从数据集网站上去查看本文的实验结果。
KITTI数据集主要覆盖了三种类型的环境：带有建筑物的市区，场景中带有植被的小路和道路宽阔且周围环境相对干净的高速公路。上图图(b)显示了一个激光雷达点云示例以及对应的市区环境的照片。数据集包含的整体行驶距离为39.2公里，在上传车辆运动轨迹之后，数据集服务器会自动计算精度和排名。本文提出的方法按照数据集服务器评估后在所有方法中排名第一，这些方法不考虑传感器的形式，其中包括目前最好的双目视觉里程计。本方法的平均位置误差为行驶距离的0.88%，这是通过在100米、200米、...、800米长度的轨迹段来生成的。
总结和展望
使用来自旋转激光雷达的点云进行运动估计和建图是很困难的，因为该问题包含了运动恢复以及激光雷达点云的运动畸变校正。本文提出的方法将该问题划分为两个并行的算法进行求解：激光雷达里程计算法以高频粗略地估计速度，同时激光建图算法以低频构建精细的地图，这两种算法的组合能够实时地进行运动估计和建图。进一步，本文提出的方法能够利用激光扫描模式和点云分布的优势进行特征提取，并且特征匹配在里程计算法中具有较快的计算速度，在建图算法中有足够的精度。最后，本方法在室内、室外以及KITTI里程计数据集上进行了测试和验证。 由于当前的方法没有识别回环，未来工作中可以引入回环来校正运动估计漂移，同时也可以通过卡尔曼滤波器融合本方法估计的位姿和IMU数据来进一步减少运动估计漂移。
