CN102981504B - 基于Zigbee通信的多机器人队形控制系统及方法 - Google Patents

Abstract

本发明涉及一种基于Zigbee通信的多机器人队形控制系统及方法；摄像头以一定的采样频率采集多个机器人的运动图像并传送至上位机，在上位机上对图像进行处理，从而得到各个机器人的位置和方向；上位机计算出各个机器人下一个采样周期内的运动速度，最后上位机通过通信模块将机器人的运动速度写入数据包并发送给机器人；机器人通过通信模块收到数据包后解析出运动速度并执行；如此反复，直至达到相应的队形控制目标；本发明能实现多机器人的队形控制，验证各种控制算法，并分析不同的通信条件对多机器人队形控制的影响，为多机器人队形控制的研究提供一种经济高效的方法。

Description

基于Zigbee通信的多机器人队形控制系统及方法

技术领域

本发明涉及一种多机器人队形控制技术，尤其涉及一种基于Zigbee通信的多机器人队形控制系统及方法。

背景技术

随着机器人技术的发展，机器人的功能越来越多，应用也越来越广泛。同时，依赖机器人所需要完成的任务越来越精密和复杂，单个机器人逐渐难以完成人们制定的目标，此时，多个机器人组成机器人系统，通过协调和合作来完成任务，逐渐成为机器人领域发展的重要趋势。同时，很多事例表明，使用多个机器人能够比增加单个机器人的能力和复杂度更加节省成本，也更能出色地完成任务。

多机器人的队形控制问题是近来控制领域的热点问题，其主要任务是使多机器人的队形控制更加的准确。所谓的多机器人的队形控制是指多个机器人在达到目标的过程中，保持预设定的某种队形，同时又要克服周围环境的干扰和影响（例如存在障碍物或者空间的物理限制）的一种控制技术。多机器人保持一定的队形有着以下的好处：一是能够充分的获取周围的环境信息；二是能够保持系统的稳定性和安全性；三是能够提高整体的工作效率；四是能够提高系统的鲁棒性。

当前，多机器人的队形控制方法有所发展，但仍然存在很多不足，如方法实现的成本过高，可扩展性不强，稳定性较差。这些劣势也抑制了多机器人的队形控制在实际工业应用领域的发展。

发明内容

本发明的目的在于针对现有技术的不足，提供一种基于Zigbee通信的多机器人队形控制系统及方法。

本发明解决其技术问题采用的技术方案是：一种基于Zigbee通信的多机器人队形控制系统，该系统主要由摄像头、通信模块、上位机和多个机器人组成；所述摄像头通过网线与上位机相连，所有机器人通过通信模块与上位机进行Zigbee通信。

一种应用上述系统的基于Zigbee通信的多机器人队形控制方法，该方法包括以下步骤：

（1）摄像头采集多个机器人运动的图像并传送至上位机；

（2）上位机对摄像头传送来的图像进行处理，得到各个机器人的位置和方向；

（3）上位机根据步骤2得到的各个机器人的位置(x_i,y_i)和方向θ_i得到每个机器人下一采样周期内的两轮的转速(ω_Li,ω_Ri)，其中ω_Li为机器人左轮的转速，ω_Ri为机器人右轮的转速；

（4）将步骤3中求出的机器人两轮的转速(ω_Li,ω_Ri)写入通信数据包，并通过通信模块发送至各个机器人；

（5）每个机器人收到上位机发送的通信数据包之后即解析数据包，并根据数据包内容进行运动，从而实现多机器人的队形控制。

相对于现有技术，本发明具有的有益效果是：本发明为一种基于Zigbee通信的多机器人队形控制系统及方法，能为多机器人控制与Zigbee通信研究者提供一个方便实用的实现方法，来验证理论研究算法的可行性，并可用于改进相关的理论研究。同时，可以检验通信存在丢包或者时延情况下的控制性能，为控制算法的改进提供数据支持。本发明中上位机与各个机器人之间的通信基于Zigbee通信，Zigbee通信具有近距离、自组织、低功耗、低复杂度、低成本等特点。本发明成本低，精度高，稳定性好，适用于各种多机器人队形的研究领域，具有良好的发展潜力。

