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

权利要求书

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任一项所述基于双边优化的点云匹配方法的步骤。