CN109282822A - 构建导航地图的存储介质、方法和设备 - Google Patents

Abstract

本发明公开了一种构建导航地图的存储介质、方法和设备。其中，该方法包括：使用基于特征点配准的地图构建算法，对已获取的目标地点的三维点云数据进行预处理；将经过预处理后的三维点云数据转化为预定格式的点云数据；以及基于八叉树数据结构的模型，将预定格式的点云数据转化为八叉树数据结构的导航地图。本发明解决了点不连续、无用点过多以及无法实现位置和姿态估计的技术问题。

Description

构建导航地图的存储介质、方法和设备

技术领域

本发明涉及3D SLAM领域，具体而言，涉及一种构建导航地图的存储介质、方法和设备。

背景技术

当前基于多线激光雷达采集构建的地图通常规模很大，一个16线雷达在五分钟内保存的点云数据能达到10G左右，然而这其中有很多区域的点并不是连续的，而且存在很多因噪声产生的无用点。除此之外，建筑物的遮挡、树木等产生的阴影也是不需要的。另一方面，3D SLAM算法构建出的点云图虽然描绘出了建筑、道路边线、道路信号标志标线等标记物的轮廓，但机器人不知道可通过区域点的具体位置，不能依据周围特征点和里程计信息实现位置和姿态估计，所以地图依然难以应用于机器人导航。

针对上述点不连续、无用点过多以及无法实现位置和姿态估计的问题，目前尚未提出有效的解决方案。

发明内容

本发明实施例提供了一种构建导航地图的存储介质、方法和设备，以至少解决点不连续、无用点过多以及无法实现位置和姿态估计的技术问题。

根据本发明实施例的一个方面，提供了一种构建导航地图的方法，包括：使用基于特征点配准的地图构建算法，对已获取的目标地点的三维点云数据进行预处理；将经过预处理后的三维点云数据转化为预定格式的点云数据；以及基于八叉树数据结构的模型，将述预定格式的点云数据转化为八叉树数据结构的导航地图。

进一步地，预定格式为PCL点云库可用的bag二进制数据格式。

进一步地，使用基于特征点配准的地图构建算法，获取目标地点的三维点云数据包括如下步骤：建立预定坐标系，确定目标地点的三维点云数据的位置信息；利用位置信息计算所述目标地点的三维点云数据的深度，并基于深度对目标地点的三维点云数据进行筛选；利用数据搜索模型，对筛选后的点云数据进行特征点配准；以及基于特征点配准后的点云数据，得到点云间相对运动的优化运动估计结果。

进一步地，预定坐标系为笛卡尔坐标系。

进一步地，数据搜索模型为Kd-Tree多维空间关键数据搜索模型。

进一步地，基于八叉树数据结构的模型，将预定格式的点云数据转化为八叉树数据结构的导航地图包括如下步骤：将预定格式的点云数据分配至多个立方体内；基于最小二乘法对多个立方体内的数据做曲面拟合；基于曲面拟合的结果，获得点云中每个点的法向量标准偏差；以及通过将法向量标准偏差与预定阈值比较，判断是否对多个立方体进行细分。

进一步地，通过将法向量标准偏差与预定阈值比较，判断是否对多个立方体进行细分的操作包括如下步骤：当法向量标准偏差大于所述预定阈值时，对多个立方体进行细分并得到最小立方体，并对最小立方体内的点云数据进行压缩；以及当法向量标准偏差小于阈值或所述多个立方体中只有单个点时，不对多个立方体进行细分。

进一步地，对最小立方体内的点云数据进行压缩的操作包括如下步骤：获取最小立方体内的点云数据；以及保留最小立方体内的点云数据中符合预定条件的点云数据，并删除不满足预定条件的点云数据。

进一步地，预定条件为：待判定的点云数据的法向量与最小立方体内的点云数据的平均法向量的差值是最小立方体内的点云数据中任意一个点云数据的法向量与最小立方体内的点云数据的平均法向量的差值中的最小值。

进一步地，基于八叉树数据结构的模型，将预定格式的点云数据转化为八叉树数据结构的导航地图还包括如下步骤：基于法向量标准偏差与预定阈值比较的结果，生成八叉树数据结构的导航地图。

