CN111813108A - 一种实时分布式多机器人队形控制方法、系统 - Google Patents

Abstract

本发明属于协同控制系统技术领域，公开了一种实时分布式多机器人队形控制方法、系统，机器人对障碍物进行精确定位，当与其他机器人通信时需要将其探测到的情况通报，收到通报的机器人对收的信息和自己探测到的信息甄别和选择；机器人进行通信后检索到两障碍区的位置不同，则将其按两障碍块处理进行互补，分析后认为可通行区域为两障碍区间的空白区域；机器人定位的障碍区一致，可通行区域也是一致的。本发明根据可通行区域的宽度决定系统目标队形的结构和尺寸，代替机器人初始化时确定有限个队形的方法，更好的环境适应性；在队形变换中仍旧使用数学规划的方式让每个机器人寻找最优点，在该点形成目标队形，以此队形穿过障碍区。

Description

一种实时分布式多机器人队形控制方法、系统

技术领域

本发明属于协同控制系统技术领域，尤其涉及一种实时分布式多机器人队形控制方法、系统。

背景技术

目前，多机器人系统是一种多个机器人在给定的环境中一起工作来完成某些给定的任务。相对于单个机器人，其表现出较多的优点，例如增加空间覆盖率和时间的吞吐量以及降低解决任务的复杂性，提升解决效率和能力。多机器人系统已经被应用于许多领域。如自动制造、柔性生产、搜索营救、环境监测、安全健康等。到目前为止，人类对机器人的研究已经取得卓越的成就，但是很多技术都是基于单个机器人，其数量上的限制导致对任务的选择有很大的局限性，单个机器人很难完成一些复杂的任务。相对于单个机器人，多个机器人可以协调合作完成单一机器人难以完成的任务，比如在水下进行营救作业时单个机器人无法保证平衡性，但是多个机器人的合作不仅可以增加稳定性还可以保证有足够的力量从水下将救援对象拖出水面，提升了救援成功的可能性。另外，多机器人还可以应用到环境监督、自动搬运和雷区映射等场景。正是多机器人此种无法被替代的特点吸引了众多研究学者的研究，用来弥补单个机器人的不足。

针对多机器人的队形控制，包括队形形成、队形保持和队形变换，众多学者提出了多种实施方法，包括领航-跟随法、基于行为法、人工势场法、虚拟结构法等。领航-跟随法操作方便，多机器人系统的运动规划主要依赖于领航机器人，其他机器人只需获取领航机器人发送的行为指令。由于整个系统只有领航机器人进行探测和决策，该方法的跟踪误差反馈较差，当机器人系统比较庞大时该方法不适用。基于行为法将机器人执行的任务分解为多个行为的集合，通过给不同的行为配置不同的权值实现不同的任务，该方法可以获得实时的反馈，但是权值的选取难以确定。人工势场法将环境模拟为一个势场，存在斥力和引力，机器人与终点位置存在吸引力，与障碍物存在斥力，并且这些力的大小与距离远近有直接关系，机器人的运动是各种力的矢量之和，该方法建模简单，容易计算，但是容易出现零势能点，只能找到局部最小点，无法到达目标位置。虚拟结构法将机器人系统形成的队形虚拟为一个刚体结构，在机器人前进过程中各机器人追随自己刚体虚拟位置，该方法比较容易分配机器人的行为，但是在障碍物比较多的环境中不适用。

通过上述分析，分布式控制的意义在于，系统中的机器人独立决策，独立执行，并且机器人间有通信，机器人间的通信可以互补信息，在每一个位置机器人可以获得更全面的环境信息，减少探测环境时的误差；机器人间通信可以处理一致性问题，机器人了解了对方的信息状态后，可以更好的决定自己的行为，最终达到合作的效果，在多障碍物的环境中可以有更好的表现。

发明内容

针对现有技术存在的问题，本发明提供了一种实时分布式多机器人队形控制方法、系统。

本发明是这样实现的，一种实时分布式多机器人队形控制方法，所述实时分布式多机器人队形控制方法包括：

机器人对障碍物进行精确定位，当与其他机器人通信时需要将其探测到的情况通报，收到通报的机器人对收的信息和自己探测到的信息甄别和选择；

当机器人系统识别和定位障碍区后，机器人通过分析障碍区可穿行区域与机器人前进方向上的最大宽度之比d，考虑变换何种队形，尽可能的在时间和耗能上减少浪费，最后到达目标区域。

进一步，所述实时分布式多机器人队形控制方法的队形选择，机器人确定前方最大可通行宽度D_max，由于机器人有安全半径ρ的约束，所以D_max等于障碍区中可通行宽度减安全半径的二倍，构成的队形中，机器人前进方向上最大的宽度为D，则伸缩比：

当机器人之间的距离恰好是安全距离时，认定其伸缩比为d_min，d≥d_min；

伸缩比d的取值范围分类为：d≥1、d＜1且d≥d_min、d＜d_min，与三种分类分别对应三种变换方式：零构变换、同构变换、异构变换；

零构变换：当d≥1时D_max≥D，说明障碍区中可通行区域的宽度远远大于机器人前进方向中最大的宽度；

