CN103365293B - 一种基于动态区域划分的机器人安全路径规划方法 - Google Patents

Abstract

本发明涉及一种基于动态区域划分的机器人安全路径规划方法，其步骤包括：1）在路径空间中随机选择一个采样点作为扩展方向，并选取离该随机采样点路径最短的临近节点；2）根据障碍物占据区域的大小对所述临近节点进行区域划分并建立路径的安全性评价准则，进而确定最优临近节点；3）根据最优临近节点及其区域划分结果选择合适的方式进行路径扩展，生成新的节点并将其添加到扩展树中，逐步生成机器人安全路径。本发明针对复杂可变的未知环境，考虑在多自由度机器人的路径规划中，使路径地图中的节点含有当地局部区域的环境信息，并用这个环境信息指导规划器高效、安全、实时地规划路径。

Description

一种基于动态区域划分的机器人安全路径规划方法

技术领域

本发明属于智能人机交互、机器人运动规划领域，具体涉及一种适用于复杂动态环境下的基于区域划分的机器人安全路径规划方法。

背景技术

机器人路径规划是机器人系统的重要组成部分，是机器人导航和目标跟踪的基础。路径规划的根本目的是为高层任务与底层描述之间建立一个相互转化的桥梁，当高层通过高层语言发出任务命令时，机器人能够通过这个转化桥梁自动的将命令转化成底层动作并智能化的执行。这是友好人机交互的基本条件之一。路径规划在计算机图形、生物医疗和航天探索等领域也有重要的应用。

目前静态环境中的规划问题得到了不错的解决。但是在实际应用中，机器人所处的环境一般不可能是不发生任何变化的静态环境。障碍物的数量和运动模式、环境的尺度等等信息对机器人而言都是不确定的，这些不确定因素使得动态环境中的路径规划远比静态环境中难。因此，实时的人机交互还面临着许多难题，在复杂的动态环境中设计安全、高效地路径规划方法就显得尤为重要。

发明内容

本发明针对复杂可变的未知环境，提供一种基于动态区域划分的机器人安全路径规划方法，考虑在多自由度机器人的路径规划中，使路径地图中的节点含有当地局部区域的环境信息，并用这个环境信息指导规划器高效、安全、实时地进行路径规划。

为实现上述目的，本发明采用如下技术方案：

一种基于动态区域划分的机器人安全路径规划方法，其步骤包括：

1）在路径空间中随机选择一个采样点作为扩展方向，并选取离该随机采样点路径最短的临近节点；

2）根据障碍物占据区域的大小对所述临近节点进行区域划分，基于临近节点的区域划分结果建立路径的安全性评价准则，根据该安全性评价准则确定最优临近节点；

3）根据最优临近节点及其区域划分结果选择合适的方式进行路径扩展，生成新的节点并将其添加到扩展树中，同时对该新生成的节点进行区域划分以备后用，从而逐步生成机器人安全路径。

上述方法实际是一个迭代运算的过程，节点的区域划分是用来指导下一个迭代运算中的节点扩展，划分了区域才能选择合适的扩展方式进行扩展。下面对各步骤做具体的说明：

1.区域划分方法。

位姿空间可以被划分为一个个栅格区域的集合。这些局部区域可以根据障碍物占据区域的大小被标记一个拥塞程度，比如分为自由、交界、狭窄路径和障碍物区域。区域的拥塞程度可以作为一个属性加入到路径地图的节点中，使路径地图对局部环境信息敏感度增强。对机器人而言，可以通过在这个区域里进行采样观测来判定这个区域属于那种类型。具体的说来，这个方法包含以下几个步骤：

1）用一个固定大小的窗口进行采样，观测窗口内采样到有效点的概率P：

$P\left(q\right)=\frac{{\mathrm{\Σ}}_W{q}_{\mathrm{valid}}}{{\mathrm{\Σ}}_W(q_{\mathrm{invalid}}+q_{\mathrm{valid}})}$

其中是q窗口的中心点，W代表观测窗口，也就是以q为中心的局部区域。在本文算法中，中心点位置一直是树中的有效节点，并且在上式的计算中算做有效采样点，因此P∈(0,1]。