附图说明

图1是本发明的整体结构框图；

图2是本发明的工作流程图；

图3是通信数据包的格式。

具体实施方式

下面结合附图详细描述本发明，本发明的目的和效果将变得更加明显。

如图1所示，描述了本发明的整体结构组成。

实现多机器人队形控制方法的系统主要由摄像头、通信模块、上位机和多个机器人组成。摄像头通过网线与上位机相连，所有机器人通过通信模块与上位机进行Zigbee通信。

摄像头每隔一个固定的采样周期即采集一帧多个机器人运动的图像，并通过网线上传至上位机，上位机调用OpenCV的函数对图像进行处理，即可得到各个机器人的位置和方向，然后运行控制算法，得到每个机器人下下一采样周期内的运动速度，写入数据包并通过Zigbee通信发送至各个机器人，机器人收到相应的数据包之后即将运动速度从数据包中解析出来，并执行。

本发明可以使用德国Basler公司的pia2400-17gc摄像头。该摄像头具有500万象素的分辨率和千兆网数据接口，用支架将摄像头固定在实验平台的上方，保证摄像头的视野能完全覆盖机器人的运动区域。将摄像头与电源相连，并用网线将摄像头与上位机连接，用于摄像头采集的图像传送。

本发明所使用的机器人可以为瑞士联邦理工大学微处理器和接口实验室的研究团队开发的Khepera II双轮移动机器人，主要部件包括两个电机，内置可充电电池，控制器，用于通信扩展的串口。Khepera II双轮移动机器人最初设计作为研究和教学工具结构提供给瑞士研究程序。它允许对现实世界中的轨迹执行、避障、传感信息处理、对行为的处理假设模拟的算法改进开发。Khepera II双轮移动机器人作为机器人实验和应用平台现在被广泛应用于世界各地。Khepera II双轮移动机器人拥有扩展功能，完全符合原来的设计目标，可以使用Khepera II双轮移动机器人的任何附件。这是一个简单易用，功能强劲的机器人应用的标准平台。使用前，先用给定的充电器给Khepera II双轮移动机器人的内置电池充电，当充电器上的指示灯由红色变为绿色时，即表示电池已经充满电，然后打开机器人侧面的电源开关，此时机器人即由内部电池供电。并选择机器人的运行模式1，即串口设置为9600波特率，共8位，1个起始位，2个停止位，无校验位。

本发明中的通信模块采用的是TI公司的eZ430-RF2500套件。eZ430-RF2500套件结合了超低功耗MSP430微控制器与无线通信功能，从而进一步简化了低功耗无线系统的开发工作。利用eZ430-RF2500套件可以很方便地实现Zigbee通信。eZ430-RF2500套件通过串口与Khepera II双轮移动机器人相连，并由Khepera II的内置电池供电。

本发明中的上位机可以由普通的台式电脑或笔记本电脑来实现。用网线将摄像头与上位机相连，并在上位机上安装摄像头的驱动程序。上位机的USB端口插有一个通信模块，用于上位机与各个机器人之间进行Zigbee通信。上位机的功能是接收摄像头传送来的图像，对图像进行处理，执行控制算法，发送数据包给各个机器人，并显示机器人的运动图像与各种运动曲线。

本发明基于Zigbee通信的多机器人队形控制方法，包括以下步骤：

1、摄像头采集多个机器人运动的图像并传送至上位机

设定摄像头的采样周期为0.5s，摄像头每隔一个采样周期即拍摄一帧多个机器人运动的图像，保存为bmp格式，并通过网线传送至上位机。

2、上位机对摄像头传送来的图像进行处理，得到各个机器人的位置和方向

上位机的摄像头图像处理部分用于处理摄像头采集的图像从而得出各个机器人的位置和方向，该部分程序采用C++语言编写，调用了部分摄像头厂商提供的API函数。在对图像处理之前，首先要完成摄像头的畸变校正。调用OpenCV提供的一个可以直接使用的校正算法，即输入摄像头传送来的图像由cvCalibrateCamera2()函数得到畸变系数并生成校正后的图像。