同构变换：当d＜1时说明机器人保持原来的队形已经不能够通过障碍区，需要变换队形，而d≥d_min则表明可以穿行的宽度大于机器人安全距离，即机器人缩小自己宽度通过障碍区；

异构变换：当d＜d_min时表明机器人系统在不改变原结构的情况下已经不能通过障碍区。

通过伸缩比可以确定机器人的目标队形，但是在不同的位置变换会影响系统的性能。选取合适的目标点将成为另一个重要的研究内容。

为了保证机器人的一致性，规定机器人在目标点的速度为0，则可以将整个变换过程抽象为一个匀减速为0的运动，其过程遵循以下运动学方程：

v_t＝v₀+at；

v₀＝0；

其中v₀表示机器人在目标点的速度，s是变换队形过程中初始点和目标点的直线距离，v_t是机器人在当前位置的速度，为已知量。

在上述抽象模型基础之上，寻找目标点的优化模型为：

s.t. k_fm＝k_ft；

v_t＝at_c；

s_c＝||P_m-P_t||₂；

||P_m-O_α||₂≥ρ；

a∈[a_min，a_max]；

机器人到达目标点是速度从v_t降为0的运动过程，其中a和t分别表示机器人在此过程中的加速度和所花费的时间，w₁和w₂为其权重值，k_fm表示终点位置和可通行区中点连线的斜率，k_ft表示终点位置和目标位置连线的斜率，v_t为机器人在当前位置的速度，S_c为机器人从当前位置到达目标位置的直线距离，P_m和P_t分别表示目标位置和当前位置，P_f表示终点位置，t_s表示到达目标点后剩余的时间。

进一步，所述实时分布式多机器人队形控制方法在确定目标点的模型中，为了使机器人在变换队形后有最好的初始状态，规定目标队形的几何中心落在终点和可通行区域中点的连线上。使用分布式控制要使每个机器人依据在队形中所处的位置独立决策自己的目标点，最终使整个系统找到合适的目标位置，机器人根据自己在队形中与对称轴的相对位置确定目标队形中与对称轴的相对方向，根据当前队形宽度和伸缩比确定与对称轴的距离，两个确定量可以计算该机器人在目标队形中应该满足的直线方程，该机器人的目标点将会在该直线上产生，根据伸缩比可以确定机器人目标点所处的直线k_fm1，则用k_fm1＝k_ft1替代模型中的k_fm＝k_ft进行独立求解；

根据上边确定的直线方程再加上模型中提到的约束条件，每个机器人确定一个满足要求的目标点，为了找到所有机器人的最终目标点，机器人在初步规划目标点后通报自己规划的目标位置；对所有机器人求解的几何中心位置坐标求取平均值作为目标几何中心；

当确定目标几何中心后机器人确定自己的目标点，当确定目标点后，各机器人根据运动模型驱动机器人从初始位置到达目标位置，在此运动过程中采用分布式的控制方式，各机器人独立决策、独立执行，在相同时间到达目标点，并且在目标位置各机器人的速度均为零，为后面继续保持队形创造最优的初始条件。

上述算法当中驱动机器人前进的模型采用模型预测控制结合数学规划的方法，其中数学规划模型为：

||x[k]-Q||₂≥ρ′；

v[k]∈[v_min，v_max]；

a[k]∈[a_min，a_max]；

v[T]＝0x[T]＝x_f；

k＝k₀，...T-1；

上式表示机器人和障碍物避碰，当障碍物为静态时，ρ′的值为ρ，当障碍物为其他机器人时ρ′的值为2ρ。

结合上述的所有技术方案，本发明所具备的优点及积极效果为：本发明将使多机器人系统保持固定的队形从初始位置到达目标位置，当多机器人系统穿过一片障碍区，比如一道窄门时，集体避障的方法不再适用，系统需要变换为结构更小的队形才可以通过。针对此种情况，本发明的解决策略将根据可通行区域的宽度决定系统目标队形的结构和尺寸，代替机器人初始化时确定有限个队形的方法，这种控制方式将使系统有更好的环境适应性。另外，在队形变换中仍旧使用数学规划的方式让每个机器人寻找最优点，使其在该点形成目标队形，然后以此队形穿过障碍区。

本发明解决了多机器人编队从初始位置到达目标位置过程中若无法避开障碍物而需要穿过时，要让机器人能够变换为合理的队形，并且依旧采用实时分布式的方法控制机器人的问题。

附图说明

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

图1是本发明实施例提供的实时分布式多机器人队形控制方法流程图。

图2是本发明实施例提供的实时分布式多机器人队形控制系统的结构示意图；

图中：1、信息甄别和选择模块；2、障碍块处理互补模块。

图3是本发明实施例提供的分布式探测原理图。

图4是本发明实施例提供的分布式探测原理图。

图5是本发明实施例提供的伸缩比说明示意图。

图6是本发明实施例提供的恢复队形原理图。

图7是本发明实施例提供的目标点选择说明图。

图8是本发明实施例提供的同构变换仿真环境示意图。

图9是本发明实施例提供的同构变换仿真结果图。

