基于STM32轮式机器人姿态系统的综合测试平台设计与实现

王锋，周翟和，陈如意

(南京航空航天大学 自动化学院，江苏 南京 210016)

0 引言

两轮自平衡机器人作为一种特殊的倒立摆式的移动机器人，具有非完整性、非线性、欠驱动和不稳定等特点，这使得它能够成为验证各种控制算法的理想平台[1]。姿态系统需要对它的运动姿态等信息有准确的估计，以实现动态平衡。但是由于感知系统采用的传感器极易受到噪声、温度等环境因素的干扰，影响系统估计的准确性。

为了提高姿态系统的开发效率和质量，分析实际的姿态测量过程，测试姿态系统的性能。结合软硬件，针对轮式机器人姿态系统，设计了一个综合测试平台，对提高轮式机器人姿态系统的可靠性具有重要意义[2]。

1 系统硬件设计

图1为所设计的综合测试平台的硬件框图。综合测试平台的硬件部分主要由测试点模块、主控模块、显示模块组成。主控制器模块是整个测试平台的核心，用来采集测试点数据，对数据进行处理以及运算，并将处理后的数据传输给显示模块，借助显示模块测试信号滤波前后的效果。

图1 测试平台硬件框图

1.1 主控模块介绍

主控模块是测试平台的核心，主要实现数据采集、数据的预处理、卡尔曼滤波处理等工作。主控模块采用STM32控制器，内嵌了ADC以及IIC通讯接口，被测试系统可以以从机的模式通过IIC与主控模块进行通讯[4]。

1.2 显示模块介绍

为了实时显示被测信号的变化，本文针对被测系统的性能，利用MATLAB的GUIDE快速开发环境设计了一套基于MATLAB图形用户界面(GUI)信号测试界面，通过设置各控件属性以及回调函数实现系统功能[4]。上位机可以通过建立好的MATLAB GUI界面可以实时显示被测系统的工作状态，将被测信号进行实时显示，并对采集到的数据进行存储以及分析；另一方面，可根据需要，随时编写相应的程序，升级测试平台的功能。

1.3 测试点传感器电路设计

测试点主要是被测系统的输入模块，即姿态传感器。被测系统采用的姿态传感器分别为MMA7361加速度计以及ENC03陀螺仪。加速度计传感器MMA7361是美国Freescale推出的一款低成本单芯片三轴加速度传感器。该微型电容式加速度传感器提供了两种加速度测量范围： ±1.5 g和6 g[5]。可通过芯片引脚的设置来选择量程范围，并输出与加速度成正比的电压信号。以X轴为例，所受加速度与输出电压关系如下[6]：

(1)

式(1) 表示，当加速度计输入量分别为为1g、0g、-1g的重力加速度信号时，理论输出模拟电压分别为2.45V、1.65V、0.85V。由式(1)也可以看出，MMA7361加速度计所受加速度从-1g～+1g，与对应输出电压成线性关系[7]。测试点MMA7361模块电路图如图2所示。

测试点陀螺仪传感器选用的是村田ENC03型号，它利用了旋转坐标系中的物体会受到科里奥利力的原理，在器件中利用压电陶瓷做成振动单元。当旋转器件时会改变振动频率从而反映出物体旋转的角速度[8]。测试点陀螺仪ENC03模块的电路如图3所示。测试点的传感器模块可以作为从机使用，通过IIC与主机STM32进行通信。

图2 测试点MMA7361模块电路

图3 测试点ENC03模块电路

为了降低30～33kHz的噪声(陀螺仪谐振频率)，在图4的ENC03模块电路中，设计了截止频率比传感器频率(50Hz)更高的低通滤波器，对陀螺仪信号进行硬件滤波处理。传感器温度漂移的部分，可以经A/D采样后，由软件对采样的原始数据进行处理，消除温漂。

2 系统软件设计

2.1 总体流程设计

