CN114353779A - 一种采用点云投影快速更新机器人局部代价地图的方法 - Google Patents

Abstract

本发明公开了一种采用点云投影快速更新机器人局部代价地图的方法，包括以下步骤：首先对点云预处理，对激光雷达或深度相机输入的原始点云进行下采样，并移除离群点。然后从点云中提取地面点云，计算非地面点云到地面点云的距离，将距离小于机器人高度的所有非地面点投影到地面点云拟合得到的平面上。最后，对投影点云再次下采样，使用采样后的点云更新代价地图的栅格。本发明通过将三维点云压缩和投影，大幅减少了点云数量，显著降低了更新代价地图的计算量，提高了机器人避障的实时性。同时保留高度方向的障碍物信息，适用于复杂的环境。

Description

一种采用点云投影快速更新机器人局部代价地图的方法

技术领域

本发明属于移动机器人自主导航领域，具体涉及一种移动机器人局部路径规划中使用的代价地图更新方法。

背景技术

移动机器人为了在动态的环境中实现自主移动，需要具备感知环境的能力。早期机器人一般采用超声波传感器或者单线激光雷达作为环境感知的设备。

随着技术的发展，高分辨率的多线激光雷达和深度相机等传感器开始装备到移动机器人，使机器人能够应对更复杂的环境。但是，海量的外部数据给机器人的算法和算力造成了很大的压力。单纯增加算力面临成本、功耗等问题，研究更有效的数据处理技术势在必行。

移动机器人无法直接利用传感器输出的原始数据进行导航，需要经过一系列的处理转化成机器人能够利用的数据格式。代价地图就是机器人进行局部避障时常用的一种数据格式，代价地图将机器人周围的空间划分为若干栅格，栅格中的数值表示障碍物信息。机器人利用栅格中的数值进行局部路径规划实现避障，移动机器人在避障时，代价地图需要实时更新，准确反映环境状态。

多线激光雷达和深度相机输出的是三维点云数据，在构建代价地图时，一种方法是直接建立三维代价地图，即将三维空间划分为很多体素，如果一个体素中包含点云，则认为其被障碍物占据，这种方法构建的代价地图大小与空间分辨率成三次方关系，计算量非常大，导致实时性差；另一种方法是从激光雷达或深度相机中抽取单个扫描线扫描的数据，建立二维代价地图，这种方法虽然计算量小，但是对环境表示能力有限，在面对垂直方向截面变化的障碍物时失效。

发明内容

本发明针对现有代价地图构建方法的缺点，提出一种采用点云投影的快速构建代价地图的方法，提高了代价地图更新的频率，加快了机器人对环境变化的响应速度。

本发明实施例可以应用于室内和室外智能移动机器人。

本发明提出的技术方案包含以下内容：

一种采用点云投影快速更新机器人局部代价地图的方法，包括以下步骤：

步骤1，从传感器中获取环境的原始点云数据，并对所述原始点云数据进行预处理；

步骤2，从预处理后的点云数据中提取地面点集，对地面点集进行分块平面拟合，得到拟合平面；

步骤3，计算非地面点到所述拟合平面的距离，将距离小于移动机器人最大高度的点向拟合平面投影；

步骤4，对投影后的点云数据进行下采样；

步骤5，使用投影点云对代价地图的栅格进行标记，更新所述机器人局部代价地图。

进一步的，所述步骤1中，使用多线激光雷达或深度相机作为传感器，获取原始点云数据，作为代价地图更新的数据输入。

进一步的，所述步骤1中的预处理为：使用体素化网格的方法对所述原始点云数据进行下采样，减少点云数量；采样后的点云数据再经过移除离群点预处理，去除干扰和噪点。

进一步的，所述步骤1中，采用传感器中的陀螺仪得到传感器相对于世界坐标系的姿态角，利用所述姿态角构造变换矩阵，将点云数据从传感器坐标系转换到世界坐标系；

计算点云中各点数据的法向量，并计算点数据的法向量与世界坐标系z轴的夹角，将所述夹角小于第一阈值，并且和点高度小于第二阈值的点标记为地面点集，对地面点集进行分块平面拟合，得到各块的区域平面方程。

