CN103278164B - 一种复杂动态场景下机器人仿生路径规划方法及仿真平台 - Google Patents

Abstract

本发明涉及一种复杂动态场景下机器人仿生路径规划方法，以及实现该方法的机器人运动规划仿真平台。该方法根据碰撞可能性和到达可能性建立路径安全性衡量准则，采用两层交互路径规划的算法框架，将运动规划分为环境探索层和局部路径规划层，两层之间利用一个自适应的路径缓冲区来进行信息交互，将安全性更新搜索树作为顶层规划器，应用到两层交互框架中的环境探索层。该仿真平台包括问题模块、规划模块和执行模块。本发明仿照人类路径选取的路径规划策略，提出了一种安全、实时的路径规划方案，克服了现有技术仅考虑路径可行性而没有关注路径持续性的不足，实现了机器人在复杂动态环境中的运动规划。

Description

一种复杂动态场景下机器人仿生路径规划方法及仿真平台

技术领域

本发明属于机器人运动规划领域，具体涉及一种适用于复杂动态环境下的机器人仿生安全路径规划方法，以及采用该方法的复杂动态环境中的机器人运动规划仿真平台。

背景技术

运动规划研究机器人如何到达预定目标这个问题的方法和技术。它是机器人导航和目标跟踪的基础，也是友好人机交互和机器人更好地为人类服务的基本条件之一，在计算机辅助设计，生物医疗，航天领域等也有重要的应用，属于机器人学核心研究领域。

目前的路径规划方法主要分为精确的几何路径规划方法和基于概率方法的路径规划方法。其中几何路径规划方法的思路是利用环境特点，主要是几何特性，先将问题空间划分为栅格或几何图形集合，然后用相应的搜索策略获取任务的路径解。这类方法的计算复杂程度随空间大小、维度等因素指数上升，因此主要应用于低维、静态的环境，实用性不强。为了降低规划成本，基于概率的规划方法引入了位姿空间的概念，其主要思想是将物理空间映射到位姿空间中，然后利用一定的概率策略采样，在有效的采样点集合中，搜索一条可行的路径，这样大大降低了机器人维度和环境复杂度带来的计算量的剧增的问题。但目前大部分基于采样的规划算法都仅仅计算一条可行路径，没有考虑环境对规划路径可能带来的影响，导致路径片段很有可能太靠近障碍物，在机器人执行任务路径的过程中可能发生碰撞，而且当环境发生变化时这些路径片段被障碍物占据失效的可能性比较大。这将带来更多的重规划，而且会导致成功到达目标的成功率偏低。更重要的是，在复杂的动态环境中，障碍物的数量和运动模式、环境的尺度等等信息都是不确定的，这些不确定会大大增加在动态环境中规划的难度，因此，在复杂的动态环境中进行安全、高效地路径规划依然面临着许多难题。

发明内容

本发明针对复杂可变的未知环境，提出一种复杂动态场景下机器人仿生路径规划方法，以及实现该方法的机器人运动规划仿真平台。该方案仿照人类路径选取的路径规划策略，是一种安全、实时的路径规划方案，它克服了现有技术仅考虑路径可行性而没有关注路径持续性的不足，在多自由度机器人仿真平台上，利用路径搜索树规划方法得到有效的局部路径，实现了机器人在复杂动态环境中的运动规划。

具体来说，本发明采用如下技术方案：

一种复杂动态场景下机器人安全路径规划方法，其步骤包括：

1）根据描述每个局部环境的障碍物拥挤程度评估碰撞可能性，根据机器人在某一时间间隔内到达环境中每个区域的概率评估到达可能性，根据所述碰撞可能性和所述到达可能性建立路径安全性衡量准则；

2）顶层规划器根据所述安全性衡量准则采用安全性更新搜索树算法进行环境探索，得到可行的粗略区域路径，并将该粗略区域路径存储到自适应的路径缓冲区中；

3）底层规划器根据机器人周围环境的拥挤程度确定局部环境的区域大小，进而依据缓冲区的粗略路径信息细化局部区域并得到一条有效路径；

4）通过该有效路径指导机器人向目标位置进行运动，当环境变化导致机器人当前执行的局部路径失效时，机器人进行反应式避障，如果避障成功则继续执行；否则返回到步骤1）重新执行规划过程，直到机器人到达目标位置。

