点云配准方法和装置与流程

本申请涉及自动驾驶领域，尤其涉及一种点云配准方法和装置。

背景技术：

一种实现自动驾驶的方法是通过周边环境的地图确定车辆的当前位置。例如，车辆通过激光雷达等传感器获取实时地图后，将该实时地图与预设地图进行配准，从而确定了车辆的当前位置。

实时地图与预设地图通常都是以点云(point cloud)的形式存在，点云是三维物体或三维场景的一种表现形式，是由空间中一组无规则分布的、表达三维物体或三维场景的空间结构和表面属性的离散点所构成。而点云配准，就是求两个点云之间的旋转平移矩阵，将源点云(例如，实时地图)变换到与目标点云(例如，预设地图)相同的坐标系下，使得两个点云中的点云数据之间的距离经过点云位姿变换后达到最小值。

常用的点云配准方法是迭代最近点(iterative closest point，ICP)算法，ICP算法的基本思想是寻找两个点云的几何特征之间的关联关系，通过迭代不同的关联关系确定优选的旋转平移矩阵。然而，上述方法仅利用了几何特征，对于几何结构重复性较强的场景，该算法的鲁棒性不足。

技术实现要素：

本申请提供了一种点云配准方法和装置，在几何结构重复性较强的场景中也具备较好的效果，具有较强的鲁棒性。

第一方面，提供了一种点云配准方法，包括：获取第一点云和第二点云；对第一点云和第二点云进行粗配准，确定第二点云相对于第一点云的初始位姿；根据初始位姿、第一点云的几何特征和语义特征、以及第二点云的几何特征和语义特征对第一点云和第二点云进行精配准，确定第二点云相对于第一点云的最终位姿。

第一点云例如是预设点云，第二点云例如是实时点云，本申请对第一点云和第二点云具体内容不做限定。上述配准方法除了利用点云的几何特征进行配准外，还利用点云的语义特征进行配准，在几何结构重复性较强的场景中也具备较好的效果。例如，当第一点云和第二点云均包含形状相同的圆形点云块时，点云配准装置可以根据这两个圆形点云块均表示电力井盖将这两个圆形点云块匹配在一起，或者，点云配准装置可以根据这两个圆形点云块分别表示污水井盖和电力井盖避免将这两个圆形点云块匹配在一起。因此，上述方案在几何结构重复性较强的场景中也具有较强的鲁棒性。

可选地，所述根据初始位姿、第一点云的几何特征和语义特征、以及第二点云的几何特征和语义特征对第一点云和第二点云进行精配准，确定第二点云相对于第一点云的最终位姿，包括：构建关于位姿变量的似然函数，其中，所述位姿变量用于指示第二点云相对于第一点云的多种位姿，似然函数用于指示多种位姿的概率，似然函数包括几何变量和语义变量，几何变量用于指示第一点云的几何特征和第二点云的几何特征之间的关联关系，语义变量用于指示第一点云的语义特征和第二点云的语义特征之间的关联关系，几何变量为观测变量，语义变量为隐变量，位姿变量的初始值为初始位姿的值；根据迭代算法确定似然函数的极大值；根据所述极大值确定最终位姿，其中，所述极大值对应的位姿变量所指示的位姿为最终位姿。

在一些复杂场景中，由于语义特征不够明确，第一点云和第二点云存在多种可能的位姿，因此，可以利用概率模型处理点云配准问题。但是，配准模块难以直接通过语义特征和几何特征配准两个点云。例如，传感器直接获得的数据可以被称为观测变量，即，能够被测量的随机变量；几何特征由点云的形状所表征，为观测变量，由于几何特征是连续变量，因此能够直接被概率模型处理；而语义特征本质上是一种离散变量，难以直接使用概率模型进行处理。因此，可以将语义特征作为一种隐变量输入概率模型，使得配准模块能够通过概率模型处理语义特征和几何特征，并估计两个点云的真实位姿。

可选地，所述根据迭代算法确定似然函数的极大值，包括：根据似然函数的对数函数、几何变量和位姿变量i确定Q函数i，Q函数i为第i次迭代时语义变量的条件概率分布的期望，位姿变量i为位姿变量在第i次迭代时的估计值，i为正整数；根据Q函数i确定位姿变量i+1，位姿变量i+1为位姿变量在第i+1次迭代时的估计值，并且，位姿变量i+1为Q函数i取极大值时位姿变量的取值；根据位姿变量i和位姿变量i+1确定似然函数的极大值，其中，当位姿变量i和位姿变量i+1的差值的绝对值小于或等于第一阈值时，或者，当Q函数i和位姿变量i+1对应的Q函数的差值的绝对值小于或等于第二阈值时，似然函数的值为极大值。

