CN111397598A - 人机共融环境中移动型机器人路径规划采样方法及系统 - Google Patents

Abstract

本发明涉及一种人机共融环境中的移动型机器人路径规划采样方法及系统,包括：计算地图中的点到最近障碍物的距离，同时，对环境中的行人进行检测，并标记环境中行人的位置；选择起点位置作为根节点，初始化搜索树；在可行区域随机选取候选节点,计算搜索树上节点到所述候选节点的累计成本，选取成本最小的点作为生长点；对所述生长点和所述候选节点之间的连线作碰撞检测，判断碰撞检测是否通过；将所述候选节点连接到所述生长点上，判断所述候选节点是否为终点；从所述搜索树上获取从根节点到终点之间的节点连线集合，形成最终路径。本发明有利于形成更为安全可靠的路径。

Description

人机共融环境中移动型机器人路径规划采样方法及系统

技术领域

本发明涉及服务型机器人的技术领域，尤其是指一种人机共融环境中的移动型机器人路径规划采样方法及系统。

背景技术

人工智能技术的突破给移动型服务机器人研究带来了巨大的机遇，目前，引导机器人、扫地机器人、导购机器人、货物搬运机器人等移动型服务机器人已经成功应用到了机场、超市、博物馆、家庭等多种环境。移动机器人路径规划技术是指机器人在无人干预的条件下，自动根据环境地图的信息及起始点位置规划出一条可行路径的过程，路径通常由一系列的点组成。相对于工业机器人来说，服务型移动机器人所在的人机共融环境复杂度更高，不确定性更大，对路径规划的实时性要求更高。

现有的经典路径规划算法可大致分为四类：基于栅格的路径规划算法，基于人工势场的路径规划算法，基于奖赏的路径规划算法和基于随机采样的路径规划算法。其中，基于随机采样的路径规划算法由于避免了对状态空间的建模，极大地减少了规划时间和内存成本，更适用于解决动态人机共融环境的路径规划问题，但是当前采样策略仅仅考虑了树的内部节点之间的关系，忽略了环境的不确定性对树的生长的影响。对于服务型移动机器人所处的人机共融环境中来说，环境中的不确定性主要包含环境建模的误差、机器人的定位误差、动态行人影响等。由于当前采样策略仅仅考虑了树的内部节点之间的关系，因此忽略了环境的不确定性对树的生长的影响，从而导致形成的路径不够安全和可靠。

发明内容

为此，本发明所要解决的技术问题在于克服现有技术中忽略了环境的不确定性对树的生长的影响，从而导致形成的路径不够安全和可靠的问题，从而提供一种引入环境不确定性因素，且能形成更为安全可靠的路径的人机共融环境中的移动型机器人路径规划采样方法及系统。

为解决上述技术问题，本发明的一种人机共融环境中的移动型机器人路径规划采样方法，包括如下步骤：计算地图中的点到最近障碍物的距离，同时，对环境中的行人进行检测，并标记环境中行人的位置；选择起点位置作为根节点，初始化搜索树；在可行区域随机选取候选节点,计算搜索树上节点到所述候选节点的累计成本，选取成本最小的点作为生长点；对所述生长点和所述候选节点之间的连线作碰撞检测，判断碰撞检测是否通过，若通过，则进入步骤S5，否则，返回步骤S3；将所述候选节点连接到所述生长点上，判断所述候选节点是否为终点，若是，则进入步骤S6，否则返回步骤S3；从所述搜索树上获取从根节点到终点之间的节点连线集合，形成最终路径。

在本发明的一个实施例中，所述步骤S1中地图中的点到最近障碍物的距离可通过线下计算完成，对于同一幅地图，计算结果可在不同的路径规划中多次复用。

在本发明的一个实施例中，对环境中的行人进行检测的方法为：步骤S11：通过环境中的分布式摄像机网络拍摄环境图片；步骤S12：应用行人检测算法对行人进行检测，输出行人在像素坐标系下的位置；步骤S13：根据摄像机标定结果，通过坐标变换，输出行人在全局坐标系下的位置。

在本发明的一个实施例中，所述步骤S2中搜索树的节点包含：节点位置、节点连接和节点成本，在搜索树初始化过程中，根节点位置设置为起点，节点连接为空，节点成本为零。

