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

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

【技术实现步骤摘要】
狭窄空间的多层次场景重建和快速分割方法、系统及装置
本专利技术属于机器人场景重建领域，具体涉及了一种狭窄空间的多层次场景重建和快速分割方法、系统及装置。
技术介绍
机器人在未知环境下的导航、决策需要依赖对所在环境的感知，具体而言就是需要结合机器人的定位信息，重建作业场景的三维环境，并从该三维环境中分割作业场景的空间组成。根据地图种类的不同，场景重建可分稀疏重建与稠密重建两类，前者一般只通过用于定位的路标点建立场景，而后者会通过传感器的所有测量点建立场景。在视觉里程计中，一般定位与建图同时共存，即在获得机器人位姿的同时已获得机器人所在环境的稀疏地图，而稠密地图则需要同时借助定位数据与传感器测量数据。稀疏重建的地图中存在大量的未知空间信息，并不能很好用于机器人导航。而稠密重建则存在精度与计算实时性的矛盾，即为了提高机器人导航精度，场景重建精度应尽可能高，而高分辨率的重建场景必然影响计算实时性，进而影响机器人的实时避障。狭窄空间下，环境物体与机器人的距离分布极其不均，机器人在移动时主要依赖距离较近的环境进行避障，距离较远的环境只要能够明确空间的占据关系即可完成路径规划与导航，并不需要高分辨率地图，而现有稠密重建算法多基于全局一致的分辨率，无法在保证计算实时性的前提下实现狭窄空间的高分辨率场景重建。
技术实现思路
为了解决现有技术中的上述问题，即狭窄空间下机器人场景重建及分割无法兼顾重建精度与计算实时性的问题，本专利技术提供了一种狭窄空间的多层次场景重建和快速分割方法，该方法包括：步骤S10，获取机器人场景重建和快速分割的传感器数据；所述传感器数据包括通过深度相机获得的彩色图像和深度图像、基于所述彩色图像和深度图像通过相机标定获得的深度相机标定数据以及基于所述彩色图像、深度图像和深度相机标定数据通过多传感器融合定位系统获得的机器人空间位置和姿态信息；所述相机标定数据包括相机内参矩阵、机器人坐标系到相机坐标系的第一映射关系；步骤S20，基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息、相机内参矩阵以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系和到图像像素坐标系的第三映射关系，并基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据；步骤S30，基于所述第二映射关系获取空间点与相机光心的距离，结合所述当前帧对应的点云数据和设定的距离阈值获取高、中、低层次的八叉树，并将各层次八叉树融合得到多层次场景重建的融合八叉树地图；步骤S40，对所述融合八叉树地图进行层次调整，对层次调整获得的点云数据应用欧式聚类方法以及体素滤波，并在所述融合八叉树地图中查找相应八叉树聚类子集，获得融合八叉树地图的空间分割结果；步骤S50，基于所述空间分割结果建立多个空间分割的子八叉树地图，所述多层次场景重建的融合八叉树地图和子八叉树地图为机器人场景重建和快速分割结果。在一些优选的实施例中，步骤S20包括：步骤S21，获取所述传感器数据中当前帧深度图像中像素点Ppixel位于像素坐标系下的像素坐标(u，v)和位于世界坐标系下的深度测量值d以及像素点Ppixel在当前帧深度图像对应的彩色图像中的色彩信息Pcolor，并结合所述第一映射关系确定世界坐标系到相机坐标系的第二映射关系Tcw；步骤S22，基于相机内参矩阵K以及所述第二映射关系Tcw获取世界坐标系到图像像素坐标系的第三映射关系Tpw，并对像素点Ppixel的图像像素坐标系表示进行坐标转换，获得像素点Ppixel的世界坐标系表示；步骤S23，重复步骤S21-步骤S22遍历当前帧深度图像中所有像素点，获得当前帧对应的点云数据C＝[tTcwP]T；其中，t为时间戳，P为各像素点对应的世界坐标表示集。在一些优选的实施例中，所述像素点Ppixel的世界坐标系表示为：其中，[xyz]为像素点Ppixel对应的空间点Pworld在空间坐标系下的x轴、y轴和z轴坐标。在一些优选的实施例中，所述各像素点对应的世界坐标表示集P为：P＝[P0…Pi]其中，Pi＝[Pworld_iPcolor_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.一种狭窄空间的多层次场景重建和快速分割方法，其特征在于，该方法包括：/n步骤S10，获取机器人场景重建和快速分割的传感器数据；所述传感器数据包括通过深度相机获得的彩色图像和深度图像、基于所述彩色图像和深度图像通过相机标定获得的深度相机标定数据以及基于所述彩色图像、深度图像和深度相机标定数据通过多传感器融合定位系统获得的机器人空间位置和姿态信息；所述相机标定数据包括相机内参矩阵、机器人坐标系到相机坐标系的第一映射关系；/n步骤S20，基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息、相机内参矩阵以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系和到图像像素坐标系的第三映射关系，并基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据；/n步骤S30，基于所述第二映射关系获取空间点与相机光心的距离，结合所述当前帧对应的点云数据和设定的距离阈值获取高、中、低层次的八叉树，并将各层次八叉树融合得到多层次场景重建的融合八叉树地图；/n步骤S40，对所述融合八叉树地图进行层次调整，对层次调整获得的点云数据应用欧式聚类方法以及体素滤波，并在所述融合八叉树地图中查找相应八叉树聚类子集，获得融合八叉树地图的空间分割结果；/n步骤S50，基于所述空间分割结果建立多个空间分割的子八叉树地图，所述多层次场景重建的融合八叉树地图和子八叉树地图为机器人场景重建和快速分割结果。/n...

【技术特征摘要】
1.一种狭窄空间的多层次场景重建和快速分割方法，其特征在于，该方法包括：
步骤S10，获取机器人场景重建和快速分割的传感器数据；所述传感器数据包括通过深度相机获得的彩色图像和深度图像、基于所述彩色图像和深度图像通过相机标定获得的深度相机标定数据以及基于所述彩色图像、深度图像和深度相机标定数据通过多传感器融合定位系统获得的机器人空间位置和姿态信息；所述相机标定数据包括相机内参矩阵、机器人坐标系到相机坐标系的第一映射关系；
步骤S20，基于相机投影模型和坐标系变换原理，通过所述机器人空间位置和姿态信息、相机内参矩阵以及第一映射关系，确定世界坐标系到相机坐标系的第二映射关系和到图像像素坐标系的第三映射关系，并基于所述第三映射关系进行所述传感器数据中当前帧数据从图像像素坐标系到世界坐标系的转换，获得当前帧对应的点云数据；
步骤S30，基于所述第二映射关系获取空间点与相机光心的距离，结合所述当前帧对应的点云数据和设定的距离阈值获取高、中、低层次的八叉树，并将各层次八叉树融合得到多层次场景重建的融合八叉树地图；
步骤S40，对所述融合八叉树地图进行层次调整，对层次调整获得的点云数据应用欧式聚类方法以及体素滤波，并在所述融合八叉树地图中查找相应八叉树聚类子集，获得融合八叉树地图的空间分割结果；
步骤S50，基于所述空间分割结果建立多个空间分割的子八叉树地图，所述多层次场景重建的融合八叉树地图和子八叉树地图为机器人场景重建和快速分割结果。


2.根据权利要求1所述的狭窄空间的多层次场景重建和快速分割方法，其特征在于，步骤S20包括：
步骤S21，获取所述传感器数据中当前帧深度图像中像素点Ppixel位于像素坐标系下的像素坐标(u，v)和位于世界坐标系下的深度测量值d以及像素点Ppixel在当前帧深度图像对应的彩色图像中的色彩信息Pcolor，并结合所述第一映射关系确定世界坐标系到相机坐标系的第二映射关系Tcw；
步骤S22，基于相机内参矩阵K以及所述第二映射关系Tcw获取世界坐标系到图像像素坐标系的第三映射关系Tpw，并对像素点Ppixel的图像像素坐标系表示进行坐标转换，获得像素点Ppixel的世界坐标系表示；
步骤S23，重复步骤S21-步骤S22遍历当前帧深度图像中所有像素点，获得当前帧对应的点云数据C＝[tTcwP]T；其中，t为时间戳，P为各像素点对应的世界坐标表示集。


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



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


4.根据权利要求2所述的狭窄空间的多层次场景重建和快速分割方法，其特征在于，所述各像素点对应的世界坐标表示集P为：
P＝[P0…Pi]
其中，Pi＝[Pworld_iPcolor_i]，i代表遍历的图像像素序号。


5.根据权利要求1所述的狭窄空间的多层次场景重建和快速分割方法，其特征在于，步骤S30包括：
步骤S31，基于所述第二映射关系获取空间点与相机光心的距离，并设定所述距离的高、中、低分层阈值；
步骤S32，根据所述高、中、低分层阈值，将所述当前帧对应的点云数据划分为高、中、低三个层次；
步骤S33，根据光线追踪算法判定点云数据所在空间的空间点是否存在测量值，若存在，则空间点被占据；若不存在测量值，且从相机光心到空间点的线段上没有物体，则空间点不被占据；
步骤S34，根据空间点的占据状态，计算其占据概率，并进行空间点状态更新；
步骤S35，重复步骤S33-步骤S34遍历点云数据所在空间的空间点，获得高、中、低三个层次的八叉树；

【专利技术属性】
技术研发人员：罗明睿，李恩，郭锐，刘佳鑫，杨国栋，梁自泽，谭民，李勇，刘海波，李胜川，周桂平，
申请(专利权)人：中国科学院自动化研究所，国网山东省电力公司，国网辽宁省电力有限公司电力科学研究院，
类型：发明
国别省市：北京;11