CN114296445B - 基于回环路网随机树的最优路径实时规划方法 - Google Patents

Abstract

一种基于回环路网随机树的最优路径实时规划方法，包括(1)回环路网预生成：①生成无环树；②在分支之间建立虚拟连接；③消除冗余和无效的虚拟连接；(2)实时优化迭代搜索：①建立起点和终点之间的不包含虚拟连接的路径；②遍历虚拟连接；③形成下一循环的迭代路径；④直到虚拟连接列表遍历完毕，此时的迭代路径即为算法优化搜索的结果，该路径通过松弛之后作为机器人运动的路径；(3)动态运行维护：①当障碍物发生变化时，修剪树分支，删除无效虚拟连接；②将修剪得到的多个子树进行重连接；③进行重生长过程，补充树节点和虚拟连接。本发明实时搜索路网内从起点到终点的最短路径，保证了树和回环路网在任意时刻的有效性、遍布性和联通性。

Description

基于回环路网随机树的最优路径实时规划方法

技术领域

本发明涉及一种用于对机器人最优路径进行实时规划的方法，属于机器人路径规划技术领域。

背景技术

机器人已经越来越多地运用到复杂动态环境中，进行长时间、高强度、高难度的任务作业。移动机器人在复杂动态环境中的移动能力，是评价其作业能力和智能水平的关键指标之一。移动机器人路径规划技术作为关键基础技术，在面临复杂动态长周期的应用场景时，存在以下问题：

(1)实时性要求高

动态环境中存在运动障碍物，以及突然出现的新障碍物。因此要求机器人具备较高的规划和控制频率，能够针对变动环境进行快速反应，避免碰撞。

(2)路径长度最优

在高强度任务环境下，机器人移动路径长度是影响执行效率的关键。通过最优路径，机器人能够尽快到达目标位置展开作业，提升机器人系统的作业效率。

(3)长生命周期规划

在密集任务场景下，机器人的规划算法框架应能够适应紧密衔接的任务序列，当任务完成、中断、更改时，机器人的规划性能应保持一致；当机器人所处环境地图完全改变时，规划算法框架能够自动适应，而非完全重新规划。

发明内容

针对机器人路径规划技术存在以上问题，本发明提出一种基于回环路网随机树的最优路径实时规划方法(简称CNRRT)，使机器人在复杂动态长周期任务环境中具备实时性、最优性和长生命周期。

本发明的基于回环路网随机树的最优路径实时规划方法，包括三个部分：回环路网预生成、实时优化迭代搜索和动态运行维护；各部分的具体步骤如下：

(1)回环路网预生成：

①生成一棵遍布整个地图的无环树；

在地图的无障碍物区域中选择一点作为树的根节点，在无障碍物区域中随机采样，从根节点开始向无障碍物区域生长树节点；生长过程中构建连接关系链表，存储各个节点的上一级节点索引、下一级节点索引和同级节点索引；所述生长树节点采用RRT生长方法，生长步长取值为15。

在无环树生成过程中加入营养素机制，以指示地图上尚未探索的无障碍物区域，地图中无障碍物区域的面积为总营养素，而障碍物区域没有营养素，每生长一个树节点，就从节点附近区域内吸收营养素；设定营养素剩余的最小值，当地图中的营养素低于最小值时，表明无环树生长到预期的地图遍布程度，生长过程终止。所述吸收营养素的数量为0至0.0015倍的总营养素。所述最小值为0-0.25。

②在树的不同分支之间建立虚拟连接，以构建回环分支；

继续在地图上随机采样，在无环树的基础上生成新节点，检查新节点和树上各节点之间的距离；如果新节点与某一个树节点的距离低于连接阈值，则在新节点和该树节点之间建立虚拟连接，被连接的两个节点所在的两个树分支通过虚拟连接组成一个回环分支；所述连接阈值为5-18。

更明确地，回环分支指被连接的两个节点之间的树路径加虚拟连接段，通过列表记录虚拟连接点位置和两个端点；

在虚拟连接过程中引入营养素机制，用于判断虚拟连接是否完成，每生成一个虚拟连接，就从虚拟连接周围区域内吸收营养素(该营养素与无环树的营养素互不影响)，当虚拟节点营养素低于阈值(阈值就是最小值，为0-0.25)，判断虚拟连接完成。

③通过筛选，消除冗余和无效的虚拟连接；

