基于惯性传感器的集群移动机器人定位系统设计

，，，，

(1.宁夏大学 物理与电子电气工程学院，银川 750021；2.北京交通大学)

基于惯性传感器的集群移动机器人定位系统设计

孟一飞1,2，谢堂健1，杨文慧1，刘丽萍1，陈燕1

(1.宁夏大学 物理与电子电气工程学院，银川 750021；2.北京交通大学)

为了对集群机器人的定位技术做进一步研究，本文对群体移动机器人的整体框架进行了设计，主要包括控制单元、通信模块和传感器单元等，重点对基于MPU9250惯性传感器的定位技术进行了研究，定位系统以ZigBee模块组成无线传感网络，用Arduino对MPU9250进行姿态数据获取并进行处理，利用ZigBee网络将位置信息传输到上位机。本文提出的机器人惯导定位技术对于解决机器人的定位问题具有一定的指导意义。

集群；Arduino；ZigBee；定位；MPU9250

引 言

集群是大自然中群体生物常见的生活方式，自然界中的生物多以群体生活，如大雁的迁徙、鱼群的觅食、蚁群的群居生活等。通过模拟这些行为，揭示在群体行为中蕴含的科学知识，将其中的技术应用在人类的生活实践中，能够推动仿生学的发展。群体里的每个机器人可以根据需要自主控制自己的行动，但还是要通过合作弥补个体智能的不同，从而使群体更加智能，这些都为一些大型且复杂任务的选择提供了更好的帮助。现在也有很多的例子可以说明群体机器人的广泛使用，例如美国DARPA公司开展的Smart Dust项目，运用MEMS技术做成了移动机器人，然后建成感知网络的方式，就是运用群体合作来进行的。还有美国MIT AI实验室微型多移动机器人系统Ants也是运用了集群这种方式。

本文重点论述了集群移动机器人中的定位问题，旨在解决机器人之间的相互定位难题，主要采用ZigBee技术与MPU9250传感器相结合的解决思路，利用Arduino对MPU9250进行了数据读取，以及通过无线传感器网络实现信息的交互传输。

1 无线网络技术概述

1.1 ZigBee无线传感网络

无线网伴随微电子技术、无线通信等技术而发展，而物联网技术的发展推动了无线传感网络(Wireless Sensor Networks, WSN)的快速发展。WSN主要是用来监测、感知和采集信息，能够实现信息的采集、处理和传输功能[1]，在智慧农业[2]、智能家居[3]、军事等领域有着重要的应用价值。本文是利用无线传感网络来监测移动机器人的加速度、角速度等物理量。

本文所用到的ZigBee节点芯片选用的是CC2530，内含增强型微控制器内核，该芯片具有良好的抗干扰性以及灵敏的接收度，8 KB RAM和多种闪存版本，具备RF收发器，硬件支持CSMA/CA，CC2530的工作电压在2～3.6 V，串口可运行于UART和SPI模式。

1.2 室内定位技术概述

图1 集群移动机器人整体框架

定位技术是机器人的重要研究方向之一，本文的研究背景是室内环境下机器人定位。参考文献[4]提出了传感器和视觉里程计结合的定位算法；参考文献[5]采用的是超声波定位技术，该技术基于超声波测距原理，GPS定位技术虽然在室外环境具有广泛的应用，但是一般的民用精度也不是很高(10 m左右)，面对复杂的室内环境，基本无使用；参考文献[6]给出了基于WiFi信号的室内机器人定位实现，建立了指纹图并引入了粒子群算法去噪；参考文献[7]设计了一种基于超宽带(UWB)的高精度室内定位系统，具有精度高、稳定性高的特点，能够满足机器人的定位需求；参考文献[8]采用智能手机定位思路，对指纹数据库进行了优化改进，其实质还是利用了WiFi差分信号强度SSD，此外，可以利用智能手机内嵌的加速度计、陀螺仪、磁力计等惯性传感器推算移动节点的位移。

虽然作为LBS(Location Based Service)最后一米的室内定位饱受关注，但其依然不够成熟。不同于GPS、AGPS等室外定位系统，室内定位系统依然没有形成统一的标准，多种室内定位技术相映生辉。目前，室内定位技术主要有蓝牙定位、射频识别、超宽带、WiFi定位等，定位算法根据是否需要测距离分为基于测距和非测距两类。前者主要有TOA (Time of Arrive) 、AOA (Angle of Arrive) 、TDOA (Time Difference of Arrive)等方法 ，后者主要有基于接收信号强度RSSI(Received Signal Strength Indicator)的定位算法等[10]。

