CN113256787B - 一种基于点云密度分析的自适应分辨率八叉树建图方法 - Google Patents

Abstract

本发明公开了一种基于点云密度分析的自适应分辨率八叉树建图方法，属于室内地图构建技术领域，具体步骤为：首先分割稠密点云地图，建立点云包围盒，在此基础上，计算点云包围盒内每个点的点云密度，得到点云包围盒内的最大点云密度和其均值、标准差；然后利用点云密度和sigmoid函数建立评价函数，利用评价函数的结果对邻近点云包围盒间的相似性进行判断，合并相似性超过设定阈值的点云包围盒；最后，利用评价函数的结果选择八叉树地图的分辨率，建立自适应分辨率的八叉树地图。本发明能够根据环境复杂程度的不同自适应地调整八叉树地图的分辨率，在充分表征环境细节的情况下，能够减少地图存储占用空间，提高建图效率。

Description

一种基于点云密度分析的自适应分辨率八叉树建图方法

技术领域

本发明属于地图构建技术领域，具体涉及一种基于点云密度分析的自适应分辨率八叉树建图方法。

背景技术

视觉即时定位与建图(Simultaneous Localization and Mapping，SLAM)技术是利用图像或视频提供的丰富环境信息实现移动机器人定位导航的技术。其中，建图是机器人实现室内路径规划和导航的前提和基础。视觉点云地图是利用摄像头采集的图像转化而成的，可用于表征信息，但单纯的点云地图无法计算机器人的可通行路径，只能进行示教，限制了其应用。而八叉树地图为三维空间的管理提供了途径。八叉树是一种用于描述三维空间的树状数据结构，八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，这八个子节点所表示的体积元素加在一起就等于父节点的体积。按照这种结构，由根节点一层层的分割下去，直到最小的叶子节点为止，整个八叉树就能够表示一整张地图，叶子节点是八叉树能表示的最小面积，即地图的分辨率。但八叉树地图的分辨率由人为设定，当地图内既有复杂环境又有简单环境时，固定的分辨率导致建图的效率低，地图占用空间大等缺陷。

发明内容

为了解决上述问题，本发明提出了一种基于点云密度分析的自适应分辨率八叉树建图方法，通过对环境点云地图的点云密度分析，根据环境复杂程度的不同自适应地设置八叉树地图的分辨率，减少地图的存储空间，提高建图效率。

本发明的技术方案如下：

一种基于点云密度分析的自适应分辨率八叉树建图方法，通过对环境点云地图的点云密度分析，根据环境复杂程度的不同自适应地设置八叉树地图的分辨率，其具体步骤如下：

步骤1：分割稠密点云地图，建立网格状的点云包围盒；

步骤2：根据点云密度计算函数计算点云包围盒内每个点的点云密度值，得到点云包围盒内的最大点云密度和点云密度的均值、标准差；

步骤3：利用sigmoid函数建立点云包围盒的评价函数，分析每个点云包围盒的点云密度和均值、标准差；

步骤4：分析相邻点云包围盒的评价函数结果，比较其相似性，对相似性满足设定的阈值条件的点云包围盒进行合并；

步骤5：在合并邻近点云包围盒后，对各个合并后点云包围盒的评价函数输出进行排序，根据评价函数的结果依次设置不同的分辨率，建立自适应分辨率的八叉树地图。

优选地，步骤1具体包括：

利用视觉深度相机获取环境的稠密点云图后，对稠密点云进行分割，分割成点云包围盒进行处理；稠密点云分割采用主元分析法，利用主元分析法求解稠密点云图的最大外接包围盒，然后对最大外接包围盒进行N等分，进而将稠密点云分解成N个点云包围盒。

优选地，主元分析法是通过协方差来计算主元，其具体过程为：

步骤1.1：根据公式(1)计算数据集的平均值

其中，P_i＝(x_i,y_i,z_i)表示单个点云的位置，稠密的点云是由M个点P₁,P₂,…,P_M组成的数据集；

步骤1.2：根据公式(2)求解数据集的协方差矩阵：

其中，

步骤1.3：对协方差矩阵Q，利用|Q-λI|＝0求解其特征值λ，其中，I表示单位矩阵；

步骤1.4：根据特征值求出Q的特征向量，对特征向量正交化、标准化，得到正交矩阵q＝[ε₁,ε₂,ε₃]；

