CN111638526B - 一种陌生环境下机器人自主建图的方法 - Google Patents

Abstract

本发明公开了机器人自主探索领域的一种陌生环境下机器人自主建图的方法，包括以下步骤：S1、根据穿墙随机树算法，生成已知区域与未知区域之间的前沿点，并对前沿点聚类优化，得到初步筛选的前沿点；S2、对初步筛选的前沿点进行评估，得出一个前沿目标点；S3、机器人向前沿目标点移动；S4、当机器人当前位置距离上一个位置的距离大于预设的运动半径时或者行进距离大于预设的步长时，判断是否有未知区域，如果有，则返回步骤S1，如果没有，停止前进，完成地图绘制。本发明的方法，与已有的随机树探索策略相比，扩大了随机树生长条件，并且设定刷新频率，可以更快的探索到机器人附近的前沿点。

Description

一种陌生环境下机器人自主建图的方法

技术领域

本发明涉及机器人自主探索领域，特别是一种陌生环境下机器人自主建图的方法。

背景技术

自主机器人探索的主要目标是使机器人能够在有限的时间内获取最完整，最准确的环境图，而无需人工干预。现在的主流方法是基于前沿(即未开发空间和已知空间之间的边界)进行探索，基于前沿的探索策略的思想是将机器人引导到未知区域以完成探索任务。目前传统的前沿探索算法是基于数字图像处理技术的前沿检测和区域提取，为了提取前沿，必须处理整个地图，并且随着地图的扩展，对其进行处理将消耗越来越多的计算资源。且对于前沿的选择多为随机法或者最近法，导致探索效率低下。

发明内容

本发明将穿墙随机树算法应用于机器人的自主前沿探索，并且取消了随机树生长点固定不变的生长规则，设置了随机树生长点刷新的合适频率，提出了一种陌生环境下机器人自主建图的方法，从而加快了自主探索的速度，更加有效的对未知环境进行建图。

为了实现上述发明目的，本发明提供了以下技术方案：

一种陌生环境下机器人自主建图的方法，包括以下步骤：

S1，以机器人所在的位置为参考点，根据穿墙随机树算法，生成已知区域与未知区域之间的前沿点，并对前沿点聚类优化，得到初步筛选的前沿点；

S2，根据预期距离和未知区域空间占比值，对初步筛选的前沿点进行评估，得出一个前沿目标点；

S3，机器人向前沿目标点移动；

S4，在机器人移动过程中，当机器人当前位置距离上一个位置的距离大于预设的运动半径并且行进距离大于预设步长时，判断是否有未知区域，如果有，则返回步骤S1，如果没有，停止前进，完成地图绘制。

进一步的，S1包括以下步骤：

S11，以机器人所在的位置为中心点，激光雷达以半径R对周围进行扫描，获得已知区域、障碍物存在位置和未知区域的2D栅格地图；

S12，以机器人所在的位置为起点，利用穿墙随机树算法，向外延展，生成一个随机树；

当随机树新树枝的两端都在已知区域当中，无论新树枝两端之间有无障碍物，都把新树枝添加到已有随机树当中；当随机树新树枝的一端在已知区域，另一端在未知区域时，随机树停止生长，根据新树枝的两个端点，得到一个前沿点，并停止基于新树枝的树枝添加；

S13，重复步骤S12，直到生成已知区域与未知区域之间的所有前沿点；

S14，将相邻的多个前沿点分为一组，得到多组前沿点，找到每一组前沿点的质心前沿点，以质心前沿点为初步筛选的前沿点。

作为优选方案，2D栅格地图使用基于滤波SLAM框架的开源SLAM算法Gmapping构建。

作为优选方案，S3中对初步筛选的前沿点进行评估的条件为：预期距离最小，并且未知区域空间占比值最大。

进一步的，未知区域空间占比值的计算步骤为：以初步筛选的前沿点为圆心，以激光雷达的检测距离为半径形成一个圆；计算圆中未知区域面积在整个圆面积的占比。

作为优选方案，前沿点评估的计算公式是

其中，

x_f代表前沿点，x_r代表机器人的当前位置，R(x_f)是前沿点x_f的评估值，I(x_f)是前沿点x_f的信息增益，N(x_f)是前沿点x_f的导航成本，

是衡量I(x_f)和N(x_f)的权重系数，h(x_f，x_r)代表滞后增益，h_rad是预设的半径值，h_gain是一个大于1的预设固定值。

作为优选方案，步骤S4中，预设步长为运动半径的1.5倍，运动半径为探索环境最短边长的0.1倍。

基于相同的构思，本发明还提出了一种陌生环境下机器人自主建图的系统，包括至少一个处理器，以及与至少一个处理器通信连接的存储器；存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够执行上述任一项的方法。

与现有技术相比，本发明的有益效果：