图10是本发明实施例提供的异构变换仿真环境示意图。

图11是本发明实施例提供的异构变换仿真结果示意图。

图12是本发明实施例提供的异构变换形成目标队形示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

针对现有技术存在的问题，本发明提供了一种实时分布式多机器人队形控制方法、系统，下面结合附图对本发明作详细的描述。

如图1所示，本发明提供的实时分布式多机器人队形控制方法包括以下步骤：

S101：机器人对障碍物进行精确定位，当与其他机器人通信时需要将其探测到的情况通报，收到通报的机器人对收的信息和自己探测到的信息甄别和选择；

S102：机器人进行通信后检索到两障碍区的位置不同，则可以将其按两障碍块处理进行互补，分析后认为可通行区域为两障碍区间的空白区域；机器人定位的障碍区一致，可通行区域也是一致的。

本发明提供的实时分布式多机器人队形控制方法业内的普通技术人员还可以采用其他的步骤实施，图1的本发明提供的实时分布式多机器人队形控制方法仅仅是一个具体实施例而已。

如图2所示，本发明提供的实时分布式多机器人队形控制系统包括：

信息甄别和选择模块1，用于实现机器人对障碍物进行精确定位，当与其他机器人通信时需要将其探测到的情况通报，收到通报的机器人对收的信息和自己探测到的信息甄别和选择。

障碍块处理互补模块2，用于实现机器人进行通信后检索到两障碍区的位置不同，则可以将其按两障碍块处理进行互补，分析后认为可通行区域为两障碍区间的空白区域；机器人定位的障碍区一致，可通行区域也是一致的。

下面结合附图对本发明的技术方案作进一步的描述。

在多机器人系统中，若要变换队形，需要解决如下问题：机器人在何种情况下变换队形、需要变换为何种队形、如何进行队形的变换。在机器人的控制当中首先要实现的是自动化进而是智能化，而在自动化控制中机器人需要有明确的控制规则，保证其在确定的情况执行确定的策略。下面对上述问题分别阐述。

运动模型。在模型预测控制中，机器人可能在相邻采样点间呈现匀速运动、匀减速运动、匀加速运动，最优的速度控制方式需要根据当前环境而定。所以根据运动学方程，机器人两采样点间应满足如下关系：

v[k+1]＝v[k]+α[k]h (1-2)

v[k]∈[v_min，v_max] (1-3)

α[k]∈[α_min，α_max] (1-4)

k＝k₀，...T-1 (1-5)

在实际环境中障碍物的形状将会呈现不规则形，为了处理方便，本发明将不规则形状的障碍物建模为一个多边形，其模型为线性不等式{x|A_ox≤b_o}，其中

所以，为了防止与此类型障碍发生碰撞应该满足如下约束：

式中||A_o||.＝(||A_o，1||₂，...，||A_o，m||₂)^T。

综上，机器人不仅可以从当前位置到达下一个采样点的位置，还可以有效避开两采样点间的障碍物，并且可以针对不同障碍物做出最合适避障方式，另外，在进行规划时还考虑了最小化问题，保证相邻两采样点的耗能最小，在实时条件下保证局部最小化。所以，机器人的运动模型为一个数学规划模型3：

v[k+1]＝v[k]+a[k]h (3-3)

||x[k]-Q||₂≥ρ′ (3-4)

v[k]∈[v_min，v_max] (3-5)

a[k]∈[a_min，a_max] (3-6)

v[T]＝0x[T]＝x_f (3-7)

k＝k₀，...T-1 (3-8)

式(3-4)表示机器人和障碍物避碰，当障碍物为静态时，ρ′的值为ρ，当障碍物为其他机器人时ρ′的值为2ρ。

障碍物的识别与定位。如果把多机器人保持队形从初始位置到达目标位置看作一项任务，则在途中遇到障碍物可以看做在实现此项任务过程中无法预测的事件，属于一种中断，障碍物的不同将导致中断类型的不同。在图3中展示了两种不同的障碍物，针对图3(a)中的障碍物，系统可以采用集体避障的方法，保持队形不变。如若是图3(b)中的障碍物，机器人系统原本的队形将无法通过障碍区，集体避障方法不再适用，需变换为结构更小的队形才可以穿过障碍区，而本设计主要解决图3(b)所示的情况。在本发明中，假设机器人可以精确识别和定位上述两种障碍物，本发明的重点是要使多机器人系统根据障碍物的不同执行不同的算法实现不同的控制策略。

本设计采用分布式的控制方式，在对环境的监测方面也将是每个机器人独立探测然后进行通信。在探测周围环境方面，多机器人可能出现如图4的情况，图4(a)中机器人R₁和R₂分别探测了障碍区的一边，图4(b)中两机器人都可以探测障碍区的两边。机器人在进行决策之前要进行有效的通信，不论是图4(a)还是图4(b)的情况，需要机器人对障碍物进行精确定位，当与其他机器人通信时需要将其探测到的情况通报，收到此通报的机器人对收的信息和自己探测到的信息甄别和选择。当遇到图4(a)中的情况时，两机器人进行通信后检索到两障碍区的位置不同，则可以将其按两障碍块处理进行互补，分析后认为可通行区域为两障碍区间的空白区域；在图4(b)中，两机器人定位的障碍区一致，可通行区域也是一致的。

