CN114879676A - 一种多机器人编队队形变换与动态避障方法 - Google Patents

Abstract

本发明涉及一种多机器人编队队形变换与动态避障方法，属于机器人技术领域。该方法使用最大优先指派算法和匈牙利算法对机器人进行位置分配，在保证最长路径距离最小的情况下，分配其他机器人的总路径距离最小；再使用互惠速度障碍法优化距离目标点较远的机器人在避障过程中损耗的时间，进一步缩短队形变换完成的时间；最后使用改进编队速度障碍法进行编队，改善编队在避障过程中短时间频繁切换队形的问题。

Description

一种多机器人编队队形变换与动态避障方法

技术领域

本发明属于机器人技术领域，涉及一种多机器人编队队形变换与动态避障方法。

背景技术

随着机器人技术的发展成熟，机器人编队已经成为当前的热门研究方向。由于机器人编队经常需要工作在障碍环境中，所以必须考虑机器人编队的避障问题。与单机器人避障不同，机器人编队可以通过变换队形适应外界环境的约束，队形变换的效率以及编队避障算法的优劣影响着机器人编队的安全。

发明内容

有鉴于此，本发明的目的在于提供一种多机器人编队队形变换与动态避障方法，使用最大优先指派算法和匈牙利算法给出位置分配问题的解决方案；使用互惠速度障碍法缩短队形变换完成的时间；使用改进编队速度障碍法进行编队，改善编队在避障过程中短时间频繁切换队形的问题。

为达到上述目的，本发明提供如下技术方案：

一种多机器人编队队形变换与动态避障方法，使用最大优先指派算法和匈牙利算法对机器人进行位置分配，在保证最长路径距离最小的情况下，分配其他机器人的总路径距离最小；再使用互惠速度障碍法优化距离目标点较远的机器人在避障过程中损耗的时间，进一步缩短队形变换完成的时间；最后使用改进编队速度障碍法进行编队，改善编队在避障过程中短时间频繁切换队形的问题。

该方法具体包括以下步骤：

S1：根据领航跟随法的

编队模型设计编队队形描述矩阵；

S2：使用最大优先指派算法和匈牙利算法对机器人进行位置分配，并使用改进互惠速度障碍法，优化距离目标点较远的机器人在避障过程中损耗的时间；

S3：使用改进编队速度障碍法进行编队，改善编队在避障过程中短时间频繁切换队形的问题；

S4：对机器人编队以及障碍物、控制器进行建模；

S5：使用编队控制器，对机器人编队进行控制，并对机器人编队的运行效率和避障效果进行评估。

进一步，步骤S1中，定义位置矩阵

来描述一个机器人在期望编队队形中的位置，其中R_id为该机器人的ID号，用于标识该机器人的身份；R_lid为该编队中领航者机器人的ID号；l_id、

分别为期望编队队形中该机器人与领航机器人之间的距离和角度。

进一步，步骤S1中，机器人编队通常由n个具有相同运动学模型的轮式移动机器人组成，其在全局坐标系∑XOY下的位置坐标矩阵可以表示为：P_l＝[x_ly_lθ_l]^T，其中x_l和y_l分别表示第l个机器人两个驱动轮连线中点在X轴上的坐标和在Y轴上的坐标，θ_l表示第l个机器人速度方向与惯性坐标系X轴的夹角。

进一步，步骤S2中，机器人编队的时耗矩阵与资源耗费矩阵分别为A_N×M和B_N×M时，机器人到目标位置的路径长度的时耗矩阵与资源耗费矩阵中的元素为：a_ij＝b_ij＝|G_j-R_i|,1≤i≤N,1≤j≤M；其中，R_i和G_j分别为机器人与目标点的位置坐标，N表示机器人编队的时耗矩阵与资源耗费矩阵的阶次。

进一步，步骤S2中，使用最大优先指派算法可以求得最小的最大消耗时间为：t_max＝max{a_ij,i＝1,2,…,N,j＝solve[i],j≠0}，其中solve[RawIndex]＝ColumnIndex；再构造方阵C_(N+n)×(M+m)，由最小的最大消耗时间约束下的条件使用匈牙利算法求解路径之和最小的分派结果，方阵C_(N+n)×(M+m)为：

其中，n和m表示普遍情况下与比原矩阵多出来的阶次。

进一步，步骤S2中，改进互惠速度障碍法，具体包括：对于两个性能和体积相同的机器人A和机器人B，A的当前位置矢量为P_A，目标点的位置矢量为G_A，B的当前位置矢量为P_B，目标点的位置矢量为G_B，则机器人A在避障过程中需要承担的责任比例

为：

机器人B在避障过程中需要承担的责任比例

为：

对于机器人A而言，由机器人B造成的速度障碍

为：

其中，V_A、V_B分别表示机器人A、B的速度，V_A′表示机器人A在速度障碍中的速度，

表示障碍物B对机器人A的速度障碍。承担责任越小的机器人在避障过程中速度的改变和额外行走的距离越小，能够更快的到达目标位置。另一个承担了更多避障责任的机器人将花费更多的时间用于避障。

