自动驾驶圈黑话：常用的点云配准方法以及未来发展方向

随着激光雷达、4D毫米波雷达逐渐成为汽车领域的顶流，其输出的点云(Point Cloud)也成为继像素之后描述车辆所处三维世界的一种主要数据格式。点云其实就是一个数据集，不同类型传感器输出的点云包含的数据略有差异。对于激光雷达来说，其输出的点云一般包括三维空间坐标（x，y，z）、反射强度信息、时间戳等数据。
 

但是，受传感器视场角限制，单个传感器往往只能获得有限视野范围内的点云，如何才能生成完整场景的三维点云，以便支持后续的物体识别、分类及路径规划？自动驾驶车辆预先存储了某一区域的高精度点云地图，如何通过车辆传感器实时获取的点云确定车辆当前的位置信息？在初始姿态信息已知，如何通过车辆传感器采集的前后两帧点云估计相对姿态变化信息？

  点云配准作为点云处理整个Pipeline中的重要一环，便是解决上述问题的良方妙药。自动驾驶圈黑话第二十四期，我们就来科普一下什么是点云配准，点云配准的作用，常用的点云配准方法以及未来的发展方向。

    定义

  点云配准（Pont Cloud Registration）,又名点云拼接、点云注册，对于两帧有重叠信息的点云，通过求解变换矩阵（旋转矩阵R和平移矩阵T），使得重叠部分点云变换到同一个统一的坐标系下。

  点云配准史上经历了人工、半自动、自动配准阶段。人工配准，指的是工程师通过手眼调整两幅点云，直至达到人眼意义上的配准。半自动配准，是指依赖仪器进行配准。自动配准，是指利用一定的算法，自动完成两帧点云的配准，这也是本文所要重点介绍的内容。

  点云配准包含粗配（Coarse Registration）准和精配准（Fine Registration）两步。粗配准指的是在两帧点云位置相差较大，相对位姿完全未知的情况下进行较为粗糙的配准，目的是为后续精配准提供较好的变换初值。精配准在给定初始变换矩阵条件下，进一步优化得到更精确的变换。

    作用

  地图构建。在高精地图制作环节中，制作点云地图是第一步。通过对不同位置采集的连续帧点云进行配准，可以将不同位置的多帧点云统一到同一坐标系，构建场景的完整点云地图。

  高精定位。对于自动驾驶来说，获取准确的定位是一切丰满理想实现的前提。刨去Tesla，其他绝大多数自动驾驶公司都将高精地图作为实现准确定位的必备武器。而通过点云配准技术，可以将车辆传感器实时扫描到的点云与已经预存的高精点云地图进行匹配，并将得到的定位结果与GNSS输出、IMU输出、里程计输出等融合，实现车辆最终高精定位结果输出。

  姿态估计。通过对车辆传感器实时获得的前后两帧点云进行配准，可以估计出车辆的相对姿态信息。

    方法

    一、ICP

  ICP（Iterative Closest Point，迭代最近点）应该是最为经典的配准方法之一，从1992年P.J.Besl提出，到今天（2022年），已经整整三十年，但ICP的光芒却丝毫未减。从最早的点到点ICP，发展出了点到线ICP，点到面ICP及面到面ICP等方法，共同奠定ICP方法在点云配准领域的划时代的意义。

  最早的点到点ICP方法的核心思想很简单：空间位置上同一个点在两帧点云中距离是最近的，利用平移旋转变换后，让这两帧点云在相同位置的坐标尽可能拉近甚至相等，以达到配准的效果。

  点到点ICP方法包含对应点搜索和变换估计两个阶段，对应点搜索是指在两帧点云中找到每个点的匹配点，变换估计就是利用对应关系来估计变换矩阵。这两个阶段将不断进行迭代，使得对应关系越来越准确，从而找到最佳的变换矩阵。 点到点ICP方法实现过程用数学语言可以描述为：

  （1）给定待配准点云PS，也就是当前传感器扫描到的点云，里面由n个三维点pi组成。同时我们有目标点云Pt，可以是预先建好的高精点云地图，也可以是其他位置传感器采集的但有重叠区域的点云，里面同样包含一系列三维点pj组成。

（2）而我们要做的就是寻找一个3x3旋转矩阵R和3x1平移矩阵t，使得下式取值最小。

（3）采用四元数法或SVD方法求解上R和t的值。

  一步到位寻找两帧点云最近点，需要将待配准点云中每个点依次和目标点云中每个点进行距离计算，不仅计算复杂度高，而且计算耗时久。因此通常设置距离阈值来加速这一过程，当待配准点云中的一个点与目标点云中的一个点距离小于一定阈值时就认为找到了对应点，不再遍历待配准点云和目标点云中每个点。

  经过一次迭代，调整一些对应点对的权重以及剔除一些不合理的对应点对，我们可以得到一个临时变换点云。通过计算代价函数，我们便可以得到临时变换点云和目标点云对应的最优变换。然后利用这个临时点云和目标点云再进行比较，再得到一个变换点云及最优变换，不断迭代这一过程，直到找到待配准点云中每一个点在目标点云中的最近点及全局最优变换。

  形象化的配准过程如下图所示。在给定良好初值的两帧点云中，通过不断迭代，待配准点云沿箭头指向方向不断向目标点云靠近，并最终实现目标点云和待配准点云的配准。
 

