多移动机器人编队算法研究与实验验证

王 艳，金 晶，黄 晨

(哈尔滨工业大学控制科学与工程系，黑龙江哈尔滨 150001)

多机器人编队控制的研究是多机器人协调问题研究的基础，具有典型性和通用性。本文对常用的多机器人编队算法进行了研究并加以改进，设计出一种融合了多种机器人编队算法优点的新算法。我们在理论仿真验证的基础上，通过自主搭建的群体智能移动机器人系统进行了实物仿真验证，展现出新编队算法的通用性和有效性。

1 编队算法概述

常用的多机器人编队算法有领航跟随法、人工势场法和基于行为法等。

1.1 领航跟随法

领航跟随法的基本思想是:在多机器人组成的群体中，某个机器人被指定为领航者，其余作为它的跟随者。跟随者以一定的距离间隔跟踪领航者的位置和方向，该方法可进行扩展，即不仅可以指定一个领航者，也可以指定多个，但群体队形的领航者只有一个。根据领航者与跟随者之间的相对位置关系，就可以形成不同的网络拓扑结构［1］。

1.2 基于行为法

该法首先为机器人规定一些期望的基本行为，一般情况下，机器人的行为包括躲避障碍、避免碰撞、驶向目标和保持队形等，当机器人的传感器接受到外界刺激时，根据传感器的输入信息做出反应，并输出反应量作为该行为的期望反应(例如方向和运动速度)。行为选择模块通过一定的机制来综合各行为的输出，并将综合结果作为机器人对环境刺激的反应而输出［2］。3)人工势场法

人工势场法主要是通过设计人工势场和势场函数来表示环境以及队形中各机器人之间的约束关系，并以此为基础进行分析和控制。它的基本思想是机器人在一个虚拟的力场中运动，障碍物被斥力势场所包围，其产生的排斥力随机器人与障碍物距离的减少而增大;目标点被引力势场所包围，其产生的吸引力随机器人与目标的接近而减少;在合力的作用下机器人沿最小化势能的方向运动［3］。

2 编队算法的改进与仿真研究

2.1 单体机器人算法研究

1)改进人工势场法

人工势场法的优点是计算简单，便于实现实时控制，但是势场函数的设计比较困难，而且存在局部极值点的问题。如图1(a)所示，如果选择的势场函数不好，就会陷入局部最小的情况，从而不能正确的驶向目标，形成死点。为了防止机器人在驶向目标的时候陷入局部最小，在此对势场函数进行改进［4］。主要是在斥力势场函数中引入了机器人与目标之间的相对距离。

引力场函数为

其中，K为位置增益，(X－Xgoat)为当前位置与目标位置的相对位置，相应的吸引力为目标势场的负梯度为

斥力势场为

其中，l是机器人与障碍物间的距离，障碍物影响范围在距离l0之内，g是位置增益系数。斥力可以写成以下形式:

其中，

矢量Frel的方向从障碍物指向机器人，矢量Fre2的方向从机器人指向目标点。

机器人所受到的合力为

经过我们改进后的人工势场法的仿真结果如图1(b)所示。根据仿真可见，改进算法可以消除局部最小点，在理论上得到了很好的避障效果。

图1 人工势场法仿真效果

2)跟随领航者法

由对摄像头所采集图片进行实时分析所得到的数据信息，根据跟随领航者法实现机器人的搜索目标与动态追踪功能。

我们首先需要对机器人进行随动系统建模［5］。由实际参数计算可以知道，电机感应电磁系数Ke为0.0215(V/rad/s)，机电时间常数 τm为 0.029s，电磁时间常数τe很小，因此可得电机模型。经计算简化得

我们定义输入为电压，输出为机器人的位置信息，因此机器人的运动的最终模型为

接下来通过Simulink设计PID控制器来观察该模型的跟踪情况，图2是系统控制图。

图2 系统仿真框图

当输入为阶跃信号时，系统的跟踪效果和跟踪误差如图3所示。跟踪频率为1Hz的正弦信号的跟踪效果和跟踪误差如图4所示。可见，其稳态误差小于3%。

图3 阶跃信号跟踪和误差仿真效果

图4 正弦信号跟踪效果和误差仿真效果

当输入为单位阶跃信号，干扰幅值为10时的系统阶跃响应和响应误差如图5所示。由图可见系统在加入干扰后响应依然稳定，稳态误差在3%以内。

图5 加扰动后阶跃响应效果和误差

当输入幅值为1的正弦信号，干扰幅值为10时的系统正弦响应和跟踪误差如图6所示。图中显示当输入为正弦信号时，系统仍具有较好的抗干扰能力。可见干扰对其影响很小，可不予考虑。

图6 加扰动后正弦响应效果和误差

通过以上仿真可以知道，该系统的稳态误差小于3%，并且有很好的抑制干扰的能力，当加入阶跃为10的干扰后响应依然稳定，稳态误差在3%以内。所以可以得出通过PID控制可以使机器人的视觉动态追踪达到很好的跟踪效果。