一种采用上述方法的复杂动态场景下机器人安全路径规划仿真平台，包括：

问题模块，负责进行环境设置和任务设置；

规划模块，包括顶层规划器、底层规划器和自适应的路径缓冲区；该顶层规划器负责根据由碰撞可能性和到达可能性建立的路径安全性衡量准则，采用安全性更新搜索树算法进行环境探索，得到可行的粗略区域路径；该自适应的路径缓冲区用于存储该粗略区域路径；该底层规划器根据机器人周围环境的拥挤程度确定局部环境的区域大小，进而依据缓冲区的粗略路径信息细化局部区域并得到一条有效路径；

执行模块，负责依据该有效路径指导机器人向目标位置进行运动，当环境变化导致机器人当前执行的局部路径失效时，机器人进行反应式避障，如果避障成功则继续执行；否则由规划模块重新执行规划过程，直到机器人到达目标位置。

本发明根据两层交互路径规划的算法框架，将运动规划分为环境探索层和局部路径规划层，两层之间利用一个自适应的路径缓冲区来进行信息交互，将安全性更新搜索树作为顶层规划器，应用到两层交互框架中的环境探索层，还可以将算法应用扩展到大尺度场景中的运动规划里。大量的实验结果表明，针对复杂动态环境，在用于评价规划算法优劣的规划成功率和平均规划时间这两项主要指标上，本发明都优于传统的规划方法，能够解决绝大多数情况下的可变环境中高自由度机器人的路径规划问题。

附图说明

图1是本发明的碰撞可能性评估示例图。

图2是本发明的安全性更新搜索树的节点扩展示例图。

图3是本发明的安全性运动规划策略示意图。

图4是本发明的仿真平台框架图。

图5是本发明的双层规划方法的流程图。

具体实施方式

下面通过具体实施例，并配合附图，对本发明做详细的说明。

本发明的技术方案包括了四部分的内容：

1、提出路径安全性的衡量准则

因为以往针对动态环境提出的基于概率的规划算法仅仅以可行性为标准，所以存在着路径失效的问题。有鉴于此，首先本发明将每个采样点的局部空间环境信息引入到规划过程，提出了用碰撞可能性（CollisionProbability，CP）来鉴定路径是否安全的准则，碰撞可能性描述每个局部环境的障碍物拥挤程度。同时，采用概率采样的观测策略，设计了一个方法用来评估碰撞可能性的大小，提高了它的计算效率。一个观察状态附近的节点可以用来估计它的概率密度函数，用该概率密度函数来评估碰撞可能性：首先在树中查找到离观察状态最近的N个节点，其中包含了有效节点和无效节点，那么这个状态的状态密度函数就可以用这些节点中无效节点占总节点数的比例大小来表征。而当这个观察节点是无效节点时，则它的CP就是概率1；当节点有效时，CP的大小就等于这个节点的概率密度值。

其次，深入分析机器人在动态环境下的时间局部特点，提出了到达可能性(ReachProbability，RP)的概念，用来评估机器人在某一时间间隔内到达环境中每个区域的概率。对于当前机器人所在的状态而言，定义一个机器人紧邻区域，也就是机器人在一个很短的时间内就可以达到的区域范围，在这个短时间内，环境的变化可以不考虑，当下一个节点是有效的，那么机器人能够到达该状态的概率RP是1；对于该区域外的状态，机器人到达时间内环境的变化不能再被忽略，因此机器人达到某一状态的概率RP与目标状态和机器人的距离成反比。结合碰撞可能性与到达可能性两个准则，对某一节点（i），提出了新的安全性(Safety)的衡量准则：Safety(i)=RP(i)*CP(i)。

图1展示了本发明中的碰撞可能性评估示例图，通过周围观测节点有效性概率来评估该节点的碰撞可能性。其中图1（a）为当观测节点为有效点的时候的碰撞可能性，图1（b）为当观察点是无效节点的时候的碰撞可能性。有效节点的碰撞可能性可以用来评估路径片段的安全性，无效节点的碰撞可能性则可用来评估其周围环境中障碍物拥挤程度。

2、提出安全性更新搜索树算法

在安全性准则的指导下，利用以往直接丢弃的失效节点，计算观测到的失效节点的百分比来评估每个有效节点的安全性，同时像以往的动态搜索树一样考虑生成路径的长度约束，从而得到安全性高且长度较短的路径。在重规划过程中，更新每个节点的安全性度量，用于指导规划新的路径。该算法可以分为两部分。