基于无线网的的定位技术研究是一个热门方向，WiFi、ZigBee、蓝牙都处于2.4 GHz ISM(Industrial Scientific Medical)频段，它们的定位基理是RSSI，物理模型就是信号衰减模型。无线网定位系统主要由锚节点和待定节点构成，利用信号的传播特性来计算距离，进而转换成位置坐标。室内环境复杂多样，这就决定了信号传播模型是制约定位精度的重要因素。

机器人超声波定位技术容易实现，技术成熟，但是在群体机器人中，机器人之间相互定位的时候存在着阻挡问题。UWB属高精度室内定位技术，它利用窄带脉冲传输数据，无须载波，该技术常采用TDOA算法。UWB定位系统主要包括接收器、标签等，该技术存在频谱利用率低、信息传输速率不高等特点，并且成本较高。除了以上提及的定位技术外，还有基于计算机视觉的方法。

2 集群移动机器人总体框架设计

整个集群移动机器人系统主要包括几大技术：控制技术、定位技术、通信技术和协同算法。集群移动功能的实现采用Arduino作控制、以ZigBee为通信方式、以鱼群算法作为协同算法。整体框架如图 1所示。控制单元采用ATmega328P作为机器人的主控芯片，该芯片是一款增强型的AVR单片机，用它控制机器人的前进、后退等动作；定位部分采用惯性传感器与ZigBee技术结合的方式，核心器件是MPU9250九轴传感器模块与CC2530，由Arduino读取MPU9250的姿态数据并换算处理，再经ZigBee节点以无线的方式将信息发送出去，各节点的信息可汇总至协调器经上位机显示。其中，Arduino与MPU9250之间的通信方式采用I2C通信，Arduino与CC2530之间采用UART通信；协同算法采用人工鱼群算法，以此来实现机器人的聚群、追尾等功能。本文主要对集群移动机器人的定位系统进行研究。

2.1 控制单元设计

Arduino是一种开源单片机控制器，而Arduino Uno又是典型的控制器，功能齐全，使用广泛，本文使用的ATmega328P工作电压为5 V，具有32 KB 片上可编程Flash, 支持UART、I2C和SPI通信。由于其具有先进的指令集以及单时钟周期指令执行时间，该芯片的数据吞吐率非常高，从而降低了系统功耗，提高了运行速度。对于一个机器人而言，仅仅有主控芯片是不够的，在本系统中，用舵机、电源、轮胎等器件组成了机器人的外部结构。对于Arduino的软件开发，要在Arduino IDE开发环境中进行。

2.2 通信模块设计

作为无线通信的主流方式之一，ZigBee技术以其众多优势在物联网等领域取得了广泛的应用，而且在机器人领域已经取得了一席之地。CC2530芯片是ZigBee芯片中应用最广的芯片之一，它比CC243x系列性能要更稳定。根据内置闪存容量区分，可将CC2530分为以下4个类型的产品：CC2530F32、CC2530F64、CC2530F128和CC2530F256。本文采用的是CC2530F256芯片，该产品支持自组网和透明传输功能。本文主要将CC2530用于终端移动节点和协调器，从而为构建ZigBee网络提供支撑。

2.3 集群机器人算法研究

集群机器人算法主要是在人工鱼群算法的基础上来模拟生物群体聚群并统一步调整体运动的过程，主要修改了人工鱼的各项参数，并更换行为评价函数和食物密度函数，实现了个体间能够保持一定距离，朝向一个方向整体运动的功能。机器人在运动过程中有4种基本行为：聚群行为、随机行为、觅食行为和追尾行为。这4种基本行为的选择由评价函数给出执行选择，其中随机行为是其他三种行为的补充行为。通过对人工鱼群算法的改进，使得个体能够较好地实现聚群，即个体间保持一定的距离，又不至于过度远离。首先完成群体的聚群，选择最优的聚群参数。在完成群体的聚群条件下，修改食物密度函数、行为评价函数，使得群体能够按照制定的路线运动。

3 基于MPU9250的定位技术研究

定位系统的硬件部分主要包括Arduino、ZigBee节点和MPU9250惯性传感器，它们分别实现执行、通信和采集数据的功能。

3.1 MPU9250惯性传感器