在本发明的一个实施例中，所述步骤S3中可行区域是指在最近障碍物距离地图上取值大于零的区域。计算搜索树上节点到所述候选节点的累计成本的方法为：采用公式

其中，n_new为候选节点；n_i为搜索树上T的节点，n_i∈T；cost(n_i)表示从根节点到n_i的累计成本；dist(.)表示两个点之间的欧氏距离；angle(.)表示两条连线之间的夹角，一条是n_new与n_i之间的连线，另一条是与n_i与n_i父节点之间的连线；d_min表示n_i到最近障碍物的距离；S_p表示n_new与n_i之间的连线上行人所占据的可行区域的面积；

和ε是常数，

取最大线速度的倒数，ε取最大角速度的倒数。

在本发明的一个实施例中，所述生长点通过如下公式计算：

在本发明的一个实施例中，对所述生长点和所述候选节点之间的连线作碰撞检测的方法为：步骤S41：取生长点和候选节点之间的连线上的点的集合C；步骤S42：判断是否所有集合C中的点对应的最近障碍物距离地图上的值都大于零，如果是，则无碰撞发生，碰撞检测通过；否则，发生碰撞，碰撞检测不通过。

在本发明的一个实施例中，所述步骤S5中将所述候选节点连接到所述生长点的方法为：步骤S51：记录候选节点n_new的位置，计算候选节点的成本cost(n_new，n_growth)；步骤S52：将n_new的父节点设为n_growth，将n_new包含进n_growth的子节点列表中，建立起候选节点与随机树之间的联系。

在本发明的一个实施例中，所述步骤S6中从搜索树上获取从根节点到终点之间的节点连线集合的方法为：步骤S61：确定终点所在的节点n_goal，设为当前节点n_current；步骤S62：在所述搜索树上，获取n_current的父节点n_parent，将n_parent添加到路径上；步骤S63：判断n_parent是否为根节点，如果不是，则令n_current＝n_parent，返回步骤S62；如果是，则输出路径，结束搜索。

本发明还提供了一种人机共融环境中的移动型机器人路径规划采样系统，包括：障碍物确定模块，用于计算地图中的点到最近障碍物的距离，同时，对环境中的行人进行检测，并标记环境中行人的位置；初始化模块，用于选择起点位置作为根节点，初始化搜索树；选取模块，用于在可行区域随机选取候选节点,计算搜索树上节点到所述候选节点的累计成本，选取成本最小的点作为生长点；碰撞检测模块，用于对所述生长点和所述候选节点之间的连线作碰撞检测，判断碰撞检测是否通过；判断模块，用于将所述候选节点连接到所述生长点上，判断所述候选节点是否为终点；集合模块，用于从所述搜索树上获取从根节点到终点之间的节点连线集合，形成最终路径。

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

本发明所述的人机共融环境中的移动型机器人路径规划采样方法及系统，由于引入了环境不确定性因素，包括节点周围可行区域的大小、行人所占据的可行区域面积，综合衡量节点内部关系和环境不确定性的对于生长点选取的影响，引导随机树向行人数量少的宽阔区域生长，因此减少了由于环境不确定性引起的机器人迷失、碰撞等问题，形成更为安全可靠的路径。

附图说明

为了使本发明的内容更容易被清楚的理解，下面根据本发明的具体实施例并结合附图，对本发明作进一步详细的说明，其中

图1是本发明人机共融环境中的移动型机器人路径规划采样方法流程图；

图2a是本发明环境地图的示意图；

图2b是本发明环境地图与其对应的到最近障碍物距离示意图；

图3是本发明生长节点的选择过程示意图；

图4是本发明基于最近障碍物距离的碰撞检测示意图；

图5是本发明搜索树和生成路径示意图。

具体实施方式

实施例一

如图1所示，本实施例提供一种人机共融环境中的移动型机器人路径规划采样方法，包括如下步骤：步骤S1：计算地图中的点到最近障碍物的距离，同时，对环境中的行人进行检测，并标记环境中行人的位置；步骤S2：选择起点位置作为根节点，初始化搜索树；步骤S3：在可行区域随机选取候选节点,计算所述搜索树上节点到所述候选节点的累计成本，选取成本最小的点作为生长点；步骤S4：对所述生长点和所述候选节点之间的连线作碰撞检测，判断碰撞检测是否通过，若通过，则进入步骤S5，否则，返回步骤S3；步骤S5：将所述候选节点连接到所述生长点上，判断所述候选节点是否为终点，若是，则进入步骤S6，否则返回步骤S3；步骤S6：从所述搜索树上获取从根节点到终点之间的节点连线集合，形成最终路径。

