CN112200874B - 狭窄空间的多层次场景重建和快速分割方法、系统及装置 - Google Patents

Abstract

本发明属于机器人场景重建领域，具体涉及了一种狭窄空间的多层次场景重建和快速分割方法、系统及装置，旨在解决狭窄空间下机器人场景重建及分割无法兼顾重建精度与计算实时性的问题。本发明包括：取彩色图像、深度图像、相机标定数据及机器人空间位置和姿态信息；通过坐标变换将传感器数据转换为单帧点云；对单帧点云进行尺度划分，分别进行光线追踪和概率更新，得到尺度融合后的多层次场景地图；对场景地图执行两次降采样和一次升采样，借助尺度进行无损变换，并基于空间分割结果建立多个子八叉树地图，实现多层次场景重建和快速分割。本发明在不损失场景必要细节的前提下，实现了稠密重建及算法加速，更有利于实际工程场合的应用。

Description

狭窄空间的多层次场景重建和快速分割方法、系统及装置

技术领域

本发明属于机器人场景重建领域，具体涉及了一种狭窄空间的多层次场景重建和快速分割方法、系统及装置。

背景技术

机器人在未知环境下的导航、决策需要依赖对所在环境的感知，具体而言就是需要结合机器人的定位信息，重建作业场景的三维环境，并从该三维环境中分割作业场景的空间组成。

根据地图种类的不同，场景重建可分稀疏重建与稠密重建两类，前者一般只通过用于定位的路标点建立场景，而后者会通过传感器的所有测量点建立场景。在视觉里程计中，一般定位与建图同时共存，即在获得机器人位姿的同时已获得机器人所在环境的稀疏地图，而稠密地图则需要同时借助定位数据与传感器测量数据。稀疏重建的地图中存在大量的未知空间信息，并不能很好用于机器人导航。而稠密重建则存在精度与计算实时性的矛盾，即为了提高机器人导航精度，场景重建精度应尽可能高，而高分辨率的重建场景必然影响计算实时性，进而影响机器人的实时避障。

狭窄空间下，环境物体与机器人的距离分布极其不均，机器人在移动时主要依赖距离较近的环境进行避障，距离较远的环境只要能够明确空间的占据关系即可完成路径规划与导航，并不需要高分辨率地图，而现有稠密重建算法多基于全局一致的分辨率，无法在保证计算实时性的前提下实现狭窄空间的高分辨率场景重建。

发明内容

为了解决现有技术中的上述问题，即狭窄空间下机器人场景重建及分割无法兼顾重建精度与计算实时性的问题，本发明提供了一种狭窄空间的多层次场景重建和快速分割方法，该方法包括：

步骤S10，获取机器人场景重建和快速分割的传感器数据；所述传感器数据包括通过深度相机获得的彩色图像和深度图像、基于所述彩色图像和深度图像通过相机标定获得的深度相机标定数据以及基于所述彩色图像、深度图像和深度相机标定数据通过多传感器融合定位系统获得的机器人空间位置和姿态信息；所述相机标定数据包括相机内参矩阵、机器人坐标系到相机坐标系的第一映射关系；

步骤S20，基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息、相机内参矩阵以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系和到图像像素坐标系的第三映射关系，并基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据；

步骤S30，基于所述第二映射关系获取空间点与相机光心的距离，结合所述当前帧对应的点云数据和设定的距离阈值获取高、中、低层次的八叉树，并将各层次八叉树融合得到多层次场景重建的融合八叉树地图；

步骤S40，对所述融合八叉树地图进行层次调整，对层次调整获得的点云数据应用欧式聚类方法以及体素滤波，并在所述融合八叉树地图中查找相应八叉树聚类子集，获得融合八叉树地图的空间分割结果；

步骤S50，基于所述空间分割结果建立多个空间分割的子八叉树地图，所述多层次场景重建的融合八叉树地图和子八叉树地图为机器人场景重建和快速分割结果。

在一些优选的实施例中，步骤S20包括：

步骤S21，获取所述传感器数据中当前帧深度图像中像素点P_pixel位于像素坐标系下的像素坐标(u，v)和位于世界坐标系下的深度测量值d以及像素点P_pixel在当前帧深度图像对应的彩色图像中的色彩信息P_color，并结合所述第一映射关系确定世界坐标系到相机坐标系的第二映射关系T_cw；

步骤S22，基于相机内参矩阵K以及所述第二映射关系T_cw获取世界坐标系到图像像素坐标系的第三映射关系T_pw，并对像素点P_pixel的图像像素坐标系表示进行坐标转换，获得像素点P_pixel的世界坐标系表示；

步骤S23，重复步骤S21-步骤S22遍历当前帧深度图像中所有像素点，获得当前帧对应的点云数据C＝[t T_cw P]^T；其中，t为时间戳，P为各像素点对应的世界坐标表示集。

在一些优选的实施例中，所述像素点P_pixel的世界坐标系表示为：

其中，[x y z]为像素点P_pixel对应的空间点P_world在空间坐标系下的x轴、y轴和z轴坐标。

在一些优选的实施例中，所述各像素点对应的世界坐标表示集P为：

P＝[P₀…P_i]

