CN111103897A - 一种障碍环境下多机器人的编队控制方法和*** - Google Patents

Abstract

本发明公开了一种障碍环境下多机器人的编队控制方法和***，包括定时接收各机器人的状态数据包，判断领航机器人是否发生故障，若领航机器人未发生故障则根据预设的编队指令确定各个跟随机器人当前的目标方位，并计算线速度和角速度，发送给对应的跟随机器人；否则根据领航机器人当前的位置坐标进行膨胀化处理并更新已知的障碍物坐标，根据更新的已知的障碍物坐标计算最大可通行间距，若最大可通行间距无法满足跟随机器人通过，则跟随机器人返航；否则根对跟随机器人重新进行编队。本发明解决了多机器人在复杂地形中领航机器人出现故障时无法继续运行的问题，大大减少***对领航机器人的依赖，显著提高了在障碍环境下***的鲁棒性和可靠性。

Description

一种障碍环境下多机器人的编队控制方法和***

技术领域

本申请属于机器人控制领域，具体涉及一种障碍环境下多机器人的编队控制方法和***。

背景技术

随着技术的革新和机器人应用领域的不断拓展，越来越多的人加入到多机器人***的研究，多机器人***也得到了飞速的发展。采用多个结构和传感能力相对简单的单个机器人组成多机器人协作***，多机器人协作***在节省机器人结构设计成本的同时也增加了可靠性。多机器人协作是多机器人研究的热点问题之一，其基本问题包括编队控制、地图构建、无人救灾以及协同搬运等，其中编队控制是研究其他问题的前提。多机器人协作目前在工业、军事、国防和生活领域都有很大的应用。

多机器人编队控制是指多机器人***依赖传感器感知周围环境和自身状态，相互协作完成编队，在存在障碍物的环境中实现面向目标的自主运动。在自然界中经常能见到编队行为，例如鱼群结队游行，大雁列队飞行，狼群编队捕食等。随着科技的进步，越来越多的人类活动也使用到了编队行为，例如航母军舰混合编队，飞机机群编队。目前，多机器人编队控制主要的方法有：基于行为法、人工势场法、领航跟随法、虚拟结构法以及分布式控制算法。

机器人代替人类工作在保护人类人身安全方面有重要意义，随着机器人技术和多机器人编队控制技术日趋成熟，将多机器人***运用到复杂环境成为可能，比如救灾现场、军事前线等环境。复杂环境有地形不规则，障碍物多，信号干扰大等特点，不仅考验机器人的性能更加考验出现意外情况时的应对策略是否完备。因此需要一种在复杂环境中机器人出现故障的应对方法。

发明内容

本申请的目的在于提供一种障碍环境下多机器人的编队控制方法和***，解决了多机器人在复杂地形中领航机器人出现故障时无法继续运行的问题，大大减少***对领航机器人的依赖，显著提高了在障碍环境下***的鲁棒性和可靠性。

为实现上述目的，本申请所采取的技术方案为：

一种障碍环境下多机器人的编队控制方法，所述多机器人包括一个领航机器人和若干个跟随机器人，所述障碍环境下多机器人的编队控制方法用于在已知障碍环境中障碍物坐标的情况下根据预设的编队指令控制多机器人运行，各机器人带有唯一的ID号，所述障碍环境下多机器人的编队控制方法，包括：

步骤S1、定时接收各机器人的状态数据包，所述状态数据包包括当前机器人的位置坐标和航向角度，根据领航机器人的状态数据包，判断所述领航机器人是否发生故障，若领航机器人未发生故障则执行步骤S2；否则执行步骤S3；

步骤S2、根据预设的编队指令确定各个跟随机器人当前的目标方位，结合各跟随机器人当前的状态数据包和当前的目标方位计算线速度和角速度，并由线速度和角速度生成指令数据包发送给对应的跟随机器人，重新执行步骤S1；

步骤S3、根据领航机器人当前的位置坐标进行膨胀化处理，利用膨胀化处理后得到的坐标更新已知的障碍物坐标，并执行步骤S4；

步骤S4、根据步骤S3中更新的已知的障碍物坐标计算最大可通行间距，若最大可通行间距无法满足跟随机器人通过，则控制跟随机器人返航；否则根据更新的已知的障碍物坐标针对跟随机器人重新进行编队，并重新执行步骤S1。

作为优选，所述编队指令由矩阵F^d表示，表达式如下：

F^d＝[F_s1 F_s2 F_s3 …]_4×n

