CN109839110B - 一种基于快速随机搜索树的多目标点路径规划方法 - Google Patents

Abstract

本发明涉及一种基于快速随机搜索树的多目标点路径规划的方法，属于机器人路径规划领域。本发明方法使用两层树的结构。底层树由多个目标点延伸出的多棵树组成，每棵树拥有一个由周围环境决定的权重，并且每棵树使用快速随机搜索树算法对自由空间进行探索。当两棵树足够近时产生一条有效的无碰撞路径，该有效路径及构成该有效路径的节点将会传递给顶层树。顶层树对这些路径和节点使用改进的最小生成树算法进行重新规划的工作，最终获得能够遍历多个目标点的最短路径。本发明中提供的多目标点路径规划方法能有效地在各种障碍物环境中运行，并且计算速度快，移动机器人能够对该路径直接进行导航。

Description

一种基于快速随机搜索树的多目标点路径规划方法

技术领域

本发明属于移动机器人路径规划领域，尤其涉及一种基于快速随机搜索树的多目标点路径规划方法。

背景技术

移动机器人的路径规划技术是智能机器人的核心技术之一，它是移动机器人完成其他任务的基础。路径规划技术的研究有很长的历史，现有的技术大致可分为三类：一种是基于图搜索的路径规划算法，比如Dijistra算法，A*算法等；另一种是基于种群的优化算法，比如遗传算法、粒子群算法等；最新的一种是基于采样的路径规划算法，比如随机路径图算法(Probabilistic Roadmaps，PRM)和快速随机搜索树算法(Rapidly Random-exploring Tree，RRT)。

所谓的多目标点路径规划就是一种全局的路径规划任务，机器人需要从一个规定的起点出发，每次到达一个不同的目标点，直到最后到达终点。这个过程中要求机器人符合一些不同的要求，比如路径最短、能耗最低等，并且在规划的路径中不能碰撞到任何障碍物。这是一种在机器人自由空间中的旅行商问题(Travelling Salesman Problem，TSP)。

针对多目标点路径规划问题，更多的是通过进化算法来解决。进化算法是一种群体进化的随机优化技术，每一代都继承上一代的优秀种群，并能够利用不同种群之间的相似性来寻找全局最优的解。常见的进化算法有蚁群算法，遗传算法，粒子群算法等。这些算法都可以用来解决多目标点的路径规划问题，但是这些算法也存在弊端。比如陷入局部最优的问题，而且进化算法很难应用到高维空间中，无法解决高维空间中的多目标点路径规划问题。

快速随机搜索树算法是Lavalle提出的基于采样的路径规划算法。该算法不需要对环境空间进行精确的建模，避免了对障碍物空间的栅格离散化。由于该算法使用对环境空间随机采样的策略，只需要判断采样后的点和连接树枝是否与障碍物产生碰撞，所以搜索速度快。最终的收敛路径也符合移动机器人的非完整性约束，具有诸多良好的特性。并且该算法也可以直接应用在高维空间中。但是RRT算法随机性较强，路径通常不是最优的，需要从多个角度对其进行优化。

发明内容

本发明的目的是克服进化算法所存在的缺陷，采用基于随机采样的算法策略来解决多目标点的路径规划问题。

本发明使用两层树的结构，底层树负责探索未知空间，顶层树负责规划出能够遍历多个目标点的最短路径；可以通过以下步骤来实现：

1)底层树的探索过程包含以下步骤：

步骤一：每个目标点、起点和终点均初始化一棵树，配合在全局地图的信息，确定出感知距离；

步骤二：由步骤一确定的感知距离，及每棵树周围的障碍物环境，计算出每棵树在整体树集合中的扩展权重，由扩展权重来决定每棵树的扩展概率；

步骤三：每棵树独立地使用快速随机搜索树算法进行空间探索，并且扩展步长统一为Δ；在每次算法迭代中只扩展一棵树，由步骤二中的扩展概率来选择一棵树进行扩展，扩展后得到一个新结点；

步骤四：如果两棵树的距离小于2×Δ，产生一条有效的无碰撞路径及一组构成该有效路径的节点；

2)顶层树的规划过程包含以下步骤：

步骤五：初始化距离探索方阵；

步骤六：根据步骤四中的有效路径及节点，更新距离探索方阵的距离信息，并将有效路径及构成该路径的节点添加到顶层树；

步骤七：重新规划多个目标点的访问顺序，获得能够遍历多个目标点的最短路径；

步骤八：在步骤七的最短路径上选取多个关键节点，使机器人可以在各个关键节点之间按顺序进行导航。

优选的，所述的步骤一中，每个目标点、起点和终点均初始化一棵树，根据全局地图信息计算出感知距离，计算方法如下：

R＝min{D_ij},1≤i,j≤n