设定显著性判断和邻近性判断两个筛选标准；显著性判断是通过检查回环分支中节点的数量，将较短的回环分支删除；邻近性判断针对具有至少一个相同端点的两个虚拟连接，检查二者分别构成的回环分支内的节点数量，如果两个回环分支的节点数之差低于接近阈值，则两个虚拟连接仅保留一个；以上两个判断的节点数量计算均基于对无环树的索引回溯，通过索引回溯生成两个被连接树节点之间的路径，并计算路径内节点个数，判断该回环分支的长度。所述接近阈值取值6。

所述索引回溯的方法为：由两节点中的任意一个节点开始，查询链表获取其上一级节点，迭代查询该节点的上一级节点，直至查询至根节点，形成一个回溯路径；以相同方法从另一节点回溯至根节点，生成另一回溯路径；从根节点开始删除两个回溯路径的重复路径点(保留最末尾的重复路径点)，剩余路径点组成两节点之间的路径。

(2)实时优化迭代搜索：

采用贪婪策略，进行实时优化迭代搜索，具体步骤为：

①通过上述索引回溯建立起点和终点之间的不包含虚拟连接的路径，作为初始迭代路径；

②遍历虚拟连接，针对每一个虚拟连接，构建从起点到终点的经过该虚拟连接的候选路径，并计算该路径与当前循环的迭代路径的替代片段；

对于某一个虚拟连接，构建连接起止点的路径，选取没有交叉点的路径作为该虚拟连接产生的候选路径；从虚拟连接点开始，分别向前向后遍历候选路径，直到搜索到与当前循环的迭代路径重合的两个重合点，此两个重合点之间的候选路径即为替代片段，两个重合点之间的迭代路径即为原有片段。

③若替代片段缩短了当前循环的迭代路径的长度，则用替代片段代替迭代路径上的原有片段，形成下一循环的迭代路径。

计算路径长度之前，先对路径进行松弛计算，然后计算路径长度；

所述松弛的具体过程是：

对于路径片段上的k个节点，从第一个节点开始，检查第一个和第k个节点之间的直线是否通过障碍区域，如果直线没有穿过障碍区域，则用第一和第k节点之间的连线替换1至k中间的原路径，如果直线穿过障碍区域，则将k减半后再次运行松弛计算，直到找到无碰撞连线，松弛计算结束，返回该连线和此时的k值；

对于一条路径，循环执行多轮松弛计算，直到路径不再发生变化时终止松弛；计算路径节点之间的欧式距离之和，即为该路径的长度。

④循环步骤②和③，直到虚拟连接列表遍历完毕，此时的迭代路径即为算法优化搜索的结果，该路径通过松弛之后作为机器人运动的路径；

(3)动态运行维护：

动态运行维护过程针对环境中的障碍物的变化，对树进行修剪和重连接，对虚拟连接进行增删操作；具体步骤为：

①当障碍物发生变化时，修剪树分支，删除无效虚拟连接；

删除落在障碍物范围内的树节点和虚拟连接点，当节点之间的连接被障碍物切断而节点本身未在障碍物区域内时，被切断的节点形成一棵子树；

②将修剪得到的多个子树进行重连接；

对于地图中所有子树，从节点最少的子树开始，向其他子树生长新支；若某个新生长的树节点与其他子树的节点距离小于连接阈值，则将子树相互连接，连接所有子树，最终恢复成一个完整的树；

③进行重生长过程，补充树节点和虚拟连接；

由于障碍物变化，部分树节点和虚拟连接点被删除，总体数量减少，营养素剩余增加并超过设定的最小值(0-0.25)，此时进行树节点补充生长和补充虚拟连接，将地图中的营养素剩余降至设定的最小值以下，防止算法长时间运行后发生退化。

本发明首先生成一棵遍布整个地图的无环树图，之后通过虚拟连接不同分支，构建分支回环。经过有效性筛选，删除无效和冗余连接。在机器人运动过程中，通过基于贪婪策略的优化迭代搜索，以当前机器人位置为起点，实时搜索路网内从起点到终点的最短路径。针对障碍物变动，通过动态运行维护对树进行修剪和重连，对回环路网进行修改和新建，保证了树和回环路网在任意时刻的有效性、遍布性和联通性。

附图说明

图1是本发明基于回环路网随机树的最优路径实时规划方法的总体框架图。

图2是本发明在某单任务机器人执行过程中的算法流程图。

图3是虚拟连接与虚拟连接路径示意图。

图4是路径迭代示意图。

图5是路径松弛计算示意图(k＝2)。

具体实施方式

本发明基于回环路网随机树的最优路径实时规划方法，简称CNRRT，用于使机器人在复杂动态长周期任务环境中具备实时性、最优性和长生命周期。