步骤1.5：以ε₁、ε₂、ε₃作为稠密点云地图的主方向，沿三个主方向求出沿x、y、z三个方向最大和最小位置，获得6个顶点坐标A_i(i＝1,…6)；

步骤1.6：利用这6个顶点构建稠密点云的长方体形状的最大外接包围盒，然后将最大外接包围盒N等分，获得N个点云包围盒。

优选地，步骤2具体包括：

首先，利用公式(4)遍历计算点云包围盒内每个点云点的密度大小：

式中，D_h,k表示第h个点云包围盒内的第k个点云点的点云密度，h＝1,2,…,N；(x_k,y_k,z_k)为第k个点云点的坐标，(x_j,y_j,z_j)表示点云包围盒内其他点云点的坐标；r_ax、r_ay、r_az表征了点云包围盒在x轴、y轴、z轴方向上的长度，M为点云包围盒内点云点的数量；

然后，比较得到该点云包围盒内的最大点云密度D_h,max，D_h,max表示第h个点云包围盒的最大点云密度，并利用公式(5)、(6)计算第h个点云包围盒内的点云密度均值μ_h与标准差σ_h：

优选地，步骤3中的sigmoid函数利用点云包围盒的最大密度D_h,max以及标准差σ_h建立，sigmoid函数的输出处于[0,1]之间，表示点云稀疏程度的概率。

优选地，步骤4具体包括：

步骤3中每个点云包围盒都有一个点云包围盒评价函数输出P_h(Y＝1|X)，通过比较近邻点云包围盒的评价函数输出P_i(Y＝1|X)、P_j(Y＝1|X)，若|P_i(Y＝1|X)-P_j(Y＝1|X)|<TH_P，则认为这两个邻近的点云包围盒相似度达到要求，将其合并，其中TH_P为预先设定的点云包围盒合并阈值。

优选地，步骤5具体包括：

合并完成后，每个点云包围盒对应着一个点云概率P_h(Y＝1|X)，且环境越复杂的点云包围盒，其值就越大；将P_h按照大小顺序排序，根据P_h的值设置八叉树地图的分辨率。

本发明所带来的有益技术效果：

通过本发明提出的自适应分辨率建图方法，能够根据地图中环境复杂程度的不同，自适应的修改八叉树地图的分辨率，减小地图存储所需空间，提高建图速度和导航效率。

附图说明

图1是本发明基于点云密度分析的自适应分辨率八叉树地图建图方法流程图。

具体实施方式

下面结合附图以及具体实施方式对本发明作进一步详细说明：

如图1所示，一种基于点云密度分析的自适应分辨率八叉树建图方法是通过对环境点云地图的点云密度分析，根据环境复杂程度的不同自适应地设置八叉树地图的分辨率，其具体步骤如下。

步骤1：分割稠密点云地图，建立网格状的点云包围盒。

环境的稠密点云图利用视觉深度相机获取。获取环境的稠密点云地图后，对稠密点云进行分割，分割成点云包围盒进行处理，以减少运算量。稠密点云分割采用主元分析法(Principal Component Analysis,PCA)，利用PCA求解稠密点云图的最大外接包围盒，然后对最大外接包围盒进行N等分，进而将稠密点云分解成N个点云包围盒。

主元分析法主要用于物体间碰撞检测时的外接图形的计算，也可用于数据降维。主元分析法是通过协方差来计算主元。用P_i＝(x_i,y_i,z_i)表示单个点云的位置，则稠密的点云可以看成由M个点P₁,P₂,…,P_M组成的数据集。首先，利用公式(1)计算数据集的平均值

求解数据集的协方差矩阵：

其中，

对协方差矩阵Q，利用|Q-λI|＝0求解其特征值λ。式中，I表示单位矩阵。根据特征值可以求出Q的特征向量，对特征向量正交化、标准化，得到正交矩阵q＝[ε₁,ε₂,ε₃]。以ε₁、ε₂、ε₃作为稠密点云地图的主方向，沿三个主方向求出沿x、y、z三个方向最大和最小位置，获得6个顶点坐标A_i(i＝1,…6)，由于ε₁、ε₂、ε₃是相互正交的，则A_i(i＝1,…6)构成长方体的6个顶点。利用这些顶点可以构建稠密点云的长方形的最大外接包围盒，然后将最大外接包围盒N等分，获得N个点云包围盒。

步骤2：根据点云密度计算函数计算点云包围盒内每个点的点云密度值，得到点云包围盒内的最大点云密度和点云密度的均值、标准差。