更进一步的，所述步骤3中，计算非地面点集中每个点到拟合平面的距离，记为h；

根据所述距离h将非地面点集中的点划分为两类，h>H的点记为安全点，h≤H的点记为碰撞点，其中，H是机器人的最大高度，将所述碰撞点投影到拟合平面上。

进一步的，所述步骤4中，使用体素化网格的方法对投影后的碰撞点云数据进行下采样。

作为本申请的一种优选实施方案，所述步骤5中，使用下采样的投影点云更新代价地图的栅格占据状态。

与现有技术相比，本发明的技术方案具有以下优点：

与完全使用三维点云构建三维体素代价地图的方法相比，通过投影处理减少了点云的数量，提高了代价地图更新的频率，加快了机器人对环境变化的响应速度，与只使用某个截面点云数据的方法相比，考虑垂直维度障碍物的分布对机器人的影响，因此适用于更复杂的环境。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为预处理后的点云图；

图2为投影后的点云示意图；

图3为投影前和投影后的点云示意图；

图4为单帧点云处理的总耗时曲线图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将深度相机传感器为例对本发明的技术方案进行详细的描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所得到的所有其它实施方式，都属于本发明所保护的范围。

图1、图2为本发明实施例提供的一种应用场景，坐标系所在位置为深度相机的原点。图1中的位于人身体表面的点为预处理后的部分点云，图2中的机器人投影中最密集的点（即图片中底面投影颜色最深的点）为经过预处理的位于人身体表面的点向地面投影后的点云。图中圆形区域是对应的是代价地图的可视化展示。视图侧面的水平方向的曲线是从深度相机深度图像的水平截面转换得到的点云数据，用于模拟单线激光雷达。

图3是点云中点的数量，其中图中对应纵坐标数值较大的实线是原始点云经过预处理后的点云大小，对应纵坐标数值较小的虚线是投影后的点云大小，纵轴是对数坐标。

图4是根据所提出的方法实现的程序处理单帧点云的时间消耗统计。

步骤1、本实施例采用的传感器具体是Intel RealSense L515型深度相机，其输出有姿态角度数据、深度图像数据和点云数据。点云数据除包含三维坐标（x, y, z）外还包含点的强度信息。后续处理只利用传感器的姿态角度和点云数据的三维坐标数据。本发明对传感器输出的点云中点的排列顺序没有要求，即点云可以是无序排列。

本发明提出的点云处理方法具有通用性，可以应用于能够输出点云的传感器，例如多线激光雷达，也可用于双目深度图像数据，只需要将深度图像转换为点云数据即可适用。

步骤2、对传感器输出的点云数据进行预处理，包括下采样和移除离群点，预处理后的点云如图1中的红色点所示。

步骤2中，预处理环节采用的下采样的方法是体素化网格，具体由开源点云库PCL的VoxelGrid类实现。

步骤2中，预处理环节中的移除离群点步骤具体采用开源点云库PCL的RadiusOutlierRemoval滤波器实现。

步骤3、从预处理后的点云中提取地面点集，对地面点集进行分块平面拟合。

步骤3中，提取地面点集的具体方法是：首先，利用传感器输出的姿态角度数据将点云从传感器坐标系转换到世界坐标系中。然后，使用PCL中的NormalEstimate类计算点云的法向量。计算法向量与重力向量的夹角δ。设置角度阈值δ _max和高度阈值d，将δ<δ _max且z<d的点标记为地面点，定义地面点集。将剩余的点定义为非地面点集。

步骤3中，提取出地面点集后，根据地面点集中点的法向量与重力向量的夹角δ进行聚类，对属于同一聚类中的点集拟合平面，得到局部区域的平面方程。本发明提出的地面点集提取方法适用于非水平地面的环境，地面可由若干水平或倾斜平面组合而成。

