CN108896052A - 一种基于动态复杂环境下的移动机器人平滑路径规划方法 - Google Patents

Abstract

本发明涉及一种基于动态复杂环境下的移动机器人平滑路径规划方法，属于机器人路径规划领域。具体步骤如下：开始；搜索最优路径；对最优路径进行平滑处理；结束。所述搜索最优路径步骤采用加入人工势场引力思想的双向RRT算法进行路径搜索，即由人工势场引力指导随机树朝着目标方向生长，避免RRT算法对全局进行随机采样；或采用改进启发函数后的双向A*算法，在距离因素的基础上增加角度因素，并将距离和角度的单位进行归一化处理，显著减少路径规划时间，提高路径的收敛速度；所述对最优路径进行平滑处理，先采用弗洛伊德平滑算法筛除冗余节点，再用三次B样条法将剩余的节点平滑连接起来，最终得到平滑的最优路径。

Description

一种基于动态复杂环境下的移动机器人平滑路径规划方法

技术领域

本发明属于机器人路径规划领域，具体涉及一种基于复杂环境下移动机器人平滑路径规划方法。

背景技术

随着机器人所处工作环境复杂程度提高、任务数量的增加，移动机器人路径规划已经成为国内外众多学者研究的重点。传统的路径规划算法有人工势场法、模糊规则法、遗传算法、神经网络、模拟退火算法、蚁群优化算法等，但这些方法都需要在一个确定的空间内对障碍物进行建模，计算复杂度与机器人自由度呈指数关系，不适合解决多自由度机器人在复杂环境中的路径规划。

人工势场法是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求“引力”和“斥力”的合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题，当目标点、障碍物和机器人同在一条直线上时，可能会产生未到达目标点，但是机器人所受合力为零的状态，机器人停止运动或在最优点处震荡。

基于快速扩展随机树(RRT)的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。它以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。RRT算法在静态环境下的应用已经较为成熟，但是将之应用于复杂的动态环境下，如机器人足球世界杯RoboCup场景，因为RRT算法较强的随机扩展性，可能会发生随机树偏离目标方向生长，增加路径规划的时间，而且RRT算法在生成随机树时，可能会扩展冗余节点加入随机树中，导致最后规划出来的路径发生震荡、曲折。

A*算法作为静态路网中求解最短路径的搜索方法，它在路径规划时引入了全局信息，会对当前节点距终点的距离做出估值，并据此判断该节点处于最短路线上的可能性的大小，并选择消耗最小的方向拓展节点，但当应用场景中为动态障碍物，或者目标点变化频繁时，此算法可能会同时产生多个消耗最小的路径，不能保证搜索到的路径为最优路径，而且如果环境中障碍物复杂，A*算法扩展的节点较多，也会导致搜索效率低下。

无论上述的RRT算法还是A*算法，最终寻找出来的最优路径都是一系列节点的序列，是一组线段的组合，存在诸多震荡和锯齿，不适合直接用来指导机器人行动。

发明内容

为了解决现有路径规划算法搜索效率低及规划出的路径不平滑的问题，本发明提供如下技术方案：一种基于动态复杂环境下的移动机器人平滑路径规划方法，包括

S1：开始，

S2：寻找最优路径，

S3：对路径进行平滑处理，

S4：结束，

所述步骤S2搜索最优路径为采用包含人工势场引力思想的双向RRT算法寻找最优路径，具体步骤为：

S201，初始化起点终点，初始化起点X_start和终点X_go_al，分别以X_start和X_go_al为根节点，初始化生主树T_s与从树T_g两棵树。

S202，以包含人工势场引力思想的新节点计算公式扩展T_s、T_g；

首先在传统RRT算法扩展新节点的方法基础上，加入人工势场引力思想：

人工势场的目标引力函数如下：

G(n)＝ρ*k_p(x_goal-x_near)/||x_goal-x_near|| (1-1)

传统RRT扩展新节点的计算公式如下：