从而，本实施例通过使用基于特征点配准方法的3D SLAM地图构建算法对特定室外区域地图建构，点云地图导入PCL点云库预处理，保存为PCL可用的数据格式，之后使用基于Octree八叉树数据结构的地图构建算法将数据再处理，输出带有八叉树数据结构的地图，实现了通过根据拟合曲面计算最小立方体内节点曲率方差，与所设阈值对比，进一步细分八叉树结构的技术效果，达到删除、压缩点云数量的目的，进而解决了点不连续、无用点过多以及无法实现位置和姿态估计的技术问题。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是用于执行根据本公开的实施例1所述的构建导航地图方法的设备的示意图；

图2是根据本发明实施例1所述的构建导航地图方法的流程示意图；

图3是根据本发明实施例2所述的构建导航地图的设备的结构示意图；

图4是根据本发明实施例2所述的构建导航地图的设备的工作具体流程的示意图；以及

图5是根据本发明实施例3所述的构建导航地图的设备的结构示意图.

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

实施例1

根据本发明实施例，还提供了一种构建导航地图的方法实施例，需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

本申请实施例1所提供的方法实施例可以在移动终端、计算机终端或者类似的运算装置中执行。图1示出了一种用于实现构建导航地图的方法的计算机终端(或移动设备)的硬件结构框图。如图1所示，计算机终端10(或移动设备10)可以包括一个或多个(图中采用102a、102b，……，102n来示出)处理器102(处理器102可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置)、用于存储数据的存储器104、以及用于通信功能的传输模块106。除此以外，还可以包括：显示器、输入/输出接口(I/O接口)、通用串行总线(USB)端口(可以作为I/O接口的端口中的一个端口被包括)、网络接口、电源和/或相机。本领域普通技术人员可以理解，图1所示的结构仅为示意，其并不对上述电子装置的结构造成限定。例如，计算机终端10还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。

应当注意到的是上述一个或多个处理器102和/或其他数据处理电路在本文中通常可以被称为“数据处理电路”。该数据处理电路可以全部或部分的体现为软件、硬件、固件或其他任意组合。此外，数据处理电路可为单个独立的处理模块，或全部或部分的结合到计算机终端10(或移动设备)中的其他元件中的任意一个内。如本申请实施例中所涉及到的，该数据处理电路作为一种处理器控制(例如与接口连接的可变电阻终端路径的选择)。

存储器104可用于存储应用软件的软件程序以及模块，如本发明实施例中的构建导航地图的方法对应的程序指令/数据存储装置，处理器102通过运行存储在存储器104内的软件程序以及模块，从而执行各种功能应用以及数据处理，即实现上述的应用程序的漏洞检测方法。存储器104可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器104可进一步包括相对于处理器102远程设置的存储器，这些远程存储器可以通过网络连接至计算机终端10。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

传输装置106用于经由一个网络接收或者发送数据。上述的网络具体实例可包括计算机终端10的通信供应商提供的无线网络。在一个实例中，传输装置106包括一个网络适配器(Network Interface Controller，NIC)，其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输装置106可以为射频(Radio Frequency，RF)模块，其用于通过无线方式与互联网进行通讯。

显示器可以例如触摸屏式的液晶显示器(LCD)，该液晶显示器可使得用户能够与计算机终端10(或移动设备)的用户界面进行交互。

在上述运行环境下，本申请提供了如图2所示的构建导航地图的方法。图2是根据本发明实施例1所述的构建导航地图的方法的流程图。

参考图2所示，本实施例提供了一种构建导航地图的方法，包括：

使用基于特征点配准的地图构建算法，对已获取的目标地点的三维点云数据进行预处理；

将经过预处理后的三维点云数据转化为预定格式的点云数据；以及

基于八叉树数据结构的模型，将预定格式的点云数据转化为八叉树数据结构的导航地图。

从而，本实施例通过使用基于特征点配准方法的3D SLAM地图构建算法对特定室外区域地图建构，点云地图导入PCL点云库预处理，保存为PCL可用的数据格式，之后使用基于Octree八叉树数据结构的地图构建算法将数据再处理，输出带有八叉树数据结构的地图，进而解决了点不连续、无用点过多以及无法实现位置和姿态估计的技术问题。

进一步地，预定格式为PCL点云库可用的bag二进制数据格式。

从而，通过保存地图格式为PCL点云库可用的bag二进制数据格式，才能转化为八叉树算法能够处理的数据类型。

进一步地，使用基于特征点配准的地图构建算法，获取目标地点的三维点云数据包括如下步骤：

建立预定坐标系，确定目标地点的三维点云数据的位置信息；

利用位置信息计算目标地点的三维点云数据的深度，并基于深度对目标地点的三维点云数据进行筛选；

利用数据搜索模型，对筛选后的点云数据进行特征点配准；以及基于特征点配准后的点云数据，得到点云间相对运动的优化运动估计结果。

