CN108594798A - 一种可实现蜂拥控制的机器人小车系统及其控制方法 - Google Patents

Abstract

本发明公开了一种可实现蜂拥控制的机器人小车系统及其控制方法。该系统包括多个两轮履带小车、上位机和无线通信系统。方法为：每个两轮履带小车通过搭载的传感器获取自身的位置、速度、方向信息，并通过无线通信系统将上述信息发送到上位机；上位机根据多个两轮履带小车发来的信息，通过蜂拥控制方法确定每个两轮履带小车的运动指令，并将运动指令发送给相应的两轮履带小车，两轮履带小车在得到运动指令后做出相应的运动；重复以上步骤进行迭代得到下一时刻新的位置、速度、方向信息，由此不断迭代运算实现位置、速度、方向信息的更新，达到控制小车运动的目的。本发明具有硬件成本低、开发效率高、控制精确、节省资源的优点，并且能够进一步升级使用。

Description

一种可实现蜂拥控制的机器人小车系统及其控制方法

技术领域

本发明属于自动化蜂蛹控制技术领域，特别是一种可实现蜂拥控制的机器人小车系统及其控制方法。

背景技术

蜂拥控制作为一种典型的多机器人集群运动算法，其在理论上已经取得了不错的成果，但在实际系统中应用仍面临下列问题：1)因实验室级别的仿真很难提供各种通信介质限制所需的空间，如电磁波的衰减、图像传感器的分辨率等，所以难以真正的体现通信的限制；2)室内定位需要较高的精度要求，大多数系统需要在室内安装信号源并使用高精度的传感器来估算信号衰减，或者使用昂贵的激光雷达等测绘方式进行定位，所以定位系统造价昂贵；3)蜂拥控制的目的是指导具有一定数量的机器人做集群运动，且机器人数量越多越好，而常见的智能小车体积大，造价高，设置复杂，很难在小场地开展仿真，所以商业智能小车不适合蜂拥控制的仿真。

发明内容

本发明的目的在于提供一种造价低廉、易于适用、且能进行自我控制、也能对反馈值进行误差处理的多机器人小车系统及其控制方法，该系统能利用蜂蛹控制算法控制多个机器人做集群运动。

实现本发明目的的技术解决方案为：一种可实现蜂拥控制的机器人小车系统，包括两轮履带小车、上位机和无线通信系统，其中：

所述两轮履带小车的数量为多个，每个两轮履带小车通过搭载的传感器获取自身的位置、速度、方向信息，并通过无线通信系统将上述信息发送到上位机；

所述上位机，根据多个两轮履带小车发来的信息，通过蜂拥控制方法确定每个两轮履带小车的运动指令，并将运动指令通过无线通信系统发送给相应的两轮履带小车，两轮履带小车在得到运动指令后做出相应的运动。

进一步地，所述两轮履带小车包括车体，以及安装在车体上的微型控制器、电机控制芯片i298n、12V电源、电机、履带及电机固定装置、履带：

所述微型控制器包括6轴陀螺仪、加速度计和蓝牙模块，6轴陀螺仪、加速度计用于测量机器人小车的状态信息，蓝牙模块用于实现蓝牙通信、输出控制信号到电机控制芯片i298n；

所述电机控制芯片i298n用于接收微型控制器的控制信号、电机的转向和转速，并且为微型控制器提供5V电源；

所述12V电源为12V/800mAH锂电池，用于为微型控制器、电机控制芯片 i298n、电机供电；

所述履带及电机固定装置用于将电机、履带固定在车体上；

所述履带由电机驱动。

进一步地，所述的所述微型控制器采用Curie Nano英特尔居里微型控制器，该微型控制器使用Arduino开源电子原型平台，蜂拥控制的仿真采用matlab语言编写。

一种可实现蜂拥控制的机器人小车系统的控制方法，包括两轮履带小车、上位机和无线通信系统，具体步骤为：

步骤1、系统中每个两轮履带小车通过搭载的传感器获取自身的位置、速度、方向信息，通过无线通信系统将上述信息发送到上位机；

步骤2、上位机根据多个两轮履带小车发来的信息，通过蜂拥控制算法计算出每个小车的运动指令，并将运动指令通过无线通信系统发送给相应的两轮履带小车；

步骤3、两轮履带小车在得到运动指令后做出相应的运动；

步骤4、重复步骤1～3进行迭代，得到下一时刻新的位置、速度、方向信息，由此不断迭代运算实现位置、速度、方向信息的更新，达到控制小车运动的目的。