第一部分：第一次执行时，需要生成一棵以目标位姿为根节点的搜索树。在扩展搜索树时计算评估每个节点的碰撞可能性和到达可能性。然后规划器随机选取位姿空间某一采样点作为扩展方向，根据碰撞可能性、到达可能性以及路径长度三方面来选择该采样点的“最佳最近节点”。最佳最近节点就是在距离较短的邻近节点集合中，用碰撞可能性和到达可能性从中选出一个最佳节点。最后搜索树以该最佳节点沿着采样点方向伸展某一步长的距离，并且将该位姿点激活并添加到搜索树当中，得到一个搜索树的新节点。无论该节点是否有效，都标记该节点的有效性，然后加入到搜索树中。通过不断的扩展新节点，搜索树得到一条从给定目标点到机器人起始点的有效路径。

第二部分：当路径因为机器人移动或者环境变化等原因而导致路径解失效，则需要执行重规划步骤。因为此时机器人已移动到某一新的位姿点，所以形成了一个包含该位姿点和目标位姿的最小新区域。对于当前搜索树上的所有节点，如果该节点不在这个有效的最小新区域内，则删除这些节点，减少因更新计算带来的代价。然后更新所有节点的安全性信息。如果搜索树没有一条有效路径能到达起始节点，则有必要扩展新的节点来增加规划的概率完备性，返回第一部分，直到找到一条新的有效路径；否则直接将新的路径传递给机器人控制层并执行该路径解。

图2展示了本发明的安全性更新搜索树的节点扩展示例，其中“qnear”表示搜索树中选取的距离较短的节点集，“qrand”表示一开始随机采样的节点，“qnearest”表示加入碰撞可能性和到达可能性考量之后的最佳最近节点。首先，规划器随机采样某一位姿点作为扩展方向（图2(a)），然后选择搜索树上离该采样点最近的K个节点，用每个节点的安全性以及到达目标的路径长度来排序，最后选择相对最优，也就是衡量距离最短的那个节点作为新激活节点的父节点(图2(b))。

3、设计两层交互路径规划框架

图3展示了本发明仿人的安全性运动规划策略。一般来说，人在动态环境中，首先找一条可到达目标区域的粗略的区域路径，然后在走到的局部范围里面，沿着已经决定的区域路径，仔细斟酌每一步该如何走，得到一条具体的有效路径。本发明利用该原理，在机器人执行运动控制的过程中，通过规划器多次执行上述过程，直到到达给定任务的目标位置。

针对任务空间比较大，同时分析人在动态环境中行走路径选择的特点，本发明提取出基于该特点的仿生安全性路径规划策略，将之利用到提出的安全性更新搜索树算法中，实现上层引导路径规划趋势，下层规划具体路径及反馈的机制。该框架执行时的交互过程，主要通过三个模块来实现：1）顶层规划器；2）底层规划器；3）执行模块（执行器）。该框架的执行过程如下：

首先，顶层规划器进行环境探索并得到一条可行的粗略区域路径，该路径主要刻画区域的困难程度。同时，将区域路径上的节点存储到一个缓冲区中。

然后，底层规划器根据机器人周围环境的拥挤程度来确定局部环境的区域大小，进而依据存储在缓冲区的引导信息（即存储的粗略区域路径）来细化局部区域并得到一条有效的路径，该路径指导机器人向目标点进行运动。在机器人执行运动的过程中，环境可能发生变化，障碍物的移动会导致当前执行的路径失效。当局部路径失效时，通过执行模块使机器人首先进行反应式避障，比如执行后退绕行等待等动作，如果避障成功，则继续执行；否则，返回到顶层，更新顶层区域引导信息，重新执行整个规划过程，直到机器人到达给定的任务目标点。

本发明考虑环境中局部区域的拥挤程度各不相同，提出一个自适应的路径缓冲区机制，根据CP和RP的值来估计区域空间内障碍物拥挤程度，以此来调整用于存储区域路径的存储空间的大小，使得机器人规划器可以根据拥挤程度来调整局部区域的大小，提高局部规化器的效率，降低重规划次数，提高规划的成功率。

4、搭建模拟多种实际的复杂动态场景下的机器人运动规划仿真平台

