实验教学机器人的无线传感网络软件设计

陈一铭，吴耀光，陈科全，郭小龙，张铁异

（广西大学 机械工程学院，广西 南宁 530000）

0 引 言

很多高校都开设了有关机器人的课程，但在使用实验教学机器人过程中，还缺少对机器人状态的实时感知[1]。本文在六自由度教学机器人实验平台上，搭建ZigBee无线传感网络，实时监测机器人运动状态[2]，对提高高校实验教学机器人教学效果以及教学管理效率具有一定意义。

1 总体方案设计

本文的实验平台采用具有6个转动关节的教学机器人来进行实验。通过多种传感器的应用，实时监测机器人在运动中的角速度、加速度、关节姿态、电机温度等重要参数。在实验教学过程中，能够让学生直观了解机器人运动状态的变化，通过ZigBee无线传感网络系统在实验教室之间形成局域网[3]，并对范围内的多台实验教学机器人进行实时状态监测。

2 无线传感网络软件设计

无线传感网络软件设计主要包括协调器节点程序设计、路由器节点程序设计、终端节点程序设计、机器人运动状态采集节点设计和驱动电机温度节点程序设计。

2.1 Z-Stack协议栈

本文在IAR Embedded Workbench for 8051开发环境下进行，开发基础是德州仪器公司推出的符合ZigBee 2007/Pro协议规范的Z-Stack协议栈，并根据课题需求对其进行程序设计、编译，通过开发底板的JTAG接口下载至无线核心模块中，最终搭建起能够感知网络内机器人运动状态的ZigBee无线传感网络。

Z-Stack是采用ZigBee 2007/Pro协议规范的协议栈，它是一种半开源的协议栈[4]。Z-Stack协议栈与ZigBee协议模型之间有相对应代码，大大降低了协议栈难度，为开发工作带来极大的方便[5]。

协议栈工作机制是基于OSAL操作系统。该操作系统通过把每个协议层都定义成不同任务，每个任务下面由许多事件组成[6]。任务的执行采用轮询机制，即不同任务有不同优先级别。

2.2 协调器节点程序设计

协调器接收路由器转发的电机温度以及机器人运动状态参数，进行处理后经过串口上传至上位机。协调器处于整个无线传感网络的中心位置，是无线传感网络中最关键的节点。整个传感网络只允许一个协调器存在，负责创建网络ID、各个节点地址分配、维护以及管理工作，并能够通过串口将数据发送到网关或者上位机。

设备上电后，首先进行硬件初始化，接着进行网络初始化，步骤如下：

（1）建立协调器，通过射频前端发送信标请求帧（Beacon Request Command），接着设置扫描期限（T_scan_duration），在扫描期限内无信标回应时，则确定无冲突网络存在，并创建ZigBee网络，之后协调器节点不断产生信标帧在网络半径内广播，告知网络存在。

（2）扫描信道，对设定的信道进行能量检测，对可用信道进行标注，接着网络层发送MLME-SCAN.Request进行主动扫描，搜索网络半径内的信标帧，通过搜索到的信息确定其他网络ZigBee设备使用信道情况，最后根据收到的信息来确定最合适而且无冲突信道，在主动扫描期间，MAC层只处理物理层数据服务接收到的信标帧。

（3）设置个域网地址（PAN ID），其是一个16位的无线网络标志符。通过协议栈配置文件f8wConfg.cfg中的ZDO_CONFIG_PAN_ID函数，可以设置PANID为0～0x3FFF之间的一个值。一个无线传感网络只允许一个PAN ID存在，而且它在所处的信道中地址ID是唯一的。此外还确定协调器短地址，短地址是网络中设备的标识号。默认协调器短地址为0x0000。

以上3个步骤完成后，网络初始化完成，进入监控模式，等待其他节点加入。

2.3 路由器节点程序设计

路由器在无线传感网络的功能是拓展网络半径，帮助网外设备加入网络，并转发其他节点数据。在本文中路由节点负责转发终端节点监测到的电机温度还有机器人运动状态参数。在定义为路由器的设备上电后，设备会运行应用层函数GenericApp_Init（）进行软、硬件的初始化，接着进行节点加入网络初始化，一共有4个步骤。

（1）扫描指定信道，在该信道里发送请求命令帧（Beacon Request）。如果射频范围内存在协调器，在收到请求命令帧之后，回复给路由器信标帧，信标帧里包含了协调器当前网络的状态信息。

（2）路由器比较收到的各个协调器信标帧信息，根据比较结果，选定最合适的网络协调器作为父节点，发出关联请求命令帧（Associate Request Command），协调器收到关联请求帧后MAC层根据当前拥有的资源（存储及能量）判断是否允许路由器节点加入，如果允许加入，则回复节点ACK。