2）定义一个窗口中心节点的有效熵，将前面的观测结果标记为节点属性，该有效熵为：

$e\left(q\right)=\left\{\begin{array}{ccc}1+P\left(q\right)\log \left[P\left(q\right)\right]& \mathrm{if}& p\left(x\right)<1/2\\ -P\left(q\right)\log \left[P\left(q\right)\right]& \mathrm{else}& \end{array}\right.$

3）根据这个有效熵进行区域分类。

规划器将得到的有效熵定义为窗口中心节点的一个属性，有效熵实际上反应了节点所处区域的拥挤状况，有效熵越大说明区域障碍物越少，反之，则障碍物越多。节点的这个属性值，体现了节点周围的拥塞程度，，即利用环境信息来描述节点，在扩展中可以设计与节点有效熵相符合的扩展方法。

2.基于安全准则的路径生成方法

最优路径的评价准则往往根据问题的实际需求而各不相同。在静态环境中，最经常使用的评价准则有最短路径，最少功耗等等。因为动态环境问题比较复杂，还没有一个简单合理的标准来衡量生成路径的优劣，研究者们往往根据待解决问题的特点而自己进行一些最优的假设。

本发明对路径的优化评价是自己设定的一个加权范数距离，这个评价准则结合了最短路径衡量和一个路径的安全性评价——路径的概率持续有效时间。这个范数距离定义为：

$d=k_1\&times;\mathrm{\&Sigma;dist}(q_i,q_{i+1})+k_2\&times;\mathrm{\&Sigma;s}\left(\stackrel{\&RightArrow;}{q_i{q}_{i+1}}\right)$

其中k₁，k₂是环境相关的常参数，dist(q_i,q_i+1)返回两个节点间的距离度量，q_i是路径上的节点，表示两个节点中间路径片段的安全性评估。最优邻近点的选取一般包含以下步骤：

1）选取路径地图上离随机采样点最近的K个节点；

2）利用节点有效熵的进行安全性度量，选取最优临近节点：

$q_{\mathrm{nearest}}\&DoubleLeftRightArrow;\min \{k_1\&times;\mathrm{\&Sigma;dist}(q_i,q_{i+1})+k_2\&times;\frac{1}{e\left(q_{\mathrm{near}}\right)}\}.$

目前的快速随机扩展树方法（Rapidly-exploringRandomTree，RRT）方法虽然在采样偏移上对采样点的分布进行了偏移，但在规划中的大部分情况下，在扩展过程中，搜索树中节点都是一样对待的。就是说整个环境中的路径地图的采样点是一样密集的，显然这并不符合人在运动中更关注有障碍物区域的特点。同样策略进行扩展的结果就是，在自由位姿空间采样过多，冗余信息影响规划器效率，困难区域采样点少，很难探索。因此不仅要用不同的扩展方法提高扩展成功的概率，还要改变规划器在不同区域里路径地图上节点的密集程度。

本发明对RRT的扩展方法进行了增强改进，提出了适用于不同拥挤程度的环境中的扩展方法和自适应的重扩展策略，提高了在困难区域扩展成功的概率，一定程度上提高了可扩展性。本发明还将路径的安全性纳入到最优路径选择的过程中，在考虑最短路径的同时，使路径尽量往远离障碍物的区域偏移。所以在考虑障碍物移动的情况下，本发明算法生成的路径比以往算法得到的路径有效的持续时间有一定提高。

附图说明

图1是本发明实施例的基于区域划分的安全路径生成算法流程图。

图2是本发明实施例的三种扩展方法示例图。

图3是本发明实施例的扩展执行流程图。

具体实施方式

下面通过实施例并结合附图，对本发明的技术方案做进一步详细描述。

本发明对RRT的扩展方法进行了增强改进，图1是本实施例的基于区域划分的安全路径生成算法流程图。对其说明如下：

（1）RRT在扩展新节点时，首先生成需要在空间中随机选择一个采样点作为扩展方向，如图1中虚线方框的搜索循环。

（2）选定扩展方向后，需要在路径地图选取一个扩展的父节点。

首先返回路径地图上离随机采样点路径最短的K个节点，加入基于节点有效熵的安全评价准则，从这K个临近节点中选取最安全，也就是区域中障碍物最少的节点作为扩展的父节点。

（3）扩展的执行。本发明提出了三种适合不同拥塞程度环境的扩展方法，如图2所示，这三种扩展方法的具体实施如下：

a)第一种方法主要是针对拥塞程度轻微的区域，可近似看作自由区域，如图2(a)所示的S₁区域。图中，T为扩展树，q_rand是随机采样点，q_nearest是选取的最优邻近点，q_new是添加到扩展树中的新生成节点，l₁是该区域扩展的扩展步长。这种区域扩展成功率比较高，因此不需要进行特别的算法改进，因此直接沿扩展方向以一定步长延伸，得到新节点，尝试将新节点插入路径地图，本发明将其称为原始扩展。