进一步，，步骤S3中，改进的编队速度障碍法具体包括：在所有方向上由障碍物导致的编队速度障碍

为：

其中，v_θ为沿着方向θ的单位速度，v_o表示障碍物O的速度，τ表示时间范围，t^-,t⁺表示发生碰撞的时间下限和上限，p_o表示障碍物O在以编队包围盒为中心的坐标系上的位置，h表示包围盒高度的一半；

队形评价函数E(F)为：

E(F)＝p_F(v_F·v_pref)

其中，p_F表示人为地给予每个编队队形一个优先级，v_F表示机器人编队的实际速度，v_pref表示机器人编队的期望速度；

待选速度的综合评价函数E为：

E＝-α₁·Δstr+α₂·P-α₃·D_v

其中，Δstr表示结构变形指标，P表示待评价速度所属队形的队形优先级，其值等于p_F，D_v表示待选速度与与期望速度的欧氏距离，α₁、α₂和α₃为各个性能指标的权重。

进一步，步骤S4中，领航者机器人的ID为k，通过组合所有机器人的位置矩阵可以将任何一个由n个机器人组成的编队队形用一个4行n列的队形描述矩阵来表示：

本发明的有益效果在于：本发明使用最大优先指派算法和匈牙利算法给出位置分配问题的解决方案；使用互惠速度障碍法优化距离目标点较远的机器人在避障过程中损耗的时间，进一步缩短了队形变换完成的时间；使用改进编队速度障碍法进行编队，改善了编队在避障过程中短时间频繁切换队形的问题。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

图1为本发明多机器人编队队形变换与动态避障方法流程图；

图2为静态障碍下改进编队速度障碍法机器人轨迹；

图3为静态障碍下改进编队速度障碍法编队包围盒中心轨迹；

图4为动态障碍下改进编队速度障碍法机器人轨迹；

图5为动态障碍下改进编队速度障碍法编队包围盒中心轨迹。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本发明的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

请参阅图1～图5，为一种多机器人编队队形变换与动态避障方法，如图1所示，该方法的具体步骤如下：

1)在使用一个领航者机器人以及两个跟随者机器人，并选取了三角型队形作为编队队形时，其队形描述矩阵为：

2)机器人编队通常由n个具有相同运动学模型的轮式移动机器人组成。对于编队中第l个轮式移动机器人而言，其在全局坐标系∑XOY下的位置坐标矩阵可以表示为：

P_l＝[x_ly_lθ_l]^T

其中，x_l和y_l分别表示机器人两个驱动轮连线中点在X轴上的坐标和在Y轴上的坐标，θ_l表示该机器人速度方向与惯性坐标系X轴的夹角。由领航跟随法的距离-角度关系模型可以得到虚拟机器人的位置坐标矩阵：

跟随者机器人与虚拟机器人之间的误差可以表示为：

在跟随者机器人中心建立局部坐标系∑X_fO_fY_f，e_fx和e_fy分别表示跟随者机器人与虚拟机器人在局部坐标系中沿X_f轴和Y_f轴的误差分量，则可以得到跟随者机器人的位姿误差方程为：

3)在本实施例中，使用了五个移动机器人，其中一个机器人为领航机器人其余四个为跟随机器人。机器人的起始速度为0m/s，最大速度为0.5m/s，机器人的半径为0.2m/s，使用三角型队形。

领航者机器人的起始位置为(0,0)，其余跟随者机器人起始位置分别为(0,1)、(0,-1)。机器人编队沿着引导线前进，返航点统一设置为(30,0)，返回引导线后编队继续沿着引导线前进。控制器参数为k1＝4.2，k2＝2.7。

障碍物为静态障碍物，位置为(20,0)，半径为3m。使用改进后的编队速度障碍法，根据机器人编队与障碍物间的距离体调节包围盒膨胀半径，编队的路径如图2所示。编队包围盒中心运动轨迹如图3所示。

从图2和图3中可以看到，机器人编队在位置(5.1,0)开始偏离规定路线躲避障碍物，在位置(29.8,0)返回规定路线。在规定路线上行驶的距离长度为21.3m，偏离规定路线的长度为24.7m。与原始编队速度障碍法相比，偏离规定路径的长度减少了1.2m。同时由于引入了最小安全距离，机器人编队在避障期间与障碍物保持了0.8m的最小间隔，保证了机器人编队的安全。

4)将障碍物改为动态障碍物，障碍物的起始位置为(25,0)，半径为3m，速度大小为0.1m/s。编队的路径如图4所示。编队包围盒中心运动轨迹如图5所示。机器人编队在位置(8.57,0)开始偏离规定路线躲避障碍物，在位置(30,0)返回规定路线。在规定路线上行驶的距离长度为24.57m。

根据以上实验，使用改进后的编队速度障碍法可以明显减少机器人编队偏离规定路线的路径长度，同时能够让机器人编队与障碍物之间保持一定距离，保证了机器人编队的安全。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (9)