队形的选择。当机器人系统可以识别和定位障碍区后，研究的重点是机器人变换为何种队形，合理的队形不仅可以保证安全让机器人通过障碍区，还可以减少变换队形的耗能。在上述提到的集中控制方式中，机器人有规定的队形库，机器人可以变换的结构和尺寸都是有限的。比如当障碍区可通信区域的宽度恰好比机器人系统的宽度大一点时，由于队形库中队形样式的限制，机器人系统只能变换为纵队通过，此种变换方式会造成了很大的耗能浪费，因为这种情况下可以通过缩小队形的方法解决，在时间和耗能上减少浪费。

下面具体介绍伸缩比，如图5，机器人需要确定前方最大可通行宽度D_max，由于机器人有安全半径ρ的约束，所以D_max等于障碍区中可通行宽度减安全半径的二倍，构成的队形中，机器人前进方向上最大的宽度为D，则伸缩比：

由于机器人系统中要防止机器人之间的碰撞，机器人之间的距离也需要大于安全距离，所以伸缩比也有范围约束，当机器人之间的距离恰好是安全距离时，认定其伸缩比为d_min，故d≥d_min。

伸缩比d的取值范围可以分类为：d≥1、d＜1且d≥d_min、d＜d_min，与三种分类分别对应三种变换方式：零构变换、同构变换、异构变换。

零构变换：当d≥1时D_max≥D，说明障碍区中可通行区域的宽度远远大于机器人前进方向中最大的宽度，也即机器人可以保持原来的队形穿过障碍区，不需要队形变换。

同构变换：当d＜1时说明机器人保持原来的队形已经不能够通过障碍区，需要变换队形，而d≥d_min则表明可以穿行的宽度大于机器人安全距离，即机器人可以缩小自己宽度通过障碍区。此种变换方式机器人的队形基本保持原来的结构，只是整体队形的大小改变。为了节约耗能，在本发明中规定当机器人系统需要同构变换时，其前进方向上机器人之间的距离不改变，只是根据伸缩比变换其队形宽度为原来的d倍，所以，在以几何中心建立的坐标系下，各机器人进行同构变换后机器人横坐标不变，纵坐标变为原来的d倍。

异构变换：当d＜d_min时表明机器人系统在不改变原结构的情况下已经不能通过障碍区，需要变换为更小宽度的结构，比如矩形变换为线性。一般情况下当需要异构变换时均变换为线性，因为可通行的宽度已经不能够让两个机器人并排过，若可通行宽度更小时，机器人系统将无法通过，系统需寻找别的路径。

使用伸缩比可以用于区分环境，根据环境的不同选择不同的变换方式，这种对变换方式进行细分可以增加机器人系统变换的灵活性，机器人根据实际情况做出最合适的选择。这种控制方式可以使机器人的变化最小化，使整个系统节约耗能，恢复队形容易。

目标点的形成。当机器人系统确定了目标队形的结构和尺寸后，需要确定目标点使机器人系统在该点变换为目标队形，然后以目标队形穿过障碍区。因为本发明中采用分布式的控制方式，各机器人可以根据目标点独立地规划路径，在形成目标队形过程中各机器人互不干扰。当机器人系统需要变换队形时表明系统可以探测到障碍区，也即目标点的规划是根据探测到的实际情况而决策的。为了保证机器人可以安全便捷地通过障碍区，机器人目标队形的几何中心要落在目标位置和障碍区中点的连线上，即图5中的k_fm，这样可以使得机器人在变换队形后穿越障碍区时有最好的起始位置，使机器人在接下来的模型预测中的路径更加平滑。机器人系统要形成编队就要保证机器人在每一个采样点上具有相同的速度，包括大小和方向，为了让机器人能够在目标位置具有相同的速度，在本控制中要机器人到达目标位置时其速度为零，在到达目标点的过程中机器人之间的速度可能不同。

如果没有评估系统，机器人可以选取的目标点有许多，甚至是无数个，系统选取不同的点使得在整个变换队形的过程中性能不一样，所以为了找到最佳的目标点，需要有一个科学的评估系统，使得机器人在选取目标点上有所约束。目前有许多评估系统应用于机器人系统变换队形，基本从两个方面考虑：时间和路径长度，衍生出来的有变换队形收敛时间比和能量消耗率。因为本发明采用的模型预测控制可以在相邻两个采样点间调节机器人的速度，本发明将考虑运动学方程(5-1)-(5-3)：

v_t＝v₀+at (5-1)

v₀＝0 (5-3)

其中v₀表示机器人在目标点的速度，s是变换队形过程中初始点和目标点的直线距离，v_t是机器人在当前位置的速度，为已知量。从式中可以得到在变换队形过程中机器人的加速度和时间成反比，加速度和前行路程成反比，所以加速度和路程在评估因素方面地位是同等的。因为避障属于机器人在执行编队任务中的“中断”情况，为了保证机器人能够通过障碍区才需要变换队形，变换队形是执行任务中迫不得已的行为，所以需要其花费的时间尽可能小。另外，在机器人的前进过程中要尽量保证耗能最小，所以模型中使用加速度所耗费的时间作为评估因素。而由上边的推导知此两个因素成反比，所以要根据实际情况确定二者的权重。寻找目标点的优化模型(6-1)-(6-8)下：