其中，建立笛卡尔坐标系，雷达扫描平面以正对雷达方向左手侧为世界坐标系X轴，垂直向上为Y轴，正前方是Z轴，计算点的高度角，划分入不同线束中。

遍历剩余的所有可用点，计算点的高度角，根据角度正负划分入0-16不同奇数、偶数线束中并储存进点云指针。查找有效特征点并剔除无效特征点，计算各点曲率，大于设定阈值的点可以作为特征点，平面上目标点曲率计算公式如下：

计算目标点的深度，相邻两个点的深度不同且两向量的夹角小于阈值时判断为不可用的遮挡点；目标点距离前后两个相邻点的距离均大于D倍该点深度时，判断为不可用的平行点。上述操作具体地描述为：计算目标点的深度，再计算前后两个相邻点的深度，记为depth1和depth2，两点深度不相同且相邻两点与坐标原点构成夹角小于阈值时，认为特征点存在遮挡，那么目标点及前后各五个点判断为不可用点，舍去。计算目标点和前后两个相邻点间的距离L1和L2，以及目标点的深度M，当L1和L2均大于D倍目标点深度M时判断目标点和前后各五个点为不可用的平行点，舍去。一帧完整的扫描区域被划分为4个区域，且每条线束都被分割为6个区间段，按照一定的曲率变化规律排列。

Kd-Tree多维空间关键数据搜索，进行特征点匹配，构建算法把当前空间以超平面分割，查找算法从父节点代表的空间范围开始搜索，逐级展开递归。之后将当前t时刻数据和t+1时刻数据关联，计算目标点距离特征线和特征面的距离，L-M运动求解估计雷达的运动距离和姿态，保存地图格式为PCL可用二进制文件bag类型。

基于特征点配准后的点云数据，得到点云间相对运动的优化运动估计结果的操作具体为：解算雷达姿态和运动估计，首先根据几何中点到直线、面的距离公式构建点云的向量式约束方程，之后对约束方程求偏导数，构建出矩阵方程，通过最小二乘法中L-M运动解算出点云间的相对运动。解算出相对运动后，将激光雷达坐标系下的坐标转换成世界坐标系下对应坐标。之后优化运动估计结果，方法是使用当前帧点云与上一时刻点云图匹配。将所有3D SLAM程序涉及到的ROS话题保存为PCL点云库可用的bag二进制数据格式。所构建约束方程分别如下所示：

需要说明的是，3D地图构建完成后，在ROS操作系统中开启两个新的终端，在终端一中使用rosbag命令回放上一步中保存好的bag数据包，在终端二的命令窗口中输入PCL的bag_to_pcd指令，保存的话题为全部话题，等待保存完成后在新的pcd文件目录下开启终端，使用Viewer图形化工具查看保存的pcd文件是否完整。PCL-Octree八叉树数据结构在点云图中主要应用于空间划分、近邻搜索和点云压缩，压缩后的点云中丢掉了空的体素以及不连续的无用点，每帧数据大小会有相应的缩减。使用Octomap八叉树导航地图构建算法包对前一步的数据做进一步处理，具体内容分为地图压缩、更新地图，并保存为八叉树地图的格式。

进一步地，预定坐标系为笛卡尔坐标系。

进一步地，数据搜索模型为Kd-Tree多维空间关键数据搜索模型。

进一步地，基于八叉树数据结构的模型，将所述预定格式的点云数据转化为八叉树数据结构的导航地图包括如下步骤：

将预定格式的点云数据分配至多个立方体内；

基于最小二乘法对多个立方体内的数据做曲面拟合；

基于曲面拟合的结果，获得点云中每个点的法向量标准偏差；以及

通过将法向量标准偏差与预定阈值比较，判断是否对多个立方体进行细分。

其中，点云数据经过pcl点云库处理后得到标准点云数据格式，之后被用于构建八叉树与曲率的计算。首先遍历每帧数据中点云位置分布，记录点的个数N，取立方体包含全部点云，棱长L_cub＝MAX(x_i,y_i,z_i)，对立方体进行n等分，得到中间立方体的棱长为L₁。计算方法如下：

遍历所有点，分别放入对应的中间立方体中，建立数组mid代表中间立方体，下标i是立方体索引号。并记录每个中间立方体内包含的点的个数。

