一种基于领航跟随的移动机器人编队控制方法
【技术领域】
[0001] 本发明设及移动机器人运动控制领域，尤其设及一种基于领航跟随的移动机器人 编队运动控制方法。
【背景技术】
[0002] 随着机器人技术的发展，移动机器人在应用范围和功能方面都得到了较大的拓展 和提高，并且广泛应用于国防、工业、服务业等行业。多移动机器人编队可W有效弥补单个 移动机器人能力的不足，使单个机器人在编队行为的群体中拥有了原来所没有的显著优 势，通过群体间机器人的协调合作可W完成单个机器人无法或难W完成的工作。因此，对于 移动机器人编队控制系统及控制方法的研究是十分必要的。
[0003] 经过对现有文献的检索，中国专利申请号为:CN201010618568.1，名称为:基于Ad-化C网络和leader-follower算法的多机器人编队方法，该发明提出了一种多机器人编队方 法，建立leader运动学模型，根据人工势场法确定follower运动轨迹，并在leader和 follower之间引入Ad-Hoc网络，建立信息反馈来保证follower对leader的跟踪过程无丢 失。但该发明在领航跟随者中引入Ad-Hoc网络的同时没有考虑网络通信异常的情况，易出 现由于网络通信异常造成的跟随者运动偏离现象。中国专利申请号为:201310281499.3,名 称为:一种基于行为的领航者一跟随者多智能体队形控制方法，该发明公开了一种基于行 为的领航者一跟随者多智能体队形控制方法，该方法通过增加候补领航者在一定程度上弥 补领航者在发生障碍后导致队形失效的缺陷。但该发明没有考虑在领航跟随编队中跟随者 对领航者信息获取失败的问题，容易导致跟随者丢失的情况。文献名称为:一种多智能体领 航跟随编队新型控制器的设计，在该算法中通过引入基于邻居的局部控制率W及基于邻居 的状态估计规则设计了一种新型的控制器，在该控制器中通过简单设定领航者和跟随者之 间的相对坐标即可方便的实现任意形状编队，但该方法将领航跟随者组成的编队形成一个 固定或可切换的连接拓扑，缺乏一定的编队灵活性且没有考虑到通信拓扑受限情况下的领 航跟随编队问题。
[0004] 通过检索发现，现有技术中关于移动机器人领航跟随编队运动控制的研究很多， 虽然都能够实现对机器人的编队控制，但都没有考虑到在领航跟随编队中通信受限情况下 的跟随者运动问题，编队稳定性差，缺乏有效的编队保障。因此，本发明提出一种基于领航 跟随的移动机器人编队控制方法。
【发明内容】
[0005] 本发明目的在于提供一种针对领航跟随编队运动中的通信异常、跟随者运动情况 W及整体编队队形保持有明显优化的基于领航跟随的移动机器人编队控制方法。
[0006] 为实现上述目的，采用了 W下技术方案:本发明所述控制方法包括全局定位系统、 无线通信系统、算法处理系统、调速系统W及标定不同颜色的移动机器人;全局定位系统利 用摄像头对不同颜色的移动机器人进行颜色识别，采集移动机器人的位姿信息;无线通信 系统将采集到的位姿信息发送给各机器人;算法处理系统根据接收到的各移动机器人位姿 信息做出运动控制及协调编队算法处理，并且不断地与调速系统进行信息交互，调速系统 采集处理各移动机器人的速度信息，对实际控制输出不断做出反馈，实时矫正速度偏差，最 终完成各移动机器人编队运动的控制。
[0007] 所述控制方法的具体步骤如下：
[0008] 步骤1，在工作平台中，利用全局定位系统获取编队中各机器人相对于平台全局坐 标系的位置坐标信息；
[0009] 步骤2,根据各机器人的位置坐标信息构建领航机器人与跟随机器人之间的相对 位置角度关系式，定义编队期望的领航机器人与跟随机器人的距离角度关系，得到误差动 力学方程，进而推导出跟随机器人的运动控制率；
[0010] 步骤3,在编队运动过程中，领航机器人向目标点自主运动并通过无线通信系统不 断向跟随机器人发送信息，跟随机器人接收领航机器人信息并计算自己的位置，记录跟随 机器人每一时刻的位置信息作为训练数据，建立基于非线性最小二乘法的跟随机器人轨迹 预测模型；
[0011] 步骤4,利用粒子群算法解决轨迹预测模型参数选取问题，将轨迹预测模型的一组 参数作为轨迹预测模型的一组解，初始化粒子群，计算每个粒子的适应度函数，根据粒子的 适应度函数值更新粒子的个体极值及全局极值；
[0012] 步骤5,利用迭代公式更新每个粒子的速度和位置；
[0013] 步骤6,不断利用下一时刻的数据更新轨迹预测模型，并向后预测轨迹信息，定义 一个通信数据异常范围，若接收到的数据在正常范围之内，则根据接收到的领航机器人数 据计算跟随机器人位置信息并向其运动;若数据异常时，则启用预测点代替异常数据点作 为下一时刻的跟随机器人轨迹点；
[0014] 步骤7,利用全局定位系统不断更新编队中各机器人的位置姿态信息，直到领航跟 随编队到达目标点。
[0015] 进一步地，步骤6中，所述的通信数据异常范围，是利用全局定位系统和无线通信 系统采集每帖图像，并且计算发送机器人位置信息的时间间隔及机器人的平均运动速度来 确定通信数据是否为异常数据。
[0016] 进一步地，步骤6中，所述的数据更新策略为，下一时刻的跟随机器人轨迹点值是 根据当前时刻的跟随机器人轨迹点值与数据超出正常范围的最大阔值关系来确定的，通过 分别判断当前时刻和下一时刻的跟随机器人轨迹点值在沿平台X、Y轴方向的距离是否满足 通信数据异常范围来做出更新决定。
[0017] 进一步地，所述的全局定位系统由上位机和一个高清CCD摄像头组成，将高清CCD 摄像头置于工作平台的顶部，对平台中每个移动机器人标定不同的颜色，利用颜色识别算 法获取机器人在平台中的位置和姿态信息，再将信息传回上位机进行处理，上位机处理后 再发送给各移动机器人，从而实现移动机器人的全局定位。
[0018] 进一步地，所述的无线通信系统包括上位机、移动机器人及无线路由器，其中移动 机器人上安装有无线收发模块，上位机和无线路由通过网线连接，上位机、移动机器人和无 线路由器组成基于局域网的通信系统，通过建立上位机与各机器人之间W及各机器人之间 的通信协议，实现上位机与机器人、机器人与机器人之间的无线通信。
[0019]进一步地，所述算法处理系统，由DSP数字信号处理器组成，安装在机器人内部，在 该DSP数字信号处理器中植入前面所述的基于领航跟随的移动机器人编队运动控制算法， 接收来自无线通信系统的数据，对机器人的自主运动及编队协调控制作出处理；
[0020] 进一步地，所述调速系统包括测速码盘模块、Arduino模块及PID调速模块；
[0021] 其中，所述的测速码盘模块由光栅盘和光电检测装置组成，安装在机器人的左右 轮电机旁，通过计算每秒光电编码器输出脉冲的个数反映当前左右轮电机的转数，并结合 车轮转动一周的周长，换算得到移动机器人电机左右轮的实际转速；
[0022] 所述的Arduino模块为Arduino Mega 2560单片机，安装在机器人内部，用于扩展 机器人的数字和模拟输入输出口，进而采集接收测速码盘输出脉冲个数，对数据进行分析 处理后，通过I2C总线发送给算法处理系统；
[0023] 所述的PID调速模块是基于BP神经网络的PID控制器，是在经典PID控制器的基础 上，将Arduino模块传回的实测速度与算法处理系统中的理论速度做误差比较，利用BP神经 网络优化PID控制器参数，从而实现机器人在不同运动状态下控制器参数的自适应调节，实 时矫正速度偏差。
[0024] 与现有技术相比，本发明具有如下优点：建立跟随机器人的轨迹预测模型，采用改 进的粒子群算法优化最小二乘预测模型，弥补了因短暂通信异常而导致的编队队