F_sn＝[f_1n f_2n f_3n f_4n]^T,n＝1,2,3,…

其中，F^d表示所有机器人整体的编队指令，F_sn表示第n个机器人的状态信息，f_1n表示第n个机器人的ID号，f_2n表示第n个机器人跟随的领航机器人的ID号，如果该机器人为领航机器人则f_2n设为0，f_3n表示第n个机器人与领航机器人之间的期望距离，如果该机器人为领航机器人则f_3n设为0，f_4n表示第n个机器人与领航机器人之间的期望航向角度，如果该机器人为领航机器人则f_4n设为0。

作为优选，所述判断领航机器人是否发生故障，包括：

若未定时接收到领航机器人的状态数据包，则判断领航机器人的通讯模块出现故障，领航机器人停止运行；

若接收到领航机器人的状态数据包中包含故障信息，则判断领航机器人出现故障，领航机器人停止运行；

若领航机器人的位置坐标在预设时间内的变化量小于阈值，则判断领航机器人出现故障，领航机器人停止运行；

否则领航机器人未出现故障。

作为优选，所述根据预设的编队指令确定各个跟随机器人当前的目标方位，包括：

所述目标方位包括期望位置和期望航向角；

计算跟随机器人的期望位置(x_v,y_v)和期望航向角θ_v公式如下：

θ_v＝θ_l

其中，x_l和y_l为领航机器人的位置坐标，θ_l为领航机器人的航向角度，

表示领航机器人与当前待计算的跟随机器人之间的期望距离，

表示领航机器人与当前待计算的跟随机器人之间的期望航向角度。

作为优选，所述根据领航机器人当前的位置坐标进行膨胀化处理，利用膨胀化处理后得到的坐标更新已知的障碍物坐标，包括：

S3.1、获取预设的位置膨胀化处理的参数R；

S3.2、确定领航机器人故障时的位置坐标(x_l,y_l)；

S3.3、以位置坐标(x_l,y_l)为圆心、R为半径进行膨胀化，得到一个集合Q，表达式如下：

Q＝{x,y|(x-x_l)²+(y-y_l)²≤R²}

S3.4、将所述集合Q作为障碍物，并将集合Q内的坐标加入到已知的障碍物坐标中，更新已知的障碍物坐标。

作为优选，所述根据步骤S3中更新的已知的障碍物坐标计算最大可通行间距，包括：

S4.1、读取原先的已知的障碍物坐标，记为[(x_z1,y_z1),(x_z2,y_z2),…,(x_zn,y_zn)]和领航机器人膨胀化的集合Q；

S4.2、计算最大可通行间距D_s，D_s的表达式为：

其中，x_l,y_l领航机器人故障时的位置坐标，R为膨胀化半径。

作为优选，所述若最大可通行间距无法满足跟随机器人通过，则控制跟随机器人返航；否则根据更新的已知的障碍物坐标针对跟随机器人重新进行编队，包括：

S4.3、将最大可通行间距D_s与当前跟随机器人队列的宽度D、同类队形的最小宽度D_m和单个机器人的可通过宽度D_r相比较；

S4.4、若D≤D_s，跟随机器人按照当前队形通过，并按照ID号就近原则修改编队指令；

S4.5、若D_m≤D_s<D，跟随机器人按照原有的队形通过，通过减小期望距离或者期望航向角度缩小队列宽度，按照ID号就近原则修改编队指令；

S4.6、若D_r≤D_s<D_m，跟随机器人将队形更换为柱形队形通过，按照ID号就近原则修改编队指令；

S4.7、若D_s<D_r，跟随机器人无法通过，跟随机器人返航。

本申请还提供一种障碍环境下多机器人的编队控制***，所述障碍环境下多机器人的编队控制***包括服务器和多个机器人，多个机器人包括一个领航机器人和若干个跟随机器人，各机器人带有唯一的ID号，服务器建立与各个机器人的socket连接；

所述服务器包括存储器和处理器，所述存储器中储存有计算机程序，所述处理器执行所述计算机程序实现上述任一项技术方案所述的障碍环境下多机器人的编队控制方法。

本申请提供的障碍环境下多机器人的编队控制方法和***，定时判断领航机器人的故障信息，根据领航机器人是否故障设置有两套运行方案，以应对领航机器人故障的情况，并且同时完成各个机器人的避障运行，解决了多机器人在复杂地形中领航机器人出现故障时无法继续运行的问题，大大减少***对领航机器人的依赖，显著提高了在障碍环境下***的鲁棒性和可靠性。

