CN117689698A - 点云配准方法、智能设备及存储介质 - Google Patents

Abstract

本申请公开的点云配准方法、智能设备及存储介质，涉及自动驾驶技术领域。该方法包括：获取源点云、目标点云以及源点云与目标点云之间的初始位姿；根据所述初始位姿将所述源点云转换到所述目标点云所在坐标系；基于所述目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数；基于所述代价函数计算雅克比矩阵和海森堡矩阵；基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量。本申请的点云配准方法基于源点云的高斯分布和目标点云的高斯分布，使用雅克比矩阵和海森堡矩阵实现整个非线性优化迭代过程，较大程度上降低了算力。

Description

点云配准方法、智能设备及存储介质

技术领域

本申请涉及自动驾驶技术领域，具体涉及一种点云配准方法、智能设备及存储介质。

背景技术

点云配准是求解两帧点云之间相对位姿的一种计算方法，可以根据匹配好的两帧点云计算获得两帧点云之间的相对位姿。根据计算过程中约束形式的不同，可以分为点对点的配准、点对分布(P2D：point to distribution))和分布对分布(D2D：distribution todistribution)的配准。不同约束形式的配准在位姿求解精度和算力上存在差异。这里的distribution一般表示为点云的分辨率格子内的点的高斯分布。其中D2D方法，考虑了两帧点云之间局部形状一致性的约束，精度会更高些，因此业内通常采用D2D形式的约束来迭代优化初始位姿，以此来提高点云配准的精度。

但是业内采用的D2D约束形式的点云配准方法，为了避免直接计算雅克比矩阵和海森堡矩阵，一般是采用第三方非线性优化库（比如Google开发的ceres）进行求导计算，同时ceres这种通用的非线性优化库，为了通用性，有一些额外的计算，因此导致使用ceres等第三方非线性优化库求解的算力一般比较大。

由此可见，现有技术的点云配准方法存在精度低或对算力要求较高的缺陷。

发明内容

为了克服现有技术中存在的缺陷，本申请提出了一种轻量级的、高精度的点云配准方案。

在第一方面，本申请提供一种点云配准方法，所述方法包括：

获取源点云、目标点云以及源点云与目标点云之间的初始位姿；

根据所述初始位姿将所述源点云转换到所述目标点云所在坐标系；

基于所述目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数；

基于所述代价函数计算雅克比矩阵和海森堡矩阵；

基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量。

优选地，所述根据所述初始相对位姿将所述源点云转换到所述目标点云所在坐标系，具体包括：

将源点云进行栅格划分得到多个源栅格，根据所述初始位姿，将所述源栅格投影到所述目标点云所在坐标系，并在目标点云中确定与所述源栅格对应的目标栅格。

优选地，所述基于所述目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数具体包括：

根据所述源栅格的高斯分布和所述目标栅格的高斯分布分别确定源栅格的协方差和目标栅格的协方差；

基于位姿参数构建所述源栅格与所述目标栅格的匹配关系函数；所述位姿参数包括旋转矩阵和平移向量；

根据所述匹配关系函数、所述源栅格的协方差和所述目标栅格的协方差构建关于位姿的代价函数。

优选地，所述基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量具体为：

基于所述雅克比矩阵和所述海森堡矩阵，利用牛顿法迭代求解所述代价函数直至满足收敛条件，获得位姿增量。

进一步地，所述迭代求解过程还包括：基于所述海森堡矩阵、所述雅克比矩阵，以及预先设定的匹配栅格点对噪声确定位姿协方差。

进一步地，所述迭代求解过程还包括：当不满足收敛条件时，判断所述位姿增量是否大于预设阈值；是则基于所述位姿增量更新所述初始位姿，并基于更新后的所述初始位姿对所述源点云和所述目标点云重新进行匹配；否则基于所述位姿增量重新计算海森堡矩阵和雅可比矩阵，并进行下一次迭代。

优选地，所述基于所述位姿增量重新计算海森堡矩阵和雅可比矩阵具体包括：

基于所述位姿增量更新高斯分布参数，所述高斯分布参数包括均值和方差；

基于更新后的高斯分布参数重新计算海森堡矩阵和雅克比矩阵。