R(n)＝ρ*(x_rand-x_near)/||x_rand-x_near|| (1-2)

综合以上公式，获得加入人工势场思想的RRT扩展新节点的计算公式为：

x_new＝x_near+ρ*[(x_rand-x_near)/||x_rand-x_near||+k_p(x_goal-x_near)/|x_goal-x_near||]

(1-3)

式(1-1)(1-2)(1-3)中，ρ表示搜索步长，即每次向前探索的距离；k_p为引力系数，引力系数取值1.0-1.8，无障碍物时可以取较大值，接近障碍物和接近目标点时取较小值；X_goal表示路径终点，即目标点；X_rand表示随机节点；X_near为当前扩展树上距离X_rand最近的已知节点；(X_goal-X_near)/||X_goal-X_near||为节点X_near到目标节点X_goal的单位向量，代表方向信息。

从T_s开始进行扩展，获取随机点X_rand，由X_start出发，根据公式(1-3)，进行一步扩展，生成新节点X_new，将扩展动作切换到T_g。

循环步骤1：扩展动作切换到T_g，以前述步骤中生成的X_new作为T_g扩展时的X_rand，由T_g中距离X_rand最近的节点出发，根据公式(1-3)，向X_rand方向进行多步扩展生成T_g上的新节点X’_new(为与T_s中生成的新节点X_new区分，T_g中生成的新节点记为X’_new)，直到与树T_s连接上，即X_new与X’_new的距离小于最小步长ρ，且中间无障碍物，跳至步骤S203；如果扩展过程中碰到障碍物，则获取随机点X_rand，以公式(1-3)，进行一次随机扩展生成X’_new，将扩展动作切换到T_s。

循环步骤2：扩展动作切换到T_s，以前述步骤中生成的X’_new作为T_s扩展步骤中的X_rand，由T_s中距离X_rand最近的节点出发向X_rand方向进行多步扩展生成X_new，直到与树T_g连接上，即X_new与X’_new的距离小于最小步长ρ，且中间无障碍物，跳至步骤S203；如果碰到障碍物，则获取随机节点X_rand，以公式(1-3)，进行一次扩展生成X_new，将扩展动作切换到T_g。

上述循环步骤1和循环步骤2依次循环进行，获得基于双向RRT算法的最优路径节点序列。

S203，判断T_s、T_g是否相交于无障碍的节点，即判断X_new与X’_new的距离是否小于最小步长ρ，且中间无障碍物；是，则继续执行步骤S3，否，跳回步骤S202；

上述寻找最优路径的方法还可以采用改进启发函数后的双向A*算法，具体步骤如下：

S201’初始化起点和终点，分别将起点和终点列入两个openlist中；

S202’分别从起点和终点出发寻找最近节点；

S203’判断一方搜索到的节点是否存在于另一方的最佳路径中，是，继续执行步骤S204；否则返回S202；

S204’从最后一个节点分别向起点和终点回溯，获得最优路径；

在所述步骤S202’中，所述寻找最近节点是以如下估价函数进行评估，所述估价函数，是用来计算从当前节点到目标节点的距离，距离最小者即为最近节点，将其纳入最优路径中：

h(j)＝ω₁*L′+ω₂*θ′ (1-4)

所述公式(1-4)中，h(j)为估价函数，ω₁和ω₂为距离和角度的加权值，范围分别是0.55-0.65和0.35-0.45且ω₁+ω₂＝1，L是当前节点到终点的距离，n是下一步所有可选节点的数量，L_i是n个可选节点中第i个节点到终点的距离，θ是起点到当前节点的线段与当前节点到终点的线段的夹角。

S3对路径进行平滑处理，即对上述步骤中获得的最优路径节点序列进行平滑处理，具体步骤如下：

S301：采用佛洛依德算法筛除多余节点，仅保留必要的节点；

S302：采用三次B样条法对所述步骤获得的最优路径节点序列进行平滑连接处理；