s.t. k_fm＝k_ft (6-2)

v_t＝at_c (6-4)

s_c＝||P_m-P_t||₂ (6-5)

||P_m-O_α||₂≥ρ (6-6)

a∈[a_min，a_max] (6-8)

机器人到达目标点是一种速度从v_t降为0的运动过程，所以本发明将整个运动过程抽象为一个匀减速直线运动的模型进行近似评估。其中a和t分别表示机器人在此过程中的加速度和所花费的时间，w₁和w₂为其权重值。k_fm表示终点位置和可通行区中点连线的斜率，k_ft表示终点位置和目标位置连线的斜率。v_t为机器人在当前位置的速度，S_c为机器人从当前位置到达目标位置的直线距离，P_m和P_t分别表示目标位置和当前位置。3-6表示选中的目标点一定不能和障碍区发生碰撞，要与之保持安全距离ρ。由于变换队形花费的时间是可以变化的，所以一定要保证机器人能够在剩余的时间内到达终点，在模型中使用最大加速度进行预测，P_f表示终点位置，t_s表示到达目标点后剩余的时间。

上述模型使用数学规划的方法寻找机器人变换队形中的目标位置，在模型中平衡了耗能和时间，考虑了机器人前进中的避碰问题，还从全局的角度考虑了当前选择目标点对整个任务的影响。另外数学规划一方面需要迭代优化，可以和模型预测控制进行结合，另一方面，数学规划找出的目标位置是认为不确定的，但是约束条件可以使其找到最优的解，此种模型可以根据实际情况确定不同的值，具有很强环境适应能力。

队形的恢复。在多机器人系统的队形变换中选择合理的目标队形可以安全地通过障碍区，当机器人系统穿过障碍区后需要恢复到原来队形继续执行编队任务。

在原始队形变换为目标队形时，可以认为是两侧的机器人分别向对称轴移动相同的距离形成一个新的队形。在以几何中心建立的坐标系中，目标队形和原始队形相比纵向距离不变，横向距离变为原来的d倍，根据d可以计算出二者横向距离差Δ的具体值，那么在恢复队形时可根据横向距离差进行预测规划。

因为在编队中所有机器人的速度都是一致的，那么在穿过障碍区时也是如此，在恢复队形时可以让对称轴两侧的机器人向远离对称轴的方向分别移动相同的距离即可，这样不仅恢复了队形，机器人的速度还会保持一致，如图6(a)。为了保证机器人在恢复队形的过程中依然向终点前进，本发明中采用边前进边恢复队形的方案，如图6(b)，在有限个相同的采样点下，所有机器人的前进距离相等，在与前进方向垂直的线上选取Δ长度，二者进行矢量相加就可以恢复为原来的队形。如果队形经过异构变换形成线性队形后需要恢复队形时原理也一样。所以在机器人系统穿过障碍区收到恢复队形的信号时，在原本路径规划的基础上添加约束进行迭代使目标点符合上述条件。

控制策略。单个机器人只需要根据自己探测到的环境做出最优的决策，不需要考虑其他个体，当控制对象扩展为多机器人时，需要立足于整体考虑系统的性能。单个机器人的最优决策可能无法使所处的多机器人系统性能达到最优，当系统处于最优性能时单个机器人也不一定是最优性能。本发明研究对象是多机器人系统，所以要考虑系统的状态。在分布式控制中，各个机器人通信协商，根据所得信息自主决策，最终使整个系统达到最优状态，所以在多机器人系统的变换队形中需要确定一种规则，每个机器人在此种规则的约束下不仅可以变换到目标队形还可以保证变换过程中系统是最优的。

在确定目标点的模型中，期望目标队形的几何中心落在终点和可通行区域中点的连线上。使用分布式控制要使每个机器人依据在队形中所处的位置独立决策自己的目标点，最终使整个系统找到合适的目标位置。机器人根据自己在队形中与对称轴的相对位置确定目标队形中与对称轴的相对方向，根据当前队形宽度和伸缩比确定与对称轴的距离，两个确定量可以计算该机器人在目标队形中应该满足的直线方程，该机器人的目标点将会在该直线上产生。如图7，机器人R₁在对称轴的上侧，根据就近原则，其目标点在中轴线的上侧，根据伸缩比可以确定机器人目标点所处的直线k_fm1，则用k_fm1＝k_ft1替代模型6中的k_fm＝k_ft进行独立求解。

根据上边确定的直线方程再加上模型中提到的约束条件，每个机器人可以确定一个满足要求的目标点，由于各机器人之间没有协商约束，可能确定的目标点无法形成目标队形。为了找到所有机器人的最终目标点，机器人在初步规划目标点后通报自己规划的目标位置。由于目标队形的结构和大小都已经确定，所以机器人可以根据规划结果确定目标队形几何中心的位置坐标。这样，系统中每个机器人可以确定一个目标几何中心，而这些几何中心的坐标可能是一致也可能是不一致，但是对于每个机器人来说当前的规划结果都是最优的，为了平衡所有机器人的规划结果，在本发明中对所有机器人求解的几何中心位置坐标求取平均值作为目标几何中心。