点到点ICP方法以简单著称，不需要对点云进行分割和特征提取，不需要训练数据，可以较好的推广到未知场景。且在初值较好情况下，严密的数学理论可以保证较好的精度和收敛性。

  但是点到点ICP方法遍历所有点云，在点云数量较大时，计算量是惊人的。同时点到点ICP方法只考虑点与点的距离，缺少对点云特征信息的利用，在所处环境不断变化以及测量误差影响下，会产生较大的配准误差。而最致命的问题是，点对点ICP方法初始值敏感，容易陷入局部最优，导致配准失败。
  当上图在初值不好的情况下，如下图所示。按照对应点距离最小原则，待配准点云将按箭头指示顺时针方向旋转。这种情况虽然会导致两边对应点距离变远，但是刨去两边的剩余大部分点距离在减少。这也就是点到点ICP方法因初值不好带来的局部最优问题。
 

而为了解决遍历点云中所有点引入的高计算量，点到点ICP方法引入特征点的概念。特征点是指点云中具有代表性的点，能够尽可能代表点云的有效空间信息，比如拐角点，凸凹分界处的点等。通过对点云做特征点提取，然后再使用点到点ICP方法，可以显著减少运算量，保持较高的精度。

  而为了优化局部最优问题，出现了一些ICP改进算法，包括点到面ICP，面到面ICP等。

  点到面ICP考虑待配准点云顶点到目标点云顶点所在面的距离，比起直接计算点到点距离，考虑了目标点云的局部结构，精度更高，不容易陷入局部最优。但是点到面ICP是一个非线性问题，速度比较慢，效率较低。

  面到面ICP考虑待配准点云顶点所在的面和目标点云顶点所在面的距离，同时考虑了待配准点云和目标点云的局部结构，精度更高，更不易陷入局部最优。但是同样作为一个非线性优化问题，速度更慢，效率更低。

  因此点到点ICP方法常用于粗配准完成，已有较好初值的精配准阶段。

  二、NDT

  NDT（Normal Distribution Transform，正态分布变换），将目标点云（可以是高精点云地图）转换为多维变量的正态分布，并采用最优化技术寻找最优变换，使得待配准点云经过变换之后在目标点云中的概率密度之和最大。

  NDT方法实现过程用数学语言可以描述为：

  （1）将目标点云划分为一个个规格的网格(对于2D点云来说，是2D平面；对于3D点云来说，是3D立方体)，同时保证每个网格中最少包含6个点。

  （2）假设每个网格中的点均服从正态分布，并计算每个网格中点的均值向量q和协方差矩阵C。

  （3）基于以上信息，对网格中每个点X进行正态分布建模N(q，C)，我们便可以得到这个网格单元中每个点的高斯概率密度函数P(X)。

 

式P(X)中，c为归一化常数。同时我们看到本式中对协方差矩阵C求逆，如果单元格中的点数太少会导致协方差矩阵条件数过大而导致误差过大，故上文要求单元格至少包含六个点。同时每个P(X) 可以看作是单元格表面的近似表达，描述了单元格表面的位置以及它的方向和平滑度。
  随后我们会给定一个初始变换矩阵，将待配准点云变换并贴近目标点云，这样一来待配准点云上的点就会落在目标点云划分的网格单元中。以一个网格单元举例，将待配准点云中落入这个网格单元中的点Y带入上文的P(X)中，可得到这个点在这个网格单元中的概率值，这个值是一个似然值。
  同时为了减少格子离散化，不连续的影响，我们对这个网格单元分别向下、向左、向下后再向左平移，这样我们可得到另外三个网络单元。而这个点在另外三个网格单元中分别会有一个概率值，将这四个值加起来，作为待配准点云这个点总的得分（Score）。
 

落在目标点云网格单元中的每个点都有一个Score值，将所有Score值相乘就是我们的目标函数了，而我们最终的目的就是找到最优的参数使得目标函数得分最大（最大似然），那我们也便认为配准了。可以理解为，在目标点云中点出现概率大的地方，待配准点云应尽量占据这些位置。
  NDT方法通过网格划分技术，可以支持更大、更稠密的点云地图配准。且在配准过程中不利用对应点的特征计算和匹配，所以相比ICP效率更高，计算资源消耗更少。且当需要配准的两帧点云重叠率低，并且结构化不明显（平面较少）时，NDP比点ICP更加准确。

  三、深度学习

  深度学习在自动驾驶领域可谓无往不利，点云配准领域也不例外。基于深度学习的点云配准算法不断被提出，包括PointNetLK、DCP、IDAM、RPM-Net、3DRegNet等，且这些深度学习模型在实验室中已经证明性能与速度上均远超ICP和NDP方法。而在复杂的实际场景效果如何，值得我们持续关注。

      初值获取

    对于ICP来说，初值的好坏在很大程度上决定了两帧点云配准的结果。对于NDT来说，虽说对初值不敏感（两帧点云位置3m以内，角度+/-45°范围），但良好的初值有利于提高配置的效率和质量。不同的车辆配置决定了不同的初值获取方法。对于只有激光雷达配置的车辆，可以采用上一帧的变换结果作为初值。
  对于配置有IMU和里程计的车型，IMU的加速度测量比较准，积分出来的速度和位移会有累计误差。里程计的速度测量比较准，而根据运动模型计算出来的加速度会有一定误差。因此针对此种配置，使用IMU的加速度测量结果和里程计的速度测量结果融合一个初值，会是非常可靠的一个结果。

  对于配置GNSS的车型，如果GNSS还能输出航向角，那么直接用GNSS的输出作为初值。对于无法输出航向角的GNSS,通过连续的位置坐标，也可以计算一个平面运动的航向角。

    展望

    单一传感器的点云配准已有成熟的算法，但可以看到，每种方法都存在局限性，无法满足自动驾驶全场景的点云配准需求，如何取长补短成为单一传感器点云配准发展的重要道路。

  随着多传感器融合成为主旋律，不同类别传感器获取的点云进行配准就显得重要且必须。但由于这方面的研究刚刚起步，仅有一些实验室阶段的优化策略和深层次的神经网络模型，如何训练出可以上路的神经网络模型，值得万众期待。
   

编辑：黄飞