其中，P_i＝[P_{world_i} P_{color_i}]，i代表遍历的图像像素序号。

在一些优选的实施例中，步骤S30包括：

步骤S31，基于所述第二映射关系获取空间点与相机光心的距离，并设定所述距离的高、中、低分层阈值；

步骤S32，根据所述高、中、低分层阈值，将所述当前帧对应的点云数据划分为高、中、低三个层次；

步骤S33，根据光线追踪算法判定点云数据所在空间的空间点是否存在测量值，若存在，则空间点被占据；若不存在测量值，且从相机光心到空间点的线段上没有物体，则空间点不被占据；

步骤S34，根据空间点的占据状态，计算其占据概率，并进行空间点状态更新；

步骤S35，重复步骤S33-步骤S34遍历点云数据所在空间的空间点，获得高、中、低三个层次的八叉树；

步骤S36，将所述高、中、低三个层次的八叉树进行尺度融合，获得多层次场景重建的融合八叉树地图。

在一些优选的实施例中，步骤S40包括：

步骤S41，对所述融合八叉树地图进行层次调整，输出高层次的八叉树的节点层次为m的低精度点云数据；

步骤S42，对所述低精度点云数据进行欧式聚类，获得所述低精度点云数据对应的聚类子集；

步骤S43，进行所述聚类子集的点云的体素滤波，将所述聚类子集的点云层次降至n，获得聚类体轮廓点云；

步骤S44，根据所述聚类体轮廓点云的中心坐标在原始高层次的八叉树中查找相应的n层节点，并展开节点至叶子层，得到高精度的八叉树聚类子集；

步骤S45，重复步骤S43-步骤S44遍历所述聚类子集中每一个聚类，获得融合八叉树地图的空间分割结果；

其中，m和n均为0-16的整数。

在一些优选的实施例中，所述节点层次m取值为14，n取值为12。

本发明的另一方面，提出了一种狭窄空间的多层次场景重建和快速分割系统，该系统包括数据获取与生成模块、坐标转换模块、场景重建模块、空间分割模块和输出模块；

所述数据获取与生成模块，配置为获取机器人场景重建和快速分割的彩色图像和深度图像、深度相机标定数据以及机器人空间位置和姿态信息；所述相机标定数据包括相机内参矩阵、机器人坐标系到相机坐标系的第一映射关系；

所述坐标转换模块，配置为基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息、相机内参矩阵以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系和到图像像素坐标系的第三映射关系，并基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据；

所述场景重建模块，配置为基于所述第二映射关系获取空间点与相机光心的距离，结合所述当前帧对应的点云数据和设定的距离阈值获取高、中、低层次的八叉树，并将各层次八叉树融合得到多层次场景重建的融合八叉树地图；

所述空间分割模块，配置对所述融合八叉树地图进行层次调整，对层次调整获得的点云数据应用欧式聚类方法以及体素滤波，并在所述融合八叉树地图中查找相应八叉树聚类子集，获得融合八叉树地图的空间分割结果；

所述输出模块，配置为基于所述空间分割结果建立多个空间分割的子八叉树地图，所述多层次场景重建的融合八叉树地图和子八叉树地图为机器人场景重建和快速分割结果。

在一些优选的实施例中，所述数据获取与生成模块包括数据获取模块、第一映射模块和数据生成模块；

所述数据获取模块，配置为通过获取通过深度相机获得机器人场景重建和快速分割的彩色图像和深度图像；

所述第一映射模块，配置为基于所述彩色图像和深度图像，通过相机标定获得深度相机标定数据；

所述数据生成模块，配置为基于所述彩色图像、深度图像和深度相机标定数据，通过多传感器融合定位系统获得机器人空间位置和姿态信息。

在一些优选的实施例中，所述坐标转换模块包括第二映射模块、第三映射模块和转换模块；

所述第二映射模块，配置为基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系；

所述第三映射模块，配置为基于所述第二映射关系，结合相机内参矩阵，确定世界坐标系到图像像素坐标系的第三映射关系；

所述转换模块，配置为基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据。

本发明的有益效果：

(1)本发明狭窄空间的多层次场景重建和快速分割方法，针对狭窄空间内机器人避障主要依赖较近的环境，对于距离较远的环境只需要明确空间的占据关系的特点，本发明将机器人环境根据设定的阈值划分为高、中、低三个层次，在高层次的环境重建时通过稠密重建尽可能提高场景重建精度，在低层次的环境重建时通过稀疏重建尽可能加快重建速度，兼顾了狭窄空间下机器人场景重建及分割的重建精度与计算实时性。

(2)本发明狭窄空间的多层次场景重建和快速分割方法，针对欧式聚类前都需进行降采样体素滤波才能够实时进行点云聚类，但降采样滤波属于有损压缩，会降低聚类体精度的问题，本发明借助八叉树结构层次可变的特性，直接获取设定节点层次的低精度点云数据，在进行低精度点云数据的欧式聚类后再进行降采样体素滤波和高精度八叉树的重构，在保证空间分割精度的基础上通过KD-Tree加速了算法，进一步提高了算法的实时性。

附图说明

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

图1是本发明狭窄空间的多层次场景重建和快速分割方法的流程示意图；

