CN114419118A - 三维点云配准方法、移动设备及存储介质 - Google Patents

Abstract

本发明提供了一种三维点云配准方法、移动设备及存储介质，包括：根据移动设备的惯性测量数据和速度数据，得到位姿变化量，以及根据源点云和目标点云，分别得到源特征点云和目标特征点云；通过最近邻距离观测函数对源特征点云和目标特征点云进行迭代优化，得到第一观测矩阵；通过第一观测矩阵，确定退化的强化数量；根据强化数量和位姿变化量，确定位姿变化量观测函数；根据最近邻距离观测函数和位姿变化量观测函数，得到优化矩阵方程；对优化矩阵方程进行求解，得到点云配准结果。与现有技术相比，本发明通过最近邻距离观测函数和位姿变化量观测函数解决了点云退化下点云配准失效的问题。

Description

三维点云配准方法、移动设备及存储介质

技术领域

本发明涉及传感器数据处理技术领域，具体地，涉及一种三维点云配准方法、移动设备及存储介质。

背景技术

激光雷达具有抗干扰能力强、感知信息丰富、测量精度高等优点，因此被广泛用于自主机器人和自动驾驶汽车的障碍物检测、高精度定位和地图构建等。激光雷达测得的数据由大量点构成的三维点云表示，每个点包含空间位置和反射强度信息。点云能够表征三维世界；但由于每台激光雷达只能在有限的视野内扫描获取数据，因此要获得完整的三维场景，必须结合多个不同视角的点云。

点云配准就是计算两帧点云之间变换矩阵的问题。点云配准主要用于：三位重建，生成完成的三维场景，例如自动驾驶中的高精度三维地图重建和机器人技术中的三维环境重建；三维定位，例如自动驾驶汽车估计其在地图上的位置以及到道路边沿的距离；位姿估计，将一个点云A与另一个点云B对齐，可以生成点云A相对于点云B的位姿信息，其中，点云A为3D实时视图，点云B表征3D环境。

然而，在隧道、宽阔广场等环境中，点云容易因为环境特征不明细而出现“退化”现象，即信息丢失现象。当点云发生退化时，点云配准将在某些维度上出现不确定和精度下降的情况，引发位姿计算失败，最终导致定位失败。

专利文献CN111340862A公开了一种基于多特征融合的点云配准方法、装置及存储介质，所述方法包括：分别从源点云和目标点云中提取若干源点云特征点以及若干目标点云特征点；提取每一特征点的局部深度特征、法线角度特征、点云密度特征以及局部颜色特征，继而根据每一特征点的局部深度特征、法线角度特征、点云密度特征以及局部颜色特征生成每一特征点对应的特征描述子；其中，特征点包括源点云特征点和目标点云特征点；根据特征描述子，将源点云特征点与目标点云特征点进行配对，生成特征点对；根据特征点对生成变换矩阵，并根据变换矩阵对源点云进行变换，生成第二源点云，继而对第二源点云以及目标点云进行精配准。但该方法并未点云退化下点云配准失效的问题。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种三维点云配准方法、移动设备及存储介质。

第一方面，本发明提供一种三维点云配准方法，包括如下步骤：

步骤1：根据移动设备的惯性测量数据和速度数据得到位姿变化量，以及根据源点云和目标点云分别得到源特征点云和目标特征点云；

步骤2：通过最近邻距离观测函数对所述源特征点云和所述目标特征点云进行迭代优化，得到第一观测矩阵；

步骤3：通过所述第一观测矩阵，确定退化位姿维度的强化数量；

步骤4：根据所述强化数量和所述位姿变化量，确定位姿变化量观测函数；

步骤5：根据所述最近邻距离观测函数和所述位姿变化量观测函数，得到优化矩阵方程；

步骤6：对所述优化矩阵方程进行求解，得到点云配准结果。

优选地，步骤1，包括：

步骤101：根据移动设备的惯性测量数据和速度数据，采用组合导航算法，得到位姿变化量；

步骤102：对源点云和目标点云进行特征提取，分别得到源特征点云和目标特征点云。

优选地，步骤3，包括：

步骤301：根据所述第一观测矩阵，得到退化因子；

步骤302：根据所述退化因子，确定退化位姿维度；

步骤303：针对所述退化位姿维度，确定强化数量。

优选地，步骤5，包括：

步骤501：根据所述最近邻距离观测函数和所述位姿变化量观测函数，确定位姿优化函数；