最小二乘法曲面拟合算法相比于平面拟合计算量大，但计算精度高，特别适用于对离散的点云曲率等的计算。这里使用中间立方体内点的法向量标准偏差来表征曲率变化大小，计算法向量标准偏差之前对每个中间立方体内的点云做曲面拟合，这里提出一种基于标准曲面方程的简单高效的曲面拟合模型，标准曲面方程为：

a₀+a₁x+a₂y+a₃xy+a₄x²+a₅y²＝z (5)

最小二乘法估计方程参数时，要求观测值z偏差加权平方和最小，转化为计算的最小值，分别计算δ对a₀等参数的偏导数，令其分别等于零，列出方程组如下：

将点的坐标带入方程(6)至(11)求解出所需要的方程参数。

进一步地，通过将所述法向量标准偏差与预定阈值比较，判断是否对所述多个立方体进行细分的操作包括如下步骤：

当所述法向量标准偏差大于所述预定阈值时，对所述多个立方体进行细分并得到最小立方体，并对所述最小立方体内的点云数据进行压缩；以及

当所述法向量标准偏差小于所述阈值或所述多个立方体中只有单个点时，不对所述多个立方体进行细分。

其中，对通过将所述法向量标准偏差与预定阈值比较，判断是否对所述多个立方体进行细分的操作做详细解释如下：求解出拟合曲面方程后，带入点的坐标分别计算各点的法向量记为(x_i,y_i,z_i)，法向量标准偏差计算公式为：

其中是平均法向量。

当立方体内点的法向量标准偏差大于阈值时，进一步细分得到最小立方体，棱长L₀＝L₁/m，其中m是当前中间立方体内包含点的数量，计算p＝ceil(L_cub/L₀)，由此确定递归层数k为大于等于p的整数。最大立方体棱长转化为L_cub＝2^k×L₀。如果法向量标准偏差计算公式小于阈值或者中间立方体中只有单个点时，不再继续细分。

进一步地，对最小立方体内的点云数据进行压缩的操作包括如下步骤：

获取最小立方体内的点云数据；以及

保留最小立方体内的点云数据中符合预定条件的点云数据，并删除不满足预定条件的点云数据。

进一步地，预定条件为：待判定的点云数据的法向量与所述最小立方体内的点云数据的平均法向量的差值是所述最小立方体内的点云数据中任意一个点云数据的法向量与所述最小立方体内的点云数据的平均法向量的差值中的最小值。

其中，细分得到最小立方体的同时，记录每个最小立方体内包含的点的ID，建立数组保存点的ID，final代表最小立方体索引号。最小立方体中包括的点是需要压缩的数据，点云数据压缩具体算法步骤为：(1)遍历(2)计算平均法向量与每个点的法向量，寻找与平均法向量差别最小的点；(3)保留此点删去其他的点。

进一步地，基于八叉树数据结构的模型，将所述预定格式的点云数据转化为八叉树数据结构的导航地图还包括如下步骤：基于法向量标准偏差与预定阈值比较的结果，生成八叉树数据结构的导航地图。

需要说明的是，八叉树分辨率设置为0.1m，即每个子节点正方体边长为10cm，所选取的场景点云规模很大，分辨率过低时八叉树构建效果不明显。每个子节点立方体未被占据时用浮点数表示为0，0.5以下认为没有被占据，0-1之间越高占据的比例越大。依照此方法将占据概率低的节点剪掉，压缩数据大小。八叉树导航地图构建算法主要包括三个参数，分别是pcd2octomap，作用是将上一步保存的pcd文件转化为octree地图，另外两个参数是input数据参数和output数据参数。包含有全部特征点的PCL可用的pcd数据文件经PCL点云库处理后，输出为input数据参数，作为程序算法三个参数之一的输入函数插入到Octomap算法中，之后更新八叉树地图。Octomap算法最后生成的八叉树地图数据的格式为bt文件，查看文件内容的方法是使用Viewer可视化图形工具打开，窗口右侧是可调节八叉树地图分辨率的滑动条，有0-16等级，为了保证八叉树地图的可视化效果，选择了等级16，即依照程序中原定的分辨率0.1m显示，可以看出，整个方法不仅有效的压缩了点云地图的数据大小，而且将所有有效点依照八叉树数据结构存储方式保存，所得到的地图为整个场景的完整八叉树地图。

从而，本实施例通过使用基于特征点配准方法的3D SLAM地图构建算法对特定室外区域地图建构，点云地图导入PCL点云库预处理，保存为PCL可用的数据格式，之后使用基于Octree八叉树数据结构的地图构建算法将数据再处理，输出带有八叉树数据结构的地图，实现了通过根据拟合曲面计算最小立方体内节点曲率方差，与所设阈值对比，进一步细分八叉树结构的技术效果，达到删除、压缩点云数量的目的，进而解决了点不连续、无用点过多以及无法实现位置和姿态估计的技术问题。