本发明方法的总体框架如图1所示，包括三个部分：回环路网预生成、实时优化迭代搜索和动态运行维护。首先生成一棵遍布整个地图的无环树图，之后通过虚拟连接不同分支，构建分支回环。经过有效性筛选，删除无效和冗余连接。在机器人运动过程中，通过基于贪婪策略的优化迭代搜索，以当前机器人位置为起点，实时搜索路网内从起点到终点的最短路径。针对障碍物变动，通过动态运行维护对树进行修剪和重连，对回环路网进行修改和新建，保证树和回环路网在任意时刻的有效性、遍布性和联通性。

图2给出了本发明在某单任务机器人执行过程的流程，以下结合图2和图1对本发明方法详细说明。

一.回环路网预生成

回环路网预生成具体包口以下步骤：

(1)生成一棵遍布整个地图的无环树(以下简称树)；

首先在地图的无障碍物区域中选择一点作为树的根节点。之后在无障碍物区域中随机采样，通过现有技术中经典的RRT生长方法(RRT是一种通过随机构建空间填充树来有效搜索非凸,高维空间的算法)，生长步长取值15，从根节点开始向无障碍物区域生长树节点。生长过程中构建连接关系链表，存储各个节点的上一级节点索引、下一级节点索引和同级节点索引。本发明在树生成过程中加入营养素机制，以指示地图上尚未探索的无障碍物区域。地图中无障碍物区域的面积为总营养素，而障碍物区域没有营养素。算法每生长一个树节点，就从节点附近区域内吸收一定数量的营养素，吸收的营养素数量为0至0.0015倍总营养素。本发明设定营养素剩余的最小值为0-0.25。当地图中的营养素低于最小值时，表明树生长到预期的地图遍布程度，生长过程终止。

(2)在树的不同分支之间建立虚拟连接，以构建回环分支；

本发明继续在地图上随机采样，在树的基础上生成新节点。检查新节点和树上各节点之间的距离。如图3，如果新节点与某一个树节点的距离低于连接阈值(连接阈值取值为5-18)，则在新节点和该树节点之间建立虚拟连接(如图3中A点所示)，被连接的两个节点(如图3中端点1和端点2)所在的两个树分支通过虚拟连接组成一个回环分支。更明确地，回环分支指被连接的两个节点(如图3中端点1和端点2)之间的树路径加虚拟连接段，如图3中实现箭头环所示。通过列表记录虚拟连接点位置和两个端点。在虚拟连接过程中引入营养素机制，用于判断虚拟连接是否完成。与树的营养素机制相似，算法每生成一个虚拟连接，就从虚拟连接周围区域内吸收营养素。该营养素与树的营养素互不影响。当虚拟节点营养素低于阈值(阈值就是最小值)，判断虚拟连接完成。

虚拟节点营养素和树节点营养素互不影响，二者各有一个阈值，阈值就是最小值，二者的阈值同样互不影响，均可在0到0.25范围内取值。

(3)通过筛选，消除冗余和无效的虚拟连接

为了消除冗余和无效的连接，本发明提出了两个筛选标准：显著性判断和邻近性判断。如图3所示，显著性判断通过检查回环分支中节点的数量，将较短的回环分支删除(如图3中的虚拟连接A)。邻近性判断针对具有至少一个相同端点的两个虚拟连接(如图3中的虚拟连接B和C)，检查二者分别构成的回环分支内的节点数量。如果两个回环分支的节点数之差低于接近阈值(接近阈值取值为6)，则两个虚拟连接仅保留一个(如图3中两个回环分支B和C的节点数之差为1)。以上两个判断的节点数量计算均基于对树的索引回溯，通过索引回溯方法生成两个被连接树节点之间的路径，并计算路径内节点个数，判断该回环分支的长度。

所述索引回溯方法具体为：由两节点中的任意一个节点开始，查询链表获取其上一级节点，迭代查询该节点的上一级节点，直至查询至根节点，形成一个回溯路径；以相同方法从另一节点回溯至根节点，生成另一回溯路径；从根节点开始删除两个回溯路径的重复路径点(保留最末尾的重复路径点)，剩余路径点组成两节点之间的路径。

二.实时优化迭代搜索

地图中的回环分支构成了一个复杂的路径网络。路径规划问题即为在路径网中搜索最短路径的问题。本发明采用贪婪策略，进行实时优化迭代搜索，其步骤为：

(1)通过索引回溯建立起点和终点之间的不包含虚拟连接的路径，作为初始迭代路径。

