CN110930444A - 一种基于双边优化的点云匹配方法、介质、终端和装置 - Google Patents

Abstract

本发明公开一种基于双边优化的点云匹配方法、介质、终端和装置。方法包括以下步骤：获取相邻两帧点云数据；将参考帧到当前帧的平移改变量和角度改变量作为初始正向刚体变换并作用在当前帧，通过迭代算法生成最优正向刚体变换；将当前帧到参考帧的平移改变量和角度改变量作为初始反向刚体变换并作用在参考帧，通过迭代算法生成最优反向刚体变换；以最优正向刚体变换和最优反向刚体变换为初始值建立双边优化函数。本发明将编码器和惯性测量单元给出的相对改变量作为点云匹配的初始刚体变换，减小机器人轨迹推算的累积误差，同时通过双边优化函数得到的最终刚体变换具有非常高的可信度，对于移动机器人的落地应用具有十分重要的意义。

Description

一种基于双边优化的点云匹配方法、介质、终端和装置

【技术领域】

本发明涉及导航定位领域，尤其涉及一种基于双边优化的点云匹配方法、介质、终端和装置。

【背景技术】

随着传感器以及嵌入式技术的不断进步，具有自主移动能力的服务机器人逐渐走入市场。机器人在自主移动的过程中需要时刻知道自身在其运行场景中的位置。其运行场景可被称为地图，其位置包含机器人相对于该地图的横向坐标、纵向坐标和角度。根据该地图计算其位置是机器人必须具备的能力之一。由于有些运行场景非常巨大，机器人可能无法在短时间内精确计算得到自身位置。此时，根据之前位置并通过相对位移的计算得到当前位置就显得比较重要，因为相邻两个位置之间的相对位移其实很小。

目前计算相对位移比较流行的方式是通过相邻时刻激光点云的匹配得到平移和旋转，该平移和旋转即为相对位移。激光扫描所得到的点云是对机器人外部环境轮廓的一种描述，该描述实质上给出了机器人离环境边缘的距离信息。根据这些距离信息，机器可以实现自身位置的确定，也可以实现实时避障的功能。因此充分利用点云的信息是机器人自主移动的一个基本能力。点云匹配是机器人同步定位与建图(SimultaneouslyLocalization and Mapping)的重要组成部分。在相继的两帧点云中，其有极其相似的形状。由于环境轮廓的刚体性质，点云匹配输出的是两帧点云之间的旋转量和平移量。若给定其中一帧点云的位置坐标，则可以计算出另一帧点云的位置。以此类推，通过机器人所搭载激光扫描出的点云序列，基于点云匹配方法我们可以得到机器人的运动轨迹。点云匹配的基本思想是通过计算两帧点云之间的刚体变换(旋转和平移)使得两帧点云尽可能重叠，其具体方案是首先将两帧点云记为参考帧和当前帧，在当前帧中选取与参考帧对应的点对，建立所有点对之间的误差函数，采用优化方法求解关于刚体变换的误差函数，然后将所得到刚体变换作用到当前帧再重复上述过程，直到所计算的刚体变换不再改变为止。

点云匹配实质上是一个优化问题。一般采用迭代方法求解此类优化问题，此时迭代过程非常强烈依赖初始值，即本发明中的初始刚体变换。如果初始值给得不好，则迭代方法容易陷入局部最优解，而此局部最优解是机器人轨迹推算累积误差的来源。因此给出一个比较接近全局最优解的初始值就很重要。另一方面，在点云匹配中，误差函数是根据对应点对建立起来的，而对应点对是根据最近点规则计算得来的。此时，最近点的搜索将占用绝大部分的计算时间，因此如何确定最近点搜索范围是点云匹配的一个基本问题。最后，点云匹配结束之后并没有一些机制用来评判或者校正匹配结果，而如何根据点云自身的信息来校正匹配结果也是一个必须考虑的问题。

【发明内容】

本发明提供了一种基于双边优化的点云匹配方法、介质、终端和装置，解决了现有技术初始刚体变换选择不合理导致机器人轨迹推算容易累积误差以及不能根据点云自身信息来校正匹配结果的技术问题。