步骤4、计算非地面点集中的点到拟合平面的距离h，将非地面点集中的点根据高度划分为两类，h>H的点记为安全点，h≤H的点记为碰撞点。其中，H是机器人最高处的高度值。将碰撞点投影到拟合平面上。

步骤5、对投影点进行下采样，具体采用PCL中的VoxelGrid类实现，下采样后的点云如图2中的蓝色点所示。

经过步骤5后的点云相比原始点云有大幅减少，作为对比，图2显示了处理前后点云中点的数量。处理前的原始点云含有约40000个点，处理后的点云含有约350个点，仅为原点云的不到1/100。图3给出了点云处理部分（步骤1~步骤5）所需的时间，试验统计值为25.4±3.3ms。

步骤6、使用投影点云对二维代价地图的栅格值进行更新。更新包括标记（mark）和清除（clear）两个操作。标记的具体操作为，遍历投影点，计算投影点所在离散栅格索引值，将栅格索引值加入占据栅格映射。清除的具体操作为，遍历占据栅格映射，采用最近邻搜索方法搜索栅格中的点到最近邻点的距离，如果大于一定阈值就将该栅格从占据栅格映射中移除。根据占据栅格按照机器人的最小包围圆半径进行膨胀得到最终的代价地图。

通过试验测试，代价地图的更新频率不低于10Hz，满足移动机器人路径规划的要求。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。另外，本申请实施例提供的上述技术方案中与现有技术中对应技术方案实现原理一致的部分并未详细说明，以免过多赘述。

对所公开的实施例的上述说明，使本领域技术人员能够实现或使用本申请。对这些实施例的多种修改对本领域技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (7)

1.一种采用点云投影快速更新机器人局部代价地图的方法，其特征在于，包括以下步骤：

步骤1，从传感器中获取环境的原始点云数据，并对所述原始点云数据进行预处理；

步骤2，从预处理后的点云数据中提取地面点集，对地面点集进行分块平面拟合，得到拟合平面；

步骤3，计算非地面点到所述拟合平面的距离，将距离小于移动机器人最大高度的点向拟合平面投影；

步骤4，对投影后的点云数据进行下采样；

步骤5，使用投影点云对代价地图的栅格进行标记，更新所述机器人局部代价地图。

2.根据权利要求1所述的一种采用点云投影快速更新机器人局部代价地图的方法，其特征在于，所述步骤1中，使用多线激光雷达或深度相机作为传感器，获取原始点云数据，作为代价地图更新的数据输入。

3.根据权利要求1所述的一种采用点云投影快速更新机器人局部代价地图的方法，其特征在于，所述步骤1中的预处理为：使用体素化网格的方法对所述原始点云数据进行下采样，减少点云数量；采样后的点云数据再经过移除离群点预处理，去除干扰和噪点。

4.根据权利要求1所述的一种采用点云投影快速更新机器人局部代价地图的方法，其特征在于，所述步骤1中，采用传感器中的陀螺仪得到传感器相对于世界坐标系的姿态角，利用所述姿态角构造变换矩阵，将点云数据从传感器坐标系转换到世界坐标系；

计算点云中各点数据的法向量，并计算点数据的法向量与世界坐标系z轴的夹角，将所述夹角小于第一阈值，并且和点高度小于第二阈值的点标记为地面点集，对地面点集进行分块平面拟合，得到各块的区域平面方程。

5.根据权利要求1所述的一种采用点云投影快速更新机器人局部代价地图的方法，其特征在于，所述步骤3中，计算非地面点集中每个点到拟合平面的距离，记为h；

根据所述距离h将非地面点集中的点划分为两类，h>H的点记为安全点，h≤H的点记为碰撞点，其中，H是机器人的最大高度，将所述碰撞点投影到拟合平面上。

6.根据权利要求1所述的一种采用点云投影快速更新机器人局部代价地图的方法，其特征在于，所述步骤4中，使用体素化网格的方法对投影后的碰撞点云数据进行下采样。

7.根据权利要求1所述的一种采用点云投影快速更新机器人局部代价地图的方法，其特征在于，所述步骤5中，使用下采样的投影点云更新代价地图的栅格占据状态。

Images