进一步地，步骤1中所述的系统中每个两轮履带小车通过搭载的传感器获取自身的位置、速度、方向信息，具体如下：

根据两轮履带小车安装的6轴陀螺仪和加速度计，获得小车的角速度和加速度，积分运算得到小车的位置和姿态。

进一步地，步骤2中所述的上位机根据多个两轮履带小车发来的信息，通过蜂拥控制算法计算出每个小车的运动指令，具体如下：

使用卡尔曼滤波，采用集中式数据融合方式，对传感器读取的小车实际值x,y,θ与上位机计算出的理论值进行融合：

上位机计算出的理论值根据下式积分得到：

其中，θ＝θ+dt*u₂，x＝x+dt*u₁*cosθ，y＝y+dt*u₁*sinθ；

x,y为小车位置，θ为小车车头角度，u₁为小车速度，u₂为小车车头角速度，dt为测量时间间隔；

然后根据t-1时刻传感器测量值s_pre的误差q，以及建模计算得到的变量值s_star 的误差p，计算得到比例系数k，并进行融合的到t时刻的变量值s：

s＝s_star*k+s_pre*(1-k)

将融合后得到的s值传送给上位机，进行运算为下一时刻发送速度指令；

设定小车初始位置为(0，0)，初始速度为0.1，初始车头角度为0，小车的当前位置为(x，y)，当前速度为u₁，当前车头角度为θ，当前车头角速度为u₂，小车的目标位置为(xr，yr)，目标速度为(vrx，vry)，小车车轮直径为d，同时引入中间变量C₁、C₂、Mr、A、a；

上位机的数学关系推导如下：

小车的控制方程为：

建立小车的位置数组速度数组加速度数组

记：

则再根据计算出小车的左右轮转速，从而控制小车运动；

由上述方程可知，通过已知的小车当前速度u₁、当前位置(x，y)、当前车头角度θ、以及小车的目标位置(xr，yr)、目标速度(vrx，vry)，和两个参数c1、c2，然后由进行迭代得到下一时刻新的u₁，由此不断迭代运算就能够实现位置、速度、方向的更新，从而达到控制小车运动的目的。

本发明与现有技术相比，其显著优点为：(1)使用Arduino作为智能小车的控制面板，降低硬件成本，提高开发效率；(2)使用小型步进电机作为小车的驱动，精确控制小车的速度，节省板载资源；(3)使用电子陀螺仪、加速度计估算智能小车的位置与速度信息，提高定位精度；(4)使用蓝牙作为无线通信，蜂拥控制的仿真采用 matlab语言编写，matlab可支持socket编程，可以在最少的改动代码的情况下将数值仿真改为实际系统仿真。

附图说明

图1是本发明可实现蜂拥控制的机器人小车系统的结构示意图。

图2是本发明中两轮履带小车的车体结构示意图。

图3是本发明可实现蜂拥控制的机器人小车系统控制方法中卡尔曼滤波原理图。

图4是上位机工作原理图。

具体实施方式

下面结合附图和实施例对本发明作进一步描述。

本发明可实现蜂拥控制的机器人小车系统，包括两轮履带小车、上位机和无线通信系统，其中：

所述两轮履带小车的数量为多个，每个两轮履带小车通过搭载的传感器获取自身的位置、速度、方向信息，并通过无线通信系统将上述信息发送到上位机；

所述上位机，根据多个两轮履带小车发来的信息，通过蜂拥控制方法确定每个两轮履带小车的运动指令，并将运动指令通过无线通信系统发送给相应的两轮履带小车，两轮履带小车在得到运动指令后做出相应的运动。

进一步地，所述两轮履带小车包括车体1，以及安装在车体上的微型控制器2、电机控制芯片i298n3、12V电源4、电机5、履带及电机固定装置6、履带7：

所述微型控制器2包括6轴陀螺仪、加速度计和蓝牙模块，6轴陀螺仪、加速度计用于测量机器人小车的状态信息，蓝牙模块用于实现蓝牙通信、输出控制信号到电机控制芯片i298n3；

所述电机控制芯片i298n3用于接收微型控制器2的控制信号、电机5的转向和转速，并且为微型控制器2提供5V电源；

所述12V电源4为12V/800mAH锂电池，用于为微型控制器2、电机控制芯片 i298n3、电机5供电；

所述履带及电机固定装置6用于将电机5、履带7固定在车体1上；

所述履带7由电机5驱动。