优选地，所述方法还包括：判断所述海森堡矩阵是否符合正定矩阵，并在所述海森堡矩阵不符合正定矩阵时对所述海森堡矩阵进行修订的步骤。

进一步地，所述判断所述海森堡矩阵是否符合正定矩阵，并在所述海森堡矩阵不符合正定矩阵时对所述海森堡矩阵进行修正的步骤具体包括：

对所述海森堡矩阵进行奇异值分解获得多个奇异值，从所述多个奇异值中选定最小奇异值，判断所述最小奇异值小于预设值，是则所述海森堡矩阵为非正定矩阵，否则所述海森堡矩阵为正定矩阵；

当所述海森堡矩阵为非正定矩阵时，将所述多个奇异值中每个奇异值都增加所述预设值得到修改后的多个奇异值，并基于修改后的多个奇异值重新确定海森堡矩阵。

优选地，所述源点云具体为基于激光雷达采集获取的实时点云数据；所述目标点云具体为与所述源点云相同的实时点云数据形态，或者具体为划分为多个目标栅格的离线点云图像序列形态。

优选地，所述初始位姿具体为基于全球卫星导航系统采集的信号获得，或者是基于一个或多个车辆传感器采集的信号获得。

在第二方面，本申请提供一种智能设备，该智能设备包括处理器和存储装置，所述存储装置适于存储多条程序代码，所述程序代码适于由所述处理器加载并运行以执行上述点云配准方法的技术方案中任一项技术方案所述的点云配准方法。

在第三方面，本申请提供一种计算机可读存储介质，该计算机可读存储介质其中存储有多条程序代码，所述程序代码适于由处理器加载并运行以执行点云配准方法的技术方案中任一项技术方案所述的点云配准方法。

本申请上述一个或多个技术方案，至少具有如下一种或多种有益效果：在实施本申请的技术方案中，首先根据所述初始位姿将所述源点云转换到所述目标点云所在坐标系，然后基于目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数，基于所述代价函数计算雅克比矩阵和海森堡矩阵；基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量。本申请基于源点云的高斯分布和目标点云的高斯分布，使用雅克比矩阵和海森堡矩阵实现整个非线性优化迭代过程，较大程度上降低了算力。并且本申请的点云配准方法在位姿协方差计算中考虑了匹配栅格点对噪声，可以较为精确地描述位姿计算结果的不确定度。

附图说明

参照附图，本申请的公开内容将变得更易理解。本领域技术人员容易理解的是：这些附图仅仅用于说明的目的，而并非意在对本申请的保护范围组成限制。此外，图中类似的数字用以表示类似的部件，其中：

图1是根据本申请的一个实施例的点云配准方法的主要步骤流程示意图；

图2是图1中步骤S103的具体实现流程图；

图3是根据本申请的一个实施例的点云配准方法的具体实现步骤流程图；

图4是根据本申请的一个实施例的点云配准装置的组成框图示意图；

图5是基于本申请实施例的点云配准方法实现的效果示意图。

具体实施方式

下面参照附图来描述本申请的一些实施方式。本领域技术人员应当理解的是，这些实施方式仅仅用于解释本申请的技术原理，并非旨在限制本申请的保护范围。

在本申请的描述中，“模块”、“处理器”可以包括硬件、软件或者两者的组合。一个模块可以包括硬件电路，各种合适的感应器，通信端口，存储器，也可以包括软件部分，比如程序代码，也可以是软件和硬件的组合。处理器可以是中央处理器、微处理器、图像处理器、数字信号处理器或者其他任何合适的处理器。处理器具有数据和/或信号处理功能。处理器可以以软件方式实现、硬件方式实现或者二者结合方式实现。非暂时性的计算机可读存储介质包括任何合适的可存储程序代码的介质，比如磁碟、硬盘、光碟、闪存、只读存储器、随机存取存储器等等。术语“A和/或B”表示所有可能的A与B的组合，比如只是A、只是B或者A和B。术语“至少一个A或B”或者“A和B中的至少一个”含义与“A和/或B”类似，可以包括只是A、只是B或者A和B。单数形式的术语“一个”、“这个”也可以包含复数形式。