测试平台的主程序流程图如图4所示，主要由IIC初始化、姿态信息初始化、姿态传感器数据采集和通讯、卡尔曼滤波处理等部分组成。此部分的卡尔曼滤波器目的是模拟被测系统中的卡尔曼滤波器，以此来测试被测系统中卡尔曼滤波器的数据处理效果。

在读取测试点数据之前，首先需要对IIC总线进行初始化，IIC的初始化需要设置总线的时钟频率、中断允许标志位以及模块的工作模式。然后利用IIC总线写指令对姿态传感器进行初始化[9]。在完成数据换算之后，利用卡尔曼滤波算法对采集的加速度计和陀螺仪数据进行处理。

图4 主程序流程图

2.2 卡尔曼滤波器设计

被测对象是轮式机器人姿态系统，其内部采用的算法是卡尔曼滤波算法。设计的综合测试平台需要测试该算法在姿态系统中的处理效果，因此需要在测试平台上，设计一个与被测系统内部相同的滤波器，便于测试算法的效果。系统状态向量的选取是设计卡尔曼滤波器的关键[10]。因为机器人倾斜的角度与倾斜的角速度存在导数关系，因此可以选择倾斜角作为状态向量的一个元素。由于倾斜的角加速度导数无法给出，所以不能作为状态向量的另外一个元素，采用加速度计来估计陀螺仪的零位偏差Gyro_bias，这样就得到了状态向量的另外一个元素[11]。机体倾斜的角度与倾斜的角速度之间存在以下关系:

Anglek=Anglek-1+(Gyrok-1-Gyro_biask-1).dt

(2)

式中，Anglek是估算出来的k时刻的倾斜角度；Anglek-1为加速度计解算出来的k-1时刻的倾斜角度；Gyrok-1为陀螺仪解算的到k-1时刻的倾斜角度；Gyro_biask-1为k-1时刻陀螺仪的位偏差；dt为采样时间。

令：

Gyro_biask=Gyro_biask-1

(3)

综合式(1)、式(2)两式，将其改写成状态矩阵的形式，获取离散卡尔曼状态空间表达式：

(4)

过程噪声协方差矩阵Q表达形式如下：

(5)

式中的Q_Gyro和Q_Angle分别是陀螺仪所测得的载体倾角过程噪声协方差以及加速度计所测得的角速度过程噪声协方差，不同的取值代表了不同的信任程度。机体处于高速变化运行状态时，Q_Gyro设置的小一些，Q_Angle设置的大一些，使得滤波器更相信陀螺仪数据；机器人处于低速变化运行状态时，可以将Q_Angle设置的小一些，使得滤波器更相信加速度计数据。

3 系统测试与实验

3.1 滤波器性能测试

被测系统中采用卡尔曼滤波算法对姿态传感器进行数据融合处理。因此需要测试姿态传感器在算法处理前后的效果。选取的测试点是被测对象的输入模块，即姿态传感器。此部分主要测试被测系统输入模块经过卡尔曼滤波算法处理的效果。

测试平台在内部模拟了一个与被测系统相同的卡尔曼滤波器，利用采集到的测试点数据来测试滤波器的性能。静态条件下，完成测试点加速度计传感器数据的采集，并利用模拟的卡尔曼滤波器对原始数据进行滤波处理。然后利用测试平台的上位机显示模块绘制原始信号和滤波处理后的信号曲线，便于对比滤波器处理前后的信号变化以及对传感器静态误差进行分析。

图5是静态零输入条件下加速度计的原始测试数据。可以看出，未做滤波处理时，MMA7361加速度计静态信号毛刺严重，误差区间在[-1 1]。图6是经过滤波器处理后的加速度计数据。可以看出，经过滤波处理后，MMA7361加速度计静态数据的毛刺大大减少，静态误差保持在[-0.2 0.4]区间。

图5 加速度计零输入测试信号

图6 加速度计滤波后数据

3.2 基于GUI的传感器测试