根据本发明实施例的另一方面，还提供了一种用于实施上述构建导航地图的存储介质，如图1所示，存储介质包括存储的程序，其中，在所述程序运行时控制所述存储介质所在设备执行权利要求1至10中任意一项所述的构建导航地图的方法。

实施例2

图3是根据本发明实施例2所述的构建导航地图的设备的结构示意图。

参考图3所示，本公开实施例提供了一种构建导航地图的设备，其特征在于，包括：多线激光雷达31，用于获取目的地点的三维点云数据；移动底盘32，用于接收远程控制器命令，并依据命令控制多线激光雷达运动；远程控制器33，用于远程控制多线激光雷达；以及嵌入式计算平台30，用于执行如权利要求1所述的构建导航地图的方法。

其中，移动底盘32为四轮差速转向移动机器人底盘，此外，移动底盘32带有外接USB串口通信接口，内置有轮式里程计和控制器321，其中控制器321为内嵌控制器321，可以与远程控制中心间通信；

多线激光雷达31为旋转式多线机械激光雷达，线激光雷达设置于移动底盘32上，扫描周围360°所有可以反射激光的物体，接收到的点云图数据分为多层，遍历垂直扫描角度内的所有目标，激光雷达线束越多，对目标描述的越精确，还原度越高，但保存的文件规模也更大，有较高垂直、角分辨率，此外，考虑到雷达底座自身遮挡和建图需要，安装位置位于移动底盘32中心上方15-20cm，使用电源322中的辅助电源直接供电，之后进行激光雷达校准，对角度、曲率和通道数量三参数完成配置；

嵌入式计算平台30为高性能嵌入式计算平台，是整个系统控制中心，一方面收集来自雷达31和远程控制器33的数据与指令，一方面与移动底盘32的控制器321间通信，高性能嵌入式计算平台30通过USB串口与控制器321直接相连，使用电源322中的辅助电源供电，系统预先安装有Linux下Ubuntu系统，并搭建了ROS机器人操作系统，版本选择运行稳定的kinetic。除此之外，机器人操作系统还应预先安装PCL点云库，用于对点云数据去噪与压缩处理。应预先安装基于特征点匹配方法的3D SLAM即时地图定位与构建算法包，用于室外3D地图的实时构建。应预先安装Octomap八叉树导航地图构建算法包，用于对3D SLAM构建得到的点云地图数据转换为带有八叉树数据结构形式地图；

远程控制器33，本发明选用joystick手柄作为远程控制器33控制移动底盘运动速度，预先根据通信协议更改其命令指令，使其具有针对于底盘实现线速度可增减以及角速度可增减的控制模式；

电池322，设置于移动底盘32上，电池322为自载10Ah 24V锂聚合物电池和12V辅助电源，移动底盘32自载锂电池组与电压转换模块为系统提供全部所需电源；

需要说明的是，设置高性能嵌入式计算平台30的静态IP地址，选择使用RS-LiDAR-16雷达，设备IP为192.168.1.200，设置计算平台的IP地址为同一网段192.168.1.102，在Ubuntu系统中方法为添加Ethernet，DHCP解析方式MANUL，网关255.255.255.0，保存设置后，显示连接成功，打开Wireshark查看网络封包资料，分析雷达31发送的数据帧格式和大小确保雷达31工作正常。建立嵌入式计算平台30和移动底盘32间数据连接，通信方式串口通信，在Ubuntu系统中为串口赋予可写入/读取权限，初始化远程控制中心。完成雷达初始化和启动移动地盘后，启动ROS操作系统中程序各个节点，开启远程控制器33，遥控移动底盘32在目标场景中以不同档位速度运动，覆盖整个场景范围。软件程序涉及到移动底盘32驱动程序包、PCL点云库、基于特征点匹配方法的3D SLAM即时地图定位与构建算法包、Octomap八叉树导航地图构建算法包，包括autodriver_node底盘驱动节点，rslidar激光雷达启动节点，laser_cloud_surround 3D SLAM建图节点，和octomap八叉树地图构建节点。

参考图4说明本实施例2所述的构建导航地图的设备的具体工作流程：

S402：激光雷达31校准；

激光雷达31校准，对角度、曲率和通道数量三参数完成配置。

S404：配置网段，开启移动底盘32控制节点；