图2是本发明狭窄空间的多层次场景重建和快速分割方法一种实施例的单帧点云建立流程图；

图3是本发明狭窄空间的多层次场景重建和快速分割方法一种实施例的多层次场景重建流程图；

图4是本发明狭窄空间的多层次场景重建和快速分割方法一种实施例的层次划分示意图；

图5是本发明狭窄空间的多层次场景重建和快速分割方法一种实施例的空间分割流程图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

本发明提供一种狭窄空间的多层次场景重建和快速分割方法，针对狭窄空间的环境特点，提出相应的多层次动态场景实时构建方案，能够在保证导航、避障所需精度要求的前提下，提高建图速度。该方法还基于八叉树结构改进了传统欧式聚类算法，使环境场景的分割能够在保留输出聚类点云细节的同时，加快运算速度。

本发明的一种狭窄空间的多层次场景重建和快速分割方法，该方法包括：

步骤S10，获取机器人场景重建和快速分割的传感器数据；所述传感器数据包括通过深度相机获得的彩色图像和深度图像、基于所述彩色图像和深度图像通过相机标定获得的深度相机标定数据以及基于所述彩色图像、深度图像和深度相机标定数据通过多传感器融合定位系统获得的机器人空间位置和姿态信息；所述相机标定数据包括相机内参矩阵、机器人坐标系到相机坐标系的第一映射关系；

步骤S20，基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息、相机内参矩阵以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系和到图像像素坐标系的第三映射关系，并基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据；

步骤S30，基于所述第二映射关系获取空间点与相机光心的距离，结合所述当前帧对应的点云数据和设定的距离阈值获取高、中、低层次的八叉树，并将各层次八叉树融合得到多层次场景重建的融合八叉树地图；

步骤S40，对所述融合八叉树地图进行层次调整，对层次调整获得的点云数据应用欧式聚类方法以及体素滤波，并在所述融合八叉树地图中查找相应八叉树聚类子集，获得融合八叉树地图的空间分割结果；

步骤S50，基于所述空间分割结果建立多个空间分割的子八叉树地图，所述多层次场景重建的融合八叉树地图和子八叉树地图为机器人场景重建和快速分割结果。

为了更清晰地对本发明狭窄空间的多层次场景重建和快速分割方法进行说明，下面结合图1对本发明实施例中各步骤展开详述。

本发明第一实施例的狭窄空间的多层次场景重建和快速分割方法，包括步骤S10-步骤S50，各步骤详细描述如下：

步骤S10，获取机器人场景重建和快速分割的传感器数据。

传感器数据包括通过深度相机获得的彩色图像和深度图像、基于所述彩色图像和深度图像通过相机标定获得的深度相机标定数据以及基于所述彩色图像、深度图像和深度相机标定数据通过多传感器融合定位系统获得的机器人空间位置和姿态信息；所述相机标定数据包括相机内参矩阵、机器人坐标系到相机坐标系的第一映射关系。机器人空间位置和姿态信息通过多传感器融合定位系统得到，具体包括：

步骤W10，获取多传感器融合定位系统的机器人多传感器数据，包括结构光深度相机采集的深度图像和彩色图像，MARG传感器采集的三轴加速度、角速度和地磁场强度以及关节编码器采集的机器人关节的角位移数据。

步骤W20，标定与校准结构光深度相机、MARG传感器和关节编码器，并基于标定与校准结果对机器人多传感器数据序列进行修正。

步骤W30，通过预设的深度相机视觉增强方法对修正后的机器人多传感器数据中的视觉数据进行增强与修复。

彩色图的自适应图像增强与修复，其具体过程包括：

步骤W31，通过多尺度高斯滤波器获取配准的彩色图和深度图对中的彩色图的各个点光源共同作用下受光面的照度分量分布图，并对所述照度分量分布图进行区域采样，获得每一个单个点光源独立作用下的照度分量，基于所述单个点光源独立作用下的照度分量通过反馈调节进行所述彩色图的主动亮度均衡。

步骤W32，计算主动亮度均衡后彩色图各像素值的均值和标准差，构建模糊推理系统，并将均值和标准差作为系统输入变量，结合预先设定的隶属度函数和模糊规则通过模糊推理获得限制对比度自适应直方图均衡化算法中的最佳裁剪阈值和伽马校正算法中的最佳伽马校正系数。

步骤W33，基于最佳伽马校正系数，通过伽马校正算法进行亮度均衡彩色图的自适应亮度均衡；基于最佳裁剪阈值，对自适应亮度均衡后的图像进行限制对比度自适应直方图均衡，并进行双边滤波，获得增强彩色图。

步骤W34，将增强彩色图降采样至设定分辨率，一般为256×192或320×240，通过MeanShift算法将降采样后的图像中的相似纹理区平滑为相同的颜色，通过FloodFill算法提取平滑后的图像中相应的颜色连通域形成纹理区掩膜。

步骤W35，通过增强彩色图的纹理特征提取配准的彩色图和深度图对中的深度图中的ROI区域，获得深度图的相似纹理区域集合，并获取每一个区域内深度测量值的范围，将大于深度相机最大量程的测量值划分为无效测量点，属于正常范围的测量值划分为有效测量点。

