CN109062204A - 一种基于跟随领航者编队的多移动机器人控制系统 - Google Patents

Abstract

本发明公开了一种基于跟随领航者编队的多移动机器人控制系统，包括实验环境图像采集模块、上位机定位模块、车式移动机器人组、通信模块、控制算法模块。其中所述实验环境图像采集模块，将用于采集多个移动机器人编队实验环境的视频图像；所述上位机定位模块，通过摄像头标定的坐标系，运用图像处理算法实时计算每个机器人的绝对位置信息；所述车式移动机器人组，由多个单体移动小车组成，自主决策完成编队任务；所述通信模块，通过无线通信方式进行数据交互以及信息共享；所述控制算法模块，基于领航跟随的编队算法协调和控制整个系统完成编队任务。本发明是基于视觉定位的适用于室内的多机器人编队系统，具有可靠性强和实时性强的特点。

Description

一种基于跟随领航者编队的多移动机器人控制系统

技术领域

本发明涉及机器人智能控制技术领域，尤其涉及一种基于跟随领航者编队的多移动机器人控制系统。

背景技术

机器人以其具有灵活性、提高生产效率、安全系数高、便于管理等特点广泛用于社会生产生活以及科学研究中。而面对日益复杂的任务和环境，传统的单机器人无法满足需求。所以，通过多个机器人之间的协作控制弥补单个机器人在动态环境中信息感知和处理、控制方面的短板，提高机器人工作效率，完成单个机器人无法或者难以完成的任务。

多机器人控制系统中，多机器人的协作控制技术是机器人技术的前沿领域，同时多机器人系统可以应用在实际应用中。在工业领域，多机器人系统可以代替人完成一些复杂的任务，比如汽车制造的装配和产品的分类、搬运等。在军事领域中，多机器人系统可以通过相互协作探索未知环境、完成危险的目标搜、降低伤亡率。在航空领域，多机器人系统通过相互协作探索未知的世界，挺高效率。在搜索求援领域，多机器人可以在震灾后完成勘探、搜索、救援等工作。

多机器协作控制系统中，编队控制技术是一个具有典型性和通用性的多机器人协作课题，受到广泛的关注。多机器人编队控制问题是多移动机器人技术领域的一个重要分支，其在联合侦查、群体盯梢、协作救援、合作搬运、传感网络等军事和民用领域具有广阔的应用背景。多机编队控制问题是多个移动机器人自主协作形成一个预期的几何图形以完成特定的目标任务，并在未知、非结构环境条件下执行作业任务时尽可能保持既定编队队形不变。目前编队的控制方法主要有跟随领航者法、基于虚拟结构法、人工势能场法、分布式控制算法等。本发明提出了一种跟随领航者的编队控制算法，通过多移动机器人平台实现编队任务。

发明内容

本发明的目的在于提供一种基于跟随领航者编队的多移动机器人控制系统，实现多机器人编队控制。

实现本发明目的的技术解决方案为：一种基于跟随领航者编队的多移动机器人控制系统，包括实验环境图像采集模块、上位机定位模块、车式移动机器人组、通信模块、控制算法模块，其中：

所述实验环境图像采集模块，用于采集多个移动机器人编队实验环境的视频图像；

所述上位机定位模块，通过摄像头标定的坐标系，运用图像处理算法实时计算每个机器人的绝对位置信息；

所述车式移动机器人组，由多个单体移动小车组成，自主决策完成编队任务；

所述通信模块，通过无线通信方式进行数据交互以及信息共享；

所述控制算法模块，基于领航跟随的编队算法来协调和控制整个系统完成编队任务。

进一步地，所述实验环境图像采集模块采用的是基于全局视觉定位的一个工业摄像头，将采集到的图像信息通过网口传输到上位机定位软件上进行图像处理。

进一步地，所述上位机定位模块是基于MFC开发的图像处理软件，包括：

图像采集单元将工业摄像头采集的元素图像信息显示出来，并且可以调节亮度、对比度等基本图像信息；

图像处理单元采用基于Opencv库编写的图像处理算法识别每个机器人的位姿信息，根据工业摄像头标定的坐标系，计算出每个移动机器人的全局坐标(x,y)；