进一步地，所述的所述微型控制器2采用Curie Nano英特尔居里微型控制器，该微型控制器使用Arduino开源电子原型平台，蜂拥控制的仿真采用matlab语言编写。

本发明可实现蜂拥控制的机器人小车系统的控制方法，包括两轮履带小车、上位机和无线通信系统，具体步骤为：

步骤1、系统中每个两轮履带小车通过搭载的传感器获取自身的位置、速度、方向信息，通过无线通信系统将上述信息发送到上位机；

步骤2、上位机根据多个两轮履带小车发来的信息，通过蜂拥控制算法计算出每个小车的运动指令，并将运动指令通过无线通信系统发送给相应的两轮履带小车；

步骤3、两轮履带小车在得到运动指令后做出相应的运动；

步骤4、重复步骤1～3进行迭代，得到下一时刻新的位置、速度、方向信息，由此不断迭代运算实现位置、速度、方向信息的更新，达到控制小车运动的目的。

进一步地，步骤1中所述的系统中每个两轮履带小车通过搭载的传感器获取自身的位置、速度、方向信息，具体如下：

根据两轮履带小车安装的6轴陀螺仪和加速度计，获得小车的角速度和加速度，积分运算得到小车的位置和姿态。

根据权利要求4所述的可实现蜂拥控制的机器人小车系统的控制方法，其特征在于，步骤2中所述的上位机根据多个两轮履带小车发来的信息，通过蜂拥控制算法计算出每个小车的运动指令，具体如下：

使用卡尔曼滤波，采用集中式数据融合方式，对传感器读取的小车实际值x,y,θ与上位机计算出的理论值进行融合：

上位机计算出的理论值根据下式积分得到：

其中，θ＝θ+dt*u₂，x＝x+dt*u₁*cosθ，y＝y+dt*u₁*sinθ；

x,y为小车位置，θ为小车车头角度，u₁为小车速度，u₂为小车车头角速度，dt为测量时间间隔；

然后根据t-1时刻传感器测量值s_pre的误差q，以及建模计算得到的变量值s_star 的误差p，计算得到比例系数k，并进行融合的到t时刻的变量值s：

s＝s_star*k+s_pre*(1-k)

将融合后得到的s值传送给上位机，进行运算为下一时刻发送速度指令；

设定小车初始位置为0，0，初始速度为0.1，初始车头角度为0，小车的当前位置为x，y，当前速度为u₁，当前车头角度为θ，当前车头角速度为u₂，小车的目标位置为xr，yr，目标速度为vrx，vry，小车车轮直径为d，同时引入中间变量C₁、C₂、 Mr、A、a；

上位机的数学关系推导如下：

小车的控制方程为：

建立小车的位置数组速度数组加速度数组

记：

则再根据计算出小车的左右轮转速，从而控制小车运动；

由上述方程可知，通过已知的小车当前速度u₁、当前位置x，y、当前车头角度θ、以及小车的目标位置xr，yr、目标速度vrx，vry，和两个参数c1、c2，然后由进行迭代得到下一时刻新的u₁，由此不断迭代运算就能够实现位置、速度、方向的更新，从而达到控制小车运动的目的。

实施例1

本发明可实现蜂拥控制的机器人小车系统，包括两轮履带小车、上位机和无线通信系统。

如图1所示，本发明中两轮履带小车的结构示意图，所述小车包括车体1、以及安装在车体上的Curie Nano英特尔居里微型控制器2、电机控制芯片i298n 3、12V电源 4。

如图2所示，为本发明中两轮履带小车的车体结构示意图，所述两轮履带小车车体包括电机5、履带及电机固定装置6、履带7。

所述两轮履带小车的车体结构，采用两轮履带小车模型，两侧履带分别由两个减速电机控制，通过电机差速实现转向。

所述电机控制芯片i298n 3采用i298n，该芯片的工作电源最低4V，最高36V，并且可以输出5V电压，为arduino提供电源。I298n芯片的六个信号输出引脚：ENA、IN1、IN2、IN3、IN4、ENB，用来接收Curie Nano英特尔居里微型控制器2的控制信号，四个输出引脚：OUT1、OUT2、OUT3、OUT4，用来输出驱动电机5工作。

所述Curie Nano英特尔居里微型控制器2选用arduino101，该芯片自带6轴加速度计和陀螺仪，方便计算小车的位置、速度、姿态信息，并且该芯片还带有蓝牙模块，为以后继续拓展开发留下空间Curie Nano英特尔居里微型控制器2电源由电机驱动芯片i298n提供，六个信号输出引脚与电机控制芯片i298n 3信号输入引脚相连，其中有两个PWM引脚连接使能端ENA、ENB用以控制两点机转速，另外四个普通引脚控制两个电机的转向。