1、本发明将应用于路径规划的穿墙随机树算法应用于机器人对未知区域的前沿探索，提出了一种新的探索策略。与基于图像处理的传统方法相比，用于探索的数据处理量减少，提高了效率，可以更有效地应用于高维探索。与已有的随机树探索策略相比，由于以往的随机树更多的是用于机器人的路径规划而不是环境探索，不会把穿越障碍物的“树枝”加入到已有的随机树当中。本发明主要用于环境探索而非路径规划，扩大了随机树生长条件，加快了随机树的生长速度从而缩短了确定前沿点的时间，提高了探索的速度。

2、取消了随机树生长点固定不变的生长规则，机器人设置运动半径和固定步长双重标准，设置了随机树生长点的刷新频率。可以更快的探索到机器人附近的前沿点，消除机器人由于自身的定位误差陷入在两个“相似”目标点不停摆动而长期停止前进的可能性，提高了对未知环境进行建图的效率。

附图说明：

图1为本发明一种陌生环境下机器人自主建图方法的流程图；

图2为本发明实施例1中边界点检测圆示意图；

图3为本发明实施例2中实验场地布置图；

图4为本发明实施例2中实验所用机器人图；

图5为本发明实施例2中激光雷达图；

图6为本发明实施例2中建图界面图；

图7(a)为本发明实施例2中开始建图时的地图示意图；

图7(b)为本发明实施例2中不显示随机树的情况下完成建图时的地图示意图；

图7(c)为本发明实施例2中实际的场地图；

图7(d)为本发明实施例2中显示随机树的情况下完成建图时的地图示意图。

具体实施方式

下面结合试验例及具体实施方式对本发明作进一步的详细描述。但不应将此理解为本发明上述主题的范围仅限于以下的实施例，凡基于本发明内容所实现的技术均属于本发明的范围。

实施例1

一种陌生环境下机器人自主建图的方法的流程图如图1所示，步骤包括：

S1，以机器人所在的位置为参考点，根据穿墙随机树算法，生成已知区域与未知区域之间的所有前沿点。

本方法使用基于滤波SLAM框架的开源SLAM算法Gmapping构建2D栅格地图，已知区域与未知区域根据2D栅格地图进行划分。在栅格图中，栅格单元具有三种状态：空闲，占用(即障碍物存在)和未知。栅格图中的前沿点定义为空闲和未知的栅格之间的边界，本发明后续使用穿墙随机树算法在地图上生成前沿点，也是基于栅格图实现的。对于封闭环境，穿墙随机树算法具有完整性，可以保证机器人在自主探索过程中探索所有区域并构建完整的地图。

具体步骤包括：

S11，以机器人当前位置为参考点，使用基于滤波SLAM框架的开源SLAM算法Gmapping构建2D栅格地图；

S12，以机器人所在的位置为中心点，激光雷达对半径为R的范围进行扫描，获取已知区域与机器人的位置关系，即通过激光雷达探明可行进的已知区域、障碍物与机器人的相对位置关系，获得已知区域、障碍物存在位置和未知区域的2D栅格地图。

S13，以机器人所在的位置为起点，利用穿墙随机树算法，向外延展，生成随机树，当随机树生成新树枝后，如果新树枝一个点在2D栅格地图的已知区域，而另一个点位于2D栅格地图未知区域时，则通过新树枝的两个点，确定前沿点。

S14，重复步骤S13，直到找到2D栅格地图中可行进的已知区域与未知区域之间的所有前沿点。

生成其中一个前沿点的步骤具体包括：首先，初始化穿墙随机树以后，将机器人的当前位置p_current作为树的根节点。其次，在当前树中找出距离新点p_rand最近的节点p_nearest，将这两点连接成一条直线，沿着这条直线从点p_nearest向p_rand移动η长度的距离得到一个新的点p_new，其中，前沿点表示为p_i，i是区分不同边界点的下标，新点p_rand是在地图上随机生成的，η代表树的增长率，当η较大时，树增长较快，但不会探索到一些小的角落；当η相对较小时，可以到达拐角处，但是生成前沿点的速度会降低。为了兼顾探索范围和树的生长速度，可以根据地图大小对树的增长率η进行设置。如果新生成的点p_new位于未知区域中，而距离新点p_new最近的节点为节点p_nearest，则说明前沿点在新点p_new和节点p_nearest连线之间，在该连线上从p_new向p_nearest进行搜索，临近第一个状态为未知的栅格单元的点即为前沿点，该前沿点将被标记在地图上。如果新生成的点p_new位于已知区域中，无论该点是否落在障碍物上，都要将p_new与p_nearest的连线作为新的“树枝”添加到随机树当中。

多次用穿墙随机树搜索，可以得到可行进的已知区域与未知区域之间的所有K个前沿点，前沿点的个数与2D栅格地图的分辨率有关。