MPU9250为InvenSense公司的第二代9轴运动跟踪设备，该传感器体积小，功耗低。MPU9250是一个集成模块，包括两部分：MPU6500三轴传感器和AK8963三轴磁力计。其中MPU6500内含3轴加速度计和3轴陀螺仪，故称其为9轴运动传感器[13]。MPU9250支持I2C和SPI通信，本文采用的是I2C方案，在该方案中，Arduino是主机，MPU9250是从机，数据的传输主要是通过SDA和SCL引脚实现的，可输出9轴数据，I2C通信方案如图2所示。MPU9250具有3个16位加速度A/D输出、3个16位陀螺仪A/D输出、3个6位磁力计A/D输出，此外该传感器提供了全量程的参数编程设置，具体的量程分配如表1所列，其中g为重力加速度。MPU9250含有一个512字节的FIFO寄存器，可以将加速度值等数据写入FIFO缓冲区。

图2 Arduino与MPU9250 I2C通信连接

名 称可编量程单位加速度计±2，±4，±8，±16g陀螺仪±250，±500，±1000°/s磁力计±4800μT

在I2C模式下，通信速率最快可达400 kHz，SCL用来传输时钟信号，SDA用来传输数据。当SDA处在下降沿，SCL置高的时候，通信开始，之后主机发出地址位和读写位。然后，主机释放SDA线，并等待来自从机的确认信号(ACK)。每次数据传输后必须跟一位读写位。从机应答即是拉低SDA到SCL高电平周期结束。当SDA处于上升沿且SCL是高电平的时候，代表通信停止。具体的I2C通信时序如图3所示。

图3 I2C数据传输时序图

3.2 基于ZigBee和MPU9250的定位原理

本文提出了将ZigBee无线传感网络与MPU9250惯导数据相结合的思路。基于MPU9250的定位系统通过陀螺仪、加速度计以及磁力计的结合实现节点定位。首先提供移动节点初始位置及速度，然后在节点运动过程中对MPU9250的信息进行获取和处理，不断更新当前位置及速度。通过检测加速度和角速度，定位系统可以检测位置变化、速度变化和姿态变化(绕各个轴的旋转)。陀螺仪用于测量移动机器人的角速率，加速度计用于测量线加速度，通过跟踪系统当前角速率及相对于运动系统测量到的当前线加速度，即确定当前线加速度。以起始速度作为初始条件，用运动学方程对惯性加速度进行积分，就可得到系统惯性速率，然后以起始位置作初始条件再次积分就可得到位置，即对加速度二次积分得到位置。惯性传感器的误差会随时间累积，故需要不断进行修正。现代惯性导航系统使用各种信号(例如全球定位系统及磁罗盘等)对其进行修正，采取控制论原理对不同信号进行权级过滤，保证惯性导航系统的精度及可靠性。

加速度是一个物体速度的变化速率。即

积分和导数相反。如果一个物体的加速度已知，那么能够利用二重积分获得物体的位置。假设初始条件为0，那么有：

上式给出了加速度定位的数学理论知识，图4给出了加速度定位的算法流程。

4 集群移动机器人定位软件设计

4.1 控制单元与ZigBee网络的通信接口设计

4.1.1 Arduino串口程序

图4 利用加速度求解 位置算法流程

Arduino与ZigBee节点的通信是通过UART方式完成的。Arduino与ZigBee节点的连接方案是：Arduino的TX引脚接CC2530的RX引脚，RX引脚接CC2530的TX引脚。Arduino控制板通过数字引脚0(RX)和1(TX)将信息传输至ZigBee节点，对于Arduino串口程序的设计，本文在Arduino 1.6.11 (Windows)环境下开发，下面仅介绍Arduino发送信息程序中用到的主要函数。

//串口通信初始化

Serial.begin(115200);

//输出数据

Serial.println();

4.1.2 ZigBee串口程序

ZigBee串口程序主要实现CC2530接收Arduino传回的数据。整个过程包括串口初始化、串口中断接收等流程，其中，串口的初始化流程如图5所示。

图5 CC2530串口初始化

选择外部32 MHz晶振作为主时钟源，代码如下：

//设置系统时钟源为32 MHz晶振

CLKCONCMD &=～0x40;

//等待晶振稳定为32 MHz

while(CLKCONSTA & 0x40);

CC2530的串口是USART0和USART1，两个串口都可以运行在UART或SPI模式，本文中使用的是UART模式，故应先配置模式。串口配置以及波特率的设置代码如下：

//外设控制寄存器 USART 0的I/O位置:0为P0口位置1

PERCFG = 0x00;

//P0_2,P0_3用作串口(外设功能)

P0SEL = 0x0C;

//P0优先作为UART0

P2DIR &= ～0xC0;

//设置为UART方式

U0CSR |= 0x80;

//波特率设为115200

U0GCR |= 11;

U0BAUD |= 216;

4.2 基于MPU9250的定位程序设计