所述12V电源4采用11.1V 850ash蓄电池。

所述Curie Nano英特尔居里微型控制器2包括小车运动模块、传感器模块、卡尔曼滤波模块、控制器模块。

使用Arduino作为智能小车的控制面板，降低硬件成本，提高开发效率；使用小型步进电机作为小车的驱动，精确控制小车的速度，节省板载资源；使用电子陀螺仪、加速度计估算智能小车的位置与速度信息，提高定位精度；使用蓝牙作为无线通信，蜂拥控制的仿真采用matlab语言编写，matlab可支持socket编程，可以在最少的改动代码的情况下将数值仿真改为实际系统仿真。

所述小车运动模块，采用两个电机分别控制左右两个履带的小车模型，通过分别控制两侧履带的速度，实现前进后退转向，每个电机由arduino引脚控制，两个高低电压输出引脚和一个pwm引脚，两个高低电压分别输出高电压和低电压，输出高电压的引脚不同，电机的转向不同，pwm输出引脚则是用来控制电机的转速。通过这种小车模型，我们可以以一种比较简单的方式控制小车，并且控制精度比较好。

所述传感器模块，根据小车安装的加速度计和陀螺仪，获得小车的加速度和角速度，积分运算得到小车的位置和姿态，这种定位方式在系统长时间运行的情况下会产生较大的误差，但是通过算法可以减少零点漂移产生的影响，并且减少系统单次运行时间，也可以避免误差快速扩大的情况出现。

所述卡尔曼滤波模块，采用集中式数据融合方式对数据进行加权融合。对传感器读取的小车实际值x,y,θ,以及控制器计算出的理论值进行融合。

这里的理论值根据该式积分得到：

θ＝θ+dt*u₂

x＝x+dt*u₁*cosθ

y＝y+dt*u₁*sinθ

其中x,y为小车位置，θ为小车车头角度，u₁为小车速度，u₂为小车车头角速度， dt为测量时间间隔；

然后根据t-1时刻传感器测量值s_pre的误差q，以及建模计算得到的变量值s_star 的误差p，计算得到比例系数k，并进行融合的到t时刻的变量值s：

s＝s_star*k+s_pre*(1-k)

将融合后得到的s值传送给控制器，进行运算为下一时刻发送速度指令。具体流程如图3所示。

设定小车初始位置为0，0，初始速度为0.1，初始车头角度为0，小车的当前位置为x，y，当前速度为u₁，当前车头角度为θ，当前车头角速度为u₂，小车的目标位置为xr，yr，目标速度为vrx，vry，小车车轮直径为d，同时引入中间变量C₁、C₂、 Mr、A、a。

上位机的数学关系推导如下：

小车的控制方程为：

建立小车的位置数组速度数组加速度数组

记：

则再根据计算出小车的左右轮转速，从而控制小车运动；。

由上述方程可知，通过已知的小车当前速度u₁、当前位置x，y、当前车头角度θ、以及小车的目标位置xr，yr、目标速度vrx，vry，和两个参数c1、c2，然后由进行迭代得到下一时刻新的u₁，由此不断迭代运算就能够实现位置、速度、方向的更新，从而达到控制小车运动的目的。具体流程图如图4。

综上所述，本发明多机器人系统蜂蛹控制综合仿真平台的设计与实现中，系统能利用蜂蛹控制算法控制多个机器人做集群运动。系统采用陀螺仪对小车进行定位，采用卡尔曼滤波对数据进行处理，利用上位机中控制算法对小车进行控制，采用蓝牙实现小车之间的通信，最终实现对该系统进行蜂蛹控制的目的。本发明具有硬件成本低、开发效率高、控制精确、节省资源的优点，并且预留了额外的传感器空间，方便进一步升级使用。

Claims (6)

1.一种可实现蜂拥控制的机器人小车系统，其特征在于，包括两轮履带小车、上位机和无线通信系统，其中：

所述两轮履带小车的数量为多个，每个两轮履带小车通过搭载的传感器获取自身的位置、速度、方向信息，并通过无线通信系统将上述信息发送到上位机；

所述上位机，根据多个两轮履带小车发来的信息，通过蜂拥控制方法确定每个两轮履带小车的运动指令，并将运动指令通过无线通信系统发送给相应的两轮履带小车，两轮履带小车在得到运动指令后做出相应的运动。