最终得到一条从起点到终点的最优平滑路径。

本发明在上述基础上还可以做如下改进：

进一步的，在双向A*算法中，归一化处理后提高了搜索的指向性，但是需要对每一个节点到达终点的距离进行计算，计算量有所增加，为了进一步提升算法效率，提前缩小搜索区域，具体的：从最小图开始，逐步扩展搜索空间：由于完全偏离X_start和X_goal之间直接路径的障碍物对实际路径的发现没有影响，因此，在一开始就对这些障碍进行考虑是过早的，为减少搜索空间，搜索空间递归地建立起来，开始只考虑最小图，它仅包含起点和终点及其之间的切线，如果该切线被一个或多个障碍物切割，则将这些障碍物计入搜索范围内并且切除切线，对于新添加的障碍物，计算并检查切线以查看它们是否被其他障碍物覆盖以添加到图形中，直到不再需要添加障碍物为止，从而获得最少的需要考虑的搜索区域。

进一步的，在双向A*算法中，进一步缩小搜索空间的方法还可以包括对节点的扩展连线进行删减，对不可能为最优的路径以及重叠的障碍物不可行路径进行过滤。如果障碍物存在重叠，则将重叠障碍物的公切线与重叠障碍物本身围成的封闭区间设置为不可行区域，减少实际路径规划的搜索空间。

相对于现有技术，采取本发明的技术方案的有益效果是：

本发明针对传统RRT算法，将局部路径规划的思想结合到全局路径规划之中，既能够搜索出全局路径，也能适应实时变化的环境，解决了机器人在路径规划中运用局部规划带来的死锁问题和全局规划的路径震荡碰撞问题。

针对A*算法，修改后的启发式函数统一了距离与角度的影响因素，加上双向搜索思想及递归的扩展搜索范围的方法，使得搜索区域明显减少，搜索效率明显提高。

路径平滑阶段筛除了冗余节点，减少了路径振荡现象发生，更进一步提高了算法效率，提高了机器人在实际环境中路径的可达性，规划出的路径能够满足较复杂障碍物环境的需要。

附图说明

图1本发明的基于动态复杂环境下的移动机器人平滑路径规划方法流程图，

图2采用引进人工势场思想的双向RRT算法进行平滑路径规划流程图，

图3采用改进启发函数后的双向A*算法进行平滑路径规划流程图，

图4实施例1基于双向RRT算法寻找最优路径并做平滑处理前后的对比图，

图5为实施例2中A*算法进行路径规划的效果对比图，其中：

图5A采用传统A*算法进行路径规划的探索范围及寻到的路径，

图5B采用改进的双向A*算法进行路径规划的探索范围及寻到的路径，

图6最小图定义示意图，

图7完全图定义示意图，

图8重叠障碍物示意图，

图中各标号所表示的含义列表如下：

S1：开始，

S2：寻找最优路径，

S3：对路径进行平滑处理，

S4：结束，

Xstart-起始点，

Xgoal-目标点，

T1、T2、T3、T4-障碍物。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

请参照图1所示，图1为本发明基于动态复杂环境下的移动机器人平滑路径规划方法流程图，基于动态复杂环境下的移动机器人平滑路径规划方法具体包括：

S1：开始，

S2：寻找最优路径，

S3：对路径进行平滑处理，

S4：结束，

所述步骤S2搜索最优路径为采用包含人工势场引力思想的双向RRT算法寻找最优路径，请参照图2所示，图2为采用引进人工势场思想的双向RRT算法进行平滑路径规划流程图，具体步骤为：

S201，初始化起点终点，初始化起点X_start和终点X_go_al，分别以X_start和X_go_al为根节点，初始化生主树T_s与从树T_g两棵树。

S202，以包含人工势场引力思想的新节点计算公式扩展T_s、T_g；

首先在传统RRT算法扩展新节点的方法基础上，加入人工势场引力思想：

人工势场的目标引力函数如下：