附图说明

图1为本申请障碍环境下多机器人的编队控制方法的流程图；

图2为本申请判断领航机器人是否故障的流程图；

图3为本申请确定跟随机器人当前的目标方位的一种状态示意图；

图4为本申请对故障领航机器人的位置进行膨胀化处理的示意图；

图5a～5c为本申请对故障后计算最小可通过距离和修改编队指令的示意图；

图6为本申请服务器与机器人的信息交互示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本申请。

如图1所示，其中一个实施例中，提供一种障碍环境下多机器人的编队控制方法，用于在已知障碍环境中障碍物坐标的情况下根据预设的编队指令控制多机器人运行。

本实施例中的多机器人包括一个领航机器人和若干个跟随机器人，并且各机器人带有唯一的ID号。具体的，障碍环境下多机器人的编队控制方法，包括：

步骤S1、定时接收各机器人的状态数据包，所述状态数据包包括当前机器人的位置坐标和航向角度，根据领航机器人的状态数据包，判断所述领航机器人是否发生故障，若领航机器人未发生故障则执行步骤S2；否则执行步骤S3。

需要说明的是，状态数据包为机器人工作状态下发送，在一实施例中，为了实时获取机器人是否在线或是否在指定区域内，各机器人还实时(包括工作状态和非工作状态)发送心跳包。

如图2所示，判断领航机器人是否发生故障，包括：

1)若未定时接收到领航机器人的状态数据包，则判断领航机器人的通讯模块出现故障，领航机器人停止运行；领航机器人的通讯模块出现故障时，无法接收外界发送的信息，即领航机器人在规定时间内没有接收到外界的信息后，即判断自身出现故障，则会停止运行。正如上述若未定时接收到领航机器人的状态数据包或心跳包时，也判断领航机器人出现故障。

2)若接收到领航机器人的状态数据包中包含故障信息，则判断领航机器人出现故障，领航机器人停止运行；领航机器人中预设有自检程序，若自检程序检测领航机器人某模块故障，则将故障信息发送并停止自身的运行。

3)若领航机器人的位置坐标在一定时间内的变化量小于阈值，则判断领航机器人可能由于外界环境的干扰出现故障，则判断领航机器人出现故障，并命令领航机器人停止运行。

4)否则领航机器人未出现故障，即未出现上述情况则判断领航机器人正常运行。

步骤S2、根据预设的编队指令确定各个跟随机器人当前的目标方位，结合各跟随机器人当前的状态数据包和当前的目标方位计算线速度和角速度，并由线速度和角速度生成指令数据包发送给对应的跟随机器人，重新执行步骤S1。

其中，所述编队指令由矩阵F^d表示，表达式如下：

F^d＝[F₂₁ F_s2 F_s3…]_4×n

F_sn＝[f_1n f_2n f_3n f_4n]^T,n＝1,2,3,…

其中，F^d表示所有机器人整体的编队指令，F_sn表示第n个机器人的状态信息，f_1n表示第n个机器人的ID号，f_2n表示第n个机器人跟随的领航机器人的ID号，如果该机器人为领航机器人则f_2n设为0，f_3n表示第n个机器人与领航机器人之间的期望距离，如果该机器人为领航机器人则f_3n设为0，f_4n表示第n个机器人与领航机器人之间的期望航向角度，如果该机器人为领航机器人则f_4n设为0。

编队指令为根据当前运行需要预先设置的，若领航机器人未出现故障，则控制各个机器人按照预设的编队指令运行，如图3所示，控制跟随机器人按照预设的编队指令运行的步骤如下：

根据预设的编队指令确定各个跟随机器人当前的目标方位，目标方位包括期望位置和期望航向角；以图3中所示的位置关系为例计算跟随机器人的目标方位，图3中领航机器人处于位置L，并且领航机器人的位置坐标为(x_l,y_l)，领航机器人的航向角为θ_l，待计算的跟随机器人当前实际处于位置F，并且当前的位置坐标为(x_f,y_f)和航向角为θ_f，待计算的跟随机器人期望所处的位置为V，并且期望位置坐标为(x_v,y_v)和期望航向角θ_v。D_lf表示领航机器人与跟随机器人实际距离，

表示领航机器人与跟随机器人之间的实际的航向角度，即为领航机器人的运动方向与领航机器人和实际跟随机器人连线的方向之间所形成的夹角。

