CN102096415B - 基于Ad-Hoc网络和leader-follower算法的多机器人编队方法 - Google Patents

Abstract

本发明请求保护一种多机器人编队方法，属于智能控制领域。本发明以leader运动轨迹控制整个编队运动轨迹，首先确定leader的运动学模型，由斥力与引力合力确定leader运动方向；建立follower跟踪leader运动模型，follower根据一定的距离与角度跟踪leader，根据人工势场法建立的运动模型确定follower运动轨迹；在leader与follower之间引入AdHoc网络，建立信息反馈，确保follower对leader的跟踪过程无丢失。采用本发明可以使多机器人系统在完成任务的过程中，顺利避开障碍物，到达目标点，并能在整个过程中保持初始队形，能实现多机器人的实时的队形控制，更好的完成一些需要多个机器人同时完成任务的场合（如搬运，救援等）。

Description

基于Ad-Hoc网络和leader-follower算法的多机器人编队方法

技术领域

本发明属于智能控制领域，具体涉及多机器人编队方法。

背景技术

自从20世纪50年代第一台工业机器人诞生以来，随着通信、计算机、传感、电子、控制等技术以及人工智能技术的快速发展，集多种先进技术为一体的机器人技术在市场需求的牵引下已经取得了很大的发展。单个的移动机器人所能获得的信息有限，能够完成的任务以及对信息的搜集能力也是有限的。随着研究的深入，单个移动机器人在某些应用上，显得力不从心，为了解决这类问题，人们考虑在目前的机器人技术水平条件下，采用多个机器人相互协作以弥补单个机器人能力的不足，多机器人系统(multi-robot system)的研究在此需求下应运而生。随着机器人的研究和应用领域的不断扩大，应用的需求也对多机器人系统的研究提出了更高的要求，为了更好地完成任务，多个机器人协作完成任务也逐渐成为研究的热点。编队控制则是一个具有典型性和通用性的多机器人协调问题，是多机器人协调问题的基础。

自1990年以来基于队形控制的多机器人系统受到众多研究者的广泛关注，美国研究机构对队形控制的研究比较早，Georgia Tech Mobile Robot Lab 结合DARPA的UGV(unmanned ground vehicle)Demo2研究了基于行为的队形控制方法。University of Southern California的Robotics Research Lab利用局部传感信息和交互进行队形控制。Brigham Young University的MAGICC实验室在Air Force Office of Scientific Research的协助下,对UAV(unmanned aerial vehicle)的队形作了深入研究。美国航空航天局NASA采用Enhanced formation flying(EFF)技术控制多个卫星,使得航天编队飞行及空间虚拟探测成为可能。但是，上述技术不能解决队形的实时控制问题，对于一些需要多个机器人同时完成任务的场合（如搬运，救援等）就不能很好的发挥多机器人的优势。 

发明内容

本发明所要解决的技术问题是，针对现有技术中，经过编队的多机器人在遇到障碍物时，很难保持实时队形控制的上述缺陷，提出一种实时控制队形的方法。

本发明实现多机器人队形的实时控制方法所采用的技术方案是，在多机器人组成的群体中，一个(或多个)机器人被指定为领航者（leader），其余机器人作为跟随者（follower），以leader为参考点，来确定其他follower的位置，这样将队形控制问题转化为follower 跟踪leader的位置和方向的问题，以此建立队形保持的模型。而leader则采用人工势场法建立运动模型，目标点或建议经过的区域对leader产生引力，而有障碍物的区域或不期望经过的区域对leader产生斥力，再根据leader与最远follower之间的距离的约束，设定避障区域，以此保证整个机器人队伍在避开障碍物的情况下，向目标位置前进。本发明还提出在leader与follower之间引入Ad Hoc网络，在编队过程中，将隐式通信与显示通信相结合，克服了leader与follower之间无反馈信息，导致机器人容易丢失的问题。

简洁地说，本发明是在多机器人组成的群体中，一个或多个机器人被指定为领航者leader，其余机器人作为跟随者follower，在leader-follower算法基础上利用人工势场法进行避障处理，由followers以一定的距离跟踪leader的位置和方向，使其达到预先设定的值，从而控制follower跟踪leader的轨迹以实现队形控制，并引入Ad Hoc网络在leader与follower之间建立信息反馈。所述方法分以下几步： 