b)第二种方法针对拥塞程度适中的区域，这种区域最典型的代表就是自由位姿空间和障碍物的交界区域。如图2(b)所示的S₂区域，其中，l₂为该区域的扩展步长，Obs.为障碍物占据的区域。从图2(b)中可以看出，该方法当原始扩展失败之后，在的垂直方向进行扩展尝试，本发明把这种扩展方式称为垂直方向扩展。在类似于障碍物边界这样的区域，因为紧邻着自由区域，在理想的情况下，的垂直方向指向自由区域的概率是1，在垂直于障碍物边界的情况下，新的扩展方向垂直于障碍物。考虑障碍物边界复杂不规则的情况，这种方式生成的q_new有效的概率依旧比较大。

c)第三种方法针对拥塞程度很大的区域，通常这种区域都是处在障碍物的包围中，如图2(c)所示的S₃区域。图中l₃为该区域的扩展步长，q'_new为最终添加到扩展树中的新生成节点。当原始扩展失败后，规划器使用二分查找找到上障碍物的边界点q₁，然后在与成一个小随机偏移角的方向上，以q_nearest为射点发出一道射线，找到射线上障碍物的另一个边界点q₂，以为方向生成。从图中可以看到，的生成过程实际上包含了对障碍物边界的粗略估计，使新节点沿着与障碍物平行的方向生成q_new，本发明称这种方法为平行障碍物扩展。在障碍物密集的情况下，这种扩展方向是最容易获得成功的。

图3示意了基于上述三种备选扩展方法的扩展流程，具体说明如下：

a)节点被选作扩展节点q_nearest时，可以使用这种情况下效率最高的原始扩展方法。当选定一个扩展方向q_rand后，从q_nearest向q_rand的方向，以扩展步长l₁为距离，生成q_new。然后将新节点q_new插入到搜索树中。

b)如果扩展失败，则将扩展步长变为l₂，用垂直方向扩展方法重新生成q_new。

c)若还是扩展失败，则将扩展步长变为l₃，用平行障碍物扩展方法重新生成q_new。

（4）当扩展成功，路径地图中加入新的节点后，需要对这个新的节点进行区域划分。其具体执行如下:

a)通过前面的扩展方法，一个新的节点q_new产生并加入到搜索树当中。这时以q_new为中心点进行区域窗口观测，得到q_new的有效熵值。

b)如果在下一轮的扩展过程中，如果q_new被选做最优临近点，则在以q_new为新的父亲节点进行扩展之前，进行区域分类计算。在计算区域分类的时候，考虑到正常的情况下，环境总是渐变的，因此在对某个节点进行区域划分的时候，加入对它的父亲节点q_nearest的有效熵的考量。