串口通信单元采用MFC中的CMSComm控件的串口通信方式将每个机器人对应的位姿信息以字符串的方式打包发送给通信基站，其中通信基站是选用STM32主控开发的无线通信模块。

进一步地，所述车式移动机器人组包括：

移动机器人车模是采用四轮车式模型，前面两个转向轮有一个舵机控制，后面两个驱动轮采用无刷直流电机控制；

单个移动机器人主控模块用来整合环境信息并控制移动机器人的行为动作。

编码器、电子罗盘等传感器模块，用来感知移动机器人的运动状态信息，并反馈给主控模块。其中编码器能够反馈速度信息，电子罗盘可以反馈角度方位信息。

电源模块和电压转换电路模块为移动机器人提供动力以及传感器所需的驱动电压。

直流电机驱动模块，用来驱动直流电机并可以通过调节电压占空比控制电机转速；

通信模块采用无线通信方式，用来与其他移动机器人和通信基站进行数据交互。

进一步地，所述舵机型号为S-D5数码电机，直流电机型号为DC7.2VRS-540马达；所述主控模块采用STM32F103RCT6芯片开发的最小系统。所述编码器选用512线兼容正交解码的增量式旋转编码器；所述电子罗盘选用带温度补偿的HMC5983芯片；所述电源模块选用7.2V-2000mAh的高倍率发电电池；所述电压转换模块选用三端

稳压芯片L7805将7.2V电压转换成5V给主控芯片和传感器供电；所述直流电机驱动模块选用BNT7971驱动芯片；所述无线通信模块选用NRF2401芯片。

进一步地，所述控制算法模块基于跟随领航者编队算法的控制方法控制机器人组完成编队任务，包括以下步骤：

步骤1、工业摄像头采集室内环境下多机器人运动的图像信息通过网络接口传送给 PC端的上位机；

步骤2、上位机定位模块，根据标定通过的坐标系结合图像处理算法识别每个移动机器人的位置信息，包括坐标信息和姿态信息，并通过串口通信将数据包发送给通信基站；

步骤3、通信基站将各个机器人的位姿信息通过无线通信模块传递给相应的机器人，领航者与跟随机器人根据位姿误差信息以及车载传感器反馈的运动状态信息根据控制算法生产控制命令调整多机器人行为完成编队任务，具体步骤为：

步骤3-1、根据车式移动机器人的驱动结构，建立移动机器人的运动模型和机器人运动状态控制方程；

步骤3-2、基于上述全局视觉定位方法，实时获取移动机器人的位姿信息；再根据预设的多机器人编队中领航机器人和跟随机器人队形的几何信息，计算各个跟随机器人当前时刻的理想位姿；然后，跟随机器人依据各自的理想位姿和实际位姿，计算得到各个跟随机器人的跟踪位姿误差；

步骤3-3、运用编队控制算法设计的跟随机器人控制器输出移动机器人速度和角速控制量以达到误差精度要求，从而实现基于领航跟随的多移动机器人编队任务；

步骤4、判断跟随机器人是否达到与领航者设定的几何队形位置，若未完成，则持续进行反馈修正；若完成，则保持队形。

更进一步的，步骤2中上位机图像处理视频信息获取移动机器人的位姿信息具体方法为：

步骤2-1、通过对工业相机进行标定建立机器人运动平面建立直角坐标系，利用Opencv中的函数对每一帧的图像进行处理；首先通过图像阈值化，分离出移动机器人与实验室环境；再通过findContours寻找轮廓函数查找移动机器的轮廓，并计算中心距，并通过相机标定的坐标系对应的关系转换成移动机器人的世界坐标；

步骤2-2、每个移动机器人上面都有对应的数字标签，通过轮廓匹配算法进行目标匹配，将坐标信息对应到相应的移动机器人。

更进一步的，步骤3-1中对机器人状态控制方程：

式中(x,y,θ)代表移动机器人在全局坐标系下的位姿信息，其中v和ω分别代表移动机器人的速度和角速度信息。