步骤502：对所述位姿优化函数进行迭代优化，得到优化矩阵方程。

优选地，所述最近邻距离观测函数表示为：

其中，n表示选取用来迭代优化的点的数量；a_i、b_i、c_i、d_i、e_i、f_i、g_i表示与所述源点云和所述目标点云有关的常量，i＝1,2,…,n；δx、δy、δz、δφ、δθ、δψ是待优化参数，分别表示第一位姿维度、第二位姿维度、第三位姿维度、第四位姿维度、第五位姿维度和第六位姿维度。

优选地，所述位姿变化量观测函数表示为：

其中，dx、dy、dz、dφ、dθ、dψ分别表示第一位姿维度、第二位姿维度、第三位姿维度、第四位姿维度、第五位姿维度和第六位姿维度的位姿变化量，N₁、N₂、N₃、N₄、N₅、N₆均为正整数，分别表示第一位姿维度、第二位姿维度、第三位姿维度、第四位姿维度、第五位姿维度和第六位姿维度的强化数量。

优选地，所述位姿优化函数表示为：

第二方面，本发明提供一种移动设备，包括：

激光雷达模块，用于扫描所述移动设备周围的环境，生成源点云；

高清地图模块，用于存储所述移动设备活动区域的高清地图，提供目标点云；

惯性测量单元，用于提供所述移动设备的惯性测量数据；

速度传感器，用于提供所述移动设备的速度数据；

控制器模块，包括数据存储器、程序存储器和处理器，所述数据存储器存储所述源点云、所述目标点云、所述惯性测量数据和所述速度数据，所述程序存储器存储有计算机程序，所述处理器执行所述计算机程序时实现所述三维点云配准方法。

第三方面，本发明提供一种存储介质，所述存储介质存储有计算机程序，所述计算机程序被执行时实现所述三维点云配准方法。

与现有技术相比，本发明具有如下的有益效果：

1、本发明提供的三维点云配准方法具有较强的鲁棒性，在隧道、广场、高架等场景下，不会由于点云的退化而造成激光测距完全或部分不可用，能够继续使用激光的测距观测来进行点云配准，充分发挥激光点云的高精度特点。

2、本发明在非退化环境下能够识别退化程度，不影响理想环境下的激光配准精度。

3、本发明中点云退化程度的计算是自动的，不需要人工赋给环境参数而定，给定位和建图的自动化带来较大程度可行性。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明提供的三维点云配准方法的流程示意图；

图2为本发明提供的三维点云配准方法中强化数量的计算流程图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

图1为本发明提供的三维点云配准方法的流程示意图，如图1所示，本发明提供了一种三维点云配准方法，包括：

步骤1：根据移动设备的惯性测量数据和速度数据，得到位姿变化量，以及根据源点云和目标点云，分别得到源特征点云和目标特征点云。

优选地，包括：采用激光雷达，扫描移动设备周围的环境，生成源点云；采用高清地图单元，用于存储移动设备活动区域的高清地图，提供目标点云；采用惯性传感器(Inertial Measurement Unit,IMU)，提供移动设备的惯性测量数据；采用速度传感器，提供移动设备的速度数据。

具体地，还包括：控制器，包括数据存储器、程序存储器和处理器，数据存储器存储源点云、目标点云、惯性测量数据和速度数据，程序存储器存储有计算机程序，处理器执行计算机程序时实现所述三维点云配准方法。

优选地，包括：步骤101：根据移动设备的惯性测量数据和速度数据，采用组合导航算法，得到位姿变化量；步骤102：对源点云和目标点云进行特征提取，分别得到源特征点云和目标特征点云。

需要知道的是，源点云和目标点云可以不经特征提取而直接使用，本发明对于源点云和目标点云的特征提取不做限制，若源点云和目标点云可以直接输入最近邻距离观测函数进行处理，则不需要对源点云和目标点云进行特征提取。

步骤2：通过最近邻距离观测函数对所述源特征点云和所述目标特征点云进行迭代优化，得到第一观测矩阵。

具体地，将所述源特征点云和所述目标特征点云送入最近邻距离观测函数，并对最近邻距离观测函数进行迭代优化，得到第一观测矩阵。