步骤W36，计算有效测量点数量与无效测量点数量的比值，若比值小于设定阈值，则终止修复；否则，通过RANSAC算法拟合深度图的相似纹理区域内的有效测量点，获得有效测量点局部点云拟合模型。

步骤W37，以有效测量点的实际值与其模型估计值之间的误差小于等于设定阈值的点作为内点，以有效测量点的实际值与其模型估计值之间的误差大于等于设定阈值的点作为外点，若内点与外点的比值小于设定阈值，则终止修复；否则，根据相机投影模型和所述局部点云拟合模型对相似纹理区域内的无效测量点进行深度值重计算。

步骤W38，重复步骤W35至步骤W38，直到深度图的相似纹理区域集合的每一个区域完成无效测量点的修复，获得增强深度图。

本发明一个实施例中，采用上述过程进行视觉数据的增强与修复，其他实施例中，还可以通过其他方法实现视觉数据的增强与修复，例如文献：“刘俊毅.彩色图像引导的深度图像增强[D].浙江大学,2014.”和“钟宇彤.飞行时间法三维相机的深度图像增强技术研究[D].2017.”等，本发明在此不一一详述。

步骤W40，进行增强与修复的视觉数据序列的预融合处理，获得第一预融合位姿序列；进行修正后的修正后的运动状态数据序列的预融合处理，获得预融合姿态序列；进行修正后的角位移数据序列的预融合处理，获得第二预融合位姿序列。

视觉数据包含彩色图像和深度图像，借助彩色图像的特征点信息和深度图像的深度值信息，可以实现相机位姿的状态估计，即可构成相应的视觉里程计。因此，本发明针对复杂环境的特点对传统的视觉里程计进行了部分改进，增强与修复的视觉数据序列的预融合处理包括：

步骤W411，提取所述增强与修复的视觉数据序列中一帧彩色图的ORB二维特征点，并在与当前帧彩色图配准的深度图中提取所述ORB二维特征点对应的深度测量值，若能提取深度测量值，则将该ORB二维特征点作为ORB三维特征点；否则，则仍作为ORB二维特征点；

步骤W412，将当前帧的所有特征点与当前帧的前一帧的所有的特征点进行匹配追踪，获得特征点对，若所述特征点对中的特征点均为ORB三维特征点，则通过ICP算法得到两帧之间的位姿变换矩阵；否则，通过PnP算法得到两帧之间的位姿变换矩阵；

步骤W413，判断ORB三维特征点的追踪是否丢失，若未丢失，则直接将追踪得到的相机位姿变换矩阵作为新的关键帧加入已有的关键帧序列中；若已丢失，则调用当前帧对应的关节编码器和MARG传感器的预融合测量值作为新关键帧序列中初始关键帧的相机位姿变换矩阵；

步骤W414，将获取的关键帧序列进行位姿图优化，获得待优化相机位姿序列，并进行所述待优化相机位姿序列的闭环修正，获得第一预融合位姿序列。

由于传感器制造精度和作业环境干扰的影响，MARG传感器原始测量数据的噪声一般较大，若直接引入融合定位系统，会降低系统整体的定位精度。因此，本发明对MARG传感器的原始数据进行了预融合处理。修正后的运动状态数据序列的预融合处理包括：

步骤W421，获取k-1时刻的第一状态量和第一控制量；所述第一状态量包括所述MARG传感器载体坐标系到地理坐标系的旋转矩阵的四元数取值以及MARG传感器在X、Y、Z轴上的角速度累计漂移值；所述第一控制量包括X、Y、Z轴角速度真值。

步骤W422，基于所述k-1时刻的第一状态量和第一控制量，根据第一状态转移函数得到第k时刻的第一状态量的预测值。

步骤W423，基于扩展卡尔曼滤波算法，通过第k时刻的第一观测量、第一观测函数、第一过程协方差矩阵、第一噪声协方差矩阵以及第k时刻第一状态量的预测值，获得第k时刻预融合的第一状态量，k个时刻的预融合的第一状态量为预融合姿态序列；第k时刻的第一观测量包括MARG传感器获取的三轴加速度序列以及地磁场强度序列。

多自由度冗余结构可以提高机器人在复杂环境的运动灵活度，本发明具体实施中即采用多自由度结构，通过机器人正运动学解算将各关节的角位移转换为机器人末端的位姿状态。修正后的角位移数据序列的预融合处理包括：

步骤W431，将关节旋转轴线或移动方向作为Z轴，则修正后的角位移数据序列即为绕坐标系Z轴旋转角，根据机器人的预先设计的机构模型，获取机器人相邻关节之间的连杆长度、Z轴偏移距离以及相邻关节的Z轴夹角；

步骤W432，基于步骤W431获得的数据结合机器人第n个关节相对于第n-1个关节的位姿变换矩阵，得到第二预融合位姿序列。

步骤W50，基于第一预融合位姿序列、第二预融合位姿序列和预融合姿态序列对机器人在环境中的空间状态进行融合定位，获得机器人在环境中的空间位置和姿态信息。