首先建立leader的运动学模型（采用                                                

控制方法来保持队形，即通过控制两个机器人之间的相对距离

和相对角度

，使之达到一个恒定值（

），从而形成和保持一个良好的编队队形），以leader为参考点，来确定其他follower的位置，由斥力与引力合力确定leader运动方向； 

然后采用人工势场法建立follower跟踪leader运动模型（基本思想是构造目标位置引力场和障碍物周围斥力场共同作用的人工势场，搜索势函数的下降方向来寻找无碰撞路径），follower根据一定的距离与角度跟踪leader，根据运动模型确定follower运动轨迹；

最后在leader与follower之间引入Ad Hoc网络，在编队过程中，将隐式通信与显示通信相结合，建立信息反馈，确保follower对leader的跟踪过程无丢失。

本发明的有益效果：本发明是多机器人队形控制技术的研究，将leader-follower算法与人工势场法相结合，解决了leader-follower算法对动态环境的适应能力不强，不能很好的避开障碍物的问题。并引入Ad Hoc网络建立机器人之间的显示通信，实现了多机器人的实时队形控制，并且使多机器人系统能灵活的应对各种环境，使在一些需要多个机器人同时完成任务的场合（如搬运，救援等）能很好完成任务。

附图说明

图1本发明采用leader-follower算法和人工势场场法相结合的多机器人编队；

图2 本发明反馈控制结构图；

图3 本发明引入Ad Hoc网络编队策略流程图。

具体实施方式

leader-follower方法是将队形中的机器人划分为两个互补的角色leader和follower，群体中的某一个或某几个机器人被指定为领航者（leader）,其余作为它或它们的跟随者（followers），该方法的基本思想是，由followers以一定的距离跟踪leader的位置和方向，使其达到预先设定的值，从而控制follower跟踪leader的轨迹以实现队形控制。这样该编队控制算法就可以用控制理论的知识来分析处理，并能稳定跟踪误差。该方法控制简单，整个运动过程中，只有领航者知道任务信息，只需给定其行为或轨迹就可以控制整个机器人群体的行为，队形的运动轨迹完全由leader决定；但leader与follower之间相对独立，没有信息反馈，leader得不到follower的跟踪误差或丢失消息而且对动态环境的适应能力不强，机器人的避碰等问题没有得到很好的解决。

人工势场法它实际上是对机器人运动环境的一种抽象描述，其基本思想是借鉴物理学方面的概念,假设机器人在一个虚拟的力场环境中运动，环境中的障碍物对机器人产生排斥力，且斥力随机器人与障碍物的距离减少而增大，目标点对机器人产生吸引力, 引力随机器人与目标点的接近而变小，从而构造出斥力和引力共同作用的人工势场，在势场力的作用下机器人沿最小化势能的方向运动。人工势场法主要是通过设计人工势场和势场函数来表示环境对队形中各机器人约束,并在此基础进行分析和控制；该方法计算简单,便于实现实时控制，尤其对处理障碍物空间的避障问题比较有效。以leader-follower方法建立队形，采用人工势场法控制leader，在leader-follower保持队形的基础上结合人工势场法避障特性，克服了leader-follower算法的避障问题。

Ad Hoc网络是一种没有基础设施的对等无线通信网络，网络中没有中央控制设备，不需要固定设施如基站的支持，各节点自由接入，自动组网，没有固定的网络拓扑，且由于节点的移动性，拓扑结构处于动态变化中。同时，Ad Hoc网络是一个多跳网络，节点覆盖范围无须直接包含目的节点，而是借助于中间节点进行转发。Ad Hoc网络可以单独组网，也可以通过网关或接口与其他网络相连。Ad Hoc网络由于其自身的特点成为多机器人系统通信首选。将Ad Hoc网络引入leader-follower方法中，克服了leader-follower算法无信息反馈，跟随机器人容易丢失的缺陷。

以下针对附图和具体实例对本发明作具体描述：