1.一种多机器人编队队形变换与动态避障方法，其特征在于，使用最大优先指派算法和匈牙利算法对机器人进行位置分配，在保证最长路径距离最小的情况下，分配其他机器人的总路径距离最小；再使用互惠速度障碍法优化距离目标点较远的机器人在避障过程中损耗的时间；最后使用改进编队速度障碍法进行编队，改善编队在避障过程中短时间频繁切换队形的问题。

2.根据权利要求1所述的多机器人编队队形变换与动态避障方法，其特征在于，该方法具体包括以下步骤：

S1：根据领航跟随法的

编队模型设计编队队形描述矩阵，并以此设计编队控制器；

S2：使用最大优先指派算法和匈牙利算法对机器人进行位置分配，并使用改进互惠速度障碍法，优化距离目标点较远的机器人在避障过程中损耗的时间；

S3：使用改进编队速度障碍法进行编队，改善编队在避障过程中短时间频繁切换队形的问题；

S4：对机器人编队以及障碍物、控制器进行建模。

3.根据权利要求2所述的多机器人编队队形变换与动态避障方法，其特征在于，步骤S1中，定义位置矩阵

来描述一个机器人在期望编队队形中的位置，其中R_id为该机器人的ID号，用于标识该机器人的身份；R_lid为编队中领航者机器人的ID号；

分别为期望编队队形中该机器人与领航机器人之间的距离和角度。

4.根据权利要求3所述的多机器人编队队形变换与动态避障方法，其特征在于，步骤S1中，机器人编队由n个具有相同运动学模型的轮式移动机器人组成，其在全局坐标系∑XOY下的位置坐标矩阵表示为：P_l＝[x_l y_l θ_l]^T，其中x_l和y_l分别表示第l个机器人两个驱动轮连线中点在X轴上的坐标和在Y轴上的坐标，θ_l表示第l个机器人速度方向与惯性坐标系X轴的夹角。

5.根据权利要求4所述的多机器人编队队形变换与动态避障方法，其特征在于，步骤S2中，机器人编队的时耗矩阵与资源耗费矩阵分别为A_N×M和B_N×M时，机器人到目标位置的路径长度的时耗矩阵与资源耗费矩阵中的元素为：a_ij＝b_ij＝|G_j-R_i|,1≤i≤N,1≤j≤M；其中，R_i和G_j分别为机器人与目标点的位置坐标，N与M表示机器人编队的时耗矩阵与资源耗费矩阵的阶次。

6.根据权利要求5所述的多机器人编队队形变换与动态避障方法，其特征在于，步骤S2中，使用最大优先指派算法求得最小的最大消耗时间为：t_max＝max{a_ij,i＝1,2,…,N,j＝solve[i],j≠0}，其中solve[RawIndex]＝ColumnIndex；再构造方阵C_(N+n)×(M+m)，由最小的最大消耗时间约束下的条件使用匈牙利算法求解路径之和最小的分派结果，方阵C_(N+n)×(M+m)为：

其中，n和m表示普遍情况下与比原矩阵多出来的阶次。

7.根据权利要求6所述的多机器人编队队形变换与动态避障方法，其特征在于，步骤S2中，改进互惠速度障碍法，具体包括：对于两个性能和体积相同的机器人A和机器人B，A的当前位置矢量为P_A，目标点的位置矢量为G_A，B的当前位置矢量为P_B，目标点的位置矢量为G_B，则机器人A在避障过程中需要承担的责任比例

为：

机器人B在避障过程中需要承担的责任比例

为：

对于机器人A而言，由机器人B造成的速度障碍

为：

其中，V_A、V_B分别表示机器人A、B的速度，V′_A表示机器人A在速度障碍中的速度，

表示障碍物B对机器人A的速度障碍。

8.根据权利要求7所述的多机器人编队队形变换与动态避障方法，其特征在于，步骤S3中，改进的编队速度障碍法具体包括：在所有方向上由障碍物导致的编队速度障碍

为：

其中，v_θ为沿着方向θ的单位速度，v_o表示障碍物O的速度，τ表示时间范围，t^-,t⁺表示发生碰撞的时间下限和上限，p_o表示障碍物O在以编队包围盒为中心的坐标系上的位置，h表示包围盒高度的一半；

队形评价函数E(F)为：

E(F)＝p_F(v_F·v_pref)

其中，p_F表示人为地给予每个编队队形一个优先级，v_F表示机器人编队的实际速度，v_pref表示机器人编队的期望速度；

待选速度的综合评价函数E为：

E＝-α₁·Δstr+α₂·P-α₃·D_v

其中，Δstr表示结构变形指标，P表示待评价速度所属队形的队形优先级，其值等于p_F，D_v表示待选速度与与期望速度的欧氏距离，α₁、α₂和α₃为各个性能指标的权重。

9.根据权利要求8所述的多机器人编队队形变换与动态避障方法，其特征在于，步骤S4中，领航者机器人的ID为k，通过组合所有机器人的位置矩阵可以将任何一个由n个机器人组成的编队队形用一个4行n列的队形描述矩阵来表示：

Images