本实施例所述人机共融环境中的移动型机器人路径规划采样方法，所述步骤S1中，计算地图中的点到最近障碍物的距离，有利于判断出静态障碍物，同时，对环境中的行人进行检测，并标记环境中行人的位置，有利于判断出动态障碍物；所述步骤S2中，选择起点位置作为根节点，初始化搜索树，从而有利于把起点加入到所述搜索树上；所述步骤S3中，在可行区域随机选取候选节点,计算所述搜索树上节点到所述候选节点的累计成本，选取成本最小的点作为生长点，有利于保证从所述根节点到当前点的成本最低；所述步骤S4中，对所述生长点和所述候选节点之间的连线作碰撞检测，判断碰撞检测是否通过，若通过，则进入步骤S5，否则，返回步骤S3，从而有利于保证所述生长点和所述候选节点之间没有障碍物；所述步骤S5中，将所述候选节点连接到所述生长点上，判断所述候选节点是否为终点，若是，则进入步骤S6，否则返回步骤S3，从而有利于确认终点；所述步骤S6中，从树上获取从根节点到终点之间的节点连线集合，形成最终路径，由于本发明引入了环境不确定性因素，包括节点周围可行区域的大小、行人所占据的可行区域面积，综合衡量节点内部关系和环境不确定性的对于生长点选取的影响，引导搜索树向行人数量少的宽阔区域生长，减少了由于环境不确定性引起的机器人迷失、碰撞等问题，因此可以形成更为安全可靠的路径。

如图2a和图2b所示，所述步骤S1中，所述地图采用二维栅格地图，在图2b中，距离的单位为栅格数，到最近障碍物距离大于零的区域即为可行区域。所述地图中的点到最近障碍物的距离可通过线下计算完成，对于同一幅地图，计算结果可在不同的路径规划中多次复用，从而有利于提高计算速率。

另外，对环境中的行人进行检测的方法为：

步骤S11：通过环境中的分布式摄像机网络拍摄环境图片；

步骤S12：应用行人检测算法对行人进行检测，输出行人在像素坐标系下的位置；

步骤S13：根据摄像机标定结果，通过坐标变换，输出行人在全局坐标系下的位置。

作为一种变形，行人检测也可以通过激光等其它传感器获得。

所述步骤S2中搜索树的节点包含：节点位置、节点连接和节点成本，在搜索树初始化过程中，根节点位置设置为起点，节点连接为空，节点成本为零，从而可以实现把起点加入到所述搜索树上。因此所述搜索树初始化之后仅包含根节点一个节点。

所述步骤S3中可行区域是指在最近障碍物距离地图上取值大于零的区域。

在可行区域随机选取候选节点的方法为：为所述可行区域内的点分配唯一索引序号，随机选取一个索引序号，其对应的点即为候选节点。

计算搜索树上节点到所述候选节点的累计成本的方法为：采用公式

其中，n_new为候选节点；n_i为搜索树T上的节点，n_i∈T；cost(n_i)表示从根节点到n_i的累计成本；dist(.)表示两个点之间的欧氏距离；angle(.)表示两条连线之间的夹角，一条是n_new与n_i之间的连线，另一条是与n_i与n_i父节点之间的连线；d_min表示m_i到最近障碍物的距离；S_p表示n_new与n_i之间的连线上行人所占据的可行区域的面积；

和ε是常数，用于权衡距离和角度差异对于成本的影响，对于机器人来说，

取最大线速度的倒数，ε取最大角速度的倒数。d_min越大，表示n_i周围的可行区域越大；行人数量越少，∑S_p占据的可行区域越小，机器人运行更安全。

所述生长点通过如下公式计算：

遍历搜索树上的节点，选择到候选节点累计成本最小的节点，作为候选节点的生长点，从而有利于保证从所述根节点到当前点的成本最低。