本发明解决上述技术问题的技术方案如下：一种基于双边优化的点云匹配方法，包括以下步骤：

步骤1，获取相邻两帧点云数据、两帧之间编码器的平移改变量以及惯性测量单元的角度改变量，其中第一帧为参考帧，第二帧为当前帧；

步骤2，将参考帧到当前帧的第一平移改变量和第一角度改变量作为初始正向刚体变换并作用在当前帧，通过第一预设迭代方法使所述当前帧充分靠近所述参考帧并生成最优正向刚体变换；

步骤3，将当前帧到参考帧的第二平移改变量和第二角度改变量作为初始反向刚体变换并作用在参考帧，通过第二预设迭代方法使所述参考帧充分靠近所述当前帧并生成最优反向刚体变换；

步骤4，以所述最优正向刚体变换和所述最优反向刚体变换为初始值并建立双边优化函数，对所述双边优化函数求解生成目标刚体变换。

在一个优选实施方式中，所述双边优化函数具体为：

采用梯度下降算法对所述双边优化函数进行求解生成目标刚体变换；其中R是关于θ₁的旋转矩阵，R'是关于θ₂的旋转矩阵，t是平移向量(x₁,y₁)，t'是平移向量(x₂,y₂)，x₁，y₁，θ₁表示最优正向刚体变换，x₂，y₂，θ₂表示最优反向刚体变换。

在一个优选实施方式中，将参考帧到当前帧的第一平移改变量和第一角度改变量作为初始正向刚体变换并作用在当前帧，通过第一预设迭代方法使所述当前帧充分靠近所述参考帧并生成最优正向刚体变换，具体为：

S201，遍历当前帧中的每一个当前点，对当前点局部范围的多个点建立拟合直线，获取所述拟合直线的法线方向，并在靠近所述法线方向的区域搜索所述参考帧中的参考点；

S202，计算当前点与每个参考点之间的距离并获取距离最小且满足预设距离阈值的目标参考点，建立当前点和目标参考点之间的对应点对，并形成对应点对的集合；

S203，对所述对应点对的集合给定误差度量，并建立第一误差函数；

S204，线性化所述第一误差函数，并采用高斯消元法对线性化后的线性方程组求解，根据求解结果对所述初始正向刚体变换进行更新；

S205，判断是否满足预设迭代终止条件，若是，则输出步骤S204更新后的正向刚体变换为最优正向刚体变换，若否，则将步骤S204更新后的正向刚体变换作为初始正向刚体变换并重复步骤S201-S205，直到满足预设迭代终止条件。

在一个优选实施方式中，所述预设迭代终止条件为所述求解结果足够小、所述求解结果保持不变或者当前迭代次数达到预设值中的任意一个。

在一个优选实施方式中，步骤S201具体为：遍历当前帧中的每一个当前点，对当前点局部范围的10-15个点建立拟合直线，获取所述拟合直线的法线方向，并从所述法线方向开始往两边逐点搜索所述参考帧中的参考点。

在一个优选实施方式中，根据迭代次数对所述预设距离阈值进行更新，迭代次数越多，所述预设距离阈值越小。

本发明实施例的第二方面提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的基于双边优化的点云匹配方法。

本发明实施例的第三方面提供了一种基于双边优化的点云匹配终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述基于双边优化的点云匹配方法的步骤。

本发明实施例的第四方面提供了一种基于双边优化的点云匹配装置，包括获取模块、正向迭代模块、反向迭代模块和解算模块，

所述获取模块用于获取相邻两帧点云数据、两帧之间编码器的平移改变量以及惯性测量单元的角度改变量，其中第一帧为参考帧，第二帧为当前帧；

所述正向迭代模块用于将参考帧到当前帧的第一平移改变量和第一角度改变量作为初始正向刚体变换并作用在当前帧，通过第一预设迭代方法使所述当前帧充分靠近所述参考帧并生成最优正向刚体变换；

所述反向迭代模块用于将当前帧到参考帧的第二平移改变量和第二角度改变量作为初始反向刚体变换并作用在参考帧，通过第二预设迭代方法使所述参考帧充分靠近所述当前帧并生成最优反向刚体变换；