本发明与现有技术相比，其显著优点在于：(1)本发明是基于全局视觉定位的多机器人控制系统，通过摄像头采集多机器人的实时位姿，具有结构简单性强、定位精度高等特点，降低了个体机器人自身传感器对环境感知的依赖程度。(2)本发明中设计了一种领航跟随编队控制方法，控制策略简单实用，跟随机器人根据跟踪控制器的输出控制量来调整与领航者机器人保持编队队形具有快速高效、误差精度小等特点。

附图说明

图1为本发明基于跟随领航者编队的多移动机器人控制系统的系统框图；

图2为本发明基于跟随领航者编队的多移动机器人控制方法的流程图；

图3为本发明基于跟随领航者编队的多移动机器人系统运行框图；

图4为本发明基于跟随领航者编队的多移动机器人的全局视觉定位流程图；

图5为本发明基于跟随领航者编队的多移动机器人的结构图；

图6为本发明基于跟随领航者编队的多移动机器人的主要模块的电路图；

图7为本发明基于跟随领航者编队算法的领航者与跟随者机器人的模型；

图8为本发明基于跟随领航者编队算法的领航者模糊控制路径规划的模型。

具体实施方式

下面结合附图给出一个非限定的实施例对本发明作进一步的阐述。

实施例

结合图1，本发明实施例的基于跟随领航者编队的多移动机器人控制系统，包括实验环境图像采集模块、上位机定位模块、车式移动机器人组、通信模块、控制算法模块，其中：

所述实验环境图像采集模块，用于采集多个移动机器人编队实验环境的视频图像；

所述上位机定位模块，通过摄像头标定的坐标系，运用图像处理算法实时计算每个机器人的绝对位置信息；

所述车式移动机器人组，由多个单体移动小车组成，自主决策完成编队任务；

所述通信模块，通过无线通信方式进行数据交互以及信息共享；

所述控制算法模块，基于领航跟随的编队算法来协调和控制整个系统完成编队任务。

进一步地，所述实验环境图像采集模块采用的是基于全局视觉定位的一个工业摄像头，将采集到的图像信息通过网口传输到上位机定位软件上进行图像处理。其中工业摄像头选用大恒相机的水星(MERCURY)系列数字摄像机。

进一步地，所述上位机定位模块是基于MFC开发的图像处理软件，包括：

图像采集单元将工业摄像头采集的元素图像信息显示出来，并且可以调节亮度、对比度等基本图像信息；

图像处理单元采用基于OpenCv库编写的图像处理算法识别每个机器人的位姿信息，根据工业摄像头标定的坐标系，计算出每个移动机器人的全局坐标(x,y)；

串口通信单元采用MFC中的CMSComm控件的串口通信方式将每个机器人对应的位姿信息以字符串的方式打包发送给通信基站，其中通信基站是选用STM32主控开发的无线通信模块。

进一步地，所述车式移动机器人组包括：

移动机器人车模是采用四轮车式模型，前面两个转向轮有一个舵机控制，后面两个驱动轮采用无刷直流电机控制；

单个移动机器人主控模块用来整合环境信息并控制移动机器人的行为动作。

编码器、电子罗盘等传感器模块，用来感知移动机器人的运动状态信息，并反馈给主控模块。其中编码器能够反馈速度信息，电子罗盘可以反馈角度方位信息。

电源模块和电压转换电路模块为移动机器人提供动力以及传感器所需的驱动电压。

直流电机驱动模块，用来驱动直流电机并可以通过调节电压占空比控制电机转速；

通信模块采用无线通信方式，用来与其他移动机器人和通信基站进行数据交互。

进一步地，所述舵机型号为S-D5数码电机，直流电机型号为DC7.2VRS-540马达；所述主控模块采用STM32F103RCT6芯片开发的最小系统。所述编码器选用512线兼容正交解码的增量式旋转编码器；所述电子罗盘选用带温度补偿的HMC5983芯片；所述电源模块选用7.2V-2000mAh的高倍率发电电池；所述电压转换模块选用三端稳压芯片L7805将7.2V电压转换成5V给主控芯片和传感器供电；所述直流电机驱动模块选用 BNT7971驱动芯片；所述无线通信模块选用NRF2401芯片。