对于某一个虚拟连接，构建连接起止点的路径。如图4所示，粗实线为当前循环的迭代路径。对于虚拟连接D，其连通起止点的路径共2条(如图4中的虚线箭头所示起点、端点一、端点二、目的地，和实线箭头所示起点、端点二、端点一、目的地)。选取没有交叉点(虚线箭头所示)的路径作为该虚拟连接产生的候选路径。从虚拟连接点开始，分别向前向后遍历候选路径，直到搜索到与当前循环的迭代路径重合的两个重合点(如图4前向、后向重合点所示)。此两个重合点之间的候选路径即为替代片段，两个重合点之间的迭代路径即为原有片段。

(2)遍历虚拟连接，针对每一个虚拟连接，构建从起点到终点的经过该虚拟连接的候选路径，并计算该路径与当前循环的迭代路径的替代片段。

(3)若替代片段缩短了当前循环的迭代路径的长度，则用替代片段代替迭代路径上的原有片段，形成下一循环的迭代路径。

在本发明中，计算路径长度之前，先使用松弛方法对路径进行松弛计算，然后计算路径长度。路径松弛过程如图5所示。本发明定义松弛计算R，对于路径片段上的k个节点，从第一个节点开始，该方法检查第一个和第k个节点之间的直线是否通过障碍区域。如果没有，则用第一和第k节点之间的连线替换1至k中间的原路径。若直线穿过障碍区域，则将k减半后再次运行R计算，直到找到无碰撞连线，R计算结束，返回该连线和此时的k值。本发明选取k＝5，对于包含m个节点的路径，本发明从第一个(i＝1)节点开始运行R计算。根据其返回的k值，从第i+k个节点开始再次向后执行R计算，直到返回k值到达第m个节点，一轮松弛结束。对于一条路径，本发明循环执行多轮松弛计算，直到路径不再发生变化时终止松弛。计算路径节点之间的欧式距离之和，即为该路径的长度。

(4)循环步骤(2)和步骤(3)，直到虚拟连接列表遍历完毕，此时的迭代路径即为算法优化搜索的结果。该路径通过松弛之后，即可作为机器人运动的路径。

三.动态运行维护

动态运行维护过程是针对环境中的障碍物的变化，对树进行修剪和重连接，对虚拟连接进行增删操作；其具体步骤为：

(1)当障碍物发生变化时，修剪树分支，删除无效虚拟连接；

本发明删除落在障碍物范围内的树节点和虚拟连接点。当节点之间的连接被障碍物切断而节点本身未在障碍物区域内时，被切断的节点形成一棵子树。

(2)将修剪得到的多个子树进行重连接；

对于地图中所有子树，本发明从节点最少的子树开始，向其他子树生长新支。若某个新生长的树节点与其他子树的节点距离小于连接阈值，则将子树相互连接。连接所有子树，最终恢复成一个完整的树。重连接过程保证了地图中所有节点在任何时刻都是互相连通的。

(3)进行重生长过程，补充树节点和虚拟连接。

由于障碍物变化，部分树节点和虚拟连接点被删除，总体数量减少，营养素剩余发生变化。本发明借助所述营养素指标，营养素剩余增加并超过设定的最小值，此时进行树节点补充生长和补充虚拟连接，将地图中的营养素剩余降至设定的最小值以下，防止算法长时间运行后发生退化。

Claims (9)

1.一种基于回环路网随机树的最优路径实时规划方法，其特征是：包括回环路网预生成、实时优化迭代搜索和动态运行维护三个部分；各部分的具体步骤如下：

(1)回环路网预生成：

①生成一棵遍布整个地图的无环树；

在地图的无障碍物区域中选择一点作为树的根节点，在无障碍物区域中随机采样，从根节点开始向无障碍物区域生长树节点；生长过程中构建连接关系链表，存储各个节点的上一级节点索引、下一级节点索引和同级节点索引；

②在树的不同分支之间建立虚拟连接，以构建回环分支；

继续在地图上随机采样，在无环树的基础上生成新节点，检查新节点和树上各节点之间的距离；如果新节点与某一个树节点的距离低于连接阈值，则在新节点和该树节点之间建立虚拟连接，被连接的两个节点所在的两个树分支通过虚拟连接组成一个回环分支；

③通过筛选，消除冗余和无效的虚拟连接；

设定显著性判断和邻近性判断两个筛选标准；显著性判断是通过检查回环分支中节点的数量，将较短的回环分支删除；邻近性判断针对具有至少一个相同端点的两个虚拟连接，检查二者分别构成的回环分支内的节点数量，如果两个回环分支的节点数之差低于接近阈值，则两个虚拟连接仅保留一个；以上两个判断的节点数量计算均基于对无环树的索引回溯，通过索引回溯生成两个被连接树节点之间的路径，并计算路径内节点个数，判断该回环分支的长度；

(2)实时优化迭代搜索：