所述解算模块用于以所述最优正向刚体变换和所述最优反向刚体变换为初始值并建立双边优化函数，对所述双边优化函数求解生成目标刚体变换。

在一个优选实施方式中，所述解算模块建立的双边优化函数具体为：

采用梯度下降算法对所述双边优化函数进行求解生成目标刚体变换；其中R是关于θ₁的旋转矩阵，R'是关于θ₂的旋转矩阵，t是平移向量(x₁,y₁)，t'是平移向量(x₂,y₂)，x₁，y₁，θ₁表示最优正向刚体变换，x₂，y₂，θ₂表示最优反向刚体变换。

在一个优选实施方式中，所述正向迭代模块包括：

搜索单元，用于遍历当前帧中的每一个当前点，对当前点局部范围的多个点建立拟合直线，获取所述拟合直线的法线方向，并在靠近所述法线方向的区域搜索所述参考帧中的参考点；

点对集合单元，用于计算当前点与每个参考点之间的距离并获取距离最小且满足预设距离阈值的目标参考点，建立当前点和目标参考点之间的对应点对，并形成对应点对的集合；

函数建立单元，用于对所述对应点对的集合给定误差度量，并建立第一误差函数；

迭代单元，用于线性化所述第一误差函数，并采用高斯消元法对线性化后的线性方程组求解，根据求解结果对所述初始正向刚体变换进行更新；

判断单元，用于判断是否满足预设迭代终止条件，若是，则输出更新后的正向刚体变换为最优正向刚体变换，若否，则将更新后的正向刚体变换作为初始正向刚体变换并驱动搜索单元、计算单元、函数建立单元以及迭代单元再次进行迭代，直到满足预设迭代终止条件。

在一个优选实施方式中，所述搜索单元具体用于遍历当前帧中的每一个当前点，对当前点局部范围的10-15个点建立拟合直线，获取所述拟合直线的法线方向，并从所述法线方向开始往两边逐点搜索所述参考帧中的参考点。

本发明提供了一种基于双边优化的点云匹配方法、介质、终端和装置，具有以下有益效果：

(1)本发明基于短时间内编码器和惯性测量单元给出的相对改变量较为准确的事实，将此改变量作为点云匹配的初始刚体变换，如此可以避免优化问题陷入局部最优解，从而可以减小机器人轨迹推算的累积误差；

(2)在每一次迭代过程中通过参考帧所在机器人朝向作为视线来搜索对应点对，如此可以减少搜索计算量，提高点云匹配效率；

(3)基于点云匹配结果是一个刚体变换，而同时交换当前帧和参考帧进行反向点云匹配得到的也是一个刚体变换，并且此两个刚体变换应该是互逆的，因此针对此两个刚体变换建立一个新的优化问题，通过求解该优化问题所得到的最终刚体变换具有非常高的可信度，从而减少了轨迹推算的累积误差同时也提高了运算速度，对于自主移动机器人的落地应用具有十分重要的意义。

为使发明的上述目的、特征和优点能更明显易懂，下文特举本发明较佳实施例，并配合所附附图，作详细说明如下。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1是实施例1提供的基于双边优化的点云匹配方法的流程示意图；

图2是实施例2提供的基于双边优化的点云匹配装置的结构示意图；

图3是实施例3提供的基于双边优化的点云匹配终端的结构示意图。

【具体实施方式】

为了使本发明的目的、技术方案和有益技术效果更加清晰明白，以下结合附图和具体实施方式，对本发明进行进一步详细说明。应当理解的是，本说明书中描述的具体实施方式仅仅是为了解释本发明，并不是为了限定本发明。

图1是本发明实施例1提供的一种基于双边优化的点云匹配方法的流程示意图，如图1所示，方法包括以下步骤：

步骤1，获取相邻两帧点云数据、两帧之间编码器的平移改变量以及惯性测量单元的角度改变量，其中第一帧为参考帧，第二帧为当前帧；

步骤2，将参考帧到当前帧的第一平移改变量和第一角度改变量作为初始正向刚体变换并作用在当前帧，通过第一预设迭代方法使所述当前帧充分靠近所述参考帧并生成最优正向刚体变换；