MPU9250驱动程序的设计主要包括图6所示的流程。

图6 MPU9250驱动软件流程

对于MPU9250的初始化，主要是依据MPU9250的使用说明和寄存器手册对一些寄存器的定义和配置。比如加速计的配置用到的ACCEL_CONFIG寄存器可以对加速度计是否自检和量程进行设置，代码如下：

//将ACCEL_CONFIG设置为0x00，即把加速度计设为不自检模式且量程为±2g

I2Cdev::writeByte(0x68,ACCEL_CONFIG, 0x00);

I2C的软件编写主要根据上文介绍的MPU9250 I2C时序图编写，主要程序包括I2C开始、I2C结束、I2C应答、I2C读取字节等。数据获取即对9轴数据进行获取，从而得到9轴输出的真实数据。下面以加速度和陀螺仪X轴的数据获取为例，给出数据获取程序。MPU9250加速度和陀螺仪X轴数据的获取程序如下：

//读取计算加速度X轴数据

*ax = (((int16_t)buffer[0]) << 8) | buffer[1];

*ax=2.0*g*(*ax)/(32768);

//读取计算陀螺仪X轴数据

图7 磁力计 数据获取流程

*gx = (((int16_t)buffer[8]) << 8) | buffer[9];

*gx=250.0*(*gx)/32768;

计算公式为：真实值=ADC值×选择的满量程/32768。

加速度计与陀螺仪分别有三个16位的ADC，每个轴使用一个，最高位为符号位，剩下15位为数值位，因此输出值为32 768～-32 768，为了得到真正的加速度值，需要在原始数据的基础上除以32 768。所以设计中，选取加速度量程为±2g时，精度为16 384 LSB/g。

磁力计数据获取流程如图7所示。

4.3 ZigBee组网程序设计

ZigBee网络的建立主要包括两大部分，一是协调器建立网络，二是节点的入网。具体的软件流程如图8和图9所示。软件开发是在IAR Embedded Workbench环境下进行，并且使用了Z-stack协议栈编程。

图8 协调器的软件流程

图9 节点的软件流程

5 实验及测试

以ax，ay，az分别代表加速度计的X轴、Y轴、Z轴数据，以gx，gy，gz分别代表陀螺仪的X轴、Y轴、Z轴数据。静止状态下的加速度值和陀螺仪数据如表2所列。由于MPU9250处于静止状态，加速度水平方向的X轴和Y轴的数据都是0，Z轴受重力影响，存在1g大小的重力加速度，而表中得到的数据是-9，属误差。X轴的角速度不为0亦是误差原因。其中，加速度值的单位是m/s2，陀螺仪数据即角速度的单位是°/s。

表2 静止状态下MPU9250的加速度值和角速度值

沿X轴负方向时的加速度值和陀螺仪数据如表3所列。沿X轴负方向做变加速直线运动时，ax显然是负值。

绕Z轴逆时针旋转时的加速度值和陀螺仪数据如表4所列。绕Z轴逆时针旋转时，gz是变化的。

表3 沿X轴负方向运动时MPU9250的加速度值和角速度值

表4 绕Z轴逆时针旋转时MPU9250的加速度值和角速度值

静止状态下，从AK8963中读出的数据如表5所列，其中磁力计数据的单位为μT。

表5 静止时MPU9250的磁力计数据

结 语

[1] 谢金龙,邓人铭.物联网无线传感网络技术与应用(ZigBee版)[M].北京:人民邮电出版社,2016.

[2] 饶强,于舒娟,张昀,等.基于无线传感网络精细农业系统盲检测[J].电视技术,2015(21):87-90,103.

[3] 肖天柱.基于无线传感网络的智能家居管理系统

DesignofSwarmMobileRobotsLocationSystemBasedonInertialSensor

MengYifei1,2,XieTangjian1,YangWenhui1,LiuLiping1,ChenYan1

(1.School of Physics and Electronic-electrical Engineering,Ningxia University,Yinchuan 750021,China;2.Beijing Jiaotong University)

In order to study the positioning technology of cluster robots,the overall framework of the swarm-robot is designed in the paper,which mainly includes the control unit,the communication module and the sensor unit.The positioning technology of the MPU9250 inertial sensor is mainly studied.The positioning system uses the ZigBee module to form the wireless sensor network and uses Arduino to acquire and process the attitude data from MPU9250.The location information will be transmitted to the host computer via ZigBee network.The robot inertial navigation technology proposed in the paper has certain guiding significance to solve the problem of robot positioning.

swarm;Arduino;ZigBee;positioning;MPU9250

TP368

A