建立点云包围盒内点云密度的计算公式，公式如下所示：

式中，D_h,k表示第h个点云包围盒内的第k个点云点的点云密度，h＝1,2,…,N。(x_k,y_k,z_k)为第k个点云点的坐标，(x_j,y_j,z_j)表示点云包围盒内其他点云点的坐标。r_ax、r_ay、r_az表征了点云包围盒在x轴、y轴、z轴方向上的长度，M为点云包围盒内点云点的数量。利用公式(4)遍历计算该点云包围盒内每个点云点的密度大小。

求解出该点云包围盒内所有点云点的密度之后，比较得到该点云包围盒内的最大点云密度D_h,max，D_h,max表示第h个点云包围盒的最大点云密度。然后计算第h个点云包围盒内的点云密度均值μ_h与标准差σ_h：

步骤3：利用sigmoid函数建立点云包围盒的评价函数，分析每个点云包围盒的点云密度和均值、标准差。

为了量化衡量环境的复杂程度，基于sigmoid函数建立点云包围盒的评价函数。利用点云包围盒的最大密度D_h,max以及标准差σ_h建立sigmoid函数，该sigmoid函数的输出处于[0,1]之间，表示点云稀疏程度的概率。其原理可以描述：

令z＝c₀D_h,max+c₁σ_h，c₀、c₁为常数，一般根据经验选取。令X表示环境内的复杂程度，Y表示对应的环境稀疏程度，用[0,1]之间的实数表示，当空间内空无一物时为0，当空间内存在内容时为1。在输入为X，输出为Y＝1时，sigmoid函数的输出可以表示为：

P_h(Y＝1|X)＝g(z)＝(1+e^-z)^-1 (7)

同理，求解输出Y＝0的结果：

P_h(Y＝0|X)＝1-g(z)＝1-(1+e^-z)^-1 (8)

由公式(7)可知，当z的值越接近正无穷，P_h(Y＝1|X)值也就越接近1，其实际意义是，由点云包围盒的最大点云密度和标准差决定的z值越大，表明该点云包围盒被填满的概率越高。

步骤4：分析相邻点云包围盒的评价函数结果，比较其相似性，对相似性满足设定的阈值条件的点云包围盒进行合并。

由于在获取点云包围盒时采用均匀等分的方式，会将连在一起的点云簇分隔开，因此，需要对点云包围盒进行合并。步骤3中每个点云包围盒都有一个点云包围盒评价函数输出P_h(Y＝1|X)，通过比较近邻点云包围盒的评价函数输出P_i(Y＝1|X)、P_j(Y＝1|X)，若|P_i(Y＝1|X)-P_j(Y＝1|X)|<TH_P，其中TH_P为预先设定的点云包围盒合并阈值，则认为这两个邻近的点云包围盒相似度达到要求，可以将其合并。

步骤5：在合并邻近点云包围盒后，对各个合并后点云包围盒的评价函数输出进行排序，根据评价函数的结果依次设置不同的分辨率，建立自适应分辨率的八叉树地图。

合并完成后，每个点云包围盒对应着一个点云概率P_h(Y＝1|X)，且环境越复杂的点云包围盒，其值就越大。将P_h按照大小顺序排序，根据P_h的值设置八叉树地图的分辨率。通过这种方式，地图中环境较复杂的位置，其八叉树地图的分辨率高，能够反映环境的细节；而地图中环境简单的位置，其八叉树地图的分辨率较低，在能够反映环境情况下，减少地图存储的空间。

当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (5)

1.一种基于点云密度分析的自适应分辨率八叉树建图方法，其特征在于，通过对环境点云地图的点云密度分析，根据环境复杂程度的不同自适应地设置八叉树地图的分辨率，其具体步骤如下：

步骤1：分割稠密点云地图，建立网格状的点云包围盒；

步骤2：根据点云密度计算函数计算点云包围盒内每个点的点云密度值，得到点云包围盒内的最大点云密度和点云密度的均值、标准差；具体包括：

首先，利用公式(4)遍历计算点云包围盒内每个点云点的密度大小：

式中，D_h,k表示第h个点云包围盒内的第k个点云点的点云密度，h＝1,2,…,N；(x_k,y_k,z_k)为第k个点云点的坐标，(x_j,y_j,z_j)表示点云包围盒内其他点云点的坐标；r_ax、r_ay、r_az表征了点云包围盒在x轴、y轴、z轴方向上的长度，M为点云包围盒内点云点的数量；