为了实时直观地显示被测信号，利用MATLAB的M语言编写串口通信的GUI，将被测点信号通过串口实时绘制信号曲线。在开启GUI实时打印被测信号前，需要确保被测点模块电路与STM32的正确连接。基于GUI的传感器数据打印显示提供了一种新的测试手段，其设计流程如图7所示。

图7 GUI设计流程图

上位机检测到的串口号为COM6，所以在GUI中要设置正确的串口号，否则就无法实时打印被测信号(图8)。由于STM32内部程序设置的串口波特率为9 600，所以GUI中也要设置为9 600。

3.3 姿态角动态测试

1) MATLAB显示

利用本文设计的综合测试平台对姿态系统的输出信号、俯仰角和横滚角进行测试并借助上位机显示。

图9、图10是综合测试平台显示的被测系统输出信号。由于轮式机器人主要以俯仰角和横滚角运动为主，因此本文设计的综合测试平台以测试横滚角和俯仰角信号为主。为了更好地测试机器人姿态系统的性能，将轮式机器人绕俯仰轴、横滚轴连续运动，分别产生俯仰角以及横滚角信号，将姿态系统中的输出信号传送至上位机中，借助MATLAB通用性曲线打印方法，绘制姿态角信号。

图9 动态俯仰角测试

图10 动态横滚角测试

2) 示波器信号实时检测

将经过卡尔曼滤波算法融合后的姿态信号，通过主控制器STM32内部的DAC1转换成模拟信号并接入示波器。借助示波器测试姿态角信息实时性强。示波器能实时检测到微弱的信号变化，如图11、图12所示。

图11 俯仰角模拟电压信号

图12 横滚角模拟电压信号

4 结语

以机器人姿态系统测试为例，以STM32控制器为核心设计了综合测试平台的硬件模块和软件模块，利用所设计的测试平台分别测试了姿态系统的输入模块信号、卡尔曼滤波算法性能以及被测系统的输出信号。此外，还介绍了基于GUI的测试界面设计流程。为实时测试被测试点信号提供了界面化的方法。

[1] Salerno A, Angeles J. The control of semi-autonomous two-wheeled robots undergoing large payload-variations[J]. IEEE International Conference on Robotics & Automation,2004, 2(2):1740-1745.

[2] 刘鲁. 基于PLC控制系统的电机测试平台的设计与调试探究[J]. 电子测试, 2014(9):3-4.

[3] 李振全, 何波贤, 赵凤全,等. 两轮平衡车姿态检测系统设计[J]. 电脑编程技巧与维护, 2015(21):7-9.

[4] 王巧花, 叶平, 黄民. 基于MATLAB的图形用户界面(GUI)设计[J]. 煤矿机械, 2005(3):60-62.

[5] 唐海玲, 赵春雨, 宋家友. 基于MMA7361加速度传感器的重力感应遥控小车设计[J]. 传感器世界, 2013, 19(6):9-12.

[6] 汪浩. 电容式微加速度计检测原理与设计分析[J]. 巢湖学院学报, 2005, 7(3):87-89.

[7] 杨继志, 郭敬. 基于MMA7260两轮自平衡小车控制系统设计[J]. 机电产品开发与创新, 2011, 24(3):144-145.

[8] 刘海刚, 宋一标, 陈恒伟,等. 单轴两轮自平衡小车姿态控制系统设计[J]. 工业控制计算机, 2013, 26(9):16-18.

[9] Han F, Liu T, Li L, et al. Design and Fabrication of a Differential Electrostatic Accelerometer for Space-Station Testing of the Equivalence Principle[J]. Sensors, 2016, 16(8)：20-23.

[10] 刘春阳, 徐军领, 程洪涛,等. MPU9250传感器的姿态检测与数据融合[J]. 河南科技大学学报(自然科学版), 2015, 36(4):14-17.

[11] 李伟. 两轮自平衡机器人的姿态检测系统的设计[J]. 中国科技纵横, 2011(15):213-213.