一种障碍物留存方法、存储介质、设备技术

本发明专利技术涉及清洁机器人的地图算法技术领域，特别是一种基于有限视场角深度相机和多层代价地图的障碍物留存方法。该方法包括：根据障碍点在世界坐标系中的坐标以及该帧点云的时间戳将每一个障碍点以std::map[1]数据结构进行储存；在移动机器人行驶过程中，通过体素滤波，地面拟合等方法去除非障碍点；在代价地图内去除中在视场多边形内的障碍点；将新一帧点云数据在视场多边形内进行标注，已经被标注的障碍点则更新标记时间；根据其上被更新的时间，超时删除；每轮代价地图更新时以最大值方对总图层进行更新；在膨胀层中，对障碍物依据梯度进行膨胀。本申请所采用的膨胀层代价衰减公式，在膨胀半径处代价过渡平滑，有助于提高机器人路径规划的平顺性。机器人路径规划的平顺性。机器人路径规划的平顺性。

【技术实现步骤摘要】
一种障碍物留存方法、存储介质、设备


[0001]本专利技术涉及清洁机器人的地图算法
，尤其是涉及了一种障碍物留存方法、存储介质、设备。。

技术介绍

[0002]路径规划是自主移动机器人技术研究中的一个核心内容，指移动机器人按照某一性能指标（如距离、时间等）搜索一条从起始状态到目标状态的最优或次优路径。随着路径规划的发展，不断涌现出如DiJkstra、A
*
、RRT等算法。并且随着深度学习等技术的加入，机器人的路径规划正向着适应性更强、更加智能的方向发展。
[0003]绝大多数的路径规划算法都要依赖对环境的建模，比较普遍一种做法是依赖栅格代价地图，以上的路径规划算法都是假定该栅格地图为真实地图进行的。但实际运行过程中，机器人会通过传感器感知更新代价地图，代价地图更新方式会在很大程度上影响路径规划的质量。
[0004]现有技术中，一般机器人传感器的数据输入到导航节点的代价地图层，往往以传感器数据帧为单位统一标记或去除，基于单帧传感器构建的代地图只会标记视场角内障碍，缺少信息，基于多帧传感器数据的代价地图往会造成重影；同时，由于深度相机的视场角有限，当机器人移动使得障碍物到深相机视野外时障碍物消失，导致机器人规划的路径会穿过障碍物，由此产生全局路径的震荡。极大影响机器人的规划效率。
[0005]具体的场景如下：代价地图的更新仅依赖当前帧的传感器数据，采用这种方法，当障碍物较大时可能会产生如下情况：由于相机视场角有限，当前只探测障碍物的左半部分，路径规划拟从右侧绕过。但当机器人转向右侧时，相机感知到障碍物的右半部分，左半部分到了视野之外，路径规划又拟从左侧绕过，如此循环往复，导致机器人左右摇摆。
[0006]此外，也可能设计一个传感器数据缓存区，将连续几帧内的障碍物数据同时标记在地图上，采用这种方法，遇到动态障碍物时，代价地图会产生严重的重影，影响规划。
[0007]上述两个问题难以同时解决。
[0008]基于此，亟需一种基于有限视场角深度相机和多层代价地图的障碍物留存方法，以解决现有技术中存在的问题。

技术实现思路