然后，比较得到该点云包围盒内的最大点云密度D_h,max，D_h,max表示第h个点云包围盒的最大点云密度，并利用公式(5)、(6)计算第h个点云包围盒内的点云密度均值μ_h与标准差σ_h：

步骤3：利用sigmoid函数建立点云包围盒的评价函数，分析每个点云包围盒的点云密度和均值、标准差；

sigmoid函数利用点云包围盒的最大密度D_h,max以及标准差σ_h建立，sigmoid函数的输出处于[0,1]之间，表示点云稀疏程度的概率；原理描述为：

令z＝c₀D_h,max+c₁σ_h，c₀、c₁为常数，根据经验选取；令X表示环境内的复杂程度，Y表示对应的环境稀疏程度，用[0,1]之间的实数表示，当空间内空无一物时为0，当空间内存在内容时为1；在输入为X，输出为Y＝1时，sigmoid函数的输出表示为：

P_h(Y＝1|X)＝g(z)＝(1+e^-z)^-1 (7)

同理，求解输出Y＝0的结果：

P_h(Y＝0|X)＝1-g(z)＝1-(1+e^-z)^-1 (8)

由公式(7)知，当z的值越接近正无穷，P_h(Y＝1|X)值越接近1，其实际意义是，由点云包围盒的最大点云密度和标准差决定的z值越大，表明该点云包围盒被填满的概率越高；

步骤4：分析相邻点云包围盒的评价函数结果，比较其相似性，对相似性满足设定的阈值条件的点云包围盒进行合并；

步骤5：在合并邻近点云包围盒后，对各个合并后点云包围盒的评价函数输出进行排序，根据评价函数的结果依次设置不同的分辨率，建立自适应分辨率的八叉树地图。

2.根据权利要求1所述的一种基于点云密度分析的自适应分辨率八叉树建图方法，其特征在于，所述步骤1具体包括：

利用视觉深度相机获取环境的稠密点云图后，对稠密点云进行分割，分割成点云包围盒进行处理；稠密点云分割采用主元分析法，利用主元分析法求解稠密点云图的最大外接包围盒，然后对最大外接包围盒进行N等分，进而将稠密点云分解成N个点云包围盒。

3.根据权利要求2所述的一种基于点云密度分析的自适应分辨率八叉树建图方法，其特征在于，所述主元分析法是通过协方差来计算主元，其具体过程为：

步骤1.1：根据公式(1)计算数据集的平均值

其中，P_i＝(x_i,y_i,z_i)表示单个点云的位置，稠密的点云是由M个点P₁,P₂,…,P_M组成的数据集；

步骤1.2：根据公式(2)求解数据集的协方差矩阵：

其中，

步骤1.3：对协方差矩阵Q，利用|Q-λI|＝0求解其特征值λ，其中，I表示单位矩阵；

步骤1.4：根据特征值求出Q的特征向量，对特征向量正交化、标准化，得到正交矩阵q＝[ε₁,ε₂,ε₃]；

步骤1.5：以ε₁、ε₂、ε₃作为稠密点云地图的主方向，沿三个主方向求出沿x、y、z三个方向最大和最小位置，获得6个顶点坐标A_i(i＝1,…6)；

步骤1.6：利用这6个顶点构建稠密点云的长方体形状的最大外接包围盒，然后将最大外接包围盒N等分，获得N个点云包围盒。

4.根据权利要求1所述的一种基于点云密度分析的自适应分辨率八叉树建图方法，其特征在于，所述步骤4具体包括：

步骤3中每个点云包围盒都有一个点云包围盒评价函数输出P_h(Y＝1|X)，通过比较近邻点云包围盒的评价函数输出P_i(Y＝1|X)、P_j(Y＝1|X)，若|P_i(Y＝1|X)-P_j(Y＝1|X)|<TH_P，则认为这两个邻近的点云包围盒相似度达到要求，将其合并，其中TH_P为预先设定的点云包围盒合并阈值。

5.根据权利要求4所述的一种基于点云密度分析的自适应分辨率八叉树建图方法，其特征在于，所述步骤5具体包括：

合并完成后，每个点云包围盒对应着一个点云概率P_h(Y＝1|X)，且环境越复杂的点云包围盒，其值就越大；将P_h按照大小顺序排序，根据P_h的值设置八叉树地图的分辨率。

Images