步骤W51，获取k-1时刻的第二状态量和第二控制量；所述第二状态量包括机器人末端执行器的位置以及机器人末端执行器的姿态欧拉角；所述第二控制量包括机器人末端执行器的速度、加速度以及角速度。

步骤W52，基于所述k-1时刻的第二状态量和第二控制量，根据第二状态转移函数得到第k时刻第二状态量的预测值。

步骤W53，获取第一预融合位姿序列、第二预融合位姿序列和预融合姿态序列作为第二观测量，对第二观测量进行数据同步，并判断能否形成同步数据，若能则状态更新矩阵为绝对状态更新矩阵，若不能则状态更新矩阵为相对状态更新矩阵。

步骤W54，基于扩展卡尔曼滤波算法，通过第k时刻的第二观测量、状态更新矩阵、第二噪声协方差矩阵以及第k时刻第二状态量的预测值，获得第k时刻融合的第二状态量。

步骤W55，将第k时刻融合后的第二状态量与第k时刻预测的第二状态量之间的偏差以及偏差绝对值的变化率作为二维Mamdani模糊方法的输入模糊变量，将第二噪声协方差矩阵作为输出模糊变量，利用模糊推理对k+1时刻的第二噪声协方差矩阵进行自适应调整。

步骤W56，依次输出第k时刻融合第二状态量，作为机器人在环境中的空间位置和姿态信息。

本发明一个实施例中，采用上述过程进行机器人空间位置和姿态信息的获取，其他实施例中，还可以通过其他方法实现机器人空间位置和姿态信息的获取，例如文献：“王京，基于传感器数据融合的单目视觉SLAM方法研究[D]，清华大学，2017.”和“李金良、孙友霞、谷明霞等，基于多传感器融合的移动机器人SLAM[J]，中国科技论文，2012.”等，本发明在此不一一详述。

步骤S20，基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息、相机内参矩阵以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系和到图像像素坐标系的第三映射关系，并基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据。

深度相机所采集的原始数据为图像像素坐标系下的空间信息，但场景重建的三维地图位于世界坐标系下，因此需要进行坐标系变换。地图场景的稠密拼接，从本质上即是通过获得的坐标系映射关系，将每次观测得到的图像像素坐标系下的像素点，变换至世界坐标系下空间点的过程。如图2所示，为本发明狭窄空间的多层次场景重建和快速分割方法一种实施例的单帧点云建立流程图，其建立过程具体包括：

步骤S21，获取所述传感器数据中当前帧深度图像中像素点P_pixel位于像素坐标系下的像素坐标(u，v)和位于世界坐标系下的深度测量值d以及像素点P_pixel在当前帧深度图像对应的彩色图像中的色彩信息P_color，并结合所述第一映射关系确定世界坐标系到相机坐标系的第二映射关系T_cw。P_pixel在像素坐标系下的表示如式(1)所示：

P_pixel＝[u v d]^T (1)

像素点P_pixel在当前帧深度图像对应的彩色图像中的色彩信息P_color如式(2)所示：

P_color＝[r g b]^T (2)

其中，[r g b]为像素点P_pixel在彩色图像中的R通道、G通道和B通道的彩色信息。

步骤S22，基于相机内参矩阵K以及所述第二映射关系T_cw获取世界坐标系到图像像素坐标系的第三映射关系T_pw，并对像素点P_pixel的图像像素坐标系表示进行坐标转换，获得像素点P_pixel的世界坐标系表示，即空间点P_world，如式(3)所示：

其中，[x y z]为像素点P_pixel对应的空间点P_world在空间坐标系下的x轴、y轴和z轴坐标。

步骤S23，重复步骤S21-步骤S22遍历当前帧深度图像中所有像素点，获得当前帧对应的点云数据C＝[t T_cw P]^T；其中，t为时间戳，P为各像素点对应的世界坐标表示集。

各像素点对应的世界坐标表示集P如式(4)所示：

P＝[P₀…P_i] (4)

其中，P_i＝[P_{world_i} P_{color_i}]，i代表遍历的图像像素序号。

步骤S30，基于所述第二映射关系获取空间点与相机光心的距离，结合所述当前帧对应的点云数据和设定的距离阈值获取高、中、低层次的八叉树，并将各层次八叉树融合得到多层次场景重建的融合八叉树地图。

重建精度与计算实时性是场景重建过程中最大的矛盾，狭长空间下，环境物体与机器人的距离分布极其不均，机器人在移动时主要依赖距离较近的环境进行避障，距离较远的环境只要能够明确空间的占据关系即可完成路径规划与导航，并不需要高分辨率地图，本发明提出相应的多层次动态场景实时构建方案用以改善这一问题。如图3所示，为本发明狭窄空间的多层次场景重建和快速分割方法一种实施例的多层次场景重建流程图，其重建过程具体包括：

步骤S31，基于所述第二映射关系获取空间点与相机光心的距离，并设定所述距离的高、中、低分层阈值。

分层阈值，一般高层次阈值不应小于机器人的工作空间，中、低层次阈值则与设备配置相关，若配置较低，需提高实时性，可将阈值尽量设小。

步骤S32，根据所述高、中、低分层阈值，将所述当前帧对应的点云数据划分为高、中、低三个层次。