迭代算法需要一个初始值，从该初始值开始迭代，直至求出似然函数的极大值。似然函数的极大值即位姿变量的多个取值中概率较大的取值，该取值对应的位姿为第一点云和第二点云存在多种可能的位姿中最有可能的位姿。

可选地，似然函数还包括第二点云的协方差，该协方差的极大似然估计值为似然函数取极大值时对应的协方差的值。

点云的协方差由生成点云的设备或者方法决定，这个协方差矩阵可以通过算法进行估计，对于未估计过协方差的设备或方法，其生成的点云(如，第二点云)的协方差是未知量，需要参与迭代算法的处理，以使得点云配准的结果更准确。

可选地，所述对第一点云和第二点云进行粗配准，包括：根据第一点云的几何特征和语义特征以及第二点云的几何特征和语义特征对第一点云和第二点云进行粗配准。

对于一些几何结构重复性较强的场景，若粗配准阶段仅基于几何特征进行配准，则可能发生将第一点云和第二点云中几何特征相似但语义特征不同的点云块匹配在一起的情况，后续精配准阶段无论如何也无法得到正确的配准结果。因此，本申请提供的方案，在粗配准阶段即根据第一点云的语义特征和第一点云的语义特征进行配准，可以避免上述问题的出现。

可选地，所述根据第一点云的几何特征和语义特征以及第二点云的几何特征和语义特征对第一点云和所述第二点云进行粗配准，包括：确定第一点云的中心和第一点云的多个点云块的中心，其中，第一点云的多个点云块中每个点云块包含的点云点具有相同的语义；确定第二点云的中心和第二点云的多个点云块的中心，其中，第二点云的多个点云块中每个点云块包含的点云点具有相同的语义；配准第一点云的中心和第二点云的中心；配准第一点云的多个点云块的中心和第二点云的多个点云块的中心，确定语义粗配准后的第一点云和第二点云；根据几何配准算法对所述语义粗配准后的第一点云和第二点云进行几何粗配准。

点云块是点云的组成部分，其包含大量具有相同含义的点。例如，地图生成模块生成了包括汽车和路标的点云，则表示汽车的点称为一个点云块，表示路标的点成为另一个点云块。点云配准模块可以先配准第一点云的中心和第二点云的中心，再配准第一点云中的点云块和第二点云中的点云块的中心，这样，可以避免在粗配准阶段将不同语义的点云块匹配在一起。

第二方面，提供了另一种点云配准方法，包括：获取第一点云和第二点云；根据第一点云的几何特征和语义特征以及第二点云的几何特征和语义特征对第一点云和第二点云进行粗配准，确定第二点云相对于第一点云的初始位姿；根据初始位姿对第一点云和第二点云进行精配准，确定第二点云相对于第一点云的最终位姿。

第一点云例如是预设点云，第二点云例如是实时点云，本申请对第一点云和第二点云具体内容不做限定。对于一些几何结构重复性较强的场景，若粗配准阶段仅基于几何特征进行配准，则可能发生将第一点云和第二点云中几何特征相似但语义特征不同的点云块匹配在一起的情况，后续精配准阶段无论如何也无法得到正确的配准结果。因此，本申请提供的方案，在粗配准阶段即根据第一点云的语义特征和第一点云的语义特征进行配准，可以避免上述问题的出现。

可选地，所述根据第一点云的几何特征和语义特征以及第二点云的几何特征和语义特征对第一点云和第二点云进行粗配准，包括：确定第一点云的中心和第一点云的多个点云块的中心，其中，第一点云的多个点云块中每个点云块包含的点云点具有相同的语义；确定第二点云的中心和第二点云的多个点云块的中心，其中，第二点云的多个点云块中每个点云块包含的点云点具有相同的语义；配准第一点云的中心和第二点云的中心；配准第一点云的多个点云块的中心和第二点云的多个点云块的中心，确定语义粗配准后的第一点云和第二点云；根据几何配准算法对语义粗配准后的第一点云和第二点云进行几何粗配准。

点云块是点云的组成部分，其包含大量具有相同含义的点。例如，地图生成模块生成了包括汽车和路标的点云，则表示汽车的点称为一个点云块，表示路标的点成为另一个点云块。点云配准模块可以先配准第一点云的中心和第二点云的中心，再配准第一点云中的点云块和第二点云中的点云块的中心，这样，可以避免在粗配准阶段将不同语义的点云块匹配在一起。

