基于点云匹配的位姿计算方法、装置、计算机设备及存储介质与流程

本发明涉及位姿计算技术领域，尤其涉及一种基于点云匹配的位姿计算方法、装置、计算机设备及存储介质。

背景技术：

传统的定位方法通常采用rtk算法，但是传统的rtk算法，由于隧道等内信号较差，因此不能解决隧道中定位问题，无法准确计算目标物的位姿。

技术实现要素：

本发明的实施例提供了一种基于点云匹配的位姿计算方法、装置、计算机设备及存储介质，旨在解决现有位姿计算无法在隧道中定位的问题。

为达到上述目的，本发明所提出的技术方案为：

第一方面，本发明提供了一种基于点云匹配的位姿计算方法，其包括以下步骤：

获取探测设备检测目标物的当前帧点云；

将当前帧点云设置为参考帧；

再次获取探测设备检测的新帧点云；

计算由当前帧点云变换至新帧点云的变换矩阵；

根据变换矩阵的计算结果，更新参考帧，以计算目标物的下一个位姿。

其中，所述步骤“计算由当前帧点云变换至新帧点云的变换矩阵”中矩阵变换的算法采用icp算法或ndt算法。

其中，所述ipc算法包括以下步骤：

点云预处理，以便滤波和清理数据；

进行点云配准，求解变换矩阵以寻找最近点；

加权处理，所述加权处理用于调整对应点的权重；

剔除不合理的配准点云，以降低噪声影响；

计算loss函数，根据loss函数获得最优变化矩阵。

其中，所述ndt算法包括以下步骤：

将扫描占用的空间细分为单元网格，基于单元网格内的点云分布计算每个单元的pdf；

根据每个单元的pdf扫描的点云的每个点进行转移矩阵变换；

根据矩阵变换结果，计算响应的概率分布函数；

根据概率分布函数，获得最优变换矩阵。

第二方面，本发明还提供了一种基于点云匹配的位姿计算装置，其包括：

当前帧点云获取单元，用于获取探测设备检测目标物的当前帧点云；

参考帧设置单元，用于将当前帧点云设置为参考帧；

新帧点云获取单元，用于再次获取探测设备检测的新帧点云；

变换矩阵计算单元，用于计算由当前帧点云变换至新帧点云的变换矩阵；

位姿计算单元，用于根据变换矩阵的计算结果，更新参考帧，以计算目标物的下一个位姿。

其中，所述变换矩阵计算单元运算并执行icp算法或ndt算法。

其中，所述变换矩阵计算单元包括：

预处理单元，用于点云预处理，以便滤波和清理数据；

点云配准单元，用于进行点云配准，求解变换矩阵以寻找最近点；

加权单元，用于加权处理，所述加权处理用于调整对应点的权重；

剔除单元，用于剔除不合理的配准点云，以降低噪声影响；

函数计算单元，用于计算loss函数，根据loss函数获得最优变化矩阵。

在另一实施例中，所述变换矩阵计算单元包括：

pdf计算单元，用于将扫描占用的空间细分为单元网格，基于单元网格内的点云分布计算每个单元的pdf；

转移矩阵变换单元，用于根据每个单元的pdf扫描的点云的每个点进行转移矩阵变换；

分布函数计算单元，用于根据矩阵变换结果，计算响应的概率分布函数；

最优矩阵计算单元，用于根据概率分布函数，获得最优变换矩阵。

第三方面，本发明还提供了一种计算机设备，所述计算机设备包括存储器及处理器，所述存储器上存储有计算机程序，所述处理器执行所述计算机程序时实现上所述的方法。

第四方面，本发明还提供了一种存储介质，所述存储介质存储有计算机程序，所述计算机程序包括程序指令，所述程序指令当被处理器执行时可实现如上所述的方法。

与现有技术相比，本发明的实施例提供了一种基于点云匹配的位姿计算、装置、计算机设备及存储介质，其通过icp或ndt算法获得前后位姿的最优变换矩阵，从而实现目标物在隧道中等的准确定位问题。

附图说明

为了更清楚地说明本发明实施例技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的基于点云匹配的位姿计算方法的主流程图；

图2为本发明实施例提供的基于点云匹配的位姿计算方法的子流程图；

图3为本发明实施例提供的基于点云匹配的位姿计算方法的子流程图；

图4为本发明实施例提供的基于基于点云匹配的位姿计算装置的示意性简图；

图5为本发明实施例提供的基于基于点云匹配的位姿计算装置的另一实施例示意性简图；以及

图6为本发明实施例提供的计算机设备的示意性框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。

还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

请参阅附图1，附图1为本发明的一种基于点云匹配的位姿计算算法的流程图，该基于点云匹配的位姿计算，其包括以下步骤：

步骤s100，获取探测设备检测目标物的当前帧点云，当前帧点云可以通过激光雷达等影响单元获取，帧点云是位姿计算原始信息数据。