如图2所示，本发明实施例的基于跟随领航者编队算法的控制方法来控制机器人组完成编队任务，包括以下步骤：

步骤1、工业摄像头采集室内环境下多机器人运动的图像信息通过网络接口传送给 PC端的上位机。

步骤2、上位机定位模块，根据标定通过的坐标系结合图像处理算法识别每个移动机器人的位姿信息，包括坐标信息和姿态信息，并通过串口通信将数据包发送给通信基站。

步骤2-1、通过对工业相机进行标定建立机器人运动平面建立直角坐标系，利用Opencv中的函数对每一帧的图像进行处理；首先通过图像阈值化，分离出移动机器人与实验室环境；再通过寻找轮廓findContours函数查找移动机器的轮廓，并计算中心距，并通过相机标定的坐标系对应的关系转换成移动机器人的世界坐标。

步骤2-2、每个移动机器人上面都有对应的数字标签，通过轮廓匹配算法进行目标匹配，将坐标信息对应到相应的移动机器人。

步骤3、通信基站将各个机器人的位姿信息通过无线通信模块传递给相应的机器人，领航者与跟随机器人根据位姿误差信息以及车载传感器反馈的运动状态信息根据控制算法生产控制命令调整多机器人行为完成编队任务，具体步骤为：

步骤3-1、根据车式移动机器人的驱动结构，建立移动机器人的运动模型和机器人运动状态控制方程：

其中(x,y,θ)代表移动机器人在全局坐标系下的位姿信息，其中v和ω分别代表移动机器人的速度和角速度信息。

步骤3-2、基于上述全局视觉定位方法，实时获取移动机器人的位姿信息；再根据预设的多机器人编队中领航机器人和跟随机器人队形的几何信息，计算各个跟随机器人当前时刻的理想位姿；然后，跟随机器人依据各自的理想位姿和实际位姿，计算得到各个跟随机器人的跟踪位姿误差；

步骤3-3、最后，运用编队控制算法设计的跟随机器人控制器输出移动机器人速度和角速控制量以达到误差精度要求，从而实现基于领航跟随的多移动机器人编队任务。

步骤4、判断跟随机器人是否达到与领航者设定的几何队形位置，若未完成，则持续进行反馈修正；若完成，则保持队形。

图3是本发明实施例的基于跟随领航者编队的多机器人控制系统的运行方框图，首先全局工业摄像头采集实时实验环境图像信息，通过轮廓匹配算法识别各个移动机器人上的数字标签进行目标识别，再通过提取移动机人外轮廓中心距并转换成世界坐标确定机器人的位姿信息。通信基站通过NRF2401无线通信模块将位姿信息分别发送给相应的移动机器人的无线通信模块。各个移动机器人接收到信息后，执行编队控制算法输出控制量驱动控制机器人的行为完成编队。

图4是上位机定位模块感知个体机器人位姿的具体流程：首先利用matlab相机标定工具箱对实验场地进行标定，并通过求解摄像机的内参和外参来确定坐标转换矩阵，以便于确定实际区域与摄像头图像的映射关系；然后，对摄像头采集的图像进行图像预处理，其中包括图像分割和形态学处理。图像分割的作用是将移动机器人与实验环境进行图像分割，具体方法可使用阈值法；形态学处理的主要目的是消除噪点，可以通过闭运算，即将图像目标区域扩张，填充内部由噪声引起的空隙，连通整个目标区域，并且扩张目标面积，然后再利用腐蚀操作对其边界进行平滑来实现。其次，基于OpenCv提供的一个FindContours函数对分割后运用闭运算处理的图像进行轮廓检测和提取。然后，对提取的编号进行轮廓识别，就必须要准确描述特定编号数字轮廓的特征，并对编号特征进行提取，最后根据模版进行特征匹配。其中，主要包括轮廓特征描述、轮廓特征提取、轮廓特征匹配三个步骤。通过用Hu不变矩来进行特征提取，因为其对轮廓的描述在平移，放缩，旋转和镜像映射时均具有不变性，所以能有效的描述轮廓的特征。对于每个轮廓，都可以算出一组Hu矩。轮廓特征提取利用GetHuMoments函数，然后利用 Hu矩与模板进行匹配。最后，根据轮廓匹配找到相应的移动机器人标号，提取中心位置和方向来确定各个机器人的位姿。