如图1为本发明基于leader-follower算法与人工势场法的队形。当多机器人队形编好后，根据leader和离leader最远的一个follower的距离确定一个圆形区域，如图中以leader机器人为中心为半径的圆形区域，即避障区域。当障碍物在避障区域以外的时，整个机器人队伍中的成员将不会与障碍物发生碰撞，则障碍物不对领航机器人产生斥力。当障碍物在避障区域之内时，表示整个队伍不能顺利通过障碍物，障碍物对领航机器人产生斥力。其具体实现过程如下：

机器人robot1（即领航机器人）在运动过程中受到的总的势场力

是由目标点引力

和避障区域内障碍物产生的斥力两部分组成。计算目标引力

，

为目标位置，

代表机器人位置，代表机器人

与目标

之间的距离，则目标产生的引力势场

和相应的目标引力为

为

                           （1）

                        （2）

                   （3）

其中为正比例因子。显然，目标引力的大小与距离成线性关系，机器人到达目标位置后其目标引力大小为零。

如图1所示，障碍物Obstacle1对它产生斥力。障碍物Obstacle2对机器人产生的斥力为零。

，表示在避障区域内的第个障碍物的位置。

表示区域内第

个障碍物和机器人

之间的距离，则斥力的势场函数

为：

 （4）

其中

是正比例因子。因此相应的斥力函数为：

   （5）

                               （6）

领航机器人在避障区域内绕开障碍物，保证了整个机器人队伍在避开障碍物的情况下，向目标位置前进。

以下是针对上述模型引入Ad Hoc网络的详细描述，如图2为本发明的反馈控制示意图，具体的编队策略如下：

（1） 领航机器人(leader)了解全局信息，接收任务（记为T），根据任务规划其初始运动方向。初始时刻，由leader发送开始命令，所有的follower在收到命令后开始跟随leader运动。在运动过程中，用领航机器人用自身携带的传感器检测障碍物，并在一定范围内避开障碍物，再继续向目标前进。

（2） 跟随机器人的任务则为跟随领航机器人，它们将采用所提的队形模型（如三角形，正方形等模型），用自身携带传感器检测跟踪领航机器人，与leader之间保持一定的角度和距离来保持队伍队形，并在运动的同时监听来自leader的消息。

（3） 在正常情况下，leader与follower之间不进行通信，follower依靠自身所配备的传感器来检测leader，进行跟踪。在前进过程中，一旦follower发现无法检测到leader，follower会主动发出跟踪失败信息（记为S），表示跟踪失败，leader在收到信息后，首先会向所有的follower发送停止前进的命令，然后通过广播方式向所有follower发送包括位置信息在内的数据包（记为P），所有的follower会根据收到的信息来调整自己当前的位置，而丢失的follower则会根据收到的信息来确定自己下一步的位置，在丢失的follower跟上整个队伍后，才继续前进。

（4）由于Ad Hoc网络的通信距离有限，一旦机器人超出通信范围，机器人将无法收到来自leader的信息，而成为一个孤立的节点，为了保证所有机器人都在通信范围内，而且使follower能使用其传感器尽可能的检测到leader，我们将整个队伍设定为较小速度(200mm/s)运动。

在整个通信过程中，机器人以打包的形式向其他机器人发送数据包(pack)，在整个机器人队伍中，leader与follower都有自己唯一的ID，在网络环境中有唯一的IP地址与之对应。跟随机器人以领航机器人为参考点，以决定其下一步的位置，因此在领航机器人发送的信息发送的数据包包括五个元素，其中：

：表示机器人

的ip地址；

：表示机器人是否为领航机器人，0为领航机器人，1为跟随者；

：分别表示机器人所在位置的x，y轴坐标；

：表示当前位置与x轴的夹角。

本发明在leader-follower算法基础上利用人工势场法进行避障处理，并引入Ad Hoc网络在leader与follower之间建立信息反馈，实验表明，该方法有效可行，对于多机器人系统完成在复杂环境下完成任务也取得了较好效果。 

Claims (3)

1.一种基于Ad Hoc网络和leader-follower算法的多机器人编队控制方法，其特征在于，所述方法是在多机器人组成的群体中，一个或多个机器人被指定为领航机器人leader，其余机器人作为跟随机器人follower，在leader-follower算法基础上利用人工势场法进行避障处理，由跟随机器人以一定的距离跟踪领航机器人的位置和方向，使其达到预先设定的值，从而控制跟随机器人跟踪领航机器人的轨迹以实现队形控制，并引入Ad Hoc网络在领航机器人与跟随机器人之间建立信息反馈；所述方法分以下几步：