如图4所示，为本发明狭窄空间的多层次场景重建和快速分割方法一种实施例的层次划分示意图，高层次的阈值设定为0.01m，中层次的阈值设定为0.05m，低层次的阈值设定为0.1m。

步骤S33，根据光线追踪算法判定点云数据所在空间的空间点是否存在测量值，若存在，则空间点被占据；若不存在测量值，且从相机光心到空间点的线段上没有物体，则空间点不被占据。

步骤S34，根据空间点的占据状态，计算其占据概率，并进行空间点状态更新。

在单次测量中，若空间点被占据，则空间点的占据概率设定为击中阈值，本发明一个实施例设定击中阈值为0.7，若空间点不被占据，则空间点的占据概率设定为穿越阈值，本发明一个实施例设定穿越阈值为0.4。在多次观测中，若空间点的历史占据概率值超过上限值或低于下限值则不再进行占据概率对数值更新，本发明一个实施例中上限值与下限值设定为0.97与0.12。对于单个空间节点，若占据概率值超过占据阈值，则认定为空间点被占据，本发明一个实施例中设定占据阈值为0.5。

步骤S35，重复步骤S33-步骤S34遍历点云数据所在空间的空间点，获得高、中、低三个层次的八叉树。

步骤S36，将所述高、中、低三个层次的八叉树进行尺度融合，获得多层次场景重建的融合八叉树地图。

尺度融合以高层次八叉树地图为基准，将中、低层次八叉树地图中节点的中心坐标加入高层次八叉树地图中，通过该操作可实现稠密的高层次地图与稀疏的低层次地图的共存，即实现尺度融合。

步骤S40，对所述融合八叉树地图进行层次调整，对层次调整获得的点云数据应用欧式聚类方法以及体素滤波，并在所述融合八叉树地图中查找相应八叉树聚类子集，获得融合八叉树地图的空间分割结果。

对地图场景进行语义识别的基础是实现对点云的聚类分割。欧式聚类是一种基于欧氏距离的基本聚类算法，由于其需要循环遍历空间点，虽然通过KD-Tree的结构加速了算法，但一般在欧式聚类前都需进行降采样体素滤波才能够实时进行点云聚类，而点云的体素滤波本身属于有损压缩，因此得到的聚类体精度较之原始数据存在下降，本发明则基于八叉树结构，提出一种能够保留输出聚类点云细节的快速欧式聚类方法。如图5所示，为本发明狭窄空间的多层次场景重建和快速分割方法一种实施例的空间分割流程图，其空间分割过程具体包括：

步骤S41，对所述融合八叉树地图进行层次调整，输出高层次的八叉树的节点层次为m的低精度点云数据。

步骤S42，对所述低精度点云数据进行欧式聚类，获得所述低精度点云数据对应的聚类子集。

步骤S43，进行所述聚类子集的点云的体素滤波，将所述聚类子集的点云层次降至n，获得聚类体轮廓点云。

步骤S44，根据所述聚类体轮廓点云的中心坐标在原始高层次的八叉树中查找相应的n层节点，并展开节点至叶子层，得到高精度的八叉树聚类子集。

步骤S45，重复步骤S43-步骤S44遍历所述聚类子集中每一个聚类，获得融合八叉树地图的空间分割结果。

其中，m和n均为0-16的整数。

当m过小，耗时的减小程度并不可观，反而会出现距离较近的聚类体粘连的现象；当n过小，虽然耗时大大减小，但升层次后的复原点云容易出现边界误判的问题。本发明一个实施例中，通过实验确定当m取值为14，n取值为12时，提取的聚类点云与直接进行欧式聚类完全一致，但耗时仅为16.5％，具有很好的工程应用意义。

步骤S50，基于所述空间分割结果建立多个空间分割的子八叉树地图，所述多层次场景重建的融合八叉树地图和子八叉树地图为机器人场景重建和快速分割结果。

在数据采集环节，本发明获取彩色图像、深度图像、相机内参以及空间位置和姿态信息。在多层次场景重建环节，本发明基于相机投影模型，通过坐标变换将传感器数据转换为单帧点云，再对单帧点云进行尺度划分，分别进行光线追踪和概率更新，得到尺度融合后的多层次场景地图。在快速欧式聚类空间分割环节，本发明对场景地图执行两次降采样和一次升采样，借助尺度的无损变换，加速聚类分割计算的同时，不损失聚类结果的精度。本发明在不损失场景必要细节的前提下，实现了稠密重建及欧式聚类的算法加速，更有利于实际工程场合的应用。

本发明第二实施例的狭窄空间的多层次场景重建和快速分割系统，该系统包括数据获取与生成模块、坐标转换模块、场景重建模块、空间分割模块和输出模块；

所述数据获取与生成模块，配置为获取机器人场景重建和快速分割的彩色图像和深度图像、深度相机标定数据以及机器人空间位置和姿态信息；所述相机标定数据包括相机内参矩阵、机器人坐标系到相机坐标系的第一映射关系；

所述坐标转换模块，配置为基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息、相机内参矩阵以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系和到图像像素坐标系的第三映射关系，并基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据；