图5为移动机器人的结构图，其中移动机器人包括车式外壳、主控模块、供电模块、无线通信模块、舵机、直流电机及其驱动模块、电子罗盘、编码器。移动机器人外壳是采用四轮车式模型，前面两个转向轮有一个S-D5数码舵机控制，后面两个驱动轮采用无刷直流电机DC7.2VRS-540控制；编码器、电子罗盘等传感器模块，用来感知移动机器人的运动状态信息，并反馈给主控模块，其中使用512线兼容正交解码的增量式旋转编码器能够反馈速度信息，带温度补偿的HMC5983芯片的电子罗盘可以反馈角度方位信息；7.2V-2000mAh的高倍率发电电池和三端稳压芯片L7805电压转换电路为移动机器人提供动力以及传感器所需的驱动电压；电机驱动模块选用BNT7971驱动芯片，用来驱动直流电机并可以通过调节电压占空比控制电机转速；通信模块采用无线通信 NRF2401模块，用来与其他移动机器人和通信基站进行数据交互。

图6为移动机器人所采用的CPU主控制器的电路图，所述控制器芯片选用STM32F103RCT6。其中主控制器模块包含STM32最小系统，电路原理中包含了电源、时钟、复位、启动模式四个部分。本发明中主控芯片STM32的部分IO口用来作为其他模块的输入输出。其中，PA0引脚配置成定时器TIM2对外部脉冲计数模式作为编码器的输入接口；PA1、PA4、PA5、PA6、PA7引脚配置成SPI通信模式作为无线传感器 NRF2401模块的接口；PC11、PC12引脚配置成IIC通信模式作为电子罗盘HMC5983 的接口；PA9、PA10引脚作为RS232串口通信模块中CH340的接口。

图7为移动机器人主要模块的电路图，其中包括RS232/USB通信模块、电机驱动模块、无线通信模块、电子罗盘模块以及电压转换模块；RS232/USB串口通信模块采用 CH340G芯片；电机驱动模块采用BNT7971驱动芯片；无线通信选用NRF2401芯片；电子罗盘选用HMC5983；

本实施例的控制方法采用领航跟随编队控制算法，具体步骤为：

首先，对多机器人领航跟随编队控制系统建立数学模型,图8所示为跟随者保持队形模型。设2个机器人在t时刻的线速度分别为v₁(t)和v₂(t)，它们的运动方向与x轴正方向的夹角为θ₁和θ₂。以机器人两轮轴心连线的中点为参考点，令机器人1为领导者，机器人2为跟随者，两机器人旋转点之间的距离为l，跟随机器人前进方向与两机器人旋转点连线的夹角为

然后，根据领航跟随机器人的数学模型，设计跟随机器人的控制率。其中领航者机器人根据路径规划进行路径跟踪，可采用模糊控制来实现。将给定曲线分解为若干个目标点，采用模糊控制方法使机器人总能到达当前自身位置距离最近的目标点。如图8, 当前所要到达的目标点为P(x_d,y_d)，移动机器人的质点M与目标点P的距离为D，线速度方向水平夹角为θ。参考点与目标点连线的夹角为α。令β＝θ-α即为机器人运动方向与目标点的偏离角。则选取D与β作为模糊控制器的输入量，机器人的线速度v(t)和角速度ω(t)作为模糊控制器的输出量。通过设计合理的模糊控制规则，来保证移动机器人在轨迹跟踪的过程总能达到目标点P，即变量D与β总是趋于零，即可达到机器人实时跟踪给定轨迹的目的。

图8中所述的对象模型中，令误差向量为在编队运动过程，若能始终保持每个跟随者与领导者之间的相对距离l和角度均收敛到给定值即满足lim_t→∞(e_l)＝0,即可达到编队控制目标。设计闭环控制率：

v₁＝ρ₂sinγ-ρ₁cosγ

其中