可选地，所述根据初始位姿对第一点云和第二点云进行精配准，确定第二点云相对于第一点云的最终位姿，包括：根据初始位姿、第一点云的几何特征和语义特征、以及第二点云的几何特征和语义特征对第一点云和第二点云进行精配准，确定第二点云相对于第一点云的最终位姿。

根据本申请提供的方案，在精配准阶段，除了利用点云的几何特征进行配准外，还利用点云的语义特征进行配准，在几何结构重复性较强的场景中也具备较好的效果。例如，当第一点云和第二点云均包含形状相同的圆形点云块时，点云配准装置可以根据这两个圆形点云块均表示电力井盖将这两个圆形点云块匹配在一起，或者，点云配准装置可以根据这两个圆形点云块分别表示污水井盖和电力井盖避免将这两个圆形点云块匹配在一起。因此，上述方案在几何结构重复性较强的场景中也具有较强的鲁棒性。

可选地，所述根据初始位姿、第一点云的几何特征和语义特征、以及第二点云的几何特征和语义特征对第一点云和第二点云进行精配准，确定第二点云相对于第一点云的最终位姿，包括：构建关于位姿变量的似然函数，其中，位姿变量用于指示第二点云相对于第一点云的多种位姿，似然函数用于指示多种位姿的概率，似然函数包括几何变量和语义变量，几何变量用于指示第一点云的几何特征和第二点云的几何特征之间的关联关系，语义变量用于指示第一点云的语义特征和第二点云的语义特征之间的关联关系，几何变量为观测变量，语义变量为隐变量，位姿变量的初始值为初始位姿的值；根据迭代算法确定似然函数的极大值；根据极大值确定最终位姿，其中，极大值对应的位姿变量所指示的位姿为最终位姿。

在一些复杂场景中，由于语义特征不够明确，第一点云和第二点云存在多种可能的位姿，因此，可以利用概率模型处理点云配准问题。但是，配准模块难以直接通过语义特征和几何特征配准两个点云。例如，传感器直接获得的数据可以被称为观测变量，即，能够被测量的随机变量；几何特征由点云的形状所表征，为观测变量，由于几何特征是连续变量，因此能够直接被概率模型处理；而语义特征本质上是一种离散变量，难以直接使用概率模型进行处理。因此，可以将语义特征作为一种隐变量输入概率模型，使得配准模块能够通过概率模型处理语义特征和几何特征，并估计两个点云的真实位姿。

可选地，所述根据迭代算法确定似然函数的极大值，包括：根据似然函数的对数函数、几何变量和位姿变量i确定Q函数i，Q函数i为第i次迭代时语义变量的条件概率分布的期望，位姿变量i为位姿变量在第i次迭代时的估计值，i为正整数；根据Q函数i确定位姿变量i+1，位姿变量i+1为位姿变量在第i+1次迭代时的估计值，并且，位姿变量i+1为Q函数i取极大值时位姿变量的取值；根据位姿变量i和位姿变量i+1确定似然函数的极大值，其中，当位姿变量i和位姿变量i+1的差值的绝对值小于或等于第一阈值时，或者，当Q函数i和位姿变量i+1对应的Q函数的差值的绝对值小于或等于第二阈值时，似然函数的值为极大值。

迭代算法需要一个初始值，从该初始值开始迭代，直至求出似然函数的极大值。似然函数的极大值即位姿变量的多个取值中概率较大的取值，该取值对应的位姿为第一点云和第二点云存在多种可能的位姿中最有可能的位姿。

可选地，似然函数还包括第二点云的协方差，该协方差的极大似然估计值为似然函数取极大值时对应的协方差的值。

点云的协方差由生成点云的设备或者方法决定，这个协方差矩阵可以通过算法进行估计，对于未估计过协方差的设备或方法，其生成的点云(如，第二点云)的协方差是未知量，需要参与迭代算法的处理，以使得点云配准的结果更准确。