该平台中用于完成规划任务的仿真机器人的一切参数均来自于真实的机器人，可以对真实机器人的各种运动特性进行模拟。同时该平台的仿真环境精确地提供了各种障碍物以及场景参数。该平台可以对各种规划算法进行定性定量的分析，从而能够评价各种算法在不同环境下优劣性。

图4展示了本发明的采用上述路径规划方法的仿真平台的架构图，该仿真平台被分为三大模块：问题模块、规划模块和执行模块。

问题模块主要进行场景设置和任务设置。在环境设置过程中，用3dsmax绘图软件根据真实机器人的实际尺寸对它的各个自由度进行建模，然后将建立的机器人3D模型通过配置文件的方式导入平台。

规划模块主要包括顶层规划器、底层规划器和缓冲区，在该模块中具体实现各种规划算法和策略，如已经为大家熟知的概率地图方法（ProbabilityRoadmap，PRM）和快速扩展随机树方法（RandomRapidly-exploringTree，RRT），其中本发明提出的安全性更新搜索树（SafetyUpdatingTree，SUT）算法就在这个模块进行实现。图4中，ERRT为基于执行的快速扩展随机树方法，DRRT为动态快速扩展随机树方法，GoalbaisedRRT为目标偏移快速扩展随机树方法。

执行模块负责依据该有效路径指导机器人向目标位置进行运动，当环境变化导致机器人当前执行的局部路径失效时，机器人进行反应式避障，如果避障成功则继续执行；否则由规划模块重新执行规划过程，直到机器人到达目标位置。

图5展示了本发明的路径规划方法的总体流程，包含执行过程和路径规划的交互过程，主要通过三个部分实现：1）顶层规划器（左虚框），2）底层规划器（右虚框），3）执行模块（下面方框）。下面对该流程做具体的说明：

首先，顶层规划器由安全性更新搜索树算法执行探索过程（environmentexploring）得到一条可行的粗略区域路径，快速获得环境拓扑，提供底层规划的启发信息，并将粗略路径存入缓冲区中。为了满足实时性的需求，本发明选择机器人扫描体积最大的K个自由度作为投影后低空间的基底，这样可以将粗略规划从高维映射到低维空间处理，大大降低了算法复杂度。该方法采用目标节点作为根节点来扩展树，在扩展方向的选取步骤，采用碰撞可能性与到达可能性的路径长度加权求和的方法，与k个最近节点方式相结合的策略。在将生成节点连接到搜索树上时，如果扩展节点离目标点距离大于预设步长，就朝扩展节点重新生成一个新的节点，否则直接将扩展节点添加到搜索树中。

然后，底层规划器根据机器人周围环境的拥挤程度来决定局部环境区域的大小，然后依据缓冲区的粗略路径信息来引导局部规划器细化局部区域（environmentexploiting）进而一条可行的路径。针对不同的环境、不同的机器人，实验平台采用已知的相对适合的局部规划方法。比如静态环境可以用概率路径地图方法及其优化算法，而带有运动约束的环境则快速搜索树方法更能胜任。为了保证每次规划都能得到一条从起始位置到子目标位置（来自缓冲区）的路径，底层规划器以起始节点作为根节点生长。扩展步长小于顶层规划器的步长并受机器人本身的约束。在扩展过程中，利用一个概率偏移的策略来利用存储的启发信息：在随机采样阶段，分别以相应不同的概率使采样偏向子目标点和缓冲区粗略路径节点随着机器人执行当前获取的路径解。环境可能发生变化，障碍物移动导致路径失效。当局部路径失效时，机器人首先进行反应式避障，如果避障成功，则继续执行当前路径；否则，返回到顶层，重新更新顶层节点用于引导的安全性信息。重复该过程直到机器人到达最终的给定目标点。

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

Claims (6)

1.一种复杂动态场景下机器人安全路径规划方法，其步骤包括：

1)根据描述每个局部环境的障碍物拥挤程度评估碰撞可能性，根据机器人在某一时间间隔内到达环境中每个区域的概率评估到达可能性，根据所述碰撞可能性和所述到达可能性建立路径安全性衡量准则；