本申请各实施例中可能涉及的相关用户个人信息，均为严格按照法律法规的要求，遵循合法、正当、必要的原则，基于业务场景的合理目的，处理用户在使用产品/服务过程中主动提供或因使用产品/服务而产生的，以及经用户授权获取的个人信息。

本申请处理的用户个人信息会因具体产品/服务场景而有所不同，需以用户使用产品/服务的具体场景为准，可能会涉及用户的账号信息、设备信息、驾驶信息、车辆信息或其他相关信息。申请人会以高度的勤勉义务对待用户的个人信息及其处理。

本申请非常重视用户个人信息的安全，已采取符合业界标准、合理可行的安全防护措施保护用户的信息，防止个人信息遭到未经授权访问、公开披露、使用、修改、损坏或丢失。

下面首先对本申请实施例提供的点云配准方法的应用场景进行介绍。

本实施例提供的点云配准算法基于NDT算法（Normal Distribution Transform）框架实现，NDT算法是一种基于正态分布的点云配准方法。其原理是：将点云转换为高斯分布的函数形式，通过计算不同高斯分布函数之间的匹配来进行点云配准，例如输入是匹配好的两帧点云，输出是两帧之间的位姿。

NDT算法是一种在三维空间中进行点云匹配的算法，其可同时应用于点云数据的三维场景的构建和定位。在三维场景的构建阶段，NDT算法的主要思想是先将点云数据分割成多个小立方体，并计算每个小立方体的高斯分布参数，然后根据高斯分布参数实现三维场景的构建。在定位阶段，NDT算法使用高斯分布来描述点云的分布特性，通过最大化相似度指标来找到点云之间的最佳配准。

NDT算法的基本思想是首先确定源点云和目标点云在初始状态下的初始位姿，然后基于该初始位姿以及迭代算法，进行点云配准。然而，该方法对初始位姿较为依赖，若初始相对位姿存在较大误差，那么会存在点云配准精度较低的问题。为此业内通常采用分布对分布（D2D，distribution to distribution）形式的约束，即基于两帧点云之间局部形状一致性的约束迭代优化初始相对位姿，以此来提高点云配准的精度。但是业内采用的基于D2D约束的NDT算法一般是采用ceres这种通用的非线性优化库进自动求导，会涉及有一些额外的计算，因此导致求解的算力一般比较大。

因此，本申请实现了一套不依赖第三方非线性优化库的D2D位姿求解方法。它显示提供了D2D约束的基础矩阵即雅克比（Jacobi）矩阵和海森堡（Hessian）矩阵，并直接使用基础矩阵运算实现了整个非线性优化过程，较大程度上降低了算力。同时，给出了考虑匹配点对噪声的位姿协方差矩阵的计算方法，较为精确地描述了位姿计算结果的不确定度。

下面结合附图对本申请提供的点云配准方法的实施例进行详细说明。

参阅附图1，图1是根据本申请的一个实施例的点云配准方法的主要步骤流程示意图。如图1所示，本申请实施例中的点云配准方法主要包括下列步骤S101至步骤S104。

步骤S101：获取源点云、目标点云以及源点云与目标点云之间的初始位姿；

具体地，所述源点云和所述目标点云是待匹配的点云点对，所述源点云具体为基于激光雷达采集获取的实时点云数据；所述目标点云具体为与所述源点云相同的实时点云数据形态，或者具体为划分为多个目标栅格的离线点云图像序列形态。所述初始位姿具体为基于全球卫星导航系统采集的信号获得，或者是基于一个或多个车辆传感器采集的信号获得，实际应用中所述初始位姿还可以是自定义的初值。

可选的，本步骤的一种实施方式为，获取激光雷达采集的实时的点云数据作为所述源点云，获取由多个目标栅格组成的离线点云图像序列作为目标点云，以及基于全球卫星导航系统采集的信号获得所述源点云和所述目标点云之间相对的初始位姿。

步骤S102：根据所述初始位姿将所述源点云转换到所述目标点云所在坐标系；

具体地，源点云为基于激光雷达采集获取的点云数据，源点云的坐标系是激光雷达的坐标系，可选的，目标点云所在坐标系为世界坐标系。所述初始位姿包括旋转矩阵和平移向量，基于初始位姿通过坐标系转换可以将源点云转换到所述目标点云所在坐标系。