当确定目标几何中心后机器人就可以确定自己的目标点，为了确保可行性，每个机器人需要验证目标点是否满足所有的约束条件。如果有机器人不能满足所有条件则需要在上次规划的基础上进行迭代重新寻找目标位置，直到满足所有约束条件。如果所有机器人满足所有的约束，为了保证一致性，系统中所有机器人在变换队形时应保证耗时一样，机器人在验证约束条件之后通报自己到达目标点的时长，所有机器人采用最长耗时作为自己变换队形的时间。由于本发明中采用模型预测控制，机器人可以根据实际时长在原本匀减速直线运动基础上做适当调整，保证在最长时长到达目标点。

当确定目标点后，各机器人根据运动模型3-4驱动机器人从初始位置到达目标位置，在此运动过程中仍旧采用分布式的控制方式，各机器人独立决策、独立执行，在相同时间到达目标点，并且在目标位置各机器人的速度均为零，为后面继续保持队形创造最优的初始条件。

算法步骤：

算法1：多机器人变换队形算法(以编号为i的机器人为例)

1.机器人i使用模型6求解目标位置

2.机器人i计算

所对应的几何中心

并进行广播

3.机器人i接收广播添加到集合P_{c_init}，其中

4.for k＝1 to N

5.根据相对位置关系计算目标位置

6.

goto 1

else

goto 7

7.机器人i广播到目标点所耗时间并且接收其他机器人的广播添加到集合T_m

8.对集合T_m中的耗时进行从大到小的排序sort(T_m)，并选取最大值T_{m_max}

9.机器人i依据模型3-4向目标点前进

-------------------------------------------------------------------

说明：上述算法中的第6步对系统确定出各机器人的目标位置进行验证，其中

表示机器人i使用模型6验证位置

是否满足所有约束条件，如果不满足约束则需要在本次规划基础上返回到第1步重新求解(goto 1)。同理，第9步中的

表示机器人i调用模型3-4，根据当前位置、速度、加速度以及所耗时长和目标位置驱动机器人前进进行变换队形。

-------------------------------------------------------------------

算法2：多机器人检测环境选择队形算法(以编号为i的机器人为例)

1.机器人i进行环境信息识别；

2.机器人i对识别的环境进行广播，并且接收其他机器人的信息；

3.机器人i甄别信息确定可通行区域的宽度L；

4.机器人i确定可通行宽度D_max＝L-2ρ；

5.

6.if d≥1

else if d_min≤d＜1

else

7.机器人i调用算法1

-------------------------------------------------------------------

说明：上述算法第6步中

表示机器人i调用算法3，其需要参数为机器人i的相对位置关系向量

与对称轴的相对位置关系

与对称轴的距离

以及伸缩比d。同理，

是调用算法4，其参数L_m为障碍区可通行区的中点位置。

-------------------------------------------------------------------

算法3：同构变换确定相对位置关系算法(以编号为i的机器人为例)

1.

2.

3.

4.

5.

6.

7.else

8.

9.

10.机器人i广播

11.机器人i接收广播添加到集合X_m

12.for k＝1 to N

13.

14.

15.

-------------------------------------------------------------------

说明：上述算法中用来更新机器人的相对位置关系，主要思路为以几何中心为圆心建立的坐标系中，各机器人与对称轴的相对位置关系不变，根据与对称轴的距离以及伸缩比确定各机器人新的位置，然后各机器人根据新的位置计算各自的相对位置关系向量(第1-9步)。

-------------------------------------------------------------------

算法4：异构变换确定相对位置关系算法(以编号为i的机器人为例)

1.机器人i求取当前位置和可通行区中点的距离

2.机器人i广播

并且接收其他机器人的广播，将信息添加到集合Dis_tm,并对Dis_tm中数据从小到大排序sort′(Dis_tm)

3.fork＝1 toN

map.push(k_i，i)4.fork＝1 toN

5.

else

-------------------------------------------------------------------

说明：上述算法主要针对零构变换和同构变换不能满足编队需求的情况，异构变换需要将所有机器人变换为一个线性队形，使机器人按顺序穿过障碍区。本算法中各机器人计算自己与障碍区可通行区中点的距离，按照距离远近进行排序形成新的队形，距离近的要排在前边，如果多个机器人与可通行区中点距离相等则各机器人按照编号进行排序。第3步将距离与机器人的编号组成键值对放在集合map中，第4步中计算各机器人与其余机器人的相对位置关系，map[k_i]-map[k_j]计算两机器人在排序中的位置差，在本算法中规定机器人在异构变换中各机器人距离为ρ，故乘以2ρ确定新队形中两机器人的距离，式中的k_fm表示机器人系统的新队形要在以几何中心为圆心所建立坐标系的x轴上，第5步中表示机器人确定与几何中心的相对位置关系，依据机器人数量N以及机器人在新队形中的排序进行分类。