G(n)＝ρ*k_p(x_goal-x_near)/||x_goal-x_near|| (1-1)

传统RRT扩展新节点的计算公式如下：

R(n)＝ρ*(x_rand-x_near)/||x_rand-x_near|| (1-2)

综合以上公式，获得加入人工势场思想的RRT扩展新节点的计算公式为：

x_new＝x_near+ρ*[(x_rand-x_near)/||x_rand-x_near||+k_p(x_goal-x_near)/||x_goal-x_near||]

(1-3)

式(1-1)(1-2)(1-3)中，ρ表示搜索步长，即每次向前探索的距离；k_p为引力系数，引力系数取值1.0-1.8，无障碍物时可以取较大值，使路径更快的向目标点前进，接近障碍物和接近目标点时取较小值，以使路径更快的绕过障碍物和避免落入局部最小情况；x_goal表示路径终点，即目标点；x_rand表示随机节点；X_near为当前扩展树上距离X_rand最近的已知节点；

(X_go_al-X_near)/||X_goal-Xnear||为节点Xnear到目标节点X_goal的单位向量(代表方向信息)。

从T_s开始进行扩展，从起始点X_start开始，获取随机点X_rand，根据公式(1-3)，进行一步扩展，生成新节点Xnew，将扩展动作切换到T_g。

循环步骤1：扩展动作切换到T_g，以前述步骤中生成的X_new作为T_g扩展步骤中的X_rand，由T_g中距离X_rand最近的节点出发，根据公式(1-3)，向X_rand方向进行多步扩展生成X’_new(为与T_s中生成的新节点X_new区分，T_g中生成的新节点记为X’_new)，直到与树T_s连接上，即X_new与X’_new的距离小于最小步长ρ，且中间无障碍物，跳至步骤S203；如果扩展过程中碰到障碍物，则获取随机点X_rand，以公式(1-3)，进行一次扩展生成X’_new，将扩展动作切换到T_s。

循环步骤2：扩展动作切换到T_s，以前述步骤中生成的X’_new作为T_s扩展步骤中的X_rand，由T_s中距离X_rand最近的节点出发向X_rand方向进行多步扩展生成X_new，直到与树T_g连接上，即X_new与X’_new的距离小于最小步长ρ，且中间无障碍物，跳至步骤S203；如果碰到障碍物，则以公式(1-3)，获取随机节点X_rand，进行一次扩展生成X_new，将扩展动作切换到T_g。

上述循环步骤1、循环步骤2两个步骤循环进行。

S203，判断T_s、T_g是否相交于无障碍的节点，即判断即X_new与X’_new的距离小于最小步长ρ，且中间无障碍物；是，继续执行步骤S3，否，跳回步骤S202；

所述步骤S3包括以下步骤：

S301:对上述步骤中获得的最优路径节点序列，采用佛洛依德算法筛除多余节点；

S302:采用三次B样条法对所述步骤获得的最优路径节点序列进行平滑连接处理。

最终得到一条从起点到终点的最优平滑路径。

请参照图4所示，图4为实施例1基于双向RRT算法寻找最优路径并做平滑处理前后的对比图，Xstart为起始点，Xgoal为终点，阴影区域为障碍物，其中细线为改进前双向RRT算法规划的路径，粗线为改进后双向RRT算法规划的路径。可以看到改进后获得的路径比改进前平滑，且总长度缩短了，具体数据如下表所示：

为了更直观的查看改进前后的差异，本实施例在仿真环境下设置了较大的距离，从上表可以看出，改进后获得的路径比改进前缩短了106米，计算时间增加了4.4秒，设机器人行走时的平均速度为5米/秒，走完106米需要用时20.1秒，故综合考虑规划用时+行走用时，本方案用少量的计算用时换取了大量的行走用时，在实际应用中机器人会以更短的时间到达目标点。

实施例2：