将生成的K个前沿点进行分组，相邻的多个前沿点分为一组，得到M组前沿点，找到每一组前沿点的质心，近似的用质心代表该组前沿点的位置。由于每组前沿点都是相邻的前沿点，位置相近似，在不影响目标点筛选的情况下，用质心代表该组前沿点的位置，这样的近似是可以的。用前沿点的质心代表各组的前沿点位置，是将所有K个前沿点进行了优化和筛选，减少了前沿点的个数，减少了计算量。

S2，以到达前沿点要经过的预期距离最短为一个筛选条件，以前沿点为圆心，未知区域空间占比最大为另一个筛选条件，通过这两个筛选条件，对初步筛选的前沿点进行评估，得出一个前沿目标点。

前沿点评估是选择前沿点的基础。一般从以下三个因素评估前沿点：前沿点的信息增益，导航成本和机器人定位的精度。在这些因素中，信息增益定义为在给定的前沿点上预期要探索的未知区域的面积。本实施例中信息增益根据前沿点周围未知区域的空间占比来体现，具体步骤如下：以前沿点为圆心，以激光雷达的检测距离为半径形成前沿点检测圆，如图2所示；通过计算该圆中未知区域占整个圆的空间比例来量化信息增益。导航成本定义为机器人从当前位置到达前沿点要经过的预期距离，用机器人从当前位置到前沿点的欧几里得距离来表示它。此外，准确的地图取决于机器人对自己姿势的准确估计。如果在前沿点的检测范围内可以检测到更多线特征或其他特征(例如断点，拐角，折线)，则机器人可以更准确地定位自己。目前在本实施例中，只选取信息增益和导航成本对前沿点进行评估，前沿点评估值R的计算公式如公式(1)所示。

其中，

x_f代表前沿点，x_r代表机器人的当前位置，I(x_f)是前沿点x_f的信息增益，N(x_f)是前沿点x_f的导航成本，

是定义的常数，代表权重，用来表示和导航成本相比信息增益的重要程度，也可以使信息增益和导航成本处于同一数量级，R(x_f)是前沿点x_f的评估值，h(x_f，x_r)代表滞后增益，如果前沿点x_f距离机器人当前位置x_r超过给定半径h_rad，则h(x_f，x_r)等于1；反之，如果前沿点x_f距离机器人当前位置x_r小于等于给定半径h_rad，则h(x_f，x_r)就等于h_gain，h_gain是一个大于1的固定值。公式中引入滞后增益函数的目的是使机器人更偏向于探索其附近的前沿点，这样做的好处是避免地图的重复探索。

S3，以机器人当前位置为起点，前沿目标点为终点，完成路径规划。

在每次向前沿目标点移动过程中，使用DWA算法(全称dynamic window approach，动态窗口法)来进行局部路径规划，以避免障碍。ROS提供的move_base包中提供的local_planner插件很好的集合了DWA算法，在实际导航中直接应用该插件。

S4，在机器人向前沿目标点移动过程中，当机器人当前位置距离上一个位置的距离大于预设的运动半径或者预设的步长时，判断是否有未知区域，如果有，则返回步骤S1，以机器人当前位置为根节点重置随机树，对前沿点重新筛选，确定新的目标前沿点并重新规划路径，如果没有，停止前进，完成地图绘制。

可以理解为，在机器人行进过程中，并不是到达目标前沿点之后才开始下一个目标前沿点的筛选，而是机器人实时进行着目标前沿点的筛选，实际上机器人并没有到达目标前沿点就调整了路线往最新的目标前沿点前进。机器人在向目标前沿点行进时，当机器人行进到设定的运动半径或者预设步长时，就重置随机树筛选出了新的目标前沿点并完成了路线规划，机器人根据最新的路径规划向最新的目标前沿点行进。

在移动过程中，激光雷达会探测到新的区域并产生新的前沿点，此时前往的目标前沿点很可能不是最优，就要对前沿点重新筛选并重新规划路径向新的目标前沿点行进。经过实验得出，此方法会导致目标点位置变化过快，甚至会产生最优点在两个前沿点之间跳动的情况，使机器人原地摆动无法前进。所以为了降低目标点的刷新频率，可以为机器人设置运动半径和固定步长双重标准，一旦机器人位置距离上一位置超过运动半径并且运动距离超过固定步长，就重置随机树筛选新的目标点并规划新的路径。经过实验，设置固定步长为运动半径的1.5倍，运动半径设置为预计探索环境最短边长的0.1倍，能够达到较好的效果。

同时，由于随机树后期生长较慢，为了使机器人更快的探索机器人附近的前沿点，需要重置随机树，并不总以起始位置为根节点，而是每次刷新目标点的同时以机器人当前位置为根节点产生新的穿墙随机树，这样可保证每次探索到的前沿点都是距离机器人较近的。目的在于使探索效率更加高效，减少机器人的移动距离。