[0009]本专利技术的目的是提供一种基于有限视场角深度相机和多层代价地图的障碍物留存方法，解决现有技术中的问题。
[0010]本专利技术公开了一种基于有限视场角深度相机和多层代价地图的障碍物留存方法，包括如下步骤：S1、根据障碍点在世界坐标系（map）中的坐标以及该帧点云的时间戳（time_stamp）将每一个障碍点以std::map[1]数据结构进行储存；该图层记为camera_layer；
S2、在移动机器人行驶过程中，有限视场角的深度相机采集到点云数据，并通过体素滤波，地面拟合等方法去除非障碍点；并输入camera_layer；S3、根据相机的视场角，在代价地图内描述一个“视场多边形”(FOV polygon),去除[1]中在视场多边形内的障碍点；S4、将新一帧点云数据在视场多边形内进行标注，已经被标注的障碍点则更新标记时间；S5、在机器人一定范围内的障碍点，除非进入视场多边形被更新或删除，否则永远保留；超出一定范围的障碍点，根据其上被更新的时间，超时删除；S6、每轮代价地图更新时，无需创建及维护本层栅地图，直接遍历[1]中的全部障碍点，以最大值方对总图层（master层）进行更新；S7、在膨胀层中，对障碍物依据梯度进行膨胀。
[0011]进一步地，所述步骤S1中map数据结构具体结构为<坐标点，时间>，其中坐标点结构为<横坐标，纵坐标>，其坐标数据类型为double，时间数据类型为int，单位为秒。
[0012]进一步地，所述步骤S2中有限视场角的深度相机采集到的点云数据至少包括障碍点的xyz坐标信息以及该帧点云的时间信息。
[0013]由于点云的稠密性，输入的点云需要进行体素滤波，同时由于部分点云表征的是机器人所在的地面，需要通过平面拟合以及限幅滤波将地面滤除，输出的稀疏点云作为代价地图更新的原始数据，有助于减小计算量，同时防止机器人误将地面当作障碍。
[0014]进一步地，所述步骤S3中的深度相机设置在机器人上部，并以一定角度向下投射其视场角一般可表征为四棱锥体。优选的，投射角度为45
°
。其与地面相交所形成的图形一般可近似为梯形（如图3所示），每次更新地图数据时，应首先清除当前“视场多边形”内的所有障碍点。
[0015]进一步地，所述步骤S4中将新一帧点云数据在“视场多边形”内进行标注，标注时应以步骤S1中所述数据结构，对障碍点坐标信息以及更新时进行储存；如果是已经被标注过的点，则更新其时间信息。
[0016]进一步地，所述步骤S5中障碍点删除的具体步骤如下：所述的超时删除功能仅对在距离机器人一定范围以外的障碍点生效；对于范围内的障碍点，除非被相机视场多边形更新删除，否则永久保留。
[0017]进一步地，所述步骤S6中对代价地图的更新，无需维护一张与静态地图同样大小的栅格代价地图，可直接遍历全部的障碍点，将障碍点的世界坐标（单位：m）转化为栅格地图坐标（单位：格）而后若总图层中的相应点为未知则不更新；若为其他，则更新为障碍点。
[0018]进一步地，所述步骤S7中，在总图层中，使用广度优先搜索的方式对障碍物对应栅格点进行膨胀，用来表征相应栅格点与障碍物的距离，其依距膨胀计算的代价值公式如下：
[0019]其中，d为被更新栅格和障碍物栅格之间的欧氏距离；inscribed_radius 是机器人的内切圆半径；inflation_radius是技术人员根据实际情况设定的膨胀层半径。
[0020]依此公式更新代价的特点如下：1、随着栅格距离障碍物距离的增大，代价值逐渐降低，且先快后慢；2、对于不同的膨胀半径，该代价公式能保证代价值在膨胀半径处平滑过渡为0，代价地图内相邻栅格代价无跳变，有助于机器人规划出更加平顺的路径3、其函数更加平滑（如附图4所示）。
[0021]本专利技术还公开了一种存储介质，保存有所述基于有限视场角深度相机和多层代价地图的障碍物留存方法的计算机程序。
[0022]本专利技术还公开了一种计算机设备，包括存储器、处理器，所述存储器包括所述存储介质。
[0023]本专利技术的实施过程如下：使用“视场多边形”对视场内的障碍物进行标记与删除判断，对视场外超出一定范围和一定时间的障碍物超时删除，使用map数据格式减少了内存空间的占用和计算效率，使用步骤S7中所述公式让障碍物膨胀层代价下降梯度更加平滑，本专利技术实现障碍物储存的过程如下：在移动机器人行驶过程中，深度相机检测到的障碍物数据以点云格式传入，经过地面拟合去除、体素滤波等预处理后传入多层代价地图。
[0024]代价地图是使用不同图层管理不同语义信息的障碍物的栅格地图，包括管理原始地图的静态层、管理雷达或点云格式障碍物信息的障碍层以及将障碍物依欧式距离进行膨胀的膨胀层等，将障碍层与静态层进行叠加最后由膨胀层进行本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种障碍物留存方法，其特征在于，包括如下步骤：S1、根据障碍点在世界坐标系（map）中的坐标以及该帧点云的时间戳（time_stamp）将每一个障碍点以std::map[1]数据结构进行储存；该图层记为camera_layer；S2、在移动机器人行驶过程中，有限视场角的深度相机采集到点云数据，并通过体素滤波，地面拟合等方法去除非障碍点；并输入camera_layer；S3、根据相机的视场角，在代价地图内描述一个“视场多边形”(FOV polygon),去除[1]中在视场多边形内的障碍点；S4、将新一帧点云数据在视场多边形内进行标注，已经被标注的障碍点则更新标记时间；S5、在机器人一定范围内的障碍点，除非进入视场多边形被更新或删除，否则永远保留；超出一定范围的障碍点，根据其上被更新的时间，超时删除；S6、每轮代价地图更新时，无需创建及维护本层栅地图，直接遍历[1]中的全部障碍点，以最大值方对总图层（master层）进行更新；S7、在膨胀层中，对障碍物依据梯度进行膨胀。2.根据权利要求1所述的障碍物留存方法，其特征在于，所述步骤S1中map数据结构具体结构为<坐标点，时间>，其中坐标点结构为<横坐标，纵坐标>，其坐标数据类型为double，时间数据类型为int，单位为秒。3.根据权利要求1所述的障碍物留存方法，其特征在于，所述步骤S2中有限视场角的深度相机采集到的点云数据至少包括障碍点的xyz坐标信息以及该帧点云的时间信息。4.根据权利要求1所述的障碍物留存方法，其特征在于，所述步骤S3中的...

【专利技术属性】
技术研发人员：请求不公布姓名，李宇浩，
申请(专利权)人：麦岩智能科技北京有限公司，
类型：发明
国别省市：