第三方面，本申请提供了一种点云配准装置，该装置可以是终端设备或服务器，也可以是芯片。该装置可以包括输入模块、粗配准模块和精配准模块。当该装置是终端设备或服务器时，粗配准模块和精配准模块可以是处理器，输入模块可以是通信接口；终端设备或服务器还可以包括存储模块，该存储模块可以是存储器；该存储模块用于存储指令，粗配准模块和精配准模块执行该存储模块所存储的指令，以使终端设备或服务器执行第一方面所述的方法。当该装置是芯片时，粗配准模块和精配准模块可以是集成于芯片上的处理器，输入模块可以是输入/输出接口、管脚或电路等；粗配准模块和精配准模块执行存储模块所存储的指令，以使芯片执行第一方面所述的方法，该存储模块可以是该芯片内的存储单元(例如，寄存器、缓存等)，也可以是位于该芯片外部的存储单元(例如，只读存储器、随机存取存储器等)。

第四方面，本申请提供了一种点云配准装置，该装置可以是终端设备或服务器，也可以是芯片。该装置可以包括输入模块、粗配准模块和精配准模块。当该装置是终端设备或服务器时，粗配准模块和精配准模块可以是处理器，输入模块可以是通信接口；终端设备或服务器还可以包括存储模块，该存储模块可以是存储器；该存储模块用于存储指令，粗配准模块和精配准模块执行该存储模块所存储的指令，以使终端设备或服务器执行第二方面所述的方法。当该装置是芯片时，粗配准模块和精配准模块可以是集成于芯片上的处理器，输入模块可以是输入/输出接口、管脚或电路等；粗配准模块和精配准模块执行存储模块所存储的指令，以使芯片执行第二方面所述的方法，该存储模块可以是该芯片内的存储单元(例如，寄存器、缓存等)，也可以是位于该芯片外部的存储单元(例如，只读存储器、随机存取存储器等)。

第五方面，本申请提供了一种计算机可读存储介质，该计算机可读存储介质中存储了计算机程序，该计算机程序被处理器执行时，使得处理器执行第一方面所述的方法。

第六方面，本申请提供了一种计算机可读存储介质，该计算机可读存储介质中存储了计算机程序，该计算机程序被处理器执行时，使得处理器执行第二方面所述的方法。

第七方面，本申请提供了一种计算机程序产品，该计算机程序产品包括：计算机程序代码，当该计算机程序代码被处理器运行时，使得处理器执行第一方面所述的方法。

第八方面，本申请提供了一种计算机程序产品，该计算机程序产品包括：计算机程序代码，当该计算机程序代码被处理器运行时，使得处理器执行第二方面所述的方法。

附图说明

图1是本申请提供的一种自动驾驶系统的示意图；

图2是本申请提供的一种配准装置的示意图；

图3是本申请提供的一种基于点云的几何特征和语义特征进行配准的方法的示意图；

图4是本申请提供的另一种基于点云的几何特征和语义特征进行配准的方法的示意图；

图5是本申请提供的两个待配准点云的示意图；

图6是本申请提供的经过粗配准后点云的示意图；

图7是本申请提供的另一种点云配准装置的示意图。

具体实施方式

下面将结合附图，对本申请中的技术方案进行描述。

图1是本申请提供的一种自动驾驶系统的示意图。

该自动驾驶系统包括传感器模块、地图生成模块和配准模块。传感器模块例如可以是视觉-深度(RGBD)传感器、激光雷达或相机，其中，激光雷达生成的原始数据即点云，RGBD传感器和相机生成的原始数据为图像，可以通过地图生成模块将图像转换为点云。

由于传感器模块生成的原始数据不包含语义，为了更好地完成配准，需要通过地图生成模块对原始数据进行处理，生成包含语义的地图，即，语义点云。语义可以被解释为数据所对应的现实世界中的事物所代表的含义以及这些含义之间的关系，例如，电力管道井盖和下水管道井盖在语义点云中的表现形式均为圆形，则该两个圆形的在几何特征上的含义相同，但语义不同。将点云数据的语义以抽象的形式表现出来的结果即语义特征，语义特征通常以标签的形式表征，属于离散变量。在可视化表示中，不同的语义特征可以以不同的颜色加以区分。地图生成模块可以通过机器学习等方法为点云中的数据添加语义特征。

配准模块用于配准实时地图和预设地图，从而获取自车位姿。

配准模块包括两个如图2所示的子模块：粗配准模块和精配准模块。其中，粗配准模块用于基于点云的语义和拓扑关系对两个点云进行初步配准，确定实时点云相对于预设点云的初始位姿；精配准模块用于基于点云的语义特征和几何特征对两个点云进行进一步地配准，确定实时点云相对于预设点云的最终位姿。