步骤3，将当前帧到参考帧的第二平移改变量和第二角度改变量作为初始反向刚体变换并作用在参考帧，通过第二预设迭代方法使所述参考帧充分靠近所述当前帧并生成最优反向刚体变换；

步骤4，以所述最优正向刚体变换和所述最优反向刚体变换为初始值并建立双边优化函数，对所述双边优化函数求解生成目标刚体变换。

上述实施例基于短时间内编码器和惯性测量单元给出的相对改变量较为准确的事实，将此改变量作为点云匹配的初始刚体变换，从而避免优化问题陷入局部最优解，减小机器人轨迹推算的累积误差；同时基于点云匹配结果是一个刚体变换，而同时交换当前帧和参考帧进行反向点云匹配得到的也是一个刚体变换，并且此两个刚体变换应该是互逆的，因此针对此两个刚体变换建立一个新的优化问题，通过求解该优化问题所得到的最终刚体变换具有非常高的可信度，从而减少了轨迹推算的累积误差同时也提高了运算速度，对于自主移动机器人的落地应用具有十分重要的意义。

以下对上述实施例的步骤进行详细说明。

首先获取相继两帧点云，记为参考帧{P_i}，当前帧{Q_i}；以及获取两帧点云之间编码器的平移改变量和惯性测量单元IMU的角度改变量。由于编码器和惯性测量单元输出的是高频数据流，因此在获取两帧点云时已经输出大量编码器的平移量和惯性测量单元的角度量，此时将这些量进行复合运算得到平移改变量和角度改变量从而用来作为初始刚体变换。

然后将编码器的第一平移改变量(x₀,y₀)和惯性测量单元IMU的第一角度改变量θ₀作为初始正向刚体变换作用在当前帧，使得当前帧充分靠近参考帧，即通过正向点云匹配生成最优正向刚体变换；同时将第二平移改变量(-x₀,-y₀)和第二角度改变量-θ₀作为初始反向刚体变换作用在参考帧，使得参考帧充分靠近当前帧，即通过反向点云匹配生成最优反向刚体变换。

采用迭代算法进行正向点云匹配包括以下步骤：

S201，对于当前帧中的每一个点，在其局部范围的多个点建立拟合直线，比如在附近的10-15个点进行直线拟合，然后获取该拟合直线的法线方向，从所述法线方向开始往两边逐点搜索所述参考帧中的参考点，具体搜索点数范围可以提前设定，如此可以提高计算效率。

S202，计算当前帧中的每一个点与参考帧的参考点列之间的距离，选取距离最小且满足预设距离阈值的两点建立对应点对。如此遍历当前帧的所有点，建立对应点对的集合{P_i',Q_i'}。同理采用迭代算法进行反向点云匹配时，可类似建立对应点对的集合{Q_i”,P_i”}。上述实施例选取平方距离函数建立对应点对集合，该函数具有凸性以及光滑性，因此方便后续线性化逼近。

S203，对上述对应点对的集合给定误差度量，并建立如下优化问题：

其中R是关于角度θ的旋转矩阵，t是平移向量(x,y)。类似可建立反向匹配过程中关于旋转矩阵R'和t'的优化问题。

S204，关于目标变量线性化上述优化问题中的第一误差函数，得到如下关于旋转角度θ和平移量(x,y)的线性方程组：

采用高斯消元法计算可得到目标变量增量(Δx,Δy,Δθ)，此时更新初始正向刚体变换为：

(x₁,y₁,θ₁)＝(x₀,y₀,θ₀)+(Δx,Δy,Δθ)。

S205，判断是否满足预设迭代终止条件，若是，则终止迭代，上述更新的正向刚体变换为优化问题的最优解，即为最优正向刚体变换；若否，则将上述更新的正向刚体变换作为初始正向刚体变换，返回至步骤201，此时在对应点对的计算中采用减半的距离阈值，重复此过程得到优化问题的最优解。上述迭代过程中，可以根据迭代次数对所述预设距离阈值进行更新，迭代次数越多，所述预设距离阈值越小，比如每次迭代都对预设距离阈值进行减半处理，如此可以逐步得到真实的对应点对，从而得到优化问题的最优解。上述优选实施例中，所述预设迭代终止条件为所述求解结果足够小、所述求解结果保持不变或者当前迭代次数达到预设值中的任意一个。采用相同方法获取最优反向刚体变换，在此不进行详细说明。