则计算跟随机器人的期望位置(x_v,y_v)和期望航向角θ_v公式如下：

θ_v＝θ_l

其中

表示领航机器人与当前待计算的跟随机器人之间的期望距离，

表示领航机器人与当前待计算的跟随机器人期望之间的航向角度，即为领航机器人的运动方向与领航机器人和期望跟随机器人连线的方向之间所形成的夹角。

计算得到跟随机器人的当前的目标方位后，即可结合各跟随机器人当前的状态数据包计算线速度和角速度，并将线速度和角速度发送至跟随机器人，完成跟随机器人的运行控制。

当完成本次运行控制后，重新执行步骤S1，即继续定时接收各机器人的状态数据包，以及重新判断领航机器人是否出现故障。该方法可定时检测领航机器人的状态，以便于及时应对突变情况，将各机器人的运行损失将至最低。

需要说明的是，本实施例采用PID控制算法根据当前位置坐标、当前航向角度和目标方位计算需要跟随机器人执行的线速度和角速度，所采用的PID控制算法为现有的计算算法，即中间的计算过程不属于本申请的改进重点，不再进一步详述。

步骤S3、根据领航机器人当前的位置坐标进行膨胀化处理，利用膨胀化处理后得到的坐标更新已知的障碍物坐标，并执行步骤S4。

由于领航机器人故障后，相对于其他机器人而言此时的领航机器人同等于障碍物，考虑到位置坐标为点，而领航机器人具有一定体积，故需要根据领航机器人当前的位置坐标扩展一个适宜的范围，以便于其他机器人的正常避障，在一个实施例中，如图4所示，提供的膨胀化处理具体如下：

S3.1、获取预设的位置膨胀化处理的参数R。

S3.2、确定领航机器人故障时的位置坐标(x_l,y_l)。

S3.3、以位置坐标(x_l,y_l)为圆心、R为半径进行膨胀化，得到一个集合Q，表达式如下：

Q＝{x,y|(x-x_l)²+(y-y_l)²≤R²}

S3.4、将以上集合Q作为障碍物，并将集合Q内的坐标加入到已知的障碍物坐标中，更新已知的障碍物坐标。

步骤S4、根据步骤S3中更新的已知的障碍物坐标计算最大可通行间距，若最大可通行间距无法满足跟随机器人通过，则控制跟随机器人返航；否则根据更新的已知的障碍物坐标针对跟随机器人重新进行编队，并重新执行步骤S1。

根据最大可通行间距控制跟随机器人运行过程具体如下：

S4.1、读取原先的已知的障碍物坐标，记为[(x_z1,y_z1),(x_z2,y_z2),…,(x_zn,y_zn)]和领航机器人膨胀化的集合Q；

S4.2、计算最大可通行间距D_s，D_s的表达式为：

其中，x_l,y_l领航机器人故障时的位置坐标，R为膨胀化半径。

S4.3、将最大可通行间距D_s与当前跟随机器人队列的宽度D、同类队形的最小宽度D_m和单个机器人的可通过宽度D_r相比较；

S4.4、如图5a所示，若D≤D_s，跟随机器人按照当前队形通过，并按照ID号就近原则修改编队指令，例如原先的编队指令中，第1个机器人为领航机器人，当该机器人故障后，按照就近原则指定第2个机器人作为新的第1个机器人，即为领航机器人，即将原先的编队指令F_s1＝[R001 0 0 0]修改成F_s1＝[R002 0 0 0]，同时将原先的第3个机器人作为新的第2个机器人，即将原先的编队指令F_s2＝[R002 R001 L^d 5π/4]修改成F_s2＝[R003 R002 L^d 5π/4]，以此类推，即按照将后一个机器人作为新的前一个机器人的原则修改编队指令。

S4.5、如图5b所示，若D_m≤D_s<D，跟随机器人按照原有的队形通过，但要缩小队列宽度，按照ID号就近原则修改编队指令，通过减小期望距离或者期望航向角度缩小队列宽度，例如将编队指令由F_s1＝[R001 0 0 0]修改成F_s1＝[R002 0 0 0]，F_s2＝[R002 R001 L^d 5π/4]修改成F_s2＝[R003 R002 l^d 5π/4](l^d为队伍可通过的最大长度)，以此类推，即按照将后一个机器人作为新的前一个机器人，同时减小期望距离或期望航向角度的原则修改编队指令。