本步骤的一种实施方式为：将源点云进行栅格划分得到多个源栅格，根据所述初始位姿，将所述源栅格投影到所述目标点云所在坐标系，并在目标点云中确定与所述源栅格对应的目标栅格。具体地，根据所述源栅格与所述目标栅格之间的投影距离取最小匹配确定与之对应的目标栅格，所述源栅格与所述目标栅格形成匹配栅格点对。

步骤S103：基于所述目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数；

具体地，本实施例中假设所述目标点云所在坐标系下的所述源点云的源栅格和所述目标点云的目标栅格是正态分布的，则可以确定所述源栅格的高斯分布参数和所述目标栅格的高斯分布参数，所述高斯分布参数包括均值和方差，根据高斯分布参数可以确定高斯分布的表达式即概率密度函数，即获得所述源栅格的高斯分布和所述目标栅格的高斯分布。

本申请实施例实现点云配准的高斯分布计算以源栅格和目标栅格为匹配栅格点对进行计算，相比以栅格中的点为匹配点对进行的高斯分布计算，较大程度上降低了算力。

本步骤的一种实施方式如图2所示，包括如下步骤S1031至步骤S1033。

步骤S1031：根据所述源栅格的高斯分布和所述目标栅格的高斯分布分别确定源栅格的协方差和目标栅格的协方差；

具体地，根据所述源栅格的高斯分布参数可以计算得到源栅格的协方差，记作，根据所述目标栅格的高斯分布计算得到目标栅格的协方差，记作/>。

步骤S1032：基于位姿参数构建所述源栅格与所述目标栅格的匹配关系函数；所述位姿参数包括旋转矩阵和平移向量；

具体地，位姿参数的向量形式表示为p，旋转矩阵表示为R，平移向量表示为t，本实施例中基于位姿参数构建的所述匹配关系函数表示为；其中，i表示源栅格，j表示目标栅格，/>表示均值。

步骤S1033：根据所述匹配关系函数、所述源栅格的协方差和所述目标栅格的协方差构建关于位姿的代价函数。

具体地，构建的关于位姿的代价函数表示如下：

，

其中表示源栅格的个数，/>表示目标栅格的个数，/>、/>为常量，/>为源栅格的协方差，/>为目标栅格的协方差，/>为匹配关系函数。

步骤S104：基于所述代价函数计算雅克比矩阵和海森堡矩阵；

具体地，基于对所述代价函数通过求导获得梯度向量，基于所述梯度向量可以计算得到雅克比矩阵和海森堡矩阵。

步骤S105：基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量。

本步骤的一种实施方式为：基于所述雅克比矩阵和所述海森堡矩阵，利用牛顿法迭代求解所述代价函数直至满足收敛条件，获得位姿增量。例如，所述收敛条件为迭代次数达到预设次数，具体地所述预设次数可以设置为等于所述源栅格的个数。

优选地，所述迭代求解过程还包括：基于所述海森堡矩阵、所述雅克比矩阵，以及预先设定的匹配栅格点对噪声确定位姿协方差，相应的，当满足收敛条件时，可以同时获得位姿增量和位姿协方差。本申请实施例提供的位姿协方差的计算方法，考虑了匹配栅格点对噪声的影响，较为精确地描述了位姿计算结果的不确定度。

所述位姿协方差可以表示如下：

，

其中，p是位姿参数的向量表示，z是预先设定的匹配栅格点对噪声，是海森堡矩阵，/>是雅克比矩阵，/>是雅克比矩阵相对于z的导数；本申请实施例中，假设/>表示匹配栅格点对在某个维度的噪声，且/>是常量，选择/>，/>，/>为随机矩阵向量。

下面结合附图3对上述步骤S101至步骤S105进行详细介绍。

如图3所示，本实施例提供的一种点云配准方法，包括以下步骤201至步骤211。

步骤201：输入源点云、目标点云和源点云与目标点云的初始位姿；

具体地，所述源点云为激光雷达采集的实时的点云数据；所述目标点云为由多个目标栅格组成的离线点云图像序列；输入的所述初始位姿是通过对车辆的轮速传感器和惯性传感器（IMU）采集的数据进行数据融合获得。