所述碰撞可能性采用概率密度函数来评估，方法是：首先在树中查找到离观察状态最近的节点，其中包含有效节点和无效节点，该状态的状态密度函数用这些节点中无效节点占总节点数的比例大小来表征；当观察节点是无效节点时，则它的碰撞可能性是概率1；当节点有效时，碰撞可能性的大小等于这个节点的概率密度值；

所述到达可能性的评估方法是：对于当前机器人所在的状态，定义一个机器人紧邻区域，即机器人在一个很短的时间内可以达到的区域范围，在这个短时间内，环境的变化不考虑，当下一个节点是有效的，那么机器人能够到达该状态的概率是1；对于该区域外的状态，机器人到达时间内环境的变化不能再被忽略，因此机器人达到某一状态的概率与目标状态和机器人的距离成反比；

对某一节点(i)，所述路径安全性衡量准则为：Safety(i)＝RP(i)*CP(i)，其中CP为碰撞可能性，RP为到达可能性；

2)顶层规划器根据所述安全性衡量准则采用安全性更新搜索树算法进行环境探索，得到可行的粗略区域路径，并将该粗略区域路径存储到自适应的路径缓冲区中；所述安全性更新搜索树算法为：

第一次执行时，生成一棵以目标位姿为根节点的搜索树，在扩展搜索树时计算评估每个节点的碰撞可能性和到达可能性；然后规划器随机选取位姿空间某一采样点作为扩展方向，根据碰撞可能性、到达可能性以及路径长度三方面来选择该采样点的最佳最近节点；最后搜索树以该节点沿着采样点方向伸展某一步长的距离，且将该位姿点激活并添加到搜索树当中，得到一个搜索树的新节点；通过不断的扩展新节点，搜索树得到一条从给定目标点到机器人起始点的有效路径；

当路径因为机器人移动或者环境变化等原因而导致路径解失效时，执行重规划步骤；如果搜索树没有一条有效路径能到达起始节点，则返回第一部分，直到找到一条新的有效路径；否则直接将新的路径传递给机器人控制层并执行该路径解；

3)底层规划器根据机器人周围环境的拥挤程度确定局部环境的区域大小，进而依据缓冲区的粗略路径信息细化局部区域并得到一条有效路径；

4)通过该有效路径指导机器人向目标位置进行运动，当环境变化导致机器人当前执行的局部路径失效时，机器人进行反应式避障，如果避障成功则继续执行；否则返回到步骤1)重新执行规划过程，直到机器人到达目标位置。

2.如权利要求1所述的方法，其特征在于：在所述重规划步骤中，对于当前搜索树上的所有节点，如果该节点不在包含机器人所在的新位姿点和目标位姿的最小新区域内，则删除这些节点，减少因更新计算带来的代价，然后更新所有节点的安全性信息。

3.如权利要求1所述的方法，其特征在于：顶层规划器选择机器人扫描体积最大的K个自由度作为投影后低空间的基底，以将粗略规划从高维映射到低维空间处理。

4.如权利要求1所述的方法，其特征在于：底层规划器根据碰撞可能性和到达可能性的值估计区域空间内障碍物拥挤程度，进而调整局部区域的大小。

5.如权利要求1所述的方法，其特征在于：底层规划器以起始节点作为根节点生长，扩展步长小于顶层规划器的步长并受机器人本身的约束；在扩展过程中，利用概率偏移的策略来利用存储的启发信息：在随机采样阶段，分别以相应不同的概率使采样偏向子目标点和缓冲区粗略路径节点随着机器人执行当前获取的路径解。

6.一种采用权利要求1所述方法的复杂动态场景下机器人安全路径规划仿真平台，包括：

问题模块，负责进行环境设置和任务设置；

规划模块，包括顶层规划器、底层规划器和自适应的路径缓冲区；该顶层规划器负责根据由碰撞可能性和到达可能性建立的路径安全性衡量准则，采用安全性更新搜索树算法进行环境探索，得到可行的粗略区域路径；该自适应的路径缓冲区用于存储该粗略区域路径；该底层规划器根据机器人周围环境的拥挤程度确定局部环境的区域大小，进而依据缓冲区的粗略路径信息细化局部区域并得到一条有效路径；

执行模块，负责依据该有效路径指导机器人向目标位置进行运动，当环境变化导致机器人当前执行的局部路径失效时，机器人进行反应式避障，如果避障成功则继续执行；否则由规划模块重新执行规划过程，直到机器人到达目标位置。