最后，以所述最优正向刚体变换和所述最优反向刚体变换为初始值并建立双边优化函数。这里点云之间的匹配实际上得到的是一个刚体变换，那么从正反两个方向进行点云匹配所得到的刚体变换应该是互逆的，因此可以建立关于此正反刚体变换的优化问题，从而得到校正的刚体变换，此即为双边优化问题。换句话说，该双边优化函数是基于相对坐标的考虑，将反向匹配过程的最优反向刚体变换通过正向匹配的最优正向刚体变换进行转换得到同一坐标系中的变换。实际上，上述两个步骤分别给出了正向匹配过程中的最优解(x_*,y_*,θ_*)和反向匹配过程中的最优解(x'_*,y'_*,θ′_*)，以此作为如下双边优化问题的初值：

其中R是关于θ₁的旋转矩阵，R'是关于θ₂的旋转矩阵，t是平移向量(x₁,y₁)，t'是平移向量(x₂,y₂)，x₁，y₁，θ₁表示最优正向刚体变换，x₂，y₂，θ₂表示最优反向刚体变换。

对上述双边优化函数进行线性化，可得到如下线性方程组

根据初始值(x_*,y_*,θ_*,x′_*,y′_*,θ′_*)，采用梯度下降算法可以得到上述优化问题的最优解，该最优解即为此两帧点云之间的最佳刚体变换。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，比如实施例1中步骤2和步骤3可以先后进行也可以同时进行，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

本发明实施例还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的基于双边优化的点云匹配方法。

图2是本发明实施例2提供的一种基于双边优化的点云匹配装置的结构示意图，如图2所示，包括获取模块100、正向迭代模块200、反向迭代模块300和解算模块400，

所述获取模块100用于获取相邻两帧点云数据、两帧之间编码器的平移改变量以及惯性测量单元的角度改变量，其中第一帧为参考帧，第二帧为当前帧；

所述正向迭代模块200用于将参考帧到当前帧的第一平移改变量和第一角度改变量作为初始正向刚体变换并作用在当前帧，通过第一预设迭代方法使所述当前帧充分靠近所述参考帧并生成最优正向刚体变换；

所述反向迭代模块300用于将当前帧到参考帧的第二平移改变量和第二角度改变量作为初始反向刚体变换并作用在参考帧，通过第二预设迭代方法使所述参考帧充分靠近所述当前帧并生成最优反向刚体变换；

所述解算模块400用于以所述最优正向刚体变换和所述最优反向刚体变换为初始值并建立双边优化函数，对所述双边优化函数求解生成目标刚体变换。

一个优选实施例中，所述正向迭代模块200包括：

搜索单元201，用于遍历当前帧中的每一个当前点，对当前点局部范围的多个点建立拟合直线，获取所述拟合直线的法线方向，并在靠近所述法线方向的区域搜索所述参考帧中的参考点；

点对集合单元202，用于计算当前点与每个参考点之间的距离并获取距离最小且满足预设距离阈值的目标参考点，建立当前点和目标参考点之间的对应点对，并形成对应点对的集合；

函数建立单元203，用于对所述对应点对的集合给定误差度量，并建立第一误差函数；

迭代单元204，用于线性化所述第一误差函数，并采用高斯消元法对线性化后的线性方程组求解，根据求解结果对所述初始正向刚体变换进行更新；

判断单元205，用于判断是否满足预设迭代终止条件，若是，则输出更新后的正向刚体变换为最优正向刚体变换，若否，则将更新后的正向刚体变换作为初始正向刚体变换并驱动搜索单元、计算单元、函数建立单元以及迭代单元再次进行迭代，直到满足预设迭代终止条件。