本发明的基于跟随领航者编队的多移动机器人控制系统，控制算法实现过程简单且思路清晰。通过改变期望距离l_d和角度的值，可以完成任意编队队形的要求。本发明通过全局视觉定位，获取各个移动机器的位姿信息，通过编队控制算法来完成多机器的协调控制，完成编队队形任务。

Claims (8)

1.一种基于跟随领航者编队的多移动机器人控制系统，其特征在于，包括实验环境图像采集模块、上位机定位模块、车式移动机器人组、通信模块、控制算法模块。

2.根据权利要求1所述的基于跟随领航者编队的多移动机器人控制系统，其特征在于，所述实验环境图像采集模块包括工业摄像头。

3.根据权利要求1所述的基于跟随领航者编队的多移动机器人控制系统，其特征在于，所述上位机定位模块包括：图像采集单元、图像处理单元和串口通信单元。

4.根据权利要求1所述的基于跟随领航者编队的多移动机器人控制系统，其特征在于，所述车式移动机器人组包括：主控模块、编码器模块、电子罗盘模块、电源模块、电压转换电路模块、直流电机驱动模块、通信模块。

5.根据权利要求4所述的基于跟随领航者编队的多移动机器人控制系统，其特征在于，所述直流电机型号为DC7.2VRS-540马达；所述编码器为512线兼容正交解码的增量式旋转编码器；所述电子罗盘为HMC5983芯片；所述电源模块为7.2V-2000mAh的高倍率发电电池；所述电压转换模块为三端稳压芯片L7805；所述直流电机驱动模块为BNT7971驱动芯片；所述无线通信模块为NRF2401芯片。

6.根据权利要求1所述的基于跟随领航者编队的多移动机器人控制系统，其特征在于，所述控制算法模块，包括以下步骤：

步骤1、工业摄像头采集室内环境下多机器人运动的图像信息通过网络接口传送给PC端的上位机；

步骤2、上位机定位模块，根据标定通过的坐标系结合图像处理算法识别每个移动机器人的位置信息，包括坐标信息和姿态信息，并通过串口通信将数据包发送给通信基站；

步骤3、通信基站将各个机器人的位姿信息通过无线通信模块传递给相应的机器人，领航者与跟随机器人根据位姿误差信息以及车载传感器反馈的运动状态信息根据控制算法生产控制命令调整多机器人行为完成编队任务，具体步骤为：

步骤3-1、根据车式移动机器人的驱动结构，建立移动机器人的运动模型和机器人运动状态控制方程；

步骤3-2、基于上述全局视觉定位方法，实时获取移动机器人的位姿信息；再根据预设的多机器人编队中领航机器人和跟随机器人队形的几何信息，计算各个跟随机器人当前时刻的理想位姿；然后，跟随机器人依据各自的理想位姿和实际位姿，计算得到各个跟随机器人的跟踪位姿误差；

步骤3-3、运用编队控制算法设计的跟随机器人控制器输出移动机器人速度和角速控制量以达到误差精度要求，从而实现基于领航跟随的多移动机器人编队任务；

步骤4、判断跟随机器人是否达到与领航者设定的几何队形位置，若未完成，则持续进行反馈修正；若完成，则保持队形。

7.根据权利要求6所述的基于跟随领航者编队的多移动机器人控制系统，其特征在于，步骤2中上位机图像处理视频信息获取移动机器人的位姿信息具体方法为：

步骤2-1、通过对工业相机进行标定建立机器人运动平面建立直角坐标系，利用Opencv中的函数对每一帧的图像进行处理；首先通过图像阈值化，分离出移动机器人与实验室环境；再通过findContours寻找轮廓函数查找移动机器的轮廓，并计算中心距，并通过相机标定的坐标系对应的关系转换成移动机器人的世界坐标；

步骤2-2、每个移动机器人上面都有对应的数字标签，通过轮廓匹配算法进行目标匹配，将坐标信息对应到相应的移动机器人。

8.根据权利要求6所述的基于跟随领航者编队的多移动机器人控制系统，其特征在于，步骤3-1中对机器人状态控制方程：

式中(x,y,θ)代表移动机器人在全局坐标系下的位姿信息，其中v和ω分别代表移动机器人的速度和角速度信息。