其中R代表感知距离，D_ij代表第i棵树的根结点与第j棵数的根结点之间的无视障碍物的欧式空间距离，n表示树木总数。

优选的，所述的步骤二中，根据感知距离R及每棵树周围的环境，计算每棵树在整体树集合的扩展权重，由权重来决定每棵树的扩展概率；计算方法如下：

其中W_i代表第i棵树的权重，C_i，obs代表第i棵数周围由R定义的正方形环境空间中障碍物的大小，正方形环境空间的边长为2×R，中心点为第i棵数的根结点；计算出的每棵树权重W_i是每棵树的扩展概率，周围环境相对空旷的树木得到的扩展机会少，反之，周围环境相对拥挤的树木得到的扩展机会多。

优选的，所述的步骤三中，每棵树使用快速随机搜索树算法进行扩展，并且扩展步长统一为Δ；在每次迭代中只扩展一棵树，由所述的步骤二中的扩展概率来选择一棵树进行扩展，扩展后得到一个新结点。

优选的，所述的步骤四具体为：当两棵树小于2×Δ时，产生一条无碰撞的路径，两棵树的距离计算公式如下：

d_m＝min{d_new,i},1≤i≤n

其中d_m表示扩展得到的新结点与其他树的距离，d_new,i代表当前新结点与第i棵树的距离，n代表树木总数，因为每次只扩展一棵树，计算两棵树之间的最短距离时只需要计算扩展得到的新结点与其他树之间的最短距离即可，新结点与一棵树最短距离由如下定义：

d_new,i＝min{||q_new-q_ij||₂},1≤j≤m

其中q_new代表新结点，q_ij代表在第i棵树中第j个结点，每两个结点的距离由欧式空间距离计算得到。

优选的，所述的步骤五为：建立一个距离探索方阵：

E＝{e_ij}_i×j,1≤i,j≤n

其中E代表距离探索方阵，是一个对称矩阵，行列数均为树木的总数n；e_ij代表第i棵树与第j棵树的距离值，表示底层树目前已经探索到的第i棵树与第j棵树的距离。

优选的，所述的步骤六中，在底层树每次获得一个无碰撞路径之后，判断新获得的路径长度与距离探索方阵E中对应位置距离值的大小，如果获得的路径更小，则更新距离探索方阵中对应位置的距离值，并将获得的路径及路径节点添加到顶层树中，反之，距离探索方阵中的值不作改变，获得的路径及路径节点也不会添加到顶层树中。

优选的，所述的步骤七具体为，在达到最大迭代次数条件之后，根据距离探索方阵E所构成的多个目标点的距离无向图，使用改进的最小生成树Kruskal算法计算出一条链表形式的树，该树可以作为遍历多个目标点的路径，改进的最小生成树Kruskal算法如下描述：将所有的路径从小到大排序，依次选择最小的权值路径，并将其连接；每次选择新路径时，保证从起点出发的最小生成树是一条链表示形式的树，如果不是链表示形式的树，抛弃这条路径，并选择下一条最小的权值路径，直至形成链表形式的树。

优选的，所述的步骤八具体为：在得到的链表形式的树上，按照起点、多个目标点、终点的遍历顺序依次进行导航，起点、多个目标点、终点均设置为关键节点；除了上述关键节点外，从树中的起点开始，按照遍历顺序检查树中的下一个节点，若两个节点连接的路径没有与障碍物产生碰撞，则继续检查下一个节点，直到产生碰撞为止，上一个不产生碰撞的节点置为关键节点；依次过程逐步遍历至终点，即可生成遍历多个目标点的路径，移动机器人对最终得到的关键节点进行导航，从而完成多目标遍历的路径规划任务。

本发明提出了一种基于快速随机搜索树的多目标点路径规划方法，其意义在于，创新性地使用快速随机搜索树算法解决多目标点规划问题，打破局限于使用传统的路径规划算法，如图搜索算法、进化算法，来解决多目标点路径规划问题。

该方法的优点在于：1、不需要像传统的路径规划算法对环境空间进行离散化的精确建模，所提出的方法只需要未经预处理的环境地图；2、将探索和规划任务分离。在底层树的探索过程中采用快速随机搜索树算法对环境空间进行多棵树同时探索，探索速度快、效率高，在顶层树的规划过程中使用改进的最小生成树Kruskal算法规划出一条路径，并在该路径上选取多个关键节点，机器人可以对这些关键节点进行导航；3、所提出的方法还可以适用于在高维空间中机器人系统的多目标点路径规划问题，如机械臂、无人飞行器等。

附图说明

图1是初始化的环境地图；

图2是底层树扩展的示意图；

图3是顶层树从底层树获得的路径信息示意图；

图4是由改进的最小生成树Kruskal算法得到的规划路径的示意图；

图5是选取关键节点之后的路径示意图。

具体实施方式