与实施例1不同的是，本实施例中步骤S2寻找最优路径采用的是改进启发函数后的双向A*算法搜索最优路径，请参考图3所示，图3为采用改进启发函数后的双向A*算法进行平滑路径规划流程图，具体步骤如下：

S201’初始化起点和终点，分别将起点和终点列入两个openlist中；

S202’分别从起点和终点出发寻找最近节点；

S203’判断一方搜索到的节点是否存在于另一方的最佳路径中，是，继续执行步骤S204；否则返回S202；

S204’从最后一个节点分别向起点和终点回溯，获得最优路径；

在所述步骤S202’中，所述寻找最近节点是以如下估价函数进行评估，所述估价函数，是用来计算从当前节点到目标节点的距离，距离最小者即为最近节点，将其纳入最优路径中：

h(j)＝ω₁*L′+ω₂*θ′ (1-4)

所述公式(1-4)中，h(j)为估价函数，ω₁和ω₂为距离和角度的加权值，范围分别是0.55-0.65和0.35-0.45且ω₁+ω₂＝1，L是当前节点到终点的距离，n是下一步所有可选节点的数量，L_i是n个可选节点中第i个节点到终点的距离，θ是起点到当前节点的线段与当前节点到终点的线段的夹角。

请参考图5所示，图5为实施例2中A*算法进行路径规划对比图，其中图5A为采用传统A*算法进行路径规划的探索范围及寻到的路径，图5B为采用改进的双向A*算法进行路径规划的探索范围及寻到的路径，图中两个实心黑框分别为起始点和终点，深色阴影区域为障碍物，浅色阴影区域为探索范围，模拟过程中，采用传统A*算法寻找到的最优路径长度为38，操作数为1048次；采用改进后的双向A*算法的寻找最优路径长度为38，操作数为458次，可以看出改进后的算法探索范围变大，搜索次数明显降低。

归一化处理后提高了搜索的指向性，但是需要对每一个节点到达终点的距离进行计算，计算量有所增加，为了进一步提升算法效率，可以提前缩小搜索区域，通过两个方法缩小搜索范围：

方法1：开始只考虑最小图区域，逐步扩展搜索空间至完全图。

由于完全偏离X_start和X_goal之间直接路径的障碍物对实际路径的发现没有影响，因此，在一开始就对这些障碍进行考虑是过早的。为减少搜索空间，开始只考虑最小图范围，即只考虑包括起点、终点即连接起点和终点之间的切线的范围，如果该切线被一个或多个障碍物切割，则将这些障碍物计入搜索范围内并且切除切线，对于新添加的障碍物，计算并检查切线以查看它们是否被其他障碍物覆盖，若有被其他障碍物覆盖，则将其添加到搜索范围内，直到不再需要添加障碍物或者扩展到完全图为止，从而获得最少的需要考虑的搜索区域。

方法2：对节点的扩展连线进行删减，对不可能为最优的路径以及重叠的障碍物造成的不可行路径进行过滤。

如果障碍物存在重叠，则重叠部分将无法通行，同时，进入由重叠障碍物围成的半包围区域内的路线也不可能是最优路线，故可以将重叠障碍物的完全图设置为不可行区域，减少路径规划的搜索空间。

关于最小图的定义，请参照图6所示，其为最小图定义示意图，图中Xstart为起点，Xgoal为终点，T1-T4为障碍物，T1-T4中，中心的黑点为障碍物本身的坐标，障碍物周围的圆圈半径代表最近阈值，如果机器人距离障碍物的距离小于最近阀值则会发生碰撞，最小图定义为仅包含起点Xstart和终点Xgoal及两者之间切线的最小搜索范围，即：起点Xstart与各个障碍物(T1-T4)圆圈没有连接。

关于完全图的定义请参照图7所示，其为完全图定义示意图，完全图定义为将起点和终点及它们之间障碍物圆圈用切线连接完整的复杂图，也是最大搜索范围，完全图将起始点Xstart与目标点Xgoal与障碍物圆圈建立了最大联系。