设备IP为192.168.1.200，设置计算平台的IP地址为同一网段192.168.1.102，在Ubuntu系统中方法为添加Ethernet，DHCP解析方式MANUL，网关255.255.255.0，保存设置后，显示连接成功，打开Wireshark查看网络封包资料，分析雷达发送的数据帧格式和大小确保雷达工作正常。

S406：开启3D建图节点；

所选择地图构建场景为某一开阔区域，大小为27*15m，周围特征点较多，障碍物少，便于移动底盘32连续运动并构建地图。各节点启动顺序为首先启动激光雷达31的节点，启动基于特征点匹配的3D SLAM建图节点，建立笛卡尔坐标系，雷达扫描平面以正对雷达方向左手侧为世界坐标系X轴，垂直向上为Y轴，正前方是Z轴，点云预处理，去除噪声点和扫描起始角度。

遍历剩余的所有可用点，计算点的高度角，根据角度正负划分入0-16不同奇数、偶数线束中并储存进点云指针。查找有效特征点并剔除无效特征点，计算各点曲率，大于设定阈值的点可以作为特征点，平面上目标点曲率计算公式参照实施例1中的公式(1)。

进一步划分，计算目标点的深度，再计算前后两个相邻点的深度，记为depth1和depth2，两点深度不相同且相邻两点与坐标原点构成夹角小于阈值时，认为特征点存在遮挡，那么目标点及前后各五个点判断为不可用点，舍去。计算目标点和前后两个相邻点间的距离L1和L2，以及目标点的深度M，当L1和L2均大于D倍目标点深度M时判断目标点和前后各五个点为不可用的平行点，舍去。一帧完整的扫描区域被划分为4个区域，且每条线束都被分割为6个区间段，按照一定的曲率变化规律排列，之后以Kd-Tree多维空间关键数据搜索的方式检索最邻近点的位置，进行特征点匹配，算法包括构建与搜索部分，构建部分将空间分割，查找算法遍历父节点和各子节点，搜索步骤为逐级展开递归的方式。

解算雷达姿态和运动估计，首先根据几何中点到直线、面的距离公式构建点云的向量式约束方程，之后对约束方程求偏导数，构建出矩阵方程，通过最小二乘法中L-M运动解算出点云间的相对运动。解算出相对运动后，将激光雷达坐标系下的坐标转换成世界坐标系下对应坐标。之后优化运动估计结果，方法是使用当前帧点云与上一时刻点云图匹配。其中，所构建约束方程如实施例1中公式(2)和(3)所示。

S408：采集并保存地图；

将所有3D SLAM程序涉及到的ROS话题保存为PCL点云库可用的bag二进制数据格式。点云数据经过pcl点云库处理后得到标准点云数据格式，之后被用于构建八叉树与曲率的计算。

S410：回放点云数据；

3D地图构建完成后，在ROS操作系统中开启两个新的终端，在终端一中使用rosbag命令回放上一步中保存好的bag数据包，在终端二的命令窗口中输入PCL的bag_to_pcd指令，保存的话题为全部话题，等待保存完成后在新的pcd文件目录下开启终端，使用Viewer图形化工具查看保存的pcd文件是否完整。

S412：构建八叉树地图结构；

PCL-Octree八叉树数据结构在点云图中主要应用于空间划分、近邻搜索和点云压缩，压缩后的点云中丢掉了空的体素以及不连续的无用点，每帧数据大小会有相应的缩减。使用Octomap八叉树导航地图构建算法包对前一步的数据做进一步处理，具体内容分为地图压缩、更新地图，并保存为八叉树地图的格式。

S414：自动调节分辨率，压缩点云数据；

(1)遍历每帧数据中点云位置分布，记录点的个数N，取立方体包含全部点云，棱长L_cub＝MAX(x_i,y_i,z_i)，对立方体进行n等分，得到中间立方体的棱长为L₁。计算方法参考实施例1公式(4)。遍历所有点，分别放入对应的中间立方体中，建立数组mid代表中间立方体，下标i是立方体索引号。并记录每个中间立方体内包含的点的个数。

(2)采用最小二乘法对点云数据进行曲面拟合算法。这里使用中间立方体内点的法向量标准偏差来表征曲率变化大小，计算法向量标准偏差之前对每个中间立方体内的点云做曲面拟合，这里提出一种基于标准曲面方程的简单高效的曲面拟合模型，其中标准曲面方程参照实施例1中的公式(5)。

