CN114115254A - 一种多机器***性编队的rrt路径规划方法 - Google Patents

Abstract

本发明涉及机器人路径规划技术领域，尤其涉及一种多机器***性编队的RRT路径规划方法，本发明针对RRT算法的随机性和障碍物周围震荡的问题，引入目标节点拓展权重，使目标节点方向和随机采样节点方向共同决定新节点产生的方向，对路径规划进行了很大程度的优化；同时针对多机器人编队引入虚拟弹簧的概念，在跟随机器人的移动路径受到障碍物影响的情况下，障碍物施加的压力致使虚拟弹簧压缩，编队队形调整至可以顺利通过的状态，并且在跟随机器人顺利通过障碍物后，障碍物施加的压力消失，编队队形恢复至初始状态，使得多机器人能够通过判断障碍物的有无来自适应调整其编队。

Description

一种多机器***性编队的RRT路径规划方法

技术领域

本发明涉及机器人路径规划技术领域，尤其涉及一种多机器***性编队的 RRT路径规划方法。

背景技术

快速搜索随机树(RRT)算法是一种基于随机采样的路径规划算法，算法以 机器人所在的起始点作为搜索随机树的根节点，在状态空间中随机采样得到新 节点，将其中可行的新节点加入到搜索树中，最终根据搜索树的父子关系进行 节点回溯，形成一条从初始节点到目标节点的路径。但在运用RRT算法形成机 器人路径的过程中，存在随机性太强、障碍物周围振荡等问题。

多机器人路径规划是对单机器人路径规划的进一步扩展，其特点是参考某 种特征指标，在所搭建状态空间中的起始点到目标点寻找一条无障碍的可行路 径。相比于单机器人，多机器人路径规划可以充分获取环境信息，提高机器人 完成任务的能力。多机器人路径规划广泛应用于工业、农业、医药、搜寻等领 域。

多机器人还涉及了编队控制，国内外对编队控制进行了广泛的研究，其控 制的主要方法有虚拟领航法、行为基础法和领航者-跟随者法。其中领航者-跟随 者法因其具有实现简单、扩展性良好等优点，成为多机器人编队的研究热门。 领航者-跟随者法是指对领航机器人进行路径规划，然后根据领航机器人的位置 及编队信息来确定跟随机器人的位置。本发明在领航者-跟随者法的基础上，引 入虚拟弹簧的概念，使得多机器人能够通过判断障碍物的有无来自适应调整其 编队。

发明内容

本发明的目的在于提供一种多机器***性编队的RRT路径规划方法，在快 速搜索随机树(RRT)算法的基础上进行创新，增强其目标导向性，缓解随机性 太强的问题，而对于机器人编队方面则引入虚拟弹簧的概念，使得多机器人能 够根据状态空间进行编队调整。

为实现上述目的，本发明提供了一种多机器***性编队的RRT路径规划方 法，包括下列步骤：

步骤1、多机器人编队，区分领航机器人和跟随机器人；

步骤2、所述领航机器人规划路径，并沿规划路径向目标节点移动；

步骤3、所述跟随机器人保持编队队形并跟随所述领航机器人向目标节点移 动；

步骤4、跟随机器人的编队路径有障碍物干扰时，所述跟随机器人依据虚拟 压力调整编队绕过障碍物；

步骤5、顺利通过障碍物后，所述跟随机器人返回原始编队路径，继续跟随 领航机器人移动；

步骤6、编队恢复队形向目标节点移动，

若跟随机器人的编队路径再次受到障碍物干扰，重复执行步骤4；

若无障碍物干扰，则保持编队队形直至目标节点。

其中，所述领航机器人根据改进快速搜索随机树算法形成规划路径，并引 入目标节点拓展权重。

其中，所述领航机器人和所述跟随机器人之间采用弹性编队模型，即设定 所述领航机器人和所述跟随机器人之间存在一个虚拟弹簧，所述虚拟压力为障 碍物施加的致使所述虚拟弹簧形变的假想作用力。

其中，所述虚拟弹簧的状态决定所述领航机器人和所述跟随机器人的位置 关系，通过所述虚拟弹簧的自由长度可确定所述领航机器人和所述跟随机器人 的初始编队距离。

其中，所述跟随机器人依据虚拟压力调整编队绕过障碍物的过程中，所述 虚拟压力致使虚拟弹簧压缩，所述跟随机器人向领航机器人移动轨迹靠拢调节， 保持现有编队队形至顺利通过障碍物。

其中，顺利通过障碍物后，所述虚拟压力消失，虚拟弹簧则自恢复至自由 长度，所述跟随机器人恢复与所述领航机器人间的初始编队距离。