下面结合具体实施过程对本发明进行进一步描述。

准备工作：环境地图为480×480，设置起点为X_start＝(50,40)，由五角星点表示，终点为X_end＝(60,430),由圆圈点表示，另外设置四个目标点X₁＝(440,410)，X₂＝(420,80)，X₃＝(240,320)，X₄＝(200,100)，它们由四个星号点表示。障碍物的位置和大小的设定如图1所示。

步骤一：每个目标点初始化一棵树，配合在全局地图的信息，确定出感知距离。计算方法如下：

R＝min{D_ij},1≤i,j≤n

其中R代表感知距离，D_ij代表第i棵树的根结点与第j棵数的根结点之间的无视障碍物的欧式空间距离，n表示树木总数，也就是多个目标点加上起始点的总数。

步骤二：由步骤一确定的感知距离R及每棵树周围的障碍物环境，计算每棵树在整体树集合的扩展权重，由权重来决定每棵树的扩展概率。计算方法如下：

其中Wi代表第i棵树的权重，它可以由感知距离R计算得出，Ci，obs代表第i棵数周围由R定义的矩形环境空间障碍物空间的大小。计算出的每棵树权重Wi是每棵树的扩展概率。周围环境相对空旷的树木得到的扩展机会少，反之，周围环境相对拥挤的树木得到的扩展机会多。

步骤三：每棵树独立地使用基础快速随机搜索树算法进行空间探索，并且扩展步长统一为Δ。本实例中将扩展步长设置为环境空间大小的1/100，即Δ＝5。如图2所示，底层树对于环境空间进行探索。

步骤四：如果两棵树的距离足够近时，产生一条有效的无碰撞路径及一组构成该有效路径的节点。方法如下：

d_m＝min{d_new,i},1≤i≤n

其中d_m表示当前结点与其他树的距离，d_new,i代表当前新结点与第i棵树的距离，n代表树木总数。因为每次只扩展一棵树，计算两棵树之间的最短距离时只需要计算最新的扩展结点与其他树之间的最短距离即可。新结点与一棵树最短距离由如下定义：

d_new,i＝min{||q_new-q_ij||₂},1≤j≤m

其中q_new代表新结点，q_ij代表在第i棵树中第j个结点。每两个结点的距离由欧式空间距离计算得到。如图3所示，顶层树或者有效路径及构成有效路径的节点。

步骤五：初始化距离探索方阵。距离方阵如下：

E＝{e_ij}_i×j,1≤i,j≤n

其中E代表距离探索方阵，是一个对称矩阵，行列数均为树木的总数n；e_ij代表第i棵树与第j棵树的探索距离，表示底层树目前已经探索到的第i棵树与第j棵树的距离。距离方阵中的值初始化为计算机所能存储的最大值。

步骤六：根据步骤四中，在底层树每次获得一个无碰撞路径之后，判断新获得的路径长度与距离探索方阵中对应位置的大小，如果获得的路径更小，则更新距离探索方阵E中对应位置的距离值，并将获得的路径及路径节点添加到顶层树中。例如，获得一个从第j棵数到第i棵树的新路径，与原先距离探索方阵中e_ji做对比，如果新路径长度更短，则更新距离探索方阵，否则，抛弃这条新路径。

步骤七：在达到最大迭代次数条件之后，根据距离探索方阵E所构成的多个目标点的距离无向图，使用改进的最小生成树Kruskal算法计算出一条无重复遍历的多目标点的规划路径。改进的最小生成树Kruskal算法如下描述：将所有的路径从小到大排序，依次选择最小的权值路径，并将其连接；每次选择新路径时，保证从起点出发的最小生成树是一条链表示形式的树，如果不是链表示形式的树，抛弃这条路径，并选择下一条最小的权值路径。如图4所示，顶层树规划出一条链表形式的树，该树可以作为规划路径。

步骤八：根据在步骤七的无重复遍历的多目标点的规划路径，按照无碰撞直接连接的原则选取路径中的关键节点，抛弃未连接的节点，从而构成遍历多个目标点的路径。移动机器人可以使用A*算法对最终得到的关键节点进行导航，从而完成多目标遍历的路径规划任务。如图5所示，直线连接的就是选取的多个关键节点，移动机器人对这些关键节点进行导航，从而完成多目标遍历的路径规划任务。

Claims (5)

1.一种基于快速随机搜索树的多目标点路径规划的方法，其特征在于：使用两层树的结构，底层树负责探索未知空间，顶层树负责规划出能够遍历多个目标点的最短路径；

1)底层树的探索过程包含以下步骤：

步骤一：每个目标点、起点和终点均初始化一棵树，配合在全局地图的信息，确定出感知距离；

每个目标点、起点和终点均初始化一棵树，根据全局地图信息计算出感知距离，计算方法如下：

R＝min{D_ij},1≤i,j≤n

