[发明专利]一种基于双边优化的点云匹配方法、介质、终端和装置

说明书

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

【技术领域】

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

【背景技术】

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

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

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

【发明内容】

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

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

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

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