实施例2

采用本发明的方法，已经实现了机器人对未知区域地图的构建。实验场地是在一个8m×8m的空闲场地，用纸箱简单布置如图3所示场地。实验所用机器人为两轮差速机器人，大小为85cm×75cm，实验机器人图如图4。其上配置镭神激光雷达，角度为180度，测量半径8m，激光雷达的图片如图5所示。

激光雷达采用USB转串口的方式将数据传输到mini-PC上，运行激光雷达的底层驱动“leishen_lidar_node”节点，实现激光雷达原始数据的读取与矫正，同时将矫正后的激光雷达数据转换成标准的ROS下的LaserScan格式并且以名为/uestc003/scan的Topic发出。然后通过/uestc003/slam_gmapping节点订阅并建图，由/detector探索前沿点并发送给节点/filter，节点/filter将其聚类筛选发送给节点/assigner，最后将评估值最高的前沿点发送给move_base包进行导航。

建图界面如图6所示，终端三个界面分别为gmapping节点和move_base节点的启动、rviz可视化界面启动和自主探索算法相关节点的运行。开始建图时如图7(a)所示，激光雷达扫描出部分障碍物。如图7(d)所示，图中的线为穿墙随机树，最终整体地图建完，没有新的前沿点需要探索，机器人静止不动，完整建图如图7(b)所示，图7(b)是不显示随机树的图。图7(c)是实际场地图。

Claims (7)

1.一种陌生环境下机器人自主建图的方法，其特征在于，包括以下步骤：

S1，以机器人所在的位置为参考点，根据穿墙随机树算法，生成已知区域与未知区域之间的前沿点，并对所述前沿点聚类优化，得到初步筛选的前沿点；

S2，根据预期距离和未知区域空间占比值，对所述初步筛选的前沿点进行评估，得出一个前沿目标点；

S3，机器人向所述前沿目标点移动；

S4，在机器人移动过程中，当机器人当前位置距离上一个位置的距离大于预设的运动半径并且行进距离大于预设步长时，判断是否有未知区域，如果有，则返回步骤S1，如果没有，停止前进，完成地图绘制；

所述步骤S1包括以下步骤：

S11，以机器人所在的位置为中心点，激光雷达以半径R对周围进行扫描，获得已知区域、障碍物存在位置和未知区域的2D栅格地图；

S12，以机器人所在的位置为起点，利用穿墙随机树算法，向外延展，生成一个随机树；

当随机树新树枝的两端都在已知区域当中，无论新树枝两端之间有无障碍物，都把新树枝添加到已有随机树当中；当随机树新树枝的一端在已知区域，另一端在未知区域时，随机树停止生长，根据新树枝的两个端点，得到一个前沿点，并停止基于新树枝的树枝添加；

S13，重复步骤S12，直到生成已知区域与未知区域之间的所有前沿点；

S14，将相邻的多个前沿点分为一组，得到多组前沿点，找到每一组前沿点的质心前沿点，以所述质心前沿点为初步筛选的前沿点。

2.如权利要求1所述的一种陌生环境下机器人自主建图的方法，其特征在于，所述2D栅格地图使用基于滤波SLAM框架的开源SLAM算法Gmapping构建。

3.如权利要求1所述的一种陌生环境下机器人自主建图的方法，其特征在于，所述步骤S2中对初步筛选的前沿点进行评估的条件为：预期距离最小，并且未知区域空间占比值最大。

4.如权利要求3所述的一种陌生环境下机器人自主建图的方法，其特征在于，所述未知区域空间占比值的计算步骤为：以初步筛选的前沿点为圆心，以激光雷达的检测距离为半径形成一个圆；计算所述圆中未知区域面积在整个圆面积的占比。

5.如权利要求4所述的一种陌生环境下机器人自主建图的方法，其特征在于，对初步筛选的前沿点进行评估的计算公式是

，

其中，

，

代表前沿点，

代表机器人的当前位置，

是前沿点

的评估值，

是前沿点

的信息增益，

是前沿点

的导航成本，

是衡量

和

的权重系数，

代表滞后增益，

是预设的半径值，

是大于1的预设的固定值。

6.如权利要求1所述的一种陌生环境下机器人自主建图的方法，其特征在于，步骤S4中，所述预设步长为运动半径的1.5倍，所述运动半径为探索环境最短边长的0.1倍。

7.一种陌生环境下机器人自主建图的系统，其特征在于，包括至少一个处理器，以及与至少一个处理器通信连接的存储器；存储器存储有可被至少一个处理器执行的指令，所述指令被至少一个处理器执行，以使至少一个处理器能够执行权利要求1-6任一项所述的方法。

Images