c)然后根据基于有效熵的区域划分策略进行区域划分。采用预设阈值的方法进行分类，设定两个经验阈值，将区域简单的划分为三种类型：如果有效熵大于阈值1，则判定为拥塞程度轻微的区域，这种区域内基本没有障碍物，一般可以当作自由的位姿空间；如果有效熵小于阈值2，判定为严重拥塞的区域类，在这种区域中很难采样到有效节点，因此路径地图难以覆盖到这种区域，比如狭窄通道问题或者L型隧道问题，就是专门研究这种区域的规划方法；否则判定为拥塞程度适中的区域类，这种区域中的障碍物占据的面积在一半上下，这是实际应用中规划问题中最经常处理的区域类型，比如障碍物与自由区域的交界区域。由于在自由区域扩展没有难度，而在拥塞太厉害的区域扩展又太费劲，因此这种区域类型里面的节点是规划器最应该关注的节点。

上述实例只是本发明的举例，尽管为说明目的公开了本发明的树结构实施例和附图，但是本领域的技术人员可以理解：在不脱离本发明及所附的权利要求的精神和范围内，各种替换、变化和修改都是可能的。因此，本发明不应局限于实施例和附图所公开的内容，本发明要求保护的范围以权利要求书界定的范围为准。

Claims (7)

1.一种基于动态区域划分的机器人安全路径规划方法，其步骤包括：

1)在路径空间中随机选择一个采样点作为扩展方向，并选取离该随机采样点路径最短的临近节点；

2)根据障碍物占据区域的大小对所述临近节点进行区域划分，基于临近节点的区域划分结果建立路径的安全性评价准则，根据该安全性评价准则确定最优临近节点；所述区域划分的方法为：

a)用一个固定大小的窗口进行采样，观测窗口内采样到有效点的概率：

其中是q窗口的中心点，W代表观测窗口；

b)定义窗口中心节点的有效熵：

c)根据所述有效熵进行区域划分；

3)根据最优临近节点及其区域划分结果选择合适的方式进行路径扩展，生成新的节点并将其添加到扩展树中，同时对该新生成的节点进行区域划分以备后用，从而逐步生成机器人安全路径。

2.如权利要求1所述的方法，其特征在于：所述安全性评价准则结合了最短路径衡量和路径的概率持续有效时间，其范数距离定义为：

其中k₁，k₂是环境相关的常参数，dist(q_i,q_i+1)返回两个节点间的距离度量，q_i是路径上的节点，表示两个节点中间路径片段的安全性评估。

3.如权利要求2所述的方法，其特征在于，确定所述最优邻近节点的方法为：

1)选取路径地图上离随机采样点最近的K个节点；

2)利用节点的有效熵进行安全性度量，选取最优临近节点：

4.如权利要求1所述的方法，其特征在于，步骤3)所述路径扩展针对三种不同的拥塞程度环境：拥塞程度轻微的区域，拥塞程度适中的区域，以及拥塞程度很大的区域。

5.如权利要求4所述的方法，其特征在于，设q_rand是随机采样点，q_nearest是选取的最优邻近点，q_new是添加到扩展树中的新生成节点，则基于所述三种不同的拥塞程度环境的扩展流程为：

a)节点被选作扩展节点q_nearest时，使用效率最高的原始扩展方法，即针对拥塞程度轻微的区域的扩展方法，当选定一个扩展方向q_rand后，从q_nearest向q_rand的方向，以扩展步长l₁为距离，生成q_new，然后插入到搜索树中；

b)如果扩展失败，则将扩展步长变为l₂，用垂直方向扩展方法重新生成q_new；所述垂直方向扩展方法是指在的垂直方向进行扩展；

c)若还是扩展失败，则将扩展步长变为l₃，用平行障碍物扩展方法重新生成q_new；所述平行障碍物扩展方法是指沿着与障碍物平行的方向生成新节点。

6.如权利要求5所述的方法，其特征在于，扩展成功并在路径地图中加入新的节点后，对该新节点进行区域划分。

7.如权利要求6所述的方法，其特征在于，对所述新节点进行区域划分的方法是：

a)新节点q_new产生并加入到搜索树中后，以q_new为中心点进行区域窗口观测，得到q_new的有效熵值；

b)在下一轮的扩展过程中，如果q_new被选做最优临近点，则在以q_new为新的父亲节点进行扩展之前进行区域分类计算，并在计算区域分类的时候加入对它的父亲节点q_nearest的有效熵的考量；

c)根据基于有效熵的区域划分策略进行区域划分。