S4.6、如图5c所示，若D_r≤D_s<D_m，跟随机器人将队形更换为柱形队形通过，按照ID号就近原则修改编队指令，例如将编队指令由F_s1＝[R001 0 0 0]修改成F_s1＝[R002 0 00]，F_s2＝[R002 R001 L^d 5π/4]修改成F_s2＝[R003 R002 L^d π],以此类推，即按照将后一个机器人作为新的前一个机器人，同时设定期望航向角度为π的原则修改编队指令。

S4.7、若D_s<D_r，跟随机器人无法通过，跟随机器人返航。

在修改编队指令后，新的领航机器人依靠自身的摄像头或雷达建图并导航至目标点，新的跟随机器人按照新的编队指令接收相应的线速度和角速度后继续行驶。

本实施例提供的障碍环境下多机器人的编队控制方法，定时判断领航机器人的故障信息，根据领航机器人是否故障设置有两套运行方案，以应对领航机器人故障的情况，并且同时完成各个机器人的避障运行，解决了多机器人在复杂地形中领航机器人出现故障时无法继续运行的问题，大大减少***对领航机器人的依赖，显著提高了在障碍环境下***的鲁棒性和可靠性。

在另一个实施例中，提供一种障碍环境下多机器人的编队控制***，所述障碍环境下多机器人的编队控制***包括服务器和多个机器人，多个机器人包括一个领航机器人和若干个跟随机器人，各机器人带有唯一的ID号，服务器建立与各个机器人的socket连接，如图6所示，服务器与各个机器人建立连接后，机器人定时发送位置坐标，航向角度，故障信息和心跳包等信息给服务器，服务器定时发送心跳包给领航机器人，同时服务器定时发送角速度，线速度，心跳包等信息给各个跟随机器人，实现机器人运行的实施监控。

其中，服务器包括存储器和处理器，所述存储器中储存有计算机程序，所述处理器执行所述计算机程序实现所述的障碍环境下多机器人的编队控制方法。

为了便于各机器人的控制运行，在另一个实施例中，通常设置领航机器人由摄像头模块，工控机模块，主控模块，电源模块，电机驱动模块等模块组成。领航机器人根据当前环境的地图规划路径向目标点移动并实时给服务器云平台发送自己的位置坐标和航向角度。

跟随机器人由摄像头模块，工控机模块，主控模块，电源模块，电机驱动模块等模块组成。跟随机器人根据服务器云平台发送的线速度和角速度跟随领航机器人运行，并实时给服务器云平台发送自己的位置坐标和航向角度。

服务器还包括通讯模块和编队模块，通讯模块与各个机器人建立连接接受各个机器人的状态数据包，编队模块计算出跟随机器人的目标方位并解算成线速度和角速度发送给跟随机器人。

关于障碍环境下多机器人的编队控制***的具体限定可参见上述对于障碍环境下多机器人的编队控制方法的限定，在此不再进行赘述。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (8)

1.一种障碍环境下多机器人的编队控制方法，所述多机器人包括一个领航机器人和若干个跟随机器人，其特征在于，所述障碍环境下多机器人的编队控制方法用于在已知障碍环境中障碍物坐标的情况下根据预设的编队指令控制多机器人运行，各机器人带有唯一的ID号，所述障碍环境下多机器人的编队控制方法，包括：

步骤S1、定时接收各机器人的状态数据包，所述状态数据包包括当前机器人的位置坐标和航向角度，根据领航机器人的状态数据包，判断所述领航机器人是否发生故障，若领航机器人未发生故障则执行步骤S2；否则执行步骤S3；

步骤S2、根据预设的编队指令确定各个跟随机器人当前的目标方位，结合各跟随机器人当前的状态数据包和当前的目标方位计算线速度和角速度，并由线速度和角速度生成指令数据包发送给对应的跟随机器人，重新执行步骤S1；

步骤S3、根据领航机器人当前的位置坐标进行膨胀化处理，利用膨胀化处理后得到的坐标更新已知的障碍物坐标，并执行步骤S4；

步骤S4、根据步骤S3中更新的已知的障碍物坐标计算最大可通行间距，若最大可通行间距无法满足跟随机器人通过，则控制跟随机器人返航；否则根据更新的已知的障碍物坐标针对跟随机器人重新进行编队，并重新执行步骤S1。

2.如权利要求1所述的障碍环境下多机器人的编队控制方法，其特征在于，所述编队指令由矩阵F^d表示，表达式如下：

F^d＝[F_s1 F_s2 F_s3…]_4×n

