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

摘要

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

主权项

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{\zeta \rho }}^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}\eta (\frac{1}{\rho (q,q_{\mathrm{obsi}})}-\frac{1}{{\rho }_s}),& \mathrm{if}& \rho (q,q_{\mathrm{obsi}})\le {\rho }_s\\ 0,& \mathrm{if}& \rho (q,q_{\mathrm{goal}})>{\rho }_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}\eta (\frac{1}{\rho (q,q_{\mathrm{obsi}})}-\frac{1}{{\rho }_s})\frac{1}{{\rho }^2(q,q_{\mathrm{obsi}})}▿\rho (q,q_{\mathrm{obsi}}),& & \\ \mathrm{if}& \rho (q,q_{\mathrm{obsi}})\le {\rho }_s& \\ 0,& \mathrm{if}& \rho (q,q_{\mathrm{obsi}})>{\rho }_s\end{array}\right.---\left(5\right)$$F_{\mathrm{rep}}=\underset{i=1···m}{\Sigma }{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轴的夹角。