-------------------------------------------------------------------

算法5：sort′(Dis_tm)算法

1.for k＝1 to N

for v＝k to N

2.for k＝1 to N

-------------------------------------------------------------------

说明：上述算法用于排序，主要服务于算法4，首先按距离排序，如果距离相等就按照编号排序。

下面结合仿真对本发明的技术效果组详细的描述，

使用MATLAB进行仿真实验，对上述提出的实时分布式多机器人的变换队形算法验证和分析。在本实验当中，整个任务等时间离散为多个采样点，相邻两采样点的时间步长为h＝0.1，传感半径为R_c＝1.2，最大速度为v_max＝5，机器人的安全半径为ρ＝0.2，则d_min的值为0.4。

同构变换实验。本仿真实验环境如图8，环境中有两机器人R_1和R_2，其初始位置分别为(0.5,4.5)、(0.5,3)，目标位置分别为(10,3)、(10,1.5)。机器人将保持初始的柱状队形从Start区域到达End区域，但是环境中存在障碍区，如图8中的黑色阴影部分，其可通行宽度为1.4，而初始队形中两机器人形成的队形宽度为1.5，显然，机器人系统需要变换为宽度更小的队形才可以穿过障碍区到达目标位置。机器人为了保证安全，在每一个位置需要与障碍物保持安全距离，在本实验中设为0.2，如图8中的虚线部分，所以，机器人可形成的最大队形宽度将变为1。本实验将利用算法4.1-4.5解决机器人R_1和R_2保持队形从初始位置到达目标位置，并且可以合理地处理障碍区约束的问题。

上述问题的仿真结果如图9，整个任务过程分为五个阶段A、B、C、D、E。在A阶段中，多机器人系统按照正常的队形保持过程从初始位置奔向目标位置。B阶段探测到障碍区，原本的集体避障方式已经不满足要求，机器人系统判定需要变换队形，首先调用算法2确定目标队形，根据探测到障碍区可通行宽度以及当前机器人队形宽度比确定伸缩比d＝1/1.5＝0.67，此时d_min≤d<1，机器人需要进行同构变换，调用算法4.3更新机器人间的相对位置关系，然后回到算法2的第7步调用算法1。两机器人根据模型6分别计算自己的目标点，然后根据相对位置关系确定各自的目标点所对应的几何中心，两机器人计算的几何中心求取平均值作为本次规划中确定的多机器人系统的目标位置。每个机器人可以根据新确定的几何中心计算自己的目标位置，此时的目标位置不一定满足所有的约束条件，故需要进行验证(算法1第6步)。如果两机器人的目标位置都满足约束条件，接下来应该确定变换队形需要的时间，如图9，在B阶段中，两机器人从B阶段起始端到B阶段终点端的距离不一样，也将导致两机器人变换队形耗时不一致，此时，需要执行算法1的7、8步找到最长耗时作为变换队形的时间，最后机器人将依据模型3进行路径规划驱动各机器人到达目标点形成目标队形。C阶段中多机器人系统保持新的队形穿过障碍区，当整个系统确定穿过障碍区后需要恢复为原队形，即D阶段，此阶段使用图6(b)所示原理，本实验中规定机器人在10个采样点内恢复为原队形。系统恢复原队形后继续执行队形保持任务，使用第三章所述算法向目标点前进，如图中的E阶段。

异构变换实验。上述实验中障碍区的宽度比较宽，机器人可以通过同构变换通过，当障碍区比较窄时，如图10，机器人的可通行宽度为0.3，可通过宽度已经不可以同时容纳两机器人并排通过，需要异构变换为一个线性队形以前后状态穿过障碍区。实验环境中除了障碍区宽度变窄外，其余条件与同构变换中的相同。实验结果如图11，该实验过程也分为A、B、C、D、E五个阶段。A阶段的过程与同构变换一致，在B阶段中经过算法2第6步判定后，机器人执行算法4，两机器人计算与障碍区可通行区中点的距离，距离近的排在前边，很明显机器人R_2比R_1更接近中点位置，故系统更新相对位置关系时在以几何中心建立的坐标系中，R_2在x轴的正半轴，R_1以相同的距离在x轴的负半轴。然后调用算法1计算各自的目标位置，并且驱动机器人到达目标位置形成目标队形，如图12。在C阶段两机器人将保持新队形从中轴线穿过障碍区，当整个系统均穿过障碍区后需要恢复为原来的队形状态，即D阶段，实施过程与上述实验一致，剩余的E阶段将继续使用第三章的队形保持算法，使其到达终点位置。

本发明设置仿真实验对本发明提出多机器人系统变化队形的算法进行验证，实验中涉及了同构变换和异构变换。从实验结果看，机器人可以根据实际探测到的环境做出最小的变换，在保证原本结构的前提下缩小队形尺寸。另外，各机器人相互独立，只有信息交换，符合完全分布式的特征，证明了本发明提出的实时分布式变换队形算法的有效性。

在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上；术语“上”、“下”、“左”、“右”、“内”、“外”、“前端”、“后端”、“头部”、“尾部”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”等仅用于描述目的，而不能理解为指示或暗示相对重要性。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。