为了提高点云配准方法的鲁棒性，尤其是在几何结构重复性较强的场景中的鲁棒性，本申请提供了一种利用点云的几何特征和语义特征对点云进行配准的方法。该方法可以应用于图1所示的自动驾驶系统，配准模块获取两个点云后，首先通过粗配准模块对该两个点云进行粗配准，确定实时点云相对于预设点云的初始位姿；随后，通过精配准模块在初始位姿的基础上对两个点云进行精配准，确定实时点云相对于预设点云的最终位姿。实时点云配准完成后，自动驾驶系统即可根据自车当前在预设地图中的位置确定自车位姿。

图3示出了一种本申请提供的基于点云的几何特征和语义特征进行配准的方法。

左侧的点云为预设点云，其中，行驶方向为创建预设点云的车辆的行驶方向。右侧的点云为实时点云，其中，行驶方向为自车的行驶方向。可见，当行驶方向相反时，相同语义的点云的相对位置会出现较大偏差。当全球导航卫星系统(global navigation satellite system，GNSS)失效仅依赖基于地图的定位时，如果行驶方向与地图方向相反，并且仅依赖几何特征进行配准，对于同样是圆形的井盖，配准模块在粗配准阶段就会将电力管道井和下水管道井配准到一起。这样，后续无论如何都无法得到两个点云的真实位姿。

按照本申请提供的配准方法，配准模块在粗配准阶段首先识别出两个点云的语义特征，根据语义特征将实时点云旋转180度，再在精配准阶段基于几何特征微调两个点云的相对位姿，使得两个点云的重合度达到最大，从而完成了点云的配准，与现有技术相比具有较强的鲁棒性。

上述实施方式仅是本申请的一个可选的示例。在一些复杂场景中，由于语义特征不明确，两个点云之间存在多种可能的位姿，因此，可以利用概率模型处理点云配准问题。但是，配准模块难以直接通过语义特征和几何特征配准两个点云。例如，传感器直接获得的数据可以被称为观测变量(observable variable)，即，能够被测量的随机变量；几何特征由点云的形状所表征，为观测变量，由于几何特征是连续变量，因此能够直接被概率模型处理；而语义特征本质上是一种离散变量，难以直接使用概率模型进行处理。因此，本申请将语义特征作为一种隐变量(hidden variable)输入概率模型，使得配准模块能够通过概率模型处理语义特征和几何特征，并估计两个点云的真实位姿。

极大似然估计法是一种能够同时处理观测变量和隐变量的概率模型，极大似然估计法的原理是：概率最大的事件最有可能发生。例如，若一次试验存在n个可能的结果A1,A2,...,An，在一次试验完成后，试验的结果为Ai，那么就可以认为事件Ai在这个n个可能结果中出现的概率最大。

因此，可以构建关于位姿变量的似然函数，位姿变量用于指示实时点云相对于预设点云的多种位姿，将该多种位姿对应的位姿变量分别输入似然函数得到的值即该多种位姿中各个位姿为真实位姿的概率，因此，似然函数用于指示多种位姿的概率，求出使得该似然函数取极大值的位姿变量，该极大值对应的位姿变量即为两个点云之间的真实位姿(或“最终位姿”)。

上述似然函数包括几何变量和语义变量，几何变量用于指示预设点云的几何特征和实时点云的几何特征之间的关联关系，语义变量用于指示预设点云的语义特征和实时点云的语义特征之间的关联关系。

包含多个变量的似然函数没有解析解，可以通过迭代算法求解。迭代算法需要一个初始值，从该初始值开始迭代，直至求出似然函数的极大值。点云配准模块可以将两个点云的初始位姿的值作为似然函数的位姿变量的初始值，下文将详细描述如何获得两个点云的初始位姿。

图4示出了本申请提供的一种点云配准方法的流程图。该方法包括：

S410，参数初始化。

该步骤可以由地图生成模块执行。地图生成模块获取传感器生成的原始数据后，可以通过机器学习方法为点云中的数据添加语义。

例如，地图生成模块获取激光雷达生成的点云后，利用深度学习算法为点云的各个部分赋予语义；或者，地图生成模块获取相机生成的图像后，利用深度学习算法对图像进行像素级语义分割，并将像素映射到3维空间形成包含语义的点云。

由于生成点云的设备或者方法存在一定的误差，因此，点云中每个点的位置具有不确定性。若点云中每个点的位置分布服从高斯分布，那么位置的真值就是高斯分布的均值。点云的协方差由生成点云的设备或者方法决定，这个协方差矩阵可以通过算法进行估计，对于已估计过协方差的设备或方法，其生成的点云的协方差可以看作已知量而不参与后续的优化计算处理。