2.根据权利要求1所述的可实现蜂拥控制的机器人小车系统，其特征在于，所述两轮履带小车包括车体(1)，以及安装在车体上的微型控制器(2)、电机控制芯片i298n(3)、12V电源(4)、电机(5)、履带及电机固定装置(6)、履带(7)：

所述微型控制器(2)包括6轴陀螺仪、加速度计和蓝牙模块，6轴陀螺仪、加速度计用于测量机器人小车的状态信息，蓝牙模块用于实现蓝牙通信、输出控制信号到电机控制芯片i298n(3)；

所述电机控制芯片i298n(3)用于接收微型控制器(2)的控制信号、电机(5)的转向和转速，并且为微型控制器(2)提供5V电源；

所述12V电源(4)为12V/800mAH锂电池，用于为微型控制器(2)、电机控制芯片i298n(3)、电机(5)供电；

所述履带及电机固定装置(6)用于将电机(5)、履带(7)固定在车体(1)上；

所述履带(7)由电机(5)驱动。

3.根据权利要求2所述的可实现蜂拥控制的机器人小车系统，其特征在于，所述的所述微型控制器(2)采用Curie Nano英特尔居里微型控制器，该微型控制器使用Arduino开源电子原型平台，蜂拥控制的仿真采用matlab语言编写。

4.一种可实现蜂拥控制的机器人小车系统的控制方法，其特征在于，包括两轮履带小车、上位机和无线通信系统，具体步骤为：

步骤1、系统中每个两轮履带小车通过搭载的传感器获取自身的位置、速度、方向信息，通过无线通信系统将上述信息发送到上位机；

步骤2、上位机根据多个两轮履带小车发来的信息，通过蜂拥控制算法计算出每个小车的运动指令，并将运动指令通过无线通信系统发送给相应的两轮履带小车；

步骤3、两轮履带小车在得到运动指令后做出相应的运动；

步骤4、重复步骤1～3进行迭代，得到下一时刻新的位置、速度、方向信息，由此不断迭代运算实现位置、速度、方向信息的更新，达到控制小车运动的目的。

5.根据权利要求4所述的可实现蜂拥控制的机器人小车系统的控制方法，其特征在于，步骤1中所述的系统中每个两轮履带小车通过搭载的传感器获取自身的位置、速度、方向信息，具体如下：

根据两轮履带小车安装的6轴陀螺仪和加速度计，获得小车的角速度和加速度，积分运算得到小车的位置和姿态。

6.根据权利要求4所述的可实现蜂拥控制的机器人小车系统的控制方法，其特征在于，步骤2中所述的上位机根据多个两轮履带小车发来的信息，通过蜂拥控制算法计算出每个小车的运动指令，具体如下：

使用卡尔曼滤波，采用集中式数据融合方式，对传感器读取的小车实际值x,y,θ与上位机计算出的理论值进行融合：

上位机计算出的理论值根据下式积分得到：

其中，θ＝θ+dt*u₂，x＝x+dt*u₁*cosθ，y＝y+dt*u₁*sinθ；

x,y为小车位置，θ为小车车头角度，u₁为小车速度，u₂为小车车头角速度，dt为测量时间间隔；

然后根据t-1时刻传感器测量值s_pre的误差q，以及建模计算得到的变量值s_star的误差p，计算得到比例系数k，并进行融合的到t时刻的变量值s：

s＝s_star*k+s_pre*(1-k)

将融合后得到的s值传送给上位机，进行运算为下一时刻发送速度指令；

设定小车初始位置为(0，0)，初始速度为0.1，初始车头角度为0，小车的当前位置为(x，y)，当前速度为u₁，当前车头角度为θ，当前车头角速度为u₂，小车的目标位置为(xr，yr)，目标速度为(vrx，vry)，小车车轮直径为d，同时引入中间变量C₁、C₂、Mr、A、a；

上位机的数学关系推导如下：

小车的控制方程为：

建立小车的位置数组速度数组加速度数组

记：

则再根据计算出小车的左右轮转速，从而控制小车运动；

由上述方程可知，通过已知的小车当前速度u₁、当前位置(x，y)、当前车头角度θ、以及小车的目标位置(xr，yr)、目标速度(vrx，vry)，和两个参数c1、c2，然后由进行迭代得到下一时刻新的u₁，由此不断迭代运算就能够实现位置、速度、方向的更新，从而达到控制小车运动的目的。