Claims (5)

1.一种实时分布式多机器人队形控制方法，其特征在于，所述实时分布式多机器人队形控制方法包括：

机器人对障碍物进行精确定位，当与其他机器人通信时需要将其探测到的情况通报，收到通报的机器人对收的信息和自己探测到的信息甄别和选择；

机器人进行通信后检索到两障碍区的位置不同，则将其按两障碍块处理进行互补，分析后认为可通行区域为两障碍区间的空白区域；机器人定位的障碍区一致，可通行区域也是一致的。

2.如权利要求1所述的实时分布式多机器人队形控制方法，其特征在于，所述实时分布式多机器人队形控制方法的队形选择，机器人确定前方最大可通行宽度D_max于机器人有安全半径ρ的约束，所以D_max等于障碍区中可通行宽度减安全半径的二倍，构成的队形中，机器人前进方向上最大的宽度为D，则伸缩比：

当机器人之间的距离恰好是安全距离时，认定其伸缩比为d_min，d≥d_min；

伸缩比d的取值范围分类为：d≥1、d＜1且d≥d_min、d＜d_min，与三种分类分别对应三种变换方式：零构变换、同构变换、异构变换；

零构变换：当d≥1时D_max≥D，说明障碍区中可通行区域的宽度远远大于机器人前进方向中最大的宽度；

同构变换：当d＜1时说明机器人保持原来的队形已经不能够通过障碍区，需要变换队形，而d≥d_min则表明可以穿行的宽度大于机器人安全距离，即机器人缩小自己宽度通过障碍区；

异构变换：当d＜d_min时表明机器人系统在不改变原结构的情况下已经不能通过障碍区。

3.如权利要求1所述的实时分布式多机器人队形控制方法，其特征在于，所述实时分布式多机器人队形控制方法机器人目标队形的几何中心落在目标位置和障碍区中点的连线上，采用的模型预测控制在相邻两个采样点间调节机器人的速度，将考虑运动学方程：

v_t＝v₀+at；

v₀＝0；

其中v₀表示机器人在目标点的速度，s是变换队形过程中初始点和目标点的直线距离，v_t是机器人在当前位置的速度，为已知量；根据实际情况确定权重，寻找目标点的优化模型：

s.t. k_fm＝k_ft；

v_t＝at_c；

s_c＝||P_m-P_t||₂；

||P_m-O_α||₂≥ρ；

a∈[a_min，a_max]；

机器人到达目标点是速度从v_t降为0的运动过程，其中a和t分别表示机器人在此过程中的加速度和所花费的时间，w₁和w₂为其权重值，k_fm表示终点位置和可通行区中点连线的斜率，k_ft表示终点位置和目标位置连线的斜率，v_t为机器人在当前位置的速度，S_c为机器人从当前位置到达目标位置的直线距离，P_m和P_t分别表示目标位置和当前位置，P_f表示终点位置，t_s表示到达目标点后剩余的时间。

4.如权利要求1所述的实时分布式多机器人队形控制方法，其特征在于，所述实时分布式多机器人队形控制方法在确定目标点的模型中，期望目标队形的几何中心落在终点和可通行区域中点的连线上，使用分布式控制要使每个机器人依据在队形中所处的位置独立决策自己的目标点，最终使整个系统找到合适的目标位置，机器人根据自己在队形中与对称轴的相对位置确定目标队形中与对称轴的相对方向，根据当前队形宽度和伸缩比确定与对称轴的距离，两个确定量可以计算该机器人在目标队形中应该满足的直线方程，该机器人的目标点将会在该直线上产生，根据伸缩比可以确定机器人目标点所处的直线k_fm1，则用k_fm1＝k_ft1替代模型中的k_fm＝k_ft进行独立求解；

根据上边确定的直线方程再加上模型中提到的约束条件，每个机器人确定一个满足要求的目标点，为了找到所有机器人的最终目标点，机器人在初步规划目标点后通报自己规划的目标位置；对所有机器人求解的几何中心位置坐标求取平均值作为目标几何中心；

当确定目标几何中心后机器人确定自己的目标点，当确定目标点后，各机器人根据运动模型驱动机器人从初始位置到达目标位置，在此运动过程中采用分布式的控制方式，各机器人独立决策、独立执行，在相同时间到达目标点，并且在目标位置各机器人的速度均为零，为后面继续保持队形创造最优的初始条件。

5.一种运行权利要求1～4任意一项所述实时分布式多机器人队形控制方法的实时分布式多机器人队形控制系统，其特征在于，所述实时分布式多机器人队形控制系统包括：

信息甄别和选择模块，用于实现机器人对障碍物进行精确定位，当与其他机器人通信时需要将其探测到的情况通报，收到通报的机器人对收的信息和自己探测到的信息甄别和选择；

障碍块处理互补模块，用于实现机器人进行通信后检索到两障碍区的位置不同，则将其按两障碍块处理进行互补，分析后认为可通行区域为两障碍区间的空白区域；机器人定位的障碍区一致，可通行区域也是一致的。

Images