对摄像头传送来的图像进行处理的具体实现如下：首先将图像旋转，然后再将图像的原点定位到实际环境的原点中，再创建一个灰度图空间，调用cvCvtColor()函数将采集的RGB图像转换为灰度图，再调用cvSmooth()函数对灰度图进行高斯平滑，然后用cvHoughCircles()函数检测圆形，遍历所有的圆后，识别出所有圆的圆心坐标和半径大小，然后根据圆的半径大小，判断对应哪个机器人，同时得到的圆心坐标即为各个机器人的位置坐标(x_i,y_i)，i＝1,2,...,N，N为机器人的个数。又由于采样周期很短，将机器人上一采样时刻位置与当前位置连线的方向作为机器人当前的运动方向θ_i，i＝1,2,...,N。

3、上位机根据步骤2得到的各个机器人的位置(x_i,y_i)和方向θ_i得到每个机器人下一采样周期内的两轮的转速(ω_Li,ω_Ri)，其中ω_Li为机器人左轮的转速，ω_Ri为机器人右轮的转速

根据机器人需要跟踪的参考轨迹和机器人队形之间的几何关系，可以计算出各个机器人在采样时刻的参考位置(x_di,y_di)和参考方向θ_di，以及参考速度v_di和参考角速度ω_di。

然后根据公式求出各个机器人的跟踪误差(x_ei,y_ei,θ_ei)，其中x_ei为机器人的参考位置与实际位置之差在机器人运动方向上的投影值，y_ei为机器人的参考位置与实际位置之差在垂直于机器人运动方向上的投影值，θ_ei为机器人的参考方向与实际方向之差，计算公式如下：

$\left[\begin{array}{c}{x}_{\mathrm{ei}}\\ y_{\mathrm{ei}}\\ {\mathrm{\θ}}_{\mathrm{ei}}\end{array}\right]=\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_i& \sin {\mathrm{\θ}}_i& 0\\ -{\sin \mathrm{\θ}}_i& \cos {\mathrm{\θ}}_i& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{\mathrm{di}}-x_i\\ y_{\mathrm{di}}-y_i\\ {\mathrm{\θ}}_{\mathrm{di}}-{\mathrm{\θ}}_i\end{array}\right];$

再根据控制算法，计算出每个机器人下一采样周期运动的线速度v_i和角速度ω_i，控制算法如下：

$v_i=v_{\mathrm{di}}+{\mathrm{\β}}_i{x}_{\mathrm{ei}}-{\mathrm{\γ}}_i{\mathrm{\ω}}_{\mathrm{di}}{y}_{\mathrm{ei}}+\underset{j=1,j\≠i}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\β}}_{i,j}(x_{\mathrm{ei}}-x_{\mathrm{ej}})-\underset{j=1,j\≠i}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\γ}}_{i,j}{\mathrm{\ω}}_{\mathrm{di}}(y_{\mathrm{ei}}-y_{\mathrm{ej}})$

${\mathrm{\ω}}_i={\mathrm{\ω}}_{\mathrm{di}}+{\mathrm{\α}}_i{\mathrm{\θ}}_{\mathrm{ei}}+\underset{j=1,j\≠i}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\α}}_{i,j}({\mathrm{\θ}}_{\mathrm{ei}}-{\mathrm{\θ}}_{\mathrm{ej}})$

其中，α_i，β_i，γ_i，α_i,j，β_i,j和γ_i,j为控制参数。

最后将机器人的线速度v_i和角速度ω_i转换为机器人两轮的转速(ω_Li,ω_Ri)，计算公式如下：

${\mathrm{\ω}}_{\mathrm{Ri}}=\frac{v_i}{R}+\frac{{\mathrm{\ω}}_{i}W}{2R}$

${\mathrm{\ω}}_{\mathrm{Li}}=\frac{v_i}{R}-\frac{{\mathrm{\ω}}_{i}W}{2R}$

其中，R为机器人车轮的半径，W为机器人两个车轮的间距，均可在机器人上测量得出。