①通过上述索引回溯建立起点和终点之间的不包含虚拟连接的路径，作为初始迭代路径；

②遍历虚拟连接，针对每一个虚拟连接，构建从起点到终点的经过该虚拟连接的候选路径，并计算该路径与当前循环的迭代路径的替代片段；

对于某一个虚拟连接，构建连接起止点的路径，选取没有交叉点的路径作为该虚拟连接产生的候选路径；从虚拟连接点开始，分别向前向后遍历候选路径，直到搜索到与当前循环的迭代路径重合的两个重合点，此两个重合点之间的候选路径即为替代片段，两个重合点之间的迭代路径即为原有片段；

③若替代片段缩短了当前循环的迭代路径的长度，则用替代片段代替迭代路径上的原有片段，形成下一循环的迭代路径；

计算路径长度之前，先对路径进行松弛计算，然后计算路径长度；

④循环步骤②和③，直到虚拟连接列表遍历完毕，此时的迭代路径即为算法优化搜索的结果，该路径通过松弛之后作为机器人运动的路径；

(3)动态运行维护：

动态运行维护过程针对环境中的障碍物的变化，对树进行修剪和重连接，对虚拟连接进行增删操作；具体步骤为：

①当障碍物发生变化时，修剪树分支，删除无效虚拟连接；

删除落在障碍物范围内的树节点和虚拟连接点，当节点之间的连接被障碍物切断而节点本身未在障碍物区域内时，被切断的节点形成一棵子树；

②将修剪得到的多个子树进行重连接；

对于地图中所有子树，从节点最少的子树开始，向其他子树生长新支；若某个新生长的树节点与其他子树的节点距离小于连接阈值，则将子树相互连接，连接所有子树，最终恢复成一个完整的树；

③进行重生长过程，补充树节点和虚拟连接；

由于障碍物变化，部分树节点和虚拟连接点被删除，总体数量减少，营养素剩余增加并超过设定的最小值，此时进行树节点补充生长和补充虚拟连接，将地图中的营养素剩余降至设定的最小值以下，防止算法长时间运行后发生退化。

2.根据权利要求1所述的基于回环路网随机树的最优路径实时规划方法，其特征是：所述生长树节点采用RRT生长方法，生长步长取值为15。

3.根据权利要求1所述的基于回环路网随机树的最优路径实时规划方法，其特征是：所述无环树生成过程中加入营养素机制，以指示地图上尚未探索的无障碍物区域，地图中无障碍物区域的面积为总营养素，而障碍物区域没有营养素，每生长一个树节点，就从节点附近区域内吸收营养素；设定营养素剩余的最小值，当地图中的营养素低于最小值时，表明无环树生长到预期的地图遍布程度，生长过程终止。

4.根据权利要求3所述的基于回环路网随机树的最优路径实时规划方法，其特征是：所述吸收营养素的数量为0至0.0015倍的总营养素。

5.根据权利要求3所述的基于回环路网随机树的最优路径实时规划方法，其特征是：所述最小值为0-0.25；所述连接阈值为5-18。

6.根据权利要求1所述的基于回环路网随机树的最优路径实时规划方法，其特征是：在虚拟连接过程中引入营养素机制，用于判断虚拟连接是否完成，每生成一个虚拟连接，就从虚拟连接周围区域内吸收营养素，当虚拟节点营养素低于阈值，判断虚拟连接完成。

7.根据权利要求1所述的基于回环路网随机树的最优路径实时规划方法，其特征是：所述接近阈值取值6。

8.根据权利要求1所述的基于回环路网随机树的最优路径实时规划方法，其特征是：所述索引回溯的方法为：由两节点中的任意一个节点开始，查询链表获取其上一级节点，迭代查询该节点的上一级节点，直至查询至根节点，形成一个回溯路径；以相同方法从另一节点回溯至根节点，生成另一回溯路径；从根节点开始删除两个回溯路径的重复路径点，剩余路径点组成两节点之间的路径。

9.根据权利要求1所述的基于回环路网随机树的最优路径实时规划方法，其特征是，所述松弛的具体过程是：

对于路径片段上的k个节点，从第一个节点开始，检查第一个和第k个节点之间的直线是否通过障碍区域，如果直线没有穿过障碍区域，则用第一和第k节点之间的连线替换1至k中间的原路径，如果直线穿过障碍区域，则将k减半后再次运行松弛计算，直到找到无碰撞连线，松弛计算结束，返回该连线和此时的k值；

对于一条路径，循环执行多轮松弛计算，直到路径不再发生变化时终止松弛；计算路径节点之间的欧式距离之和，即为该路径的长度。