F_sn＝[f_1n f_2n f_3n f_4n]^T，n＝1，2，3，…

其中，F^d表示所有机器人整体的编队指令，F_sn表示第n个机器人的状态信息，f_1n表示第n个机器人的ID号，f_2n表示第n个机器人跟随的领航机器人的ID号，如果该机器人为领航机器人则f_2n设为0，f_3n表示第n个机器人与领航机器人之间的期望距离，如果该机器人为领航机器人则f_3n设为0，f_4n表示第n个机器人与领航机器人之间的期望航向角度，如果该机器人为领航机器人则f_4n设为0。

3.如权利要求1所述的障碍环境下多机器人的编队控制方法，其特征在于，所述判断领航机器人是否发生故障，包括：

若未定时接收到领航机器人的状态数据包，则判断领航机器人的通讯模块出现故障，领航机器人停止运行；

若接收到领航机器人的状态数据包中包含故障信息，则判断领航机器人出现故障，领航机器人停止运行；

若领航机器人的位置坐标在预设时间内的变化量小于阈值，则判断领航机器人出现故障，领航机器人停止运行；

否则领航机器人未出现故障。

4.如权利要求2所述的障碍环境下多机器人的编队控制方法，其特征在于，所述根据预设的编队指令确定各个跟随机器人当前的目标方位，包括：

所述目标方位包括期望位置和期望航向角；

计算跟随机器人的期望位置(x_v，y_v)和期望航向角θ_v公式如下：

θ_v＝θ_l

其中，x_l和y_l为领航机器人的位置坐标，θ_l为领航机器人的航向角度，

表示领航机器人与当前待计算的跟随机器人之间的期望距离，

表示领航机器人与当前待计算的跟随机器人之间的期望航向角度。

5.如权利要求1所述的障碍环境下多机器人的编队控制方法，其特征在于，所述根据领航机器人当前的位置坐标进行膨胀化处理，利用膨胀化处理后得到的坐标更新已知的障碍物坐标，包括：

S3.1、获取预设的位置膨胀化处理的参数R；

S3.2、确定领航机器人故障时的位置坐标(x_l，y_l)；

S3.3、以位置坐标(x_l，y_l)为圆心、R为半径进行膨胀化，得到一个集合Q，表达式如下：

Q＝{x，y|(x-x₁)²+(y-y_l)²≤R²}

S3.4、将所述集合Q作为障碍物，并将集合Q内的坐标加入到已知的障碍物坐标中，更新已知的障碍物坐标。

6.如权利要求5所述的障碍环境下多机器人的编队控制方法，其特征在于，所述根据步骤S3中更新的已知的障碍物坐标计算最大可通行间距，包括：

S4.1、读取原先的已知的障碍物坐标，记为[(x_z1，y_z1)，(x_z2，y_z2)，…，(x_zn，y_zn)]和领航机器人膨胀化的集合Q；

S4.2、计算最大可通行间距D_s，D_s的表达式为：

其中，x_l，y_l领航机器人故障时的位置坐标，R为膨胀化半径。

7.如权利要求6所述的障碍环境下多机器人的编队控制方法，其特征在于，所述若最大可通行间距无法满足跟随机器人通过，则控制跟随机器人返航；否则根据更新的已知的障碍物坐标针对跟随机器人重新进行编队，包括：

S4.3、将最大可通行间距D_s与当前跟随机器人队列的宽度D、同类队形的最小宽度D_m和单个机器人的可通过宽度D_r相比较；

S4.4、若D≤D_s，跟随机器人按照当前队形通过，并按照ID号就近原则修改编队指令；

S4.5、若D_m≤D_s＜D，跟随机器人按照原有的队形通过，通过减小期望距离或者期望航向角度缩小队列宽度，按照ID号就近原则修改编队指令；

S4.6、若D_r≤D_s＜D_m，跟随机器人将队形更换为柱形队形通过，按照ID号就近原则修改编队指令；

S4.7、若D_s＜D_r，跟随机器人无法通过，跟随机器人返航。

8.一种障碍环境下多机器人的编队控制***，其特征在于，所述障碍环境下多机器人的编队控制***包括服务器和多个机器人，多个机器人包括一个领航机器人和若干个跟随机器人，各机器人带有唯一的ID号，服务器建立与各个机器人的socket连接；

所述服务器包括存储器和处理器，所述存储器中储存有计算机程序，所述处理器执行所述计算机程序实现权力要求1至7任一项所述的障碍环境下多机器人的编队控制方法。

Images