2.2 群体机器人算法研究

基于单体移动机器人的避障和跟踪两个基本功能，本实验平台群体编队功能采用跟随领航者法、基于行为法和人工势场法三种方法综合而成的融合算法实现。

基于人工势场法实现单体移动机器人的实时避碰行为。基于跟随领航者法设定一个领航机器人，若干个跟随机器人，实现跟踪目标行为。领航机器人的行为主要就是躲避障碍和驶向目标，而跟随机器人的行为则包括躲避障碍、保持队形和驶向目标。

根据基于行为法实现融合算法，在每一个时间间隔都分别计算基本行为模式下机器人下一步的运动路径，再加上一定的权值，求出合力，从而得到机器人下一步的运动路径［6］。根据基于行为法所形成的控制量与基于领航的协调策略相结合，形成控制器，控制小车的运动。该融合算法结构图如图7所示。

图7 多机器人编队结构模型

从实现来讲，我们在仿真中设定一个机器人为领航机器人，其余两个机器人为跟踪机器人，领航机器人的主要运动轨迹就是避障并且驶向目标，而跟随机器人则按照一定的编队方式(三角形，线形)跟随领航机器人，其中还得避开障碍物，其效果如图8所示。

3 编队算法实物验证

3.1 实验平台

硬件平台采用自主开发设计的ARM嵌入式核心板和外围电路控制板构成。车体平台选用了1:10四驱电动模型遥控车VH－32作为车架，将电机更换为速度稍慢的545SA－2588B型号电机。此外扩展了Futaba S3003舵机、URM37V3.2超声传感器和ZC301摄像头。

图8 移动机器人三角编队仿真图

控制主板以三星公司基于ARM920T核的S3C2440作为CPU核心处理器，围绕S3C2440设计了JTAG及复位模块、Nandflash模块和Norflash模块和SDRAM模块。外围电路设计为四部分:串口模块、USB接口模块、网卡驱动模块和电源模块。

软件平台由LINUX实时操作系统、多线程编程技术、驱动和算法程序构成。软件的设计主要是驱动程序和应用程序的编写。

驱动程序有:PWM电机驱动程序、舵机控制驱动程序、超声测距传感器数据采集驱动程序和USB及摄像头驱动程序。主要是通过Linux下的字符设备驱动的编写，完成子函数并填充file_operations的各个域，使得应用程序可以对硬件设备进行操作。

应用程序设计基本可以分为三个部分:传感器数据采集模块、图像分析处理模块和运动控制模块。采用Linux下多线程编程技术，创建了三个线程来运行三个模块，其中负责机器人的运动控制模块的作为主线程。

3.2 实物验证效果

1)驶向目标与实时避碰

单体机器人通过人工势场法实现了机器人驶向目标和实时避碰功能，功能实现框图如图9所示。单体机器人通过人工势场法实现躲避障碍物功能实物效果如图10所示。

图9 机器人避障功能的实现

2)搜索目标与动态追踪

通过跟随领航者法对摄像头所采集图片信息进行实时分析，实现机器人的搜索目标与动态追踪功能，实现框图如图11所示。

群体移动机器人通过基于行为法将改进人工势场法和领航跟随法融合实现队形保持功能效果如图12所示。机器人还能在障碍物随机分布的环境下完成多机器人编队，在实验中，我们采用随机行走的人员作为多机器人编队所遭遇障碍物。

图10 躲避障碍物功能实物图

图11 跟踪功能实现框图

图12 编队实物效果图

4 结语

本文对人工势场法进行了有效地改进，提出了一种基于领航跟随法、人工势场法和基于行为法的新融合算法。该算法不仅有效地实现了单体机器人实时避障和动态跟踪的功能，还适用于全局环境未知，障碍物随机分布条件下的多机器人编队，效果良好，本文对于多机器人编队的研究工作具有重要的参考借鉴价值。

［1］ Jaydev P.Desai，Jim Ostrowski，Vijay Kumar.Controlling formations of multiple mobile robots［C］，IEEE International Conference on Robotics and Automation，1998:2864～2869

［2］ BALCH T，ARKIN R C.BehaviorP-based formation control for multi－ robot teams［J］.IEEE Transactions on Robotics and Automation，1998，14(6):926-939

［3］ 贾秋玲，闫建国，王新民.基于势函数的多机器人系统的编队控制［J］.沈阳:机器人，2006:111-114

［4］ 吴镜开，黄远灿，王世兴.基于势场法的移动机器人避障路径规划［J］.北京:微计算机信息，2007，23(2):228～230

［5］ 宗光华.机器人的创意设计与实践［M］.北京:北京航空航天大学出版社，2004:1～3

［6］ CAO Z Q，ZHANG B，WANG S.Cooperative hunting of multiple mobile robots in an unknown environment［J］.Acta Automatica Sinica，2003，29(4):536～543