步骤202：基于初始位姿，将源点云转换到目标点云所在坐标系，并分别计算源点云的源栅格和与之匹配的目标点云的目标栅格的高斯分布，根据高斯分布构建代价函数；

具体地，目标点云所在坐标系为世界坐标系，将源点云进行栅格划分得到多个源栅格，基于初始位姿将源点云转换到世界坐标系，并在目标点云中确定与源栅格匹配的目标栅格，所述源栅格与所述目标栅格形成匹配栅格点对，并将源点云和目标点云转换为NDT形态，即分布计算源栅格的高斯分布和目标栅格的高斯分布。

可以理解的是，基于高斯分布的概率密度函数可以构建代价函数。

步骤203：计算雅克比矩阵和海森堡矩阵；

具体地，基于所述代价函数进行线性方程函数迭代求解过程中可以计算获得雅克比矩阵和海森堡矩阵。

步骤204：判断所述海森堡矩阵是否满足正定矩阵，是则执行步骤206，否则执行步骤205；

具体地，对所述海森堡矩阵进行奇异值（SVD）分解可以获得多个奇异值，从所述多个奇异值中选定最小奇异值，判断所述最小奇异值是否小于1e-10，是则所述海森堡矩阵为非正定矩阵，否则所述海森堡矩阵为正定矩阵。

步骤205：对所述海森堡矩阵进行修正；

具体地，将所述多个奇异值中每个奇异值都增加所述预设值得到修改后的多个奇异值，并基于修改后的多个奇异值重新确定海森堡矩阵。

步骤206：基于所述雅克比矩阵和所述海森堡矩阵，利用牛顿法迭代求解所述代价函数；

步骤207：判断是否满足收敛条件，是则执行步骤211，否则执行步骤208；

例如，收敛条件设置为迭代次数达到预设次数。

步骤208：判断当前求解获得的位姿增量是否大于预设阈值，是则执行步骤209，否则执行步骤210；

步骤209：基于所述位姿增量更新所述初始位姿，然后返回执行步骤202；

步骤210：更新每次迭代的中间变量，基于所述位姿增量重新计算海森堡矩阵和雅克比矩阵，然后返回执行步骤204；

具体地，所述中间变量是指多次迭代计算过程中每迭代一次发生改变的一个矩阵变量，例如，迭代次数记作，则第一次迭代记作/>，该中间变量的初值为预先设定的数值，记作先验方差/>，经过一次迭代先验方差变为后验证方差，例如经过第次迭代后可以获得后验方差/>。

本实施例中所述中间变量的更新即从先验方差变为后验方差采用如下公式实现：

，

其中，为先验方差，/>表示海森堡矩阵，z表示预先设定的匹配栅格点对噪声，是对称矩阵，其转置的逆等于本身的逆；/>，/>。

步骤211：输出位姿增量和位姿协方差。

具体地，输出的位姿增量记为Δp，根据海森堡矩阵H和雅克比矩阵J可以获得该位姿增量：。

本实施例中，点云配准方法还输出位姿计算不确定度的描述即所述位姿协方差，以保证下游使用方能够判断当前结果的可信程度。这种不确定度一般使用一个6x6的协方差矩阵表达。先验技术中常用的描述方法是将点云配准过程中计算的海森堡矩阵取逆作为协方差矩阵，但是这种方法的缺点是，它仅仅考虑了点云配准方程的数值误差，没有考虑由于匹配点对的噪声导致的匹配点云误差引起的不确定度，因此该方法提供的描述准确度较低。而本申请的点云配准方法考虑了匹配点对的噪声即上述计算过程中的匹配栅格点对噪声z，因此，本申请实施例的点云配准方法可以对位姿计算的不确定度进行准确描述。

如图5所示是采用本申请实施例提供的点云配准方法进行点云配准可达到的效果示意图，如图所示，浅色图像部分用于表示的是协方差的三倍结果，深色图像部分用于表示的是位姿误差，即通过本申请方法获得的位姿结果与用于评估精度的真值之间的差值，通过图中深色部分和浅色部分的变化关系可以看出，采用本申请实施例提供的方法可以实现对位姿误差较为准确的描述。

进一步，本申请还提供了一种点云配准装置。