在更加优选实施例中，所述搜索单元201具体用于遍历当前帧中的每一个当前点，对当前点局部范围的10-15个点建立拟合直线，获取所述拟合直线的法线方向，并从所述法线方向开始往两边逐点搜索所述参考帧中的参考点。

本发明实施例还提供了一种基于双边优化的点云匹配终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述基于双边优化的点云匹配方法的步骤。图3是本发明实施例3提供的基于双边优化的点云匹配终端的结构示意图，如图3所示，该实施例的基于双边优化的点云匹配终端8包括：处理器80、可读存储介质81以及存储在所述可读存储介质81中并可在所述处理器80上运行的计算机程序82。所述处理器80执行所述计算机程序82时实现上述各个方法实施例中的步骤，例如图1所示的步骤1至步骤4。或者，所述处理器80执行所述计算机程序82时实现上述各装置实施例中各模块的功能，例如图2所示模块100至400的功能。

示例性的，所述计算机程序82可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述可读存储介质81中，并由所述处理器80执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序82在所述基于双边优化的点云匹配终端8中的执行过程。

所述基于双边优化的点云匹配终端8可包括，但不仅限于，处理器80、可读存储介质81。本领域技术人员可以理解，图3仅仅是基于双边优化的点云匹配终端8的示例，并不构成对基于双边优化的点云匹配终端8的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述基于双边优化的点云匹配终端还可以包括电源管理模块、运算处理模块、输入输出设备、网络接入设备、总线等。

所称处理器80可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述可读存储介质81可以是所述基于双边优化的点云匹配终端8的内部存储单元，例如基于双边优化的点云匹配终端8的硬盘或内存。所述可读存储介质81也可以是所述基于双边优化的点云匹配终端8的外部存储设备，例如所述基于双边优化的点云匹配终端8上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，所述可读存储介质81还可以既包括所述基于双边优化的点云匹配终端8的内部存储单元也包括外部存储设备。所述可读存储介质81用于存储所述计算机程序以及所述基于双边优化的点云匹配终端所需的其他程序和数据。所述可读存储介质81还可以用于暂时地存储已经输出或者将要输出的数据。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

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

在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

本发明并不仅仅限于说明书和实施方式中所描述，因此对于熟悉领域的人员而言可容易地实现另外的优点和修改，故在不背离权利要求及等同范围所限定的一般概念的精神和范围的情况下，本发明并不限于特定的细节、代表性的设备和这里示出与描述的图示示例。

Claims (10)

1.一种基于双边优化的点云匹配方法，其特征在于，包括以下步骤：

步骤1，获取相邻两帧点云数据、两帧之间编码器的平移改变量以及惯性测量单元的角度改变量，其中第一帧为参考帧，第二帧为当前帧；

步骤2，将参考帧到当前帧的第一平移改变量和第一角度改变量作为初始正向刚体变换并作用在当前帧，通过第一预设迭代方法使所述当前帧充分靠近所述参考帧并生成最优正向刚体变换；

步骤3，将当前帧到参考帧的第二平移改变量和第二角度改变量作为初始反向刚体变换并作用在参考帧，通过第二预设迭代方法使所述参考帧充分靠近所述当前帧并生成最优反向刚体变换；

步骤4，以所述最优正向刚体变换和所述最优反向刚体变换为初始值并建立双边优化函数，对所述双边优化函数求解生成目标刚体变换。

2.根据权利要求1所述的基于双边优化的点云匹配方法，其特征在于，所述双边优化函数具体为：

其中R是关于θ₁的旋转矩阵，R'是关于θ₂的旋转矩阵，t是平移向量(x₁,y₁)，t'是平移向量(x₂,y₂)，(x₁，y₁，θ₁)表示最优正向刚体变换，(x₂，y₂，θ₂)表示最优反向刚体变换；采用梯度下降算法对所述双边优化函数进行求解生成目标刚体变换。

3.根据权利要求1或2所述的基于双边优化的点云匹配方法，其特征在于，将参考帧到当前帧的第一平移改变量和第一角度改变量作为初始正向刚体变换并作用在当前帧，通过第一预设迭代方法使所述当前帧充分靠近所述参考帧并生成最优正向刚体变换，具体为：