如图3所示，圆形表示根节点，三角形表示所述搜索树上的其它节点，星形表示候选节点，灰色圆圈表示行人，实线表示节点n₀到候选节点的连接，点划线表示节点n₁到候选节点的连接。由于n₀到候选节点的连接线上有多个行人，即ΣS_p值较大，n₀到候选节点的累计成本大于n₁到候选节点的累计成本，因此选择n₁作为生长点。

所述步骤S4中对所述生长点和所述候选节点之间的连线作碰撞检测的方法为：

步骤S41：取生长点和候选节点之间的连线上的点的集合C；

步骤S42：判断是否所有集合C中的点对应的最近障碍物距离地图上的值都大于零，如果是，则无碰撞发生，碰撞检测通过；否则，发生碰撞，碰撞检测不通过。

上述作碰撞检测时，优先选择仅仅针对静态障碍物作检测，由于对于人流量大的环境来说，连线与行人发生碰撞的概率极高，同时对动态行人做碰撞检测容易引起规划效率下降，甚至规划失败。因此，对于搜索树的生长过程可以不对动态障碍物进行碰撞检测。

如图4所示，为A点到B点的碰撞检测示意图，首先将A、B两点映射到最近障碍物距离地图上，选取AB连线所经过的栅格，如图中灰色方格所示，遍历灰色方格的值，如果所有值都大于零，则没有发生碰撞，碰撞检测通过；否则发生碰撞，碰撞检测不通过。

所述步骤S5中将所述候选节点连接到所述生长点的方法为：

步骤S51：记录候选节点n_new的位置，计算候选节点的成本cost(n_new，n_growth)；

步骤S52：将n_new的父节点设为n_growth，将n_new包含进n_growth的子节点列表中，建立起候选节点与随机树之间的联系。

所述步骤S6中从搜索树上获取从根节点到终点之间的节点连线集合的方法为：

步骤S61：确定终点所在的节点n_goal，设为当前节点n_current；

步骤S62：在所述搜索树上，获取n_current的父节点n_parent，将n_parent添加到路径上；

步骤S63：判断n_parent是否为根节点，如果不是，则令n_current＝n_parent，返回步骤S62；如果是，则输出路径，结束搜索。

如图5所示，圆形节点实线表示最终生成的路径，三角形表示搜索树上的节点，虚线表示搜索树上节点之间的连接关系，灰色圆圈表示行人，路径规划策略能够避开行人较多的区域，生成较为安全可靠的路径。

实施例二

基于同一发明构思，本实施例提供一种人机共融环境中的移动型机器人路径规划采样系统，其解决问题的原理与所述人机共融环境中的移动型机器人路径规划采样方法相同，重复之处不再赘述。

本实施例所述人机共融环境中的移动型机器人路径规划采样系统，包括：

障碍物确定模块，用于计算地图中的点到最近障碍物的距离，同时，对环境中的行人进行检测，并标记环境中行人的位置；

初始化模块，用于选择起点位置作为根节点，初始化搜索树；

选取模块，用于在可行区域随机选取候选节点,计算搜索树上节点到所述候选节点的累计成本，选取成本最小的点作为生长点；

碰撞检测模块，用于对所述生长点和所述候选节点之间的连线作碰撞检测，判断碰撞检测是否通过；

判断模块，用于将所述候选节点连接到所述生长点上，判断所述候选节点是否为终点；

集合模块，用于从所述搜索树上获取从根节点到终点之间的节点连线集合，形成最终路径。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (10)

1.一种人机共融环境中的移动型机器人路径规划采样方法，其特征在于，包括如下步骤：

步骤S1：计算地图中的点到最近障碍物的距离，同时，对环境中的行人进行检测，并标记环境中行人的位置；

步骤S2：选择起点位置作为根节点，初始化搜索树；

步骤S3：在可行区域随机选取候选节点,计算搜索树上节点到所述候选节点的累计成本，选取成本最小的点作为生长点；

步骤S4：对所述生长点和所述候选节点之间的连线作碰撞检测，判断碰撞检测是否通过，若通过，则进入步骤S5，否则，返回步骤S3；

步骤S5：将所述候选节点连接到所述生长点上，判断所述候选节点是否为终点，若是，则进入步骤S6，否则返回步骤S3；