所述场景重建模块，配置为基于所述第二映射关系获取空间点与相机光心的距离，结合所述当前帧对应的点云数据和设定的距离阈值获取高、中、低层次的八叉树，并将各层次八叉树融合得到多层次场景重建的融合八叉树地图；

所述空间分割模块，配置对所述融合八叉树地图进行层次调整，对层次调整获得的点云数据应用欧式聚类方法以及体素滤波，并在所述融合八叉树地图中查找相应八叉树聚类子集，获得融合八叉树地图的空间分割结果；

所述输出模块，配置为基于所述空间分割结果建立多个空间分割的子八叉树地图，所述多层次场景重建的融合八叉树地图和子八叉树地图为机器人场景重建和快速分割结果。

其中，数据获取与生成模块包括数据获取模块、第一映射模块和数据生成模块；

所述数据获取模块，配置为通过获取通过深度相机获得机器人场景重建和快速分割的彩色图像和深度图像；

所述第一映射模块，配置为基于所述彩色图像和深度图像，通过相机标定获得深度相机标定数据；

所述数据生成模块，配置为基于所述彩色图像、深度图像和深度相机标定数据，通过多传感器融合定位系统获得机器人空间位置和姿态信息。

坐标转换模块包括第二映射模块、第三映射模块和转换模块；

所述第二映射模块，配置为基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系；

所述第三映射模块，配置为基于所述第二映射关系，结合相机内参矩阵，确定世界坐标系到图像像素坐标系的第三映射关系；

所述转换模块，配置为基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据。

所属技术领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统的具体工作过程及有关说明，可以参考前述方法实施例中的对应过程，在此不再赘述。

需要说明的是，上述实施例提供的狭窄空间的多层次场景重建和快速分割系统，仅以上述各功能模块的划分进行举例说明，在实际应用中，可以根据需要而将上述功能分配由不同的功能模块来完成，即将本发明实施例中的模块或者步骤再分解或者组合，例如，上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块，以完成以上描述的全部或者部分功能。对于本发明实施例中涉及的模块、步骤的名称，仅仅是为了区分各个模块或者步骤，不视为对本发明的不当限定。

本发明第三实施例的一种存储装置，其中存储有多条程序，所述程序适于由处理器加载并执行以实现上述的狭窄空间的多层次场景重建和快速分割方法。

本发明第四实施例的一种处理装置，包括处理器、存储装置；处理器，适于执行各条程序；存储装置，适于存储多条程序；所述程序适于由处理器加载并执行以实现上述的狭窄空间的多层次场景重建和快速分割方法。

所属技术领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的存储装置、处理装置的具体工作过程及有关说明，可以参考前述方法实施例中的对应过程，在此不再赘述。

本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的模块、方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，软件模块、方法步骤对应的程序可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

术语“第一”、“第二”等是用于区别类似的对象，而不是用于描述或表示特定的顺序或先后次序。

术语“包括”或者任何其它类似用语旨在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备/装置不仅包括那些要素，而且还包括没有明确列出的其它要素，或者还包括这些过程、方法、物品或者设备/装置所固有的要素。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征做出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。

Claims (10)

1.一种狭窄空间的多层次场景重建和快速分割方法，其特征在于，该方法包括：

步骤S10，获取机器人场景重建和快速分割的传感器数据；所述传感器数据包括通过深度相机获得的彩色图像和深度图像、基于所述彩色图像和深度图像通过相机标定获得的深度相机标定数据以及基于所述彩色图像、深度图像和深度相机标定数据通过多传感器融合定位系统获得的机器人空间位置和姿态信息；所述相机标定数据包括相机内参矩阵、机器人坐标系到相机坐标系的第一映射关系；

步骤S20，基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息、相机内参矩阵以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系和到图像像素坐标系的第三映射关系，并基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据；

步骤S30，基于所述第二映射关系获取空间点与相机光心的距离，结合所述当前帧对应的点云数据和设定的距离阈值获取高、中、低层次的八叉树，并将各层次八叉树融合得到多层次场景重建的融合八叉树地图；

步骤S40，对所述融合八叉树地图进行层次调整，对层次调整获得的点云数据应用欧式聚类方法以及体素滤波，并在所述融合八叉树地图中查找相应八叉树聚类子集，获得融合八叉树地图的空间分割结果；

步骤S50，基于所述空间分割结果建立多个空间分割的子八叉树地图，所述多层次场景重建的融合八叉树地图和子八叉树地图为机器人场景重建和快速分割结果。

2.根据权利要求1所述的狭窄空间的多层次场景重建和快速分割方法，其特征在于，步骤S20包括：

步骤S21，获取所述传感器数据中当前帧深度图像中像素点P_pixel位于像素坐标系下的像素坐标(u，v)和位于世界坐标系下的深度测量值d以及像素点P_pixel在当前帧深度图像对应的彩色图像中的色彩信息P_color，并结合所述第一映射关系确定世界坐标系到相机坐标系的第二映射关系T_cw；

步骤S22，基于相机内参矩阵K以及所述第二映射关系T_cw获取世界坐标系到图像像素坐标系的第三映射关系T_pw，并对像素点P_pixel的图像像素坐标系表示进行坐标转换，获得像素点P_pixel的世界坐标系表示；