首先建立领航机器人的运动学模型，以领航机器人为参考点，来确定其他跟随机器人的位置，由斥力与引力合力确定领航机器人运动方向；所述建立领航机器人的运动学模型是采用

控制方法来保持队形，即通过控制两个机器人之间的相对距离l和相对角度

，使之达到一个恒定值

从而形成和保持一个良好的编队队形；

然后采用人工势场法建立跟随机器人跟踪领航机器人运动模型，其基本思想是构造目标位置引力场和障碍物周围斥力场共同作用的人工势场，搜索势函数的下降方向来寻找无碰撞路径，跟随机器人根据一定的距离与角度跟踪领航机器人，根据运动模型确定跟随机器人运动轨迹；

所述采用人工势场法建立跟随机器人跟踪领航机器人运动模型是以leader-follower的方法建立全局队形，内部各自按照自己的方式建立模型，形成一个统一的整体；其是当多机器人队形编好后，根据领航机器人和离领航机器人最远的一个跟随机器人的距离确定一个圆形区域，即以领航机器人为中心ρ_s为半径的圆形区域，即避障区域；当障碍物在避障区域以外时，整个机器人队伍中的成员将不会与障碍物发生碰撞，则障碍物不对领航机器人产生斥力；当障碍物在避障区域之内时，表示整个队伍不能顺利通过障碍物，障碍物对领航机器人产生斥力；其具体实现过程如下：

机器人robot1，即领航机器人在运动过程中受到的总的势场力F_total是由目标点引力F_goal和避障区域内障碍物产生的斥力F_rep两部分组成；计算目标点引力F_goal，q_goal为目标位置，q代表机器人位置，ρ(q，q_goal)代表机器人与目标之间的距离，则目标产生的引力势场U_goal(q)和相应的目标点引力为F_goal(q)

F_total=F_goal+F_rep    （1）

$U_{\mathrm{goal}\left(q\right)}=\frac{1}{2}{\mathrm{\ζ\ρ}}^2(q,q_{\mathrm{goal}})---\left(2\right)$

F_goal(q)=-▽U_goal(q)=ζρ(q,q_goal)（3）

其中ζ为正比例因子；

障碍物Obstacle1对机器人产生斥力，障碍物Obstacle2对机器人产生的斥力为零，q_obsi，i=1,…,m表示在避障区域内的第i个障碍物的位置，ρ(q,q_obsi)表示区域内第i个障碍物和机器人之间的距离，则斥力的势场函数U_repi为：