步骤S6：从所述搜索树上获取从根节点到终点之间的节点连线集合，形成最终路径。

2.根据权利要求1所述的人机共融环境中的移动型机器人路径规划采样方法，其特征在于：所述步骤S1中地图中的点到最近障碍物的距离可通过线下计算完成，对于同一幅地图，计算结果可在不同的路径规划中多次复用。

3.根据权利要求1所述的人机共融环境中的移动型机器人路径规划采样方法，其特征在于：对环境中的行人进行检测的方法为：

步骤S11：通过环境中的分布式摄像机网络拍摄环境图片；

步骤S12：应用行人检测算法对行人进行检测，输出行人在像素坐标系下的位置；

步骤S13：根据摄像机标定结果，通过坐标变换，输出行人在全局坐标系下的位置。

4.根据权利要求1所述的人机共融环境中的移动型机器人路径规划采样方法，其特征在于：所述步骤S2中搜索树的节点包含：节点位置、节点连接和节点成本，在搜索树初始化过程中，根节点位置设置为起点，节点连接为空，节点成本为零。

5.根据权利要求1所述的人机共融环境中的移动型机器人路径规划采样方法，其特征在于：所述步骤S3中可行区域是指在最近障碍物距离地图上取值大于零的区域。计算搜索树上节点到所述候选节点的累计成本的方法为：采用公式

其中，n_new为候选节点；n_i为搜索树上T的节点，n_i∈T；cost(n_i)表示从根节点到n_i的累计成本；dist(.)表示两个点之间的欧氏距离；angle(.)表示两条连线之间的夹角，一条是n_new与n_i之间的连线，另一条是与n_i与n_i父节点之间的连线；d_min表示n_i到最近障碍物的距离；S_p表示n_new与n_i之间的连线上行人所占据的可行区域的面积；

和ε是常数，

取最大线速度的倒数，ε取最大角速度的倒数。

6.根据权利要求5所述的人机共融环境中的移动型机器人路径规划采样方法，其特征在于：所述生长点通过如下公式计算：

7.根据权利要求1所述的人机共融环境中的移动型机器人路径规划采样方法，其特征在于：对所述生长点和所述候选节点之间的连线作碰撞检测的方法为：

步骤S41：取生长点和候选节点之间的连线上的点的集合C；

步骤S42：判断是否所有集合C中的点对应的最近障碍物距离地图上的值都大于零，如果是，则无碰撞发生，碰撞检测通过；否则，发生碰撞，碰撞检测不通过。

8.根据权利要求1所述的人机共融环境中的移动型机器人路径规划采样方法，其特征在于：所述步骤S5中将所述候选节点连接到所述生长点的方法为：

步骤S51：记录候选节点n_new的位置，计算候选节点的成本cost(n_new，n_growth)；

步骤S52：将n_new的父节点设为n_growth，将n_new包含进n_growth的子节点列表中，建立起候选节点与随机树之间的联系。

9.根据权利要求1所述的人机共融环境中的移动型机器人路径规划采样方法，其特征在于：所述步骤S6中从搜索树上获取从根节点到终点之间的节点连线集合的方法为：

步骤S61：确定终点所在的节点n_goal，设为当前节点n_current；

步骤S62：在所述搜索树上，获取n_current的父节点n_parent，将n_parent添加到路径上；

步骤S63：判断n_parent是否为根节点，如果不是，则令n_current＝n_parent，返回步骤S62；如果是，则输出路径，结束搜索。

10.一种人机共融环境中的移动型机器人路径规划采样系统，其特征在于，包括：

障碍物确定模块，用于计算地图中的点到最近障碍物的距离，同时，对环境中的行人进行检测，并标记环境中行人的位置；

初始化模块，用于选择起点位置作为根节点，初始化搜索树；

选取模块，用于在可行区域随机选取候选节点，计算搜索树上节点到所述候选节点的累计成本，选取成本最小的点作为生长点；

碰撞检测模块，用于对所述生长点和所述候选节点之间的连线作碰撞检测，判断碰撞检测是否通过；

判断模块，用于将所述候选节点连接到所述生长点上，判断所述候选节点是否为终点；

集合模块，用于从所述搜索树上获取从根节点到终点之间的节点连线集合，形成最终路径。

Images