S420，粗配准。

粗配准可以被解释为：使两个点云的重合度达到预设要求的配准方法。即，粗配准就是找到一个变换关系，使得点云变换后重合度最大，重合度以变换后符合距离条件的匹配点的个数决定。

可选地，在粗配准阶段，首先根据语义将两个点云相同语义的部分配准，在此基础上使用利用点云的几何特征继续配准，例如，可以利用通用性较好的四点全等(4point congruent，4PC)算法进行配准。

图5示出了两个待配准的点云。点云中不同的点的集合通过不同的形状表示，而不同的语义则通过不同的填充内容表示。例如，三角形表示一个点集合，矩形表示另外一个点集合；左斜线表示一种语义，右斜线表示另一种语义。点云中的点集合也可称为点云块。

根据语义对两个点云进行粗配准时，先确定两个点云的整体中心以及各个具有相同语义的点云块的中心。随后，配准两个点云的整体中心，再依次配准各个具有相同语义的点云块的中心，使得两个点云的所有中心的距离误差项最小即可。上述距离误差项指的是两类差值的和，其中，第一类差值为实时点云的中心与预设点云的中心的坐标差值，第二类差值为实时点云和预设点云中具有相同语义的点云块的中心的坐标差值。

随后，配准模块可以通过ICP算法的粗配准方法(例如，4PC算法)进行配准，得到位姿变量的初始值，即，确定了如图6所示的初始位姿。图6中，两个实心圆的中心的坐标差值即第一类差值，五对具有相同语义的点云块的中心的差值即第二类差值，距离误差项指的是该六个差值的和。

由上述粗配准示例可以看出，基于语义特征的粗配准方法能够确定最优解的位置和梯度下降方向，如，图5中两个点云各自具有两个矩形点云块，基于语义特征的粗配准能够避免将语义不同的矩形点云块匹配在一起，从而能够确定全局最优解的附近的初始估计值。

粗配准完成后，还需要通过精配准进一步提高点云配准的精确度。

S430，精配准。

精配准可以被解释为：在粗配准的基础上进一步提高点云配准的精确度的配准方法。本申请采用概率模型对点云进行精配准。即构建关于位姿变量的似然函数，并求该似然函数的极大值。本申请给出了通过最大期望(expectation maximization，EM)算法求解似然函数的极大值的示例，但求似然函数的极大值的方法不限于此，其它迭代算法同样能够确定似然函数的极大值。

通过EM算法进行精配准可分为四个步骤：a)构建似然函数；b)进行E步骤；c)进行M步骤；d)判断位姿变量是否达到迭代终止条件。

下面将详细介绍这四个步骤。

a)构建似然函数。

似然函数可以描述为：点云匹配点对的语义关联(语义特征的关联关系)和几何关联(几何特征的关联关系)在给定位姿以及点云协方差下的概率分布连乘结果。

本质上，在EM算法中，语义关联的概率分布作为隐变量，E步骤通过计算语义关联的后验概率分布来确定给定位姿变量和点云协方差条件下的似然函数的下确界，而M步骤则在这个下确界的基础上逐步逼近真实值，使得位姿变量收敛。

首先介绍相关变量的定义。

假设两个点云的点均服从高斯分布，即，

其中，P表示一个点云，Pi表示点云P中的一个点，Q表示另一个点云，Qi表示点云Q中的一个点，表示均值，z表示点云协方差，∑p、∑q表示协方差。

粗配准后可得匹配点对Di∈D,Di＝{di,si}，其中di表示几何关联，si表示语义关联。

对于几何关联，有下式成立。

其中，ζ表示位姿变量的李代数表示方法，exp(ζ^∧)表示李代数的指数映射，T表示矩阵的转置。

对于语义关联，使用语义标签l描述不同的语义，有后续会使用概率模型来量化这种关联关系。

定义匹配点对的概率分布模型为

p(Di|ζ,z)＝p(di,si|ζ,z) (3)

定义似然函数为

为便于求解，对似然函数取对数处理，得

假设已知点云中共有k类语义，因此可将上式改写为

不妨设函数Qi(sij)，利用詹森不等式(Jensen inequality)，有下式成立

该不等式取到等号的条件为

其中c为常数。

对该式变形并连加后可以得到取等号条件下的函数表达式Qi(sij)，如下式所示

Qi(sij)即第i次迭代时似然函数的Q函数。