本发明中最近邻距离观测函数可以通过公式(1)表示，其中，n表示选取用来迭代优化的点的数量；a_i、b_i、c_i、d_i、e_i、f_i、g_i表示与源点云和目标点云有关的常量，其中i＝1,2,…,n；δx、δy、δz、δφ、δθ、δψ是待优化参数，分别表示第一位姿维度、第二位姿维度、第三位姿维度、第四位姿维度、第五位姿维度和第六位姿维度。

进一步地，对最近邻距离观测函数进行迭代优化的最优结果为公式(2)。

将公式(2)写成矩阵形式为Ax＝g，其中，A为第一观测矩阵，通过公式(3)表示，x＝[δx δy δz δφ δθ δψ]^T，g＝[g₁ g₂ … g_n]^T。

步骤3：通过所述第一观测矩阵，确定退化位姿维度的强化数量。

优选地，包括：步骤301：根据所述第一观测矩阵，得到退化因子；步骤302：根据所述退化因子，确定退化位姿维度；步骤303：针对所述退化位姿维度，确定强化数量。

示例性的，本发明根据第一观测矩阵A计算退化因子，再根据退化因子确定出现退化的退化位姿维度并计算退化位姿维度对应的强化数量。

具体地，图2为本发明提供的三维点云配准方法中强化数量的计算流程图，如图2所示，首先，计算矩阵A^TA的特征值λ_i和特征向量v_i，其中i＝1,2,…,6；然后，构造矩阵M＝[v₁ v₂ v₃ v₄ v₅ v₆]，以及矩阵N＝M；之后，对于i＝1,2,…,6，依次判断每个特征值λ_i是否小于第一阈值λ_th，若所有特征值λ_i均大于λ_th，则确定所有的位姿维度均没有退化，对应位姿维度的强化数量N_j均为0，并结束计算，其中j＝1,2,…,6；若λ_i≤λ_th，则将矩阵N中第i个列向量置为0，并构造列向量v_a＝[1 1 1 1 1 1]^T，再计算退化因子k_d＝M^-1Nv_a；进一步地，对于j＝1,2,…,6，依次判断退化因子k_d的每个元素k_dj是否小于第二阈值k_th，若第j个元素k_dj<k_th，则确定第j个位姿维度为退化位姿维度，对应退化位姿维度的强化数量为N_j＝round(mλ_th/λ_i)，其中，m为强化因子，round为取整函数，若k_dj≥k_th，则确定第j个位姿维度没有退化，对应的强化数量为0。

步骤4：根据所述强化数量和所述位姿变化量，确定位姿变化量观测函数。

具体地，根据步骤3的结果，位姿变化量观测函数的构造可以通过公式(4)表示，其中，dx、dy、dz、dφ、dθ、dψ分别表示第一位姿维度、第二位姿维度、第三位姿维度、第四位姿维度、第五位姿维度和第六位姿维度的位姿变化量；N₁、N₂、N₃、N₄、N₅、N₆均为正整数，分别表示第一位姿维度、第二位姿维度、第三位姿维度、第四位姿维度、第五位姿维度和第六位姿维度的强化数量。

步骤5：根据所述最近邻距离观测函数和所述位姿变化量观测函数，得到优化矩阵方程。

优选地，包括：步骤501：根据所述最近邻距离观测函数和所述位姿变化量观测函数，确定位姿优化函数；步骤502：对所述位姿优化函数进行迭代优化，得到优化矩阵方程。

具体地，位姿优化函数由最近邻距离观测函数和位姿变化量观测函数共同构成，位姿优化函数的构造可以采用公式(5)表示。

进一步地，对位姿优化函数进行迭代优化，最优结果为公式(6)所示，将公式(6)写成矩阵形式，得到优化矩阵方程：Bx＝h。

其中，矩阵B表示第二观测矩阵，可以通过公式(7)表示；h可以通过公式(8)表示。

步骤6：对所述优化矩阵方程进行求解，得到点云配准结果。

具体地，对优化矩阵方程Bx＝h进行求解，即可得到点云配准结果。

本发明提供了一种移动设备，包括：激光雷达模块，用于扫描移动设备周围的环境，生成源点云；高清地图模块，用于存储移动设备活动区域的高清地图，提供目标点云；惯性测量单元，用于提供移动设备的惯性测量数据；速度传感器，用于提供移动设备的速度数据；控制器模块，包括数据存储器、程序存储器和处理器，数据存储器存储源点云、目标点云、惯性测量数据和速度数据，程序存储器存储有计算机程序，处理器执行计算机程序时实现所述三维点云配准方法。