最小二乘法估计方程参数时，要求观测值z偏差加权平方和最小，转化为计算的最小值，分别计算δ对a₀等参数的偏导数，令其分别等于零，列出方程组如实施例1中公式(6)至(11)，将点的坐标带入方程(6)至(11)求解出所需要的方程参数。

(3)通过将所述法向量标准偏差与预定阈值比较，判断是否对所述多个立方体进行细分：求解出拟合曲面方程后，带入点的坐标分别计算各点的法向量记为(x_i,y_i,z_i)，法向量标准偏差计算公式实施例1中公式(12)所示，其中是平均法向量。

当立方体内点的法向量标准偏差大于阈值时，进一步细分得到最小立方体，棱长L₀＝L₁/m，其中m是当前中间立方体内包含点的数量，计算p＝ceil(L_cub/L₀)，由此确定递归层数k为大于等于p的整数。最大立方体棱长转化为L_cub＝2^k×L₀。如果法向量标准偏差计算公式小于阈值或者中间立方体中只有单个点时，不再继续细分。

(4)细分得到最小立方体的同时，记录每个最小立方体内包含的点的ID，建立数组保存点的ID，final代表最小立方体索引号。最小立方体中包括的点是需要压缩的数据，点云数据压缩具体算法步骤为：i.遍历ii.计算平均法向量与每个点的法向量，寻找与平均法向量差别最小的点；iii.保留此点删去其他的点。

(5)基于法向量标准偏差与预定阈值比较的结果，生成八叉树数据结构的导航地图。

从而，本实施例通过本实施例所述的设备构建整个场景下的三维Octree八叉树导航地图，达到了为自动驾驶、机器人导航提供地图依据的目的，实现了通过根据拟合曲面计算最小立方体内节点曲率方差，与所设阈值对比，进一步细分八叉树结构的技术效果，达到删除、压缩点云数量的目的，进而解决了点不连续、无用点过多以及无法实现位置和姿态估计的技术问题。

实施例3

图3示出了根据本实施例所述的构建导航地图的设备的示意图，该设备也与图2所示的方法对应，参考图3所示，该设备包括：

处理器41；以及

存储器42，与处理器41连接，用于为处理器41提供处理以下处理步骤的指令：使用基于特征点配准的地图构建算法，对已获取的目标地点的三维点云数据进行预处理；将经过预处理后的三维点云数据转化为预定格式的点云数据；以及基于八叉树数据结构的模型，将预定格式的点云数据转化为八叉树数据结构的导航地图。

进一步地，预定格式为PCL点云库可用的bag二进制数据格式。

进一步地，使用基于特征点配准的地图构建算法，获取目标地点的三维点云数据包括如下步骤：建立预定坐标系，确定目标地点的三维点云数据的位置信息；利用位置信息计算所述目标地点的三维点云数据的深度，并基于深度对目标地点的三维点云数据进行筛选；利用数据搜索模型，对筛选后的点云数据进行特征点配准；以及基于特征点配准后的点云数据，得到点云间相对运动的优化运动估计结果。

进一步地，预定坐标系为笛卡尔坐标系。

进一步地，数据搜索模型为Kd-Tree多维空间关键数据搜索模型。

进一步地，基于八叉树数据结构的模型，将预定格式的点云数据转化为八叉树数据结构的导航地图包括如下步骤：将预定格式的点云数据分配至多个立方体内；基于最小二乘法对多个立方体内的数据做曲面拟合；基于曲面拟合的结果，获得点云中每个点的法向量标准偏差；以及通过将法向量标准偏差与预定阈值比较，判断是否对多个立方体进行细分。

进一步地，通过将法向量标准偏差与预定阈值比较，判断是否对多个立方体进行细分的操作包括如下步骤：当法向量标准偏差大于所述预定阈值时，对多个立方体进行细分并得到最小立方体，并对最小立方体内的点云数据进行压缩；以及当法向量标准偏差小于阈值或所述多个立方体中只有单个点时，不对多个立方体进行细分。

进一步地，对最小立方体内的点云数据进行压缩的操作包括如下步骤：获取最小立方体内的点云数据；以及保留最小立方体内的点云数据中符合预定条件的点云数据，并删除不满足预定条件的点云数据。

进一步地，预定条件为：待判定的点云数据的法向量与最小立方体内的点云数据的平均法向量的差值是最小立方体内的点云数据中任意一个点云数据的法向量与最小立方体内的点云数据的平均法向量的差值中的最小值。

进一步地，基于八叉树数据结构的模型，将预定格式的点云数据转化为八叉树数据结构的导航地图还包括如下步骤：基于法向量标准偏差与预定阈值比较的结果，生成八叉树数据结构的导航地图。