本发明的一种多机器***性编队的RRT路径规划方法，本发明针对RRT算 法的随机性和障碍物周围震荡的问题，引入目标节点拓展权重，使目标节点方 向和随机采样节点方向共同决定新节点产生的方向，对路径规划进行了很大程 度的优化；同时针对多机器人编队引入虚拟弹簧的概念，在跟随机器人的移动 路径受到障碍物影响的情况下，障碍物施加的压力致使虚拟弹簧压缩，编队队 形调整至可以顺利通过的状态，并且在跟随机器人顺利通过障碍物后，障碍物 施加的压力消失，编队队形恢复至初始状态，使得多机器人能够通过判断障碍 物的有无来自适应调整其编队。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施 例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述 中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付 出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明的一种多机器***性编队的RRT路径规划方法的流程示意图。

图2是本发明的多机器***性编队的RRT路径规划方法的具体执行过程示意 图。

图3是本发明的机器***性编队遭遇障碍物时各状态参数示意图。

图4是本发明的机器***性编队越过障碍物后各状态参数示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自 始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。 下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理 解为对本发明的限制。

请参阅图1，本发明提出了一种多机器***性编队的RRT路径规划方法， 包括下列步骤：

S1：多机器人编队，区分领航机器人和跟随机器人；

S2：所述领航机器人规划路径，并沿规划路径向目标节点移动；

S3：所述跟随机器人保持编队队形并跟随所述领航机器人向目标节点移动；

S4：跟随机器人的编队路径有障碍物干扰时，所述跟随机器人依据虚拟压 力调整编队绕过障碍物；

S5：顺利通过障碍物后，所述跟随机器人返回原始编队路径，继续跟随领 航机器人移动；

S6：编队恢复队形向目标节点移动，

若跟随机器人的编队路径再次受到障碍物干扰，重复执行S4；

若无障碍物干扰，则保持编队队形直至目标节点。

所述领航机器人根据改进快速搜索随机树算法形成规划路径，并引入目标 节点拓展权重。

所述领航机器人和所述跟随机器人之间采用弹性编队模型，即设定所述领 航机器人和所述跟随机器人之间存在一个虚拟弹簧，所述虚拟压力为障碍物施 加的致使所述虚拟弹簧形变的假想作用力。

所述虚拟弹簧的状态决定所述领航机器人和所述跟随机器人的位置关系， 通过所述虚拟弹簧的自由长度可确定所述领航机器人和所述跟随机器人的初始 编队距离。

所述跟随机器人依据虚拟压力调整编队绕过障碍物的过程中，所述虚拟压 力致使虚拟弹簧压缩，所述跟随机器人向领航机器人移动轨迹靠拢调节，保持 现有编队队形至顺利通过障碍物。

顺利通过障碍物后，所述虚拟压力消失，虚拟弹簧则自恢复至自由长度， 所述跟随机器人恢复与所述领航机器人间的初始编队距离。

本发明的主要思想是让多机器人编队能够沿规划路径移动，同时借助虚拟 弹簧，避开障碍物顺利到达最终节点。首先针对RRT算法的随机性和障碍物周 围震荡的问题，引入目标节点拓展权重，使目标节点方向和随机采样节点方向 共同决定新节点产生的方向，对路径规划进行了很大程度的优化；同时针对多 机器人编队引入虚拟弹簧的概念，在跟随机器人的移动路径受到障碍物影响的 情况下，障碍物施加的压力致使虚拟弹簧压缩，编队队形调整至可以顺利通过 的状态；并且在跟随机器人顺利通过障碍物后，障碍物施加的压力消失，编队 队形恢复至初始状态，具体的执行过程请参阅图2。

以下从两个具体方面进一步说明：

1、领航机器人RRT路径规划模型

为增强路径新节点扩展的目标导向性，假设领航机器人第n+1时刻路径节点 q(n+1)的扩展方向由第n时刻路径节点q(n)指向目标节点q_goal的方向、第n时刻路 径节点q(n)指向随机采样节点q_rand的方向共同决定，可表示为

q(n+1)＝q(n)+λ(ω_gμ+(1-ω_g)ν) (1)

其中，λ为节点扩展步长，w_g为目标节点扩展权重，μ为q(n)指向q_goal的单位 矢量，

ν为q(n)指向q_rand的单位矢量，

p_bias为目标偏置 概率，p为随机概率，若p＜p_bias，q_rand＝q_goal；否则，q_rand为随机采样点。

假设领航机器人第n+1时刻路径节点q(n+1)的坐标为(x^m(n+1),y^m(n+1))，q(n)的坐标为(x^m(n),y^m(n))，q_goal的坐标为

q_rand的坐标为

式(1)可改 写为

其中，

2、跟随机器***性编队模型

假设领航机器人A与跟随机器人B和跟随机器人C之间各存在一虚拟弹簧， 虚拟弹簧的自由长度为l_e、劲度系数为k，A(n-1)B(n-1)间的距离为