本发明公开的基于双向A*算法寻找最优路径的方法中对搜索范围的递归建立，就是由图6所示的最小图开始逐渐对障碍物圆圈扩展切线连接边，直到图7所示的完全图的过程。

请参照图8所示，其为重叠障碍物示意图，图中两个圆圈代表两个障碍物，圆圈相交，代表障碍物重叠，因为机器人不可能穿越两个障碍物重叠区域，且进入由重叠障碍物围成的半包围区域内的路线也不可能是最优路线，故可以将图8所示重叠障碍物本身及由重叠障碍物的外公切线和障碍物本身围成的封闭区域提前设置为不可行区域，减少搜索范围。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于动态复杂环境下的移动机器人平滑路径规划方法，其特征在于，具体步骤为：

S1：开始，

S2：寻找最优路径，

S3：对路径进行平滑处理，

S4：结束，

所述步骤S2寻找最优路径为采用包含人工势场引力思想的双向RRT算法寻找最优路径，步骤分解如下：

S201，初始化起点终点，以起点和终点分别建立两棵树T_s与T_g；

S202，以包含人工势场引力思想的新节点计算公式扩展T_s、T_g；

S203，判断T_s、T_g是否相交于无障碍的节点，是，继续执行步骤S3，否，跳回步骤S202；

所述步骤S202中所述新节点计算公式如下：

x_new＝x_near+ρ*[(x_rand-x_near)/||x_rand-x_near||+k_p(x_goal-x_near)/||x_goal-x_near||] (1-3)

所述公式(1-3)中，所述X_new为要扩展的新节点，所述X_rand表示随机节点，所述X_near为当前扩展树上距离所述X_rand最近的已知节点，所述X_goal表示路径终点，所述ρ表示搜索步长，所述K_p为引力系数；

所述步骤S203判断T_s、T_g是否相交于无障碍的节点，是指当所述T_s树搜索的新节点X_new与所述T_g树搜索的新节点X’_new距离＜ρ，且中间无障碍物时，认为两棵树相交于无障碍的节点；

所述步骤S3对路径进行平滑处理包括如下步骤：

S301采用佛洛依德算法对所述步骤S2获得的路径节点序列的多余节点进行筛除；

S302对所述步骤S301处理后剩余的路径节点用三次B样条法进行平滑连接。

2.如权利要求1所述的基于动态复杂环境下的移动机器人平滑路径规划方法，其特征在于所述步骤S2为：采用改进的双向A*算法搜索最优路径，具体步骤如下：

S201’初始化起点和终点，分别将起点和终点列入两个openlist中；

S202’分别从起点和终点出发寻找最近节点；

S203’一方搜索到的节点是否在另一方的最佳路径中，是-继续执行步骤S204，否-返回S202；

S204’从最后的节点回溯得到最优路径；

在所述步骤S202’中，所述寻找最近节点是以如下估价函数进行评估，所述估价函数，是用来计算从当前节点到目标节点的距离，距离最小者即为最近节点，将其纳入最优路径中：

h(j)＝ω₁*L′+ω₂*θ′ (1-4)

所述公式(1-4)中，h(j)为估价函数，ω₁和ω₂为距离和角度的加权值，范围分别是0.55-0.65和0.35-0.45且ω₁+ω₂＝1，L是当前节点到终点的距离，n是下一步所有可选节点的数量，L_i是n个可选节点中，第i个节点到终点的距离，θ是起点到当前节点的线段与当前节点到终点的线段的夹角。

3.如权利要求2所述的基于动态复杂环境下的移动机器人平滑路径规划方法，其特征在于所述双向A*算法的搜索空间从仅包含起点和终点及其之间切线的最小图开始，递归的扩展。

4.如权利要求2或3所述的基于动态复杂环境下的移动机器人平滑路径规划方法，其特征在于所述A*算法预先将完全图中重叠障碍物和由所述重叠障碍物的外公切线和所述重叠障碍物本身围成的封闭区域设置为不可行区域。