多个机器人的运动图像和各种运动曲线均可以在上位机上显示出来，以方便分析其性能。上位机显示部分可以分成4个部分，每一部分又可以细分为更多的组件。第一部分用于显示多个机器人的运动图像，是由摄像头采集图像并传送至上位机的。第二部分用于选择一个或者多个机器人，并且可以选择是否显示机器人的运动信息。第三部分是参数设置，用于设定系统的各种参数，包括通信时是否存在丢包，时延以及各个控制参数。start按钮表示系统开始工作，stop按钮表示系统停止工作。最后一部分组件是一个文本显示框，用于显示上位机发送给各个机器人的所有通信数据包。

4、将步骤3中求出的机器人两轮的转速(ω_Li,ω_Ri)写入通信数据包，并通过通信模块发送至各个机器人

如图3所示，是上位机发送给各个机器人的通信数据包的格式。每个机器人都有一个对应的ID，以便于和其他机器人相区别。多个机器人的ID分别表示为1、2、3…N。为方便上位机与各个机器人之间进行Zigbee通信，定义一个通信数据包的格式。

该通信数据包的总长度为10个字节，其中对应机器人的ID占据1个字节，‘/’占据1个字节，机器人两轮的转速(ω_Li,ω_Ri)占据5个字节，最后‘0x0D 0x0A\0’占据了3个字节。

当通信数据包写完之后，即由上位机的通信模块发送至各个机器人。

5、每个机器人收到上位机发送的通信数据包之后即解析数据包，并根据数据包内容进行运动，从而实现多机器人的队形控制。

每个机器人上所插的通信模块都会接收到上位机发送的通信数据包，然后将数据包中第一个字节中的ID解析出来，进行判断：如果数据包中的ID和机器人自身的ID相同，则此时通信模块上的绿色LED灯亮，表示数据包是上位机发给该机器人的，然后继续对该数据包按照定义的格式进行解析，得到相应的机器人两轮的转速(ω_Li,ω_Ri)，并将(ω_Li,ω_Ri)通过串口传送给该机器人的控制器，控制该机器人的电机在下一个采样周期内按照转速(ω_Li,ω_Ri)运动；如果数据包中的ID和机器人自身的ID不相同，则此时通信模块上的红色LED灯亮，表示数据包不是上位机发给该机器人的，然后将该数据包丢弃，机器人的运动速度保持不变。

本发明能实现多机器人的队形控制，验证各种控制算法，并分析不同的通信条件对多机器人队形控制的影响，为多机器人队形控制的研究提供一种经济高效的途径。

Claims (4)

1.一种基于Zigbee通信的多机器人队形控制方法，该方法在多机器人队形控制系统上实现，所述多机器人队形控制系统主要由摄像头、通信模块、上位机和多个机器人组成；所述摄像头通过网线与上位机相连，所有机器人通过通信模块与上位机进行Zigbee通信；其特征在于，该方法包括以下步骤：

(1)摄像头采集多个机器人运动的图像并传送至上位机；

(2)上位机对摄像头传送来的图像进行处理，得到各个机器人的位置和方向；

(3)上位机根据步骤2得到的各个机器人的位置(x_i,y_i)和方向θ_i得到每个机器人下一采样周期内的两轮的转速(ω_Li,ω_Ri)，其中ω_Li为机器人左轮的转速，ω_Ri为机器人右轮的转速；具体为：

根据机器人需要跟踪的参考轨迹和机器人队形之间的几何关系，计算出各个机器人在采样时刻的参考位置(x_di,y_di)和参考方向θ_di，以及参考速度v_di和参考角速度ω_di；

然后根据公式求出各个机器人的跟踪误差(x_ei,y_ei,θ_ei)，其中x_ei为机器人的参考位置与实际位置之差在机器人运动方向上的投影值，y_ei为机器人的参考位置与实际位置之差在垂直于机器人运动方向上的投影值，θ_ei为机器人的参考方向与实际方向之差，计算公式如下：

$\left[\begin{array}{c}{x}_{\mathrm{ei}}\\ y_{\mathrm{ei}}\\ {\mathrm{\θ}}_{\mathrm{ei}}\end{array}\right]=\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_i& \sin {\mathrm{\θ}}_i& 0\\ -\sin {\mathrm{\θ}}_i& \cos {\mathrm{\θ}}_i& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{\mathrm{di}}-x_i\\ y_{\mathrm{di}}-y_i\\ {\mathrm{\θ}}_{\mathrm{di}}-{\mathrm{\θ}}_i\end{array}\right];$