本发明提供了一种存储介质，存储介质存储有计算机程序，计算机程序被执行时实现所述三维点云配准方法。

与现有技术相比，本发明具有以下有益效果：

1、本发明的三维点云配准方法具有较强的鲁棒性，在隧道、广场、高架等场景下，不会由于点云的退化而造成激光测距完全或部分不可用，能够继续使用激光的测距观测来进行点云配准，充分发挥激光点云的高精度特点。

2、本发明在非退化环境下能够识别退化程度，不影响理想环境下的激光配准精度。

3、本发明点云退化程度的计算是自动的，不需要人工赋给环境参数而定，给定位和建图的自动化带来较大程度可行性。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法子模块M进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (9)

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

步骤1：根据移动设备的惯性测量数据和速度数据，得到位姿变化量，以及根据源点云和目标点云，分别得到源特征点云和目标特征点云；

步骤2：通过最近邻距离观测函数对所述源特征点云和所述目标特征点云进行迭代优化，得到第一观测矩阵；

步骤3：通过所述第一观测矩阵，确定退化位姿维度的强化数量；

步骤4：根据所述强化数量和所述位姿变化量，确定位姿变化量观测函数；

步骤5：根据所述最近邻距离观测函数和所述位姿变化量观测函数，得到优化矩阵方程；

步骤6：对所述优化矩阵方程进行求解，得到点云配准结果。

2.根据权利要求1所述的三维点云配准方法，其特征在于，所述步骤1，包括：

步骤101：根据所述移动设备的惯性测量数据和所述速度数据，采用组合导航算法，得到所述位姿变化量；

步骤102：对所述源点云和所述目标点云进行特征提取，分别得到所述源特征点云和所述目标特征点云。

3.根据权利要求1所述的三维点云配准方法，其特征在于，所述步骤3，包括：

步骤301：根据所述第一观测矩阵，得到退化因子；

步骤302：根据所述退化因子，确定退化位姿维度；

步骤303：针对所述退化位姿维度，确定所述强化数量。

4.根据权利要求1所述的三维点云配准方法，其特征在于，所述步骤5，包括：

步骤501：根据所述最近邻距离观测函数和所述位姿变化量观测函数，确定位姿优化函数；

步骤502：对所述位姿优化函数进行迭代优化，得到优化矩阵方程。

5.根据权利要求1或4所述的三维点云配准方法，其特征在于，所述最近邻距离观测函数表示为：

其中，n表示选取用来迭代优化的点的数量；a_i、b_i、c_i、d_i、e_i、f_i、g_i表示与所述源点云和所述目标点云有关的常量，i＝1，2，…，n；δx、δy、δz、δφ、δθ、δψ是待优化参数，分别表示第一位姿维度、第二位姿维度、第三位姿维度、第四位姿维度、第五位姿维度和第六位姿维度。

6.根据权利要求5所述的三维点云配准方法，其特征在于，所述位姿变化量观测函数表示为：

其中，dx、dy、dz、dφ、dg、dψ分别表示第一位姿维度、第二位姿维度、第三位姿维度、第四位姿维度、第五位姿维度和第六位姿维度的位姿变化量；N₁、N₂、N₃、N₄、N₅、N₆均为正整数，分别表示第一位姿维度、第二位姿维度、第三位姿维度、第四位姿维度、第五位姿维度和第六位姿维度的强化数量。

7.根据权利要求6所述的三维点云配准方法，其特征在于，所述位姿优化函数表示为：

8.一种移动设备，其特征在于，所述移动设备，包括：

激光雷达模块，用于扫描所述移动设备周围的环境，生成源点云；

高清地图模块，用于存储所述移动设备活动区域的高清地图，提供目标点云；

惯性测量单元，用于提供所述移动设备的惯性测量数据；

速度传感器，用于提供所述移动设备的速度数据；

控制器模块，包括数据存储器、程序存储器和处理器，所述数据存储器存储所述源点云、所述目标点云、所述惯性测量数据和所述速度数据，所述程序存储器存储有计算机程序，所述处理器执行所述计算机程序时实现如权利要求1-7中任一项所述三维点云配准方法。

9.一种存储介质，其特征在于，所述存储介质存储有计算机程序，所述计算机程序被执行时实现如权利要求1-7中任一项所述三维点云配准方法。

Images