（3）节点收到协调器的ACK后，等待协调器连接响应，之后协调器会给节点分配一个16位的网络短地址（Short Addr），用于网络内节点的识别。

（4）节点收到网络短地址后回复给协调器ACK，节点加入网络成功。建立成功后该节点以及协调器会同时在全网范围内广播该节点入网成功信息，并在广播数据帧（Broadcast）里携带有该入网节点的网络短地址。

2.4 终端节点程序设计

终端节点也就是无线传感网络的感知节点，主要任务是负责采集各个传感器的信息和终端数据的发送。

在搭建的整个无线传感网络系统中要实现的功能：一是采集传感器的各项数据；二是加入无线传感网络并定时向父节点发送数据。

终端节点入网流程与路由器入网流程一致，而传感器数据采集功能则根据不同的传感器软件设计。这里进行采集节点的发送数据功能设计，终端节点工作流程如图1所示。

终端节点加入网络后，协议栈进入OSAL的轮询工作机制，这里定义SAMPLEAPP_SEND_PERI-ODIC_MSG_EVT事件函数来执行定期发送数据返回父节点，如果deviceID=1，即判断这个是MPU6050节点，执行SampleApp_Send_MPU6050_Message（）函数，并定义osal_start_timerEx来进行定时，如果是DS18B20温度节点，则执行SampleApp_Send_Temp_Message（）函数来实现往父节点发送数据功能，每1 000 ms往父节点发送一次传感器数据。数据经应用层事件处理后，往下一层执行，最后经物理层处理成比特流后添加载波送至射频前端，发送至父节点。

图1 终端节点工作流程

2.5 机器人运动状态采集节点设计

该节点程序设计主要有4个工作流程，首先是通过加速度计以及陀螺仪获取节点的加速度、角速度；其次是传感器自带运动处理器DMP进行原始数据融合处理，输出表示姿态的四元数；接着在CC2530无线核心模块进行反三角运算，得出欧拉角；最后节点对原始数据以及欧拉角一共9个数据进行打包，发送至父节点。

2.5.1 加速度、角速度获取

针对于原始加速度、角速度的数据获取，主要是在CC2530上电后，定义MPU_Init（void）函数对传感器进行初始化设置，包括初始化I2C总线、复位传感器、选择相应量程等基本操作[8]。这里选择了加速度计±2g量程，由对应数据手册知灵敏度为16 384 LSB/g，陀螺仪角速度量程选择为 ±2 000(°)/s，其灵敏度是 16.4 LSB ·(°)-1·s-1。ax，ay，az分别对应 x，y，z三轴加速度，gx，gy，gz分别对应绕 x，y，z三轴角速度。

2.5.2 运动姿态解算

在3D表示姿态的图形学中，处理坐标系旋转方法有两种：方向余弦法和四元数法[9]。用方向余弦法计算姿态矩阵时，虽然可以实现全姿态工作，但是运算量巨大，需要求解多个微分方程，不适合在能力有限的系统运行；而四元数姿态运算获取旋转轴及旋转角度，计算量比前者小很多，故后者适合在运算能力有限的处理器上运行。

MPU6050芯片内置有独立的数字运动处理器（DMP），陀螺仪的三轴角速度、加速度计的三轴加速度数据经DMP自身运动处理算法处理后生成表示姿态的角度值，输出格式为q30的四元数，但四元数只在运算上有优势，无法直观表示物体的运动姿态[10]。

为了直观表达姿态角度值，需要在CC2530无核心模块上将四元数换算成欧拉角。

四元数q表示为：

旋转的四元数为单位四元数且模为1，可表示为：

“四元数表示”可转为“欧拉角表示”，如下：

式中：α为z轴的偏航角；ψ为y轴的俯仰角；θ为x轴的滚转角。

2.6 驱动电机温度节点程序设计

在本设计中，电机温度采集节点有两个，分别检测大臂步进电机以及中臂步进电机的温度。这里只就一个节点设计进行论述。

终端设备上电后，通过函数初始化DS18B20温度传感器，完成初始化输出，之后通过函数unsigned char ReadDS18B20（void）读取传感器采集到的温度数据，先读低8位温度数据，后读高8位温度数据，经过转换成温度数值后，由协议栈SampleApp Send_Temp_Message（）函数进行数据打包处理，最后将温度信息送至射频前端，发送至路由器。打包时，除了包含该节点温度值数据外，还有加入的节点device ID，方便区分不同节点的温度。传感器软件工作流程如图2所示。

图2 温度传感器软件工作流程

3 结 语

本文以六自由度机器人为载体，通过运用多种传感器，设计实验机器人无线传感系统的软件部分。通过采集机器人的实时状态，感知机器人运动状态，对信息进行无线传输，为以后研究打下了良好的基础。