步骤s200，将当前获得的源点云设置为参考帧；将首次获得帧点云信息作为第一次计算变换矩阵的基础。

步骤s300，再次获取探测设备检测的新帧点云，也即通过激光雷达获取目标物的相邻两次的帧点云信息。

步骤s400，计算当前帧点云变换至新帧点云的变换矩阵；相邻位姿之间在空间上存在一定的变换关系，该变化关系也是对下一个位姿进行运算的重要方式，因此需要计算二者之间转换的变换矩阵，包括方向矩阵和平移矩阵，若在迭代过程采用牛顿解法，则是指梯度矩阵和海森矩阵，通常采用icp算法和ndt算法，以及改良的icp算法和改良的ndt算法。本实施例以最为基础的icp算法和ndt算法为例进行说明。

步骤s500，根据变换矩阵的计算结果，更新参考帧，以计算目标物的下一个位姿。更新参考帧是将最近获取的帧点云信息作为下一次位姿计算的参考，选用最近的位姿作为参考帧，经过迭代运算可以提高位姿计算的准确性和可靠性。

请参阅图2，图2是本实施例的基于点云匹配的位姿计算方法的子流程图，具体其为icp(iterativeclosestpoin)算法流程，也称之为迭代最近点，所述ipc算法包括以下步骤：

步骤s411，点云预处理，以便滤波和清理数据；

步骤s412，进行点云配准，求解变换矩阵以寻找最近点；

步骤s413，加权处理，所述加权处理用于调整对应点的权重；

步骤s414，剔除不合理的配准点云，以降低噪声影响；

步骤s415，计算loss函数(也即损失函数)，根据loss函数获得最优变化矩阵。

请参阅图3，其为基于点云匹配的位姿计算方法的子例程图，具体的为ndt(normaldistributiontransform)算法流程图,也即正态分布变换算法，所述ndt算法包括以下步骤：

步骤s421，将扫描占用的空间细分为单元网格，基于单元网格内的点云分布计算每个单元的pdf(概率密度函数)；icp算法常针对于环境不变的，但是在现实中常存在环境不听变化的情景，而ndt算法可以很好解决这些问题。这一步的主要目的在将参考点云网格化，然后计算每个网格的多维正态分布函数。

步骤s422，根据每个单元的pdf扫描的点云的每个点进行转移矩阵变换；这里的矩阵变化采用牛顿法求解。

步骤s423，根据矩阵变换结果，计算响应的概率分布函数；也即根据计算结果，更新牛顿求解法中的梯度矩阵g和海森矩阵h，并计算新的步长。

步骤s424，根据概率分布函数，获得最优变换矩阵。具体的，主要判断结果是否收敛或达到迭代次数，如果达到了，则获得最优矩阵，否则继续执行步骤s423，继续求解，直到满足预设条件。

请参阅图4，图4为本发明的第一种基于点云匹配的位姿计算装置的结构示意图，该基于点云匹配的位姿计算装置100其包括：

当前帧点云获取单元101，用于获取探测设备检测目标物的当前帧点云，当前帧点云通常可以通过激光雷达等影响单元获取。

参考帧点云设置102，用于将当前帧点云设置为参考帧。

新帧点云获取单元103，再次获取探测设备检测的新帧点云，具体的是通过激光雷达获取目标物的相邻两次的帧点云信息。

变换矩阵计算单元104，用于计算当前帧点云变换至新帧点云的变换矩阵；相邻位姿之间在空间上存在一定的变换关系，该变化关系也是对下一个位姿进行运算的重要方式，因此需要计算二者之间转换的变换矩阵，包括方向矩阵和平移矩阵，若在迭代过程采用牛顿解法，则是指梯度矩阵和海森矩阵，通常采用icp算法和ndt算法，以及改良的icp算法和改良的ndt算法。本实施例以最为基础的icp算法和ndt算法为例进行说明。

位姿计算单元105，根据变换矩阵的计算结果，更新参考帧，以计算目标物的下一个位姿。更新参考帧是将最近获取的帧点云信息作为下一次位姿计算的参考，选用最近的位姿作为参考帧，经过迭代运算可以提高位姿计算的准确性和可靠性。

如图4所示，在其中一实施例中，所述变换矩阵计算单元104包括：

预处理单元1041，用于点云预处理，以便滤波和清理数据；

点云配准单元1042，用于进行点云配准，求解变换矩阵以寻找最近点；

加权单元1043，用于加权处理，所述加权处理用于调整对应点的权重；

剔除单元1044，用于剔除不合理的配准点云，以降低噪声影响；

函数计算单元1045，用于计算loss函数，根据loss函数获得最优变化矩阵。

在另一实施例中，请参阅图5，一种基于点云匹配的位姿计算装置包括：

当前帧点云获取单元101，用于获取探测设备检测目标物的当前帧点云，当前帧点云通常可以通过激光雷达等影响单元获取。

参考帧点云设置102，用于将当前帧点云设置为参考帧。