步骤S23，重复步骤S21-步骤S22遍历当前帧深度图像中所有像素点，获得当前帧对应的点云数据C＝[t T_cw P]^T；其中，t为时间戳，P为各像素点对应的世界坐标表示集。

3.根据权利要求2所述的狭窄空间的多层次场景重建和快速分割方法，其特征在于，所述像素点P_pixel的世界坐标系表示为：

其中，[x y z]为像素点P_pixel对应的空间点P_world在空间坐标系下的x轴、y轴和z轴坐标。

4.根据权利要求2所述的狭窄空间的多层次场景重建和快速分割方法，其特征在于，所述各像素点对应的世界坐标表示集P为：

P＝[P₀ … P_i]

其中，P_i＝[P_{world_i} P_{color_i}]，i代表遍历的图像像素序号。

5.根据权利要求1所述的狭窄空间的多层次场景重建和快速分割方法，其特征在于，步骤S30包括：

步骤S31，基于所述第二映射关系获取空间点与相机光心的距离，并设定所述距离的高、中、低分层阈值；

步骤S32，根据所述高、中、低分层阈值，将所述当前帧对应的点云数据划分为高、中、低三个层次；

步骤S33，根据光线追踪算法判定点云数据所在空间的空间点是否存在测量值，若存在，则空间点被占据；若不存在测量值，且从相机光心到空间点的线段上没有物体，则空间点不被占据；

步骤S34，根据空间点的占据状态，计算其占据概率，并进行空间点状态更新；

步骤S35，重复步骤S33-步骤S34遍历点云数据所在空间的空间点，获得高、中、低三个层次的八叉树；

步骤S36，将所述高、中、低三个层次的八叉树进行尺度融合，获得多层次场景重建的融合八叉树地图。

6.根据权利要求1所述的狭窄空间的多层次场景重建和快速分割方法，其特征在于，步骤S40包括：

步骤S41，对所述融合八叉树地图进行层次调整，输出高层次的八叉树的节点层次为m的低精度点云数据；

步骤S42，对所述低精度点云数据进行欧式聚类，获得所述低精度点云数据对应的聚类子集；

步骤S43，进行所述聚类子集的点云的体素滤波，将所述聚类子集的点云层次降至n，获得聚类体轮廓点云；

步骤S44，根据所述聚类体轮廓点云的中心坐标在原始高层次的八叉树中查找相应的n层节点，并展开节点至叶子层，得到高精度的八叉树聚类子集；

步骤S45，重复步骤S43-步骤S44遍历所述聚类子集中每一个聚类，获得融合八叉树地图的空间分割结果；

其中，m和n均为0-16的整数。

7.根据权利要求6所述的狭窄空间的多层次场景重建和快速分割方法，其特征在于，所述节点层次m取值为14，n取值为12。

8.一种狭窄空间的多层次场景重建和快速分割系统，其特征在于，该系统包括数据获取与生成模块、坐标转换模块、场景重建模块、空间分割模块和输出模块；

所述数据获取与生成模块，配置为获取机器人场景重建和快速分割的彩色图像和深度图像、深度相机标定数据以及机器人空间位置和姿态信息；所述相机标定数据包括相机内参矩阵、机器人坐标系到相机坐标系的第一映射关系；

所述坐标转换模块，配置为基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息、相机内参矩阵以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系和到图像像素坐标系的第三映射关系，并基于所述第三映射关系进行传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据；

所述场景重建模块，配置为基于所述第二映射关系获取空间点与相机光心的距离，结合所述当前帧对应的点云数据和设定的距离阈值获取高、中、低层次的八叉树，并将各层次八叉树融合得到多层次场景重建的融合八叉树地图；

所述空间分割模块，配置对所述融合八叉树地图进行层次调整，对层次调整获得的点云数据应用欧式聚类方法以及体素滤波，并在所述融合八叉树地图中查找相应八叉树聚类子集，获得融合八叉树地图的空间分割结果；

所述输出模块，配置为基于所述空间分割结果建立多个空间分割的子八叉树地图，所述多层次场景重建的融合八叉树地图和子八叉树地图为机器人场景重建和快速分割结果。

9.根据权利要求8所述的狭窄空间的多层次场景重建和快速分割系统，其特征在于，所述数据获取与生成模块包括数据获取模块、第一映射模块和数据生成模块；

所述数据获取模块，配置为通过获取通过深度相机获得机器人场景重建和快速分割的彩色图像和深度图像；

所述第一映射模块，配置为基于所述彩色图像和深度图像，通过相机标定获得深度相机标定数据；

所述数据生成模块，配置为基于所述彩色图像、深度图像和深度相机标定数据，通过多传感器融合定位系统获得机器人空间位置和姿态信息。

10.根据权利要求9所述的狭窄空间的多层次场景重建和快速分割系统，其特征在于，所述坐标转换模块包括第二映射模块、第三映射模块和转换模块；

所述第二映射模块，配置为基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系；

所述第三映射模块，配置为基于所述第二映射关系，结合相机内参矩阵，确定世界坐标系到图像像素坐标系的第三映射关系；

所述转换模块，配置为基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据。

Images