$U_{\mathrm{repi}}\left(q\right)=\left\{\begin{array}{ccc}\frac{1}{2}\mathrm{\η}(\frac{1}{\mathrm{\ρ}(q,q_{\mathrm{obsi}})}-\frac{1}{{\mathrm{\ρ}}_s}),& \mathrm{if}& \mathrm{\ρ}(q,q_{\mathrm{obsi}})\≤{\mathrm{\ρ}}_s\\ 0,& \mathrm{if}& \mathrm{\ρ}(q,q_{\mathrm{goal}})>{\mathrm{\ρ}}_s\end{array}\right.---\left(4\right)$

其中η是正比例因子，因此相应的斥力函数F_repi为：

$F_{\mathrm{repi}}\left(q\right)=-{\▿U}_{\mathrm{repi}}\left(q\right)=\left\{\begin{array}{ccc}\mathrm{\η}(\frac{1}{\mathrm{\ρ}(q,q_{\mathrm{obsi}})}-\frac{1}{{\mathrm{\ρ}}_s})\frac{1}{{\mathrm{\ρ}}^2(q,q_{\mathrm{obsi}})}\▿\mathrm{\ρ}(q,q_{\mathrm{obsi}}),& & \\ \mathrm{if}& \mathrm{\ρ}(q,q_{\mathrm{obsi}})\≤{\mathrm{\ρ}}_s& \\ 0,& \mathrm{if}& \mathrm{\ρ}(q,q_{\mathrm{obsi}})>{\mathrm{\ρ}}_s\end{array}\right.---\left(5\right)$

$F_{\mathrm{rep}}=\underset{i=1\·\·\·m}{\mathrm{\Σ}}{F}_{\mathrm{repi}}---\left(6\right)$

领航机器人在避障区域内绕开障碍物，保证整个机器人队伍在避开障碍物的情况下，向目标位置前进；

最后在领航机器人与跟随机器人之间引入Ad Hoc网络，在编队过程中，将隐式通信与显示通信相结合，建立信息反馈，确保跟随机器人对领航机器人的跟踪过程无丢失；

所述在领航机器人与跟随机器人之间引入Ad Hoc网络是，引入AdHoc网络作为信息反馈，当跟随机器人丢失时，主动发出跟踪失败消息，领航机器人在收到消息后，向所有的跟随机器人发送数据包，包括领航机器人的坐标位置(x,y)及当前位置与x轴的夹角θ；

在领航机器人与跟随机器人之间引入Ad Hoc网络，建立信息反馈的方法如下：

（1）领航机器人了解全局信息，接收任务，记为T，根据任务规划其初始运动方向；初始时刻，由领航机器人发送开始命令，所有的跟随机器人在收到命令后开始跟随领航机器人运动；在运动过程中，领航机器人用自身携带的传感器检测障碍物，并在一定范围内避开障碍物，再继续向目标前进；

（2）跟随机器人的任务则为跟随领航机器人，它们将采用人工势场法建立的模型，用自身携带传感器检测跟踪领航机器人，与领航机器人之间保持一定的角度和距离来保持队伍队形，并在运动的同时监听来自领航机器人的消息；

（3）在正常情况下，领航机器人与跟随机器人之间不进行通信，跟随机器人依靠自身所配备的传感器来检测领航机器人，进行跟踪；在前进过程中，一旦跟随机器人发现无法检测到领航机器人，跟随机器人会主动发出跟踪失败信息，记为S，表示跟踪失败，领航机器人在收到信息后，首先会向所有的跟随机器人发送停止前进的命令，然后通过广播方式向所有跟随机器人发送包括位置信息在内的数据包，记为P1，所有的跟随机器人会根据收到的信息来调整自己当前的位置，而丢失的跟随机器人则会根据收到的信息来确定自己下一步的位置，在丢失的跟随机器人跟上整个队伍后，才继续前进；

（4）在整个通信过程中，领航机器人以打包的形式向其他机器人发送数据包，缩写为P2，在整个机器人队伍中，领航机器人与跟随机器人都有自己唯一的ID，在网络环境中有唯一的IP地址与之对应；跟随机器人以领航机器人为参考点，以决定其下一步的位置，因此在领航机器人发送的数据包包括五个元素pack={R_ID,R_i,R_ix,R_iy,θ_i}，其中：

R_ID：表示机器人R_i的ip地址；

R_i：表示机器人R_i是否为领航机器人，0为领航机器人，1为跟随机器人；

R_ix,R_iy：分别表示机器人R_i所在位置的x，y轴坐标；

θ_i：表示R_i当前位置与x轴的夹角。

2.根据权利要求1所述的编队控制方法，其特征在于，所述建立领航机器人的运动学模型的步骤为，在环境空间中，建立势场：设势场函数为Φ，目标点产生吸引力，障碍物产生斥力，领航机器人所受到的合力为目标点对机器人的引力及障碍物对它的斥力之和，合力决定了机器人的运动方向。

3.根据权利要求1所述的编队控制方法，其特征在于，在建立领航机器人的运动学模型的步骤中，使两个机器人保持一定的队形，控制的需要达到的最终目标为：

其中为预先设定的两机器人之间的保持一定队形需要达到的距离与角度；在

控制方法中，只要给定参考机器人的角速度、速度、位置和航向，该机器人就可以在保证向着与参考机器人相对距离为ld和相对方向为的位置移动，实现队形的保持，达到最终目标，并不需要刻意的注意，但是存在一定的缺陷，没有涉及到避障的问题，对动态环境适应力不强，所以引入下一步的人工势场法进行改进。

Images