A(n-1)C(n-1) 间距离为

A(n-1)B(n-1)与A(n-1)A(n-2)的夹角为θ₁,A(n-1)C(n-1)与A(n-1)A(n-2) 的夹角为θ₂，其中A(n-2)为领航机器人A在n-2时刻的坐标点，A(n-1)、B(n-1)和 C(n-1)分别表示领航机器人A、跟随机器人B和C在n-1时刻的坐标点，如图3所 示。基于弹簧工作原理，弹性编队含队形调整和队形保持两种状态。在队形调 整方面，若跟随机器人受到障碍物施加的虚拟压力，虚拟弹簧则被压缩，即跟 随机器人向领航机器人移动轨迹靠拢至B(n)，如图3所示。在队形保持方面，若 跟随机器人受到的虚拟压力消失，虚拟弹簧则自恢复至自由长度，即跟随机器 人将恢复至B(n+1)，如图4所示。

假设机器人A、B和C第n-1时刻坐标分别为

和

第n时刻坐标分别为

和

O为 跟随机器人的集合，Obstacle为障碍物集合，Eage(B(n-1),B(n))为跟随机器人B从第 n-1时刻到第n时刻的轨迹集合，

为跟随机器人B从第n-1时刻到第n时刻 的坐标距离，β为领航机器人移动轨迹与水平线夹角，α₁为跟随机器人B从第n-1 时刻到第n时刻的坐标连线与水平线夹角。在队形调整过程中，跟随机器人B在 第n-1时刻与第n时刻之间受到障碍物的虚拟压力

虚拟弹簧被其压缩，跟 随机器人B向领航机器人移动轨迹靠拢至B(n)，在跟随机器人B移动到B(n)的过 程中，将偏离其原始移动轨迹，形成轨迹偏移角

根据图4，不难得出虚 拟压力

表示为

其中，

以得到在障碍物施加虚拟压力的情况下，领航机器人-跟随机器人连线与领 航机器人移动轨迹的夹角:

由此，可得出跟随机器人的坐标为:

通过障碍物后，跟随机器人受到的障碍物压力消失，此时虚拟弹簧恢复至 自由长度，即领航机器人-跟随机器人连线与领航机器人移动轨迹的夹角恢复至 θ_i。

以上所揭露的仅为本发明一种较佳实施例而已，当然不能以此来限定本发 明之权利范围，本领域普通技术人员可以理解实现上述实施例的全部或部分流 程，并依本发明权利要求所作的等同变化，仍属于发明所涵盖的范围。

Claims (6)

1.一种多机器***性编队的RRT路径规划方法，其特征在于，包括下列步骤：

步骤1、多机器人编队，区分领航机器人和跟随机器人；

步骤2、所述领航机器人规划路径，并沿规划路径向目标节点移动；

步骤3、所述跟随机器人保持编队队形并跟随所述领航机器人向目标节点移动；

步骤4、跟随机器人的编队路径有障碍物干扰时，所述跟随机器人依据虚拟压力调整编队绕过障碍物；

步骤5、顺利通过障碍物后，所述跟随机器人返回原始编队路径，继续跟随领航机器人移动；

步骤6、编队恢复队形向目标节点移动，

若跟随机器人的编队路径再次受到障碍物干扰，重复执行步骤4；

若无障碍物干扰，则保持编队队形直至目标节点。

2.如权利要求1所述的多机器***性编队的RRT路径规划方法，其特征在于，

所述领航机器人根据改进快速搜索随机树算法形成规划路径，并引入目标节点拓展权重。

3.如权利要求1所述的多机器***性编队的RRT路径规划方法，其特征在于，

所述领航机器人和所述跟随机器人之间采用弹性编队模型，即设定所述领航机器人和所述跟随机器人之间存在一个虚拟弹簧，所述虚拟压力为障碍物施加的致使所述虚拟弹簧形变的假想作用力。

4.如权利要求3所述的多机器***性编队的RRT路径规划方法，其特征在于，

所述虚拟弹簧的状态决定所述领航机器人和所述跟随机器人的位置关系，通过所述虚拟弹簧的自由长度可确定所述领航机器人和所述跟随机器人的初始编队距离。

5.如权利要求4所述的多机器***性编队的RRT路径规划方法，其特征在于，

所述跟随机器人依据虚拟压力调整编队绕过障碍物的过程中，所述虚拟压力致使虚拟弹簧压缩，所述跟随机器人向领航机器人移动轨迹靠拢调节，保持现有编队队形至顺利通过障碍物。

6.如权利要求5所述的多机器***性编队的RRT路径规划方法，其特征在于，

顺利通过障碍物后，所述虚拟压力消失，虚拟弹簧则自恢复至自由长度，所述跟随机器人恢复与所述领航机器人间的初始编队距离。

Images