再根据控制算法，计算出每个机器人下一采样周期运动的线速度v_i和角速度ω_i，控制算法如下：

$v_i=v_{\mathrm{di}}+{\mathrm{\β}}_i{x}_{\mathrm{ei}}-{\mathrm{\γ}}_i{\mathrm{\ω}}_{\mathrm{di}}{y}_{\mathrm{ei}}+\underset{j=1,j\≠i}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\β}}_{i,j}(x_{\mathrm{ei}}-x_{\mathrm{ej}})-\underset{j=1,j\≠i}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\γ}}_{i,j}{\mathrm{\ω}}_{\mathrm{di}}(y_{\mathrm{ei}}-y_{\mathrm{ej}})$

${\mathrm{\ω}}_i={\mathrm{\ω}}_{\mathrm{di}}+{\mathrm{\α}}_i{\mathrm{\θ}}_{\mathrm{ei}}+\underset{j=1,j\≠i}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\α}}_{i,j}({\mathrm{\θ}}_{\mathrm{ei}}-{\mathrm{\θ}}_{\mathrm{ej}})$

其中，α_i，β_i，γ_i，α_i,j，β_i,j和γ_i,j为控制参数；

最后将机器人的线速度v_i和角速度ω_i转换为机器人两轮的转速(ω_Li,ω_Ri)，计算公式如下：

${\mathrm{\ω}}_{\mathrm{Ri}}=\frac{v_i}{R}+\frac{{\mathrm{\ω}}_{i}W}{2R}$

${\mathrm{\ω}}_{\mathrm{Li}}=\frac{v_i}{R}+\frac{{\mathrm{\ω}}_{i}W}{2R}$

其中，R为机器人车轮的半径，W为机器人两个车轮的间距，均能够在机器人上测量得出；

(4)将步骤3中求出的机器人两轮的转速(ω_Li,ω_Ri)写入通信数据包，并通过通信模块发送至各个机器人；

(5)每个机器人收到上位机发送的通信数据包之后即解析数据包，并根据数据包内容进行运动，从而实现多机器人的队形控制。

2.根据权利要求1所述的方法，其特征在于，所述步骤(1)具体为：设定摄像头的采样周期为0.5s，摄像头每隔一个采样周期即拍摄一帧多个机器人运动的图像，保存为bmp格式，并通过网线传送至上位机。

3.根据权利要求1所述的方法，其特征在于，所述步骤(2)具体为：首先将图像旋转，然后再将图像的原点定位到实际环境的原点中，再创建一个灰度图空间，调用cvCvtColor()函数将采集的RGB图像转换为灰度图，再调用cvSmooth()函数对灰度图进行高斯平滑，然后用cvHoughCircles()函数检测圆形，遍历所有的圆后，识别出所有圆的圆心坐标和半径大小，然后根据圆的半径大小，判断对应哪个机器人，同时得到的圆心坐标即为各个机器人的位置坐标(x_i,y_i)，i＝1,2,...,N，N为机器人的个数；将机器人上一采样时刻位置与当前位置连线的方向作为机器人当前的运动方向θ_i，i＝1,2,...,N。

4.根据权利要求1所述的方法，其特征在于，所述步骤(5)具体为：每个机器人上所插的通信模块都会接收到上位机发送的通信数据包，然后将数据包中第一个字节中的ID解析出来，进行判断：如果数据包中的ID和机器人自身的ID相同，则此时通信模块上的绿色LED灯亮，表示数据包是上位机发给该机器人的，然后继续对该数据包按照定义的格式进行解析，得到相应的机器人两轮的转速(ω_Li,ω_Ri)，并将(ω_Li,ω_Ri)通过串口传送给该机器人的控制器，控制该机器人的电机在下一个采样周期内按照转速(ω_Li,ω_Ri)运动；如果数据包中的ID和机器人自身的ID不相同，则此时通信模块上的红色LED灯亮，表示数据包不是上位机发给该机器人的，然后将该数据包丢弃，机器人的运动速度保持不变。