其中R代表感知距离，D_ij代表第i棵树的根结点与第j棵数的根结点之间的无视障碍物的欧式空间距离，n表示树木总数；

步骤二：由步骤一确定的感知距离，及每棵树周围的障碍物环境，计算出每棵树在整体树集合中的扩展权重，由扩展权重来决定每棵树的扩展概率；

根据感知距离R及每棵树周围的环境，计算每棵树在整体树集合的扩展权重，由扩展权重来决定每棵树的扩展概率；计算方法如下：

其中W_i代表第i棵树的扩展权重，C_i，obs代表第i棵数周围由R定义的正方形环境空间中障碍物的大小，正方形环境空间的边长为2×R，中心点为第i棵数的根结点；计算出的每棵树扩展权重W_i是每棵树的扩展概率，周围环境相对空旷的树木得到的扩展机会少，反之，周围环境相对拥挤的树木得到的扩展机会多；

步骤三：每棵树独立地使用快速随机搜索树算法进行空间探索，并且扩展步长统一为Δ；在每次迭代中只扩展一棵树，由步骤二中的扩展概率来选择一棵树进行扩展，扩展后得到一个新结点；

步骤四：如果两棵树的距离小于2×Δ，产生一条有效的无碰撞路径及一组构成该有效路径的节点；

2)顶层树的规划过程包含以下步骤：

步骤五：初始化距离探索方阵；

步骤六：根据步骤四中的有效路径及节点，更新距离探索方阵的距离信息，并将有效路径及构成该路径的节点添加到顶层树；

步骤七：重新规划多个目标点的访问顺序，获得能够遍历多个目标点的最短路径；

所述的步骤七具体为：在达到最大迭代次数条件之后，根据距离探索方阵E所构成的多个目标点的距离无向图，使用改进的最小生成树Kruskal算法计算出一条链表形式的树，该树可以作为遍历多个目标点的路径，改进的最小生成树Kruskal算法如下描述：将所有的路径从小到大排序，依次选择最小的权值路径，并将其连接；每次选择新路径时，保证从起点出发的最小生成树是一条链表示形式的树，如果不是链表示形式的树，抛弃这条路径，并选择下一条最小的权值路径，直至形成链表形式的树；

步骤八：在步骤七的最短路径上选取多个关键节点，使机器人可以在各个关键节点之间按顺序进行导航。

2.如权利要求1所述的基于快速随机搜索树的多目标点路径规划的方法，其特征在于所述的步骤四具体为：当两棵树小于2×Δ时，产生一条无碰撞的路径，两棵树的距离计算公式如下：

d_m＝min{d_new,i},1≤i≤n

其中d_m表示扩展得到的新结点与其他树的距离，d_new,i代表当前新结点与第i棵树的距离，n代表树木总数，因为每次只扩展一棵树，计算两棵树之间的最短距离时只需要计算扩展得到的新结点与其他树之间的最短距离即可，新结点与一棵树最短距离由如下定义：

d_new,i＝min{||q_new-q_ij||₂},1≤j≤m

其中q_new代表新结点，q_ij代表在第i棵树中第j个结点，每两个结点的距离由欧式空间距离计算得到。

3.如权利要求1所述的基于快速随机搜索树的多目标点路径规划的方法，其特征在于所述的步骤五为：建立一个距离探索方阵：

E＝{e_ij}_i×j,1≤i,j≤n

其中E代表距离探索方阵，是一个对称矩阵，行列数均为树木的总数n；e_ij代表第i棵树与第j棵树的距离值，表示底层树目前已经探索到的第i棵树与第j棵树的距离。

4.如权利要求1所述的基于快速随机搜索树的多目标点路径规划的方法，其特征在于：所述的步骤六中，在底层树每次获得一个无碰撞路径之后，判断新获得的路径长度与距离探索方阵E中对应位置距离值的大小，如果获得的路径更小，则更新距离探索方阵中对应位置的距离值，并将获得的路径及路径节点添加到顶层树中，反之，距离探索方阵中的值不作改变，获得的路径及路径节点也不会添加到顶层树中。

5.如权利要求1所述的基于快速随机搜索树的多目标点路径规划的方法，其特征在于所述的步骤八具体为：

在得到的链表形式的树上，按照起点、多个目标点、终点的遍历顺序依次进行导航，起点、多个目标点、终点均设置为关键节点；除了上述关键节点外，从树中的起点开始，按照遍历顺序检查树中的下一个节点，若两个节点连接的路径没有与障碍物产生碰撞，则继续检查下一个节点，直到产生碰撞为止，上一个不产生碰撞的节点置为关键节点；依次过程逐步遍历至终点，即可生成遍历多个目标点的路径，移动机器人对最终得到的关键节点进行导航，从而完成多目标遍历的路径规划任务。

Images