参阅附图4，图4是根据本申请的一个实施例的点云配置装置的主要结构框图，该装置包括输入模块301、迭代计算模块302和输出模块303。其中各个模块功能如下：

所述输入与匹配模块301，被配置为接收源点云、目标点云以及源点云与目标点云之间的初始位姿，并基于初始位置进行点云匹配。本实施例中所述输入模块301具体包括获取模块3011和转换模块3012，其中：

所述获取模块3011，具体用于获取源点云、目标点云以及源点云与目标点云之间的初始位置；

所述转换模块3012，具体用于根据所述初始位姿将所述源点云转换大所述目标点云所在坐标系。

所述迭代计算模块302，被配置为基于输入模块301获取的信息进行迭代计算以获得位姿增量。本实施例中所述迭代计算模块302具体包括第一计算模块3021和第二计算模块3022。

所述第一计算模块3021，具体用于基于所述目标点云所在坐标系下的所源点云的高斯分布和所述目标点云的高斯分布构建代价函数；

所述第二计算模块3022，具体用于基于所述代价函数计算雅克比矩阵和海森堡矩阵，并基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量。

所述输出模块303，被配置为输出所述迭代计算模块计算得到的位姿增量。

为了便于说明，上述点云配准装置的介绍仅示出了与本申请实施例相关的部分，具体技术细节未揭示的，请参照本申请实施例方法部分。

本领域技术人员能够理解的是，本申请实现上述一实施例的方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器、随机存取存储器、电载波信号、电信信号以及软件分发介质等。

进一步，本申请还提供了一种智能设备，所述智能设备可以包括至少一个处理器；以及，与至少一个处理器通信连接的存储器；其中，存储器中存储有计算机程序，计算机程序被至少一个处理器执行时实现上述任一实施例所述的方法。本申请所述的智能设备，可以包括驾驶设备、智能车、机器人等设备。

在本申请的一些实施例中，所述智能设备还包括至少一个传感器，所述传感器用于感知信息。所述传感器与本申请提到的任何一种类型的处理器通信连接。可选地，所述智能设备还包括自动驾驶系统，自动驾驶系统用于引导智能设备自行行驶或辅助驾驶。所述处理器与所述传感器和/或自动驾驶系统通信，用于完成上述任一实施例所述的方法。

在本申请实施例中智能设备可以是包括各种电子设备形成的控制装置设备。在一些可能的实施方式中，智能设备可以包括多个存储装置和多个处理器。而执行上述方法实施例的点云配准方法的程序可以被分割成多段子程序，每段子程序分别可以由处理器加载并运行以执行上述方法实施例的点云配准方法的不同步骤。具体地，每段子程序可以分别存储在不同的存储装置中，每个处理器可以被配置成用于执行一个或多个存储装置中的程序，以共同实现上述方法实施例的点云配准方法。

上述多个处理器可以是部署于同一个设备上的处理器，例如上述智能设备可以是由多个处理器组成的高性能设备，上述多个处理器可以是该高性能设备上配置的处理器。此外，上述多个处理器也可以是部署于不同设备上的处理器，例如上述智能设备可以是服务器集群，上述多个处理器可以是服务器集群中不同服务器上的处理器。

进一步，本申请还提供了一种计算机可读存储介质。

在根据本申请的一个计算机可读存储介质实施例中，计算机可读存储介质可以被配置成存储执行上述方法实施例的点云配准方法的程序，该程序可以由处理器加载并运行以实现上述点云配准方法。为了便于说明，仅示出了与本申请实施例相关的部分，具体技术细节未揭示的，请参照本申请实施例方法部分。该计算机可读存储介质可以是包括各种电子设备形成的存储装置设备，可选的，本申请实施例中计算机可读存储介质是非暂时性的计算机可读存储介质。

至此，已经结合附图所示的优选实施方式描述了本申请的技术方案，但是，本领域技术人员容易理解的是，本申请的保护范围显然不局限于这些具体实施方式。在不偏离本申请的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本申请的保护范围之内。

Claims (13)

1.一种点云配准方法，其特征在于，所述方法包括：

获取源点云、目标点云以及源点云与目标点云之间的初始位姿；

根据所述初始位姿将所述源点云转换到所述目标点云所在坐标系；

基于所述目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数；