新帧点云获取单元103，再次获取探测设备检测的新帧点云，具体的是通过激光雷达获取目标物的相邻两次的帧点云信息。

变换矩阵计算单元104，用于计算当前帧点云变换至新帧点云的变换矩阵；相邻位姿之间在空间上存在一定的变换关系，该变化关系也是对下一个位姿进行运算的重要方式，因此需要计算二者之间转换的变换矩阵，包括方向矩阵和平移矩阵，若在迭代过程采用牛顿解法，则是指梯度矩阵和海森矩阵，通常采用icp算法和ndt算法，以及改良的icp算法和改良的ndt算法。本实施例以最为基础的icp算法和ndt算法为例进行说明。

位姿计算单元105，根据变换矩阵的计算结果，更新参考帧，以计算目标物的下一个位姿。更新参考帧是将最近获取的帧点云信息作为下一次位姿计算的参考，选用最近的位姿作为参考帧，经过迭代运算可以提高位姿计算的准确性和可靠性。

其中，所述变换矩阵计算单元104包括：

pdf计算单元141，用于将扫描占用的空间细分为单元网格，基于单元网格内的点云分布计算每个单元的pdf；

转移矩阵变换单元142，用于根据每个单元的pdf扫描的点云的每个点进行转移矩阵变换；

分布函数计算单元143，用于根据矩阵变换结果，计算响应的概率分布函数；

最优矩阵计算单元144，用于根据概率分布函数，获得最优变换矩阵。

请参阅图6，本实施例还提供了一种计算机设备，该计算机设备500包括通过系统总线501连接的处理器502、存储器和网络接口505，其中，存储器可以包括非易失性存储介质503和内存储器504。

该网络接口505用于与其它设备进行网络通信。本领域技术人员可以理解，图4中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备500的限定，具体的计算机设备500可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

该非易失性存储介质503可存储操作系统5031和计算机程序5032。该计算机程序5032包括程序指令，该程序指令被执行时，可使得处理器502执行一种基于点云匹配的位姿计算。

该处理器502用于提供计算和控制能力，以支撑整个计算机设备500的运行。

该内存储器504为非易失性存储介质503中的计算机程序5032的运行提供环境，该计算机程序5032被处理器502执行时，可使得处理器502执行以下步骤：

步骤s100，获取探测设备检测目标物的当前帧点云，当前帧点云可以通过激光雷达等影响单元获取，帧点云是位姿计算原始信息数据。

步骤s200，将当前获得的源点云设置为参考帧；将首次获得帧点云信息作为第一次计算变换矩阵的基础。

步骤s300，再次获取探测设备检测的新帧点云，也即通过激光雷达获取目标物的相邻两次的帧点云信息。

步骤s400，计算当前帧点云变换至新帧点云的变换矩阵；相邻位姿之间在空间上存在一定的变换关系，该变化关系也是对下一个位姿进行运算的重要方式，因此需要计算二者之间转换的变换矩阵，包括方向矩阵和平移矩阵，若在迭代过程采用牛顿解法，则是指梯度矩阵和海森矩阵，通常采用icp算法和ndt算法，以及改良的icp算法和改良的ndt算法。本实施例以最为基础的icp算法和ndt算法为例进行说明。

步骤s500，根据变换矩阵的计算结果，更新参考帧，以计算目标物的下一个位姿。更新参考帧是将最近获取的帧点云信息作为下一次位姿计算的参考，选用最近的位姿作为参考帧，经过迭代运算可以提高位姿计算的准确性和可靠性。

本发明还提供了一种存储介质，所述存储介质存储有计算机程序，所述计算机程序包括程序指令，所述程序指令当被处理器执行时可实现如下基于点云匹配的位姿计算：步骤s100，获取探测设备检测目标物的当前帧点云；步骤s200，将当前获得的源点云设置为参考帧；步骤s300，再次获取探测设备检测的新帧点云；步骤s400，计算当前帧点云变换至新帧点云的变换矩阵；步骤s500，根据变换矩阵的计算结果，更新参考帧，以计算目标物的下一个位姿。

所述存储介质可以是u盘、移动硬盘、只读存储器(read-onlymemory，rom)、磁碟或者光盘等各种可以存储程序代码的计算机可读存储介质。

与现有技术相比，本发明的实施例提供了一种基于点云匹配的位姿计算、装置、计算机设备及存储介质，其通过icp或ndt算法获得前后位姿的最优变换矩阵，从而实现目标物在隧道中等的准确定位问题。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本发明所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的。例如，各个单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。

本发明实施例方法中的步骤可以根据实际需要进行顺序调整、合并和删减。本发明实施例装置中的单元可以根据实际需要进行合并、划分和删减。另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。

该集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，终端，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。

上述内容，仅为本发明的较佳实施例，并非用于限制本发明的实施方案，本领域普通技术人员根据本发明的主要构思和精神，可以十分方便地进行相应的变通或修改，故本发明的保护范围应以权利要求书所要求的保护范围为准。