S201，遍历当前帧中的每一个当前点，对当前点局部范围的多个点建立拟合直线，获取所述拟合直线的法线方向，并在靠近所述法线方向的区域搜索所述参考帧中的参考点；

S202，计算当前点与每个参考点之间的距离并获取距离最小且满足预设距离阈值的目标参考点，建立当前点和目标参考点之间的对应点对，并形成对应点对的集合；

S203，对所述对应点对的集合给定误差度量，并建立第一误差函数；

S204，线性化所述第一误差函数，并采用高斯消元法对线性化后的线性方程组求解，根据求解结果对所述初始正向刚体变换进行更新；

S205，判断是否满足预设迭代终止条件，若是，则输出步骤S204更新后的正向刚体变换为最优正向刚体变换，若否，则将步骤S204更新后的正向刚体变换作为初始正向刚体变换并重复步骤S201-S205，直到满足预设迭代终止条件。

4.根据权利要求3所述的基于双边优化的点云匹配方法，其特征在于，所述预设迭代终止条件为所述求解结果足够小、所述求解结果保持不变或者当前迭代次数达到预设值中的任意一个。

5.根据权利要求4所述的基于双边优化的点云匹配方法，其特征在于，步骤S201具体为：遍历当前帧中的每一个当前点，对当前点局部范围的10-15个点建立拟合直线，获取所述拟合直线的法线方向，并从所述法线方向开始往两边逐点搜索所述参考帧中的参考点。

6.根据权利要求5所述的基于双边优化的点云匹配方法，其特征在于，根据迭代次数对所述预设距离阈值进行更新，迭代次数越多，所述预设距离阈值越小。

7.一种计算机可读存储介质，其特征在于，存储有计算机程序，所述计算机程序被处理器执行时，实现权利要求1-6任一项所述的基于双边优化的点云匹配方法。

8.一种基于双边优化的点云匹配终端，其特征在于，包括权利要求7所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现如权利要求1-6任一项所述基于双边优化的点云匹配方法的步骤。

9.一种基于双边优化的点云匹配装置，其特征在于，包括获取模块、正向迭代模块、反向迭代模块和解算模块，

所述获取模块用于获取相邻两帧点云数据、两帧之间编码器的平移改变量以及惯性测量单元的角度改变量，其中第一帧为参考帧，第二帧为当前帧；

所述正向迭代模块用于将参考帧到当前帧的第一平移改变量和第一角度改变量作为初始正向刚体变换并作用在当前帧，通过第一预设迭代方法使所述当前帧充分靠近所述参考帧并生成最优正向刚体变换；

所述反向迭代模块用于将当前帧到参考帧的第二平移改变量和第二角度改变量作为初始反向刚体变换并作用在参考帧，通过第二预设迭代方法使所述参考帧充分靠近所述当前帧并生成最优反向刚体变换；

所述解算模块用于以所述最优正向刚体变换和所述最优反向刚体变换为初始值并建立双边优化函数，对所述双边优化函数求解生成目标刚体变换。

10.根据权利要求9所述的基于双边优化的点云匹配装置，其特征在于，所述正向迭代模块包括：

搜索单元，用于遍历当前帧中的每一个当前点，对当前点局部范围的多个点建立拟合直线，获取所述拟合直线的法线方向，并在靠近所述法线方向的区域搜索所述参考帧中的参考点；

点对集合单元，用于计算当前点与每个参考点之间的距离并获取距离最小且满足预设距离阈值的目标参考点，建立当前点和目标参考点之间的对应点对，并形成对应点对的集合；

函数建立单元，用于对所述对应点对的集合给定误差度量，并建立第一误差函数；

迭代单元，用于线性化所述第一误差函数，并采用高斯消元法对线性化后的线性方程组求解，根据求解结果对所述初始正向刚体变换进行更新；

判断单元，用于判断是否满足预设迭代终止条件，若是，则输出更新后的正向刚体变换为最优正向刚体变换，若否，则将更新后的正向刚体变换作为初始正向刚体变换并驱动搜索单元、计算单元、函数建立单元以及迭代单元再次进行迭代，直到满足预设迭代终止条件。

Images