基于所述代价函数计算雅克比矩阵和海森堡矩阵；

基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量。

2.根据权利要求1所述的方法，其特征在于，所述根据所述初始相对位姿将所述源点云转换到所述目标点云所在坐标系，具体包括：

将源点云进行栅格划分得到多个源栅格，根据所述初始位姿，将所述源栅格投影到所述目标点云所在坐标系，并在目标点云中确定与所述源栅格对应的目标栅格。

3.根据权利要求2所述的方法，其特征在于：所述基于所述目标点云所在坐标系下的所述源点云的高斯分布和所述目标点云的高斯分布构建代价函数具体包括：

根据所述源栅格的高斯分布和所述目标栅格的高斯分布分别确定源栅格的协方差和目标栅格的协方差；

基于位姿参数构建所述源栅格与所述目标栅格的匹配关系函数；所述位姿参数包括旋转矩阵和平移向量；

根据所述匹配关系函数、所述源栅格的协方差和所述目标栅格的协方差构建关于位姿的代价函数。

4.根据权利要求1所述的方法，其特征在于：所述基于所述雅克比矩阵和所述海森堡矩阵对所述代价函数进行优化迭代获得位姿增量具体为：

基于所述雅克比矩阵和所述海森堡矩阵，利用牛顿法迭代求解所述代价函数直至满足收敛条件，获得位姿增量。

5.根据权利要求4所述的方法，其特征在于：所述迭代求解过程还包括：基于所述海森堡矩阵、所述雅克比矩阵，以及预先设定的匹配栅格点对噪声确定位姿协方差。

6.根据权利要求4或5所述的方法，其特征在于，所述迭代求解过程还包括：

当不满足收敛条件时，判断所述位姿增量是否大于预设阈值；

是则基于所述位姿增量更新所述初始位姿，并基于更新后的所述初始位姿对所述源点云和所述目标点云重新进行匹配；

否则基于所述位姿增量重新计算海森堡矩阵和雅可比矩阵，并进行下一次迭代。

7.根据权利要求6所述的方法，其特征在于，所述基于所述位姿增量重新计算海森堡矩阵和雅可比矩阵具体包括：

基于所述位姿增量更新高斯分布参数，所述高斯分布参数包括均值和方差；

基于更新后的高斯分布参数重新计算海森堡矩阵和雅克比矩阵。

8.根据权利要求1所述的方法，其特征在于，所述方法还包括：判断所述海森堡矩阵是否符合正定矩阵，并在所述海森堡矩阵不符合正定矩阵时对所述海森堡矩阵进行修订的步骤。

9.根据权利要求8所述的方法，其特征在于，所述判断所述海森堡矩阵是否符合正定矩阵，并在所述海森堡矩阵不符合正定矩阵时对所述海森堡矩阵进行修正的步骤具体包括：

对所述海森堡矩阵进行奇异值分解获得多个奇异值，从所述多个奇异值中选定最小奇异值，判断所述最小奇异值小于预设值，是则所述海森堡矩阵为非正定矩阵，否则所述海森堡矩阵为正定矩阵；

当所述海森堡矩阵为非正定矩阵时，将所述多个奇异值中每个奇异值都增加所述预设值得到修改后的多个奇异值，并基于修改后的多个奇异值重新确定海森堡矩阵。

10.根据权利要求1所述的方法，其特征在于：所述源点云具体为基于激光雷达采集获取的实时点云数据；所述目标点云具体为与所述源点云相同的实时点云数据形态，或者具体为划分为多个目标栅格的离线点云图像序列形态。

11.根据权利要求1所述的方法，其特征在于：所述初始位姿具体为基于全球卫星导航系统采集的信号获得，或者是基于一个或多个车辆传感器采集的信号获得。

12.一种智能设备，其特征在于，包括：

至少一个处理器；

以及，与所述至少一个处理器通信连接的存储器；

其中，所述存储器中存储有计算机程序，所述计算机程序被所述至少一个处理器执行时实现权利要求1至8中任一项所述的点云配准方法。

13.一种计算机可读存储介质，其中存储有多条程序代码，其特征在于，所述程序代码适于由处理器加载并运行以执行权利要求1至8中任一项所述的点云配准方法。