b)进行E步骤。

计算Qi(sij)＝p(sij|ζ,z,di)。即固定位姿变量ζ和点云协方差z，利用K维索引树(K-dimension tree)求解几何关联di，并求语义关联的后验概率p(sij|ζ,z,di)。

在这一步中引入局部邻域概念。取几何匹配点对di，其邻域满足：最多取Pi的Nd个邻域点；以Pi为中心，邻域点到中心的最大距离为Rd。

令语义关联数据sij表示Qi对应的语义lQi关联于Pi局部邻域中的第j个语义假设局部邻域中语义分布服从高斯分布，则

p(sij|ζ,z,di)～N(μs,ij,∑s,ij) (9)

在Pi局部邻域中找到语义关联sij的第j个语义点云块，其中心作为μs,ij，μs,ij为语义关联的概率分布均值，其协方差∑s,ij则与第j个语义点云块的形状和大小有关，将Pi代入即可求得各类语义关联的概率。

这里需要注意，若局部邻域内无某类语义的点云，则令p(sij|ζ,z,di)＝0。对Pi处所有的语义数据关联后验概率，取归一化的比例ts,i，有

则p(sij|ζ,z,di)＝ts,i·p(sij|ζ,z,di)，并相应地更新均值和协方差。

至此可得权重归一化后的Qi(sij)。

c)进行M步骤。

求解极大似然函数

公式(11)由公式(7)推导出。当公式(7)中的大于或等于号取等号时，将对数函数的分子和分母分解即可推导出公式(11)。其中，ωij＝Qi(sij)。即固定语义数据关联sij，以权重ωij的形式引入似然函数，求解ζ以及z的极大似然。

对于联合分布p(di,sij|ζ,z)，有p(di,sij|ζ,z)～N(μuni,ij,∑uni,ij)，其均值和协方差由下式得到

μuni,ij为联合分布p(di,sij|ζ,z)的均值，∑uni,ij为联合分布p(di,sij|ζ,z)的协方差，μd,i为几何关联的概率分布均值，μs,ij为语义关联的概率分布均值，∑d表示几何关联的协方差，∑s,ij表示语义关联的协方差，∑d,(s,ij)表示几何关联与语义关联的协方差，∑(s,ij),d表示语义关联与几何关联的协方差。

结合E步骤中得到的权重ωij，可利用最大似然估计(maximize likelihood estimation，MLE)的方法得到位姿变量ζ的估计值，进而完成一次迭代。对于上文所述的极大似然函数，通常的求解方法有：求导法，对位姿变量求导，进而得到位姿变量的最优估计值；或者，转换为最小二乘问题，利用高斯-牛顿(Gauss-Newton，GN)算法或列文伯格-马夸尔特(Levenberg-Marquardt，LM)算法等方法求解位姿变量的最优估计值。

d)判断位姿变量是否达到迭代终止条件。

若第i次迭代时的位姿变量和第i+1次迭代时的位姿变量的差值的绝对值小于或等于第一阈值，或者，若第i次迭代时的Q函数和第i+1次迭代时的Q函数的差值的绝对值小于或等于第二阈值，则可以确定似然函数的值为极大值，并停止迭代，输出第i次迭代时的位姿变量。

若位姿变量或Q函数不满足上述条件，则继续迭代，即，继续执行E步骤和M步骤。

S440，输出位姿变量。

该位姿变量即旋转平移矩阵，实时点云乘以该旋转平移矩阵即可得到配准后的点云，即，确定了实时点云相对于预设点云的最终位姿。

上文详细介绍了本申请提供的点云配准方法的示例。可以理解的是，点云配准装置为了实现上述功能，其包含了执行各个功能相应的硬件结构和/或软件模块。本领域技术人员应该很容易意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，本申请能够以硬件或硬件和计算机软件的结合形式来实现。某个功能究竟以硬件还是计算机软件驱动硬件的方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

本申请可以根据上述方法示例对点云配准装置进行功能单元的划分，例如，可以将各个功能划分为各个功能单元，也可以将两个或两个以上的功能集成在一个处理单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。需要说明的是，本申请中对单元的划分是示意性的，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。

图7示出了本申请提供的一种点云配准装置的结构示意图。点云配准装置700可用于实现上述方法实施例中描述的方法。该点云配准装置700可以是芯片、服务器或终端设备。