从而，本实施例通过使用基于特征点配准方法的3D SLAM地图构建算法对特定室外区域地图建构，点云地图导入PCL点云库预处理，保存为PCL可用的数据格式，之后使用基于Octree八叉树数据结构的地图构建算法将数据再处理，输出带有八叉树数据结构的地图，实现了通过根据拟合曲面计算最小立方体内节点曲率方差，与所设阈值对比，进一步细分八叉树结构的技术效果，达到删除、压缩点云数量的目的，进而解决了点不连续、无用点过多以及无法实现位置和姿态估计的技术问题。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种构建导航地图的方法，其特征在于，包括：

使用基于特征点配准的地图构建算法，对已获取的目标地点的三维点云数据进行预处理；

将经过预处理后的三维点云数据转化为预定格式的点云数据；以及

基于八叉树数据结构的模型，将所述预定格式的点云数据转化为八叉树数据结构的导航地图，其中

所述预定格式为PCL点云库可用的bag二进制数据格式。

2.根据权利要求1所述的方法，其特征在于，使用基于特征点配准的地图构建算法，获取目标地点的三维点云数据包括如下步骤：

建立笛卡尔坐标系，确定所述目标地点的三维点云数据的位置信息；

利用所述位置信息计算所述目标地点的三维点云数据的深度，并基于所述深度对所述目标地点的三维点云数据进行筛选；

利用Kd-Tree多维空间关键数据搜索模型，对筛选后的点云数据进行特征点配准；以及

基于特征点配准后的点云数据，得到点云间相对运动的优化运动估计结果。

3.根据权利要求2所述的方法，其特征在于，基于八叉树数据结构的模型，将所述预定格式的点云数据转化为八叉树数据结构的导航地图包括如下步骤：

将所述预定格式的点云数据分配至多个立方体内；

基于最小二乘法对所述多个立方体内的数据做曲面拟合；

基于所述曲面拟合的结果，获得点云中每个点的法向量标准偏差；以及

通过将所述法向量标准偏差与预定阈值比较，判断是否对所述多个立方体进行细分。

4.根据权利要求3所述的方法，其特征在于，通过将所述法向量标准偏差与预定阈值比较，判断是否对所述多个立方体进行细分的操作包括如下步骤：

当所述法向量标准偏差大于所述预定阈值时，对所述多个立方体进行细分并得到最小立方体，并对所述最小立方体内的点云数据进行压缩；以及

当所述法向量标准偏差小于所述阈值或所述多个立方体中只有单个点时，不对所述多个立方体进行细分。

5.根据权利要求4所述的方法，其特征在于，对所述最小立方体内的点云数据进行压缩的操作包括如下步骤：

获取所述最小立方体内的点云数据；以及

保留所述最小立方体内的点云数据中符合预定条件的点云数据，并删除不满足预定条件的点云数据。

6.根据权利要求5所述的方法，其特征在于，所述预定条件为：

待判定的点云数据的法向量与所述最小立方体内的点云数据的平均法向量的差值是所述最小立方体内的点云数据中任意一个点云数据的法向量与所述最小立方体内的点云数据的平均法向量的差值中的最小值。

7.根据权利要求6所述的方法，其特征在于，基于八叉树数据结构的模型，将所述预定格式的点云数据转化为八叉树数据结构的导航地图还包括如下步骤：

基于所述法向量标准偏差与预定阈值比较的结果，生成所述八叉树数据结构的导航地图。

8.一种存储介质，其特征在于，所述存储介质包括存储的程序，其中，在所述程序运行时控制所述存储介质所在设备执行权利要求1至10中任意一项所述的构建导航地图的方法。

9.一种构建导航地图的设备，其特征在于，包括：

多线激光雷达，用于获取目的地点的三维点云数据；

移动底盘，用于接收远程控制器命令，并依据所述命令控制所述多线激光雷达运动；

远程控制器，用于远程控制所述多线激光雷达；以及

嵌入式计算平台，用于执行如权利要求1所述的构建导航地图的方法。

10.一种构建导航地图的设备，其特征在于，包括：

处理器；以及

存储器，与所述处理器连接，用于为所述处理器提供处理以下处理步骤的指令：

使用基于特征点配准的地图构建算法，对已获取的目标地点的三维点云数据进行预处理；

将经过预处理后的三维点云数据转化为预定格式的点云数据；以及

基于八叉树数据结构的模型，将所述预定格式的点云数据转化为八叉树数据结构的导航地图。