点云配准装置700包括一个或多个处理器701，该一个或多个处理器701可支持点云配准装置700实现图3或图4所对应方法实施例中的方法。处理器701可以是通用处理器或者专用处理器。例如，处理器701可以是中央处理器(central processing unit，CPU)、数字信号处理器(digital signal processor，DSP)、专用集成电路(application specific integrated circuit，ASIC)、现场可编程门阵列(field programmable gate array，FPGA)或者其它可编程逻辑器件，例如，分立门、晶体管逻辑器件或分立硬件组件。处理器701可以用于对点云配准装置进行控制，执行软件程序，处理软件程序的数据。

点云配准装置700还可以包括输入模块，用以实现信号的输入(接收)。可选地，装置700还可以包括输入模块，用以实现信号的输出(发送)。例如，通信模块705可以是输入/输出电路，或者，通信模块705可以是通信接口。

点云配准装置700中可以包括一个或多个存储器702，其中存有程序704，程序704可被处理器701运行，生成指令703，使得处理器701根据指令703执行上述方法实施例中描述的方法。可选地，存储器702中还可以存储有数据。可选地，处理器701还可以读取存储器702中存储的数据，该数据可以与程序704存储在相同的存储地址，该数据也可以与程序704存储在不同的存储地址。

处理器701和存储器702可以单独设置，也可以集成在一起，例如，集成在单板或者系统级芯片(system on chip，SOC)上。

在一种可能的设计中，处理器701用于通过通信模块705获取第一点云和第二点云；随后，对所述第一点云和所述第二点云进行粗配准，确定所述第二点云相对于所述第一点云的初始位姿；随后，根据所述初始位姿、所述第一点云的几何特征和语义特征、以及所述第二点云的几何特征和语义特征对所述第一点云和所述第二点云进行精配准，确定所述第二点云相对于所述第一点云的最终位姿。

在另一种可能的设计中，处理器701用于通过通信模块705获取第一点云和第二点云；随后，根据所述第一点云的几何特征和语义特征以及所述第二点云的几何特征和语义特征对所述第一点云和所述第二点云进行粗配准，确定所述第二点云相对于所述第一点云的初始位姿；随后，根据所述初始位姿对所述第一点云和所述第二点云进行精配准，确定所述第二点云相对于所述第一点云的最终位姿。

对第一点云和第二点云进行配准的具体方法可以参见上述方法实施例中的相关描述。方法实施例的各步骤可以通过处理器701中的硬件形式的逻辑电路或者软件形式的指令完成。

本申请还提供了一种计算机程序产品，该计算机程序产品被处理器701执行时实现本申请中任一方法实施例所述的点云配准方法。

该计算机程序产品可以存储在存储器702中，例如是程序704，程序704经过预处理、编译、汇编和链接等处理过程最终被转换为能够被处理器701执行的可执行目标文件。

本申请还提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被计算机执行时实现本申请中任一方法实施例所述的点云配准方法。该计算机程序可以是高级语言程序，也可以是可执行目标程序。

该计算机可读存储介质例如是存储器702。存储器702可以是易失性存储器或非易失性存储器，或者，存储器702可以同时包括易失性存储器和非易失性存储器。其中，非易失性存储器可以是只读存储器(read-only memory，ROM)、可编程只读存储器(programmable ROM，PROM)、可擦除可编程只读存储器(erasable PROM，EPROM)、电可擦除可编程只读存储器(electrically EPROM，EEPROM)或闪存。易失性存储器可以是随机存取存储器(random access memory，RAM)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(static RAM，SRAM)、动态随机存取存储器(dynamic RAM，DRAM)、同步动态随机存取存储器(synchronous DRAM，SDRAM)、双倍数据速率同步动态随机存取存储器(double data rate SDRAM，DDR SDRAM)、增强型同步动态随机存取存储器(enhanced SDRAM，ESDRAM)、同步连接动态随机存取存储器(synchlink DRAM，SLDRAM)和直接内存总线随机存取存储器(direct rambus RAM，DR RAM)。

本领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的系统、装置和模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的方法实施例的一些特征可以忽略，或不执行。以上所描述的装置实施例仅仅是示意性的，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，多个单元或组件可以结合或者可以集成到另一个系统。另外，各单元之间的耦合或各个组件之间的耦合可以是直接耦合，也可以是间接耦合，上述耦合包括电的、机械的或其它形式的连接。

应理解，在本申请的各种实施例中，各过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请的实施例的实施过程构成任何限定。

另外，本文中的术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。

总之，以上所述仅为本申请技术方案的较佳实施例而已，并非用于限定本申请的保护范围。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。