基于NI myRIO的两轮自平衡小车设计

王学卿，庄晓东

(青岛大学 电子信息学院，青岛 266590)

0 引言

近年来，两轮自平衡电动车以其行走灵活、便利、节能等特点得到了很大的发展，但其本身是一种非线性、多变量和强耦合的类倒立摆系统[1-3]，其控制难度大、算法复杂。两轮车保持直立平衡的前提是要通过传感器获得车体稳定精确的姿态，但由于陀螺仪和加速度计等惯性传感器的固有特性，测量的数值会随时间产生不同程度的偏差和漂移[4]，本文通过使用互补滤波算法融合两者的数据解决这个问题。其次简单有效、健壮的控制算法是两轮车保持平衡的关键，本设计首先使用角度PD控制器对车身倾角进负反馈控制，实验结果显示单纯使用该控制算法不能让两轮车维持很好的平衡，因此又引入速度PI控制器，通过让车体保持速度为零，从而增强平衡性能。

LabVIEW图形化的编程环境和模块化的软件设计流程，非常符合工程思维[5-6]，配合NI的硬件平台myRIO大大缩短了本设计的开发和调试周期。

1 系统硬件设计

系统的硬件部部分主要包括核心控制器myRIO、电源管理模块、mpu6050六轴传感器、带电磁(霍尔）编码器的直流减速电机以及TB6612电机驱动模块。下图是系统硬件的整体框图。

图1 .1 系统硬件整体框图

1.1 核心控制器myRIO

NI myRIO是NI（美国国家仪器有限公司）针对教学和学生创新应用而推出的嵌入式系统开发平台，内嵌Xilinx Zynq芯片，这款芯片集成了双核ARM Cortex-A9处理器以及Xilinx FPGA[7-8]，LabVIEW程序可以自动编译并在ARM实时处理器中执行。控制器除了常见的模拟输入、模拟输出、数字I/O之外，还包括I2C总线、SPI总线、PWM、编码器、UART等接口，是一款强大的嵌入式实时处理器。

1.2 电源管理模块

本模块的核心是DC-DC升压电路，主要负责给各部分模块提供稳定供电。该模块输入电压为11.1v ，由3S航模锂电池提供；经过DC-DC升压后输出电压为12v，直接为myRIO控制器和TB6612电机驱动模块供电。

1.3 mpu6050六轴传感器

MPU-60X0 系列是全球首例 9 轴运动处理传感器。它集成了 3 轴 MEMS陀螺仪,3 轴 MEMS 加速度计,以及一个可扩展的数字运动处理器 DMP(Digital Motion Processor)[9]。能够同时输出x,y,z三个轴上的加速度与角速度数字信号。由该模块采集两轮车的当前姿态数据包括z轴上的加速度与y轴的角速度，并通过I2C接口将数据传送给myRIO。

1.4 带电磁(霍尔）编码器的直流减速电机

直流减速电机为双轮平衡车提供动力，减速比为30：1，电机尾部自带了 13 线的AB相增量式磁（霍尔）编码器车轮转一圈，电机可以输出 390个脉冲，编码器的额定工作电压是 5V,集成了上拉电阻和比较整形功能，可以直接输出方波，通过与myRIO的编码器接口相连，便可以获取平衡车的速度信息。

1.5 TB6612电机驱动电路

TB6612FNG是东芝半导体公司生产的一款直流电机驱动器件，具有大电流MOSFET-H桥结构[10]，双通道电路输出，每通道输出最高1A的连续驱动电流，4种电机控制模式：正转/反转/制动/停止。myRIO控制器将一定频率、占空比的PWM信号输入到该模块，改模块继而控制电机转速和方向。

2 软件和算法设计

本系统的程序部分采用myRIO版本的图形化编程开发环境LabVIEW 2017开发，它可以直观而又高效地修改控制参数、实时显示平衡车的速度和姿态数据。LabVIEW中已经内置了多种现成的函数，并且针对NI myRIO各种外围I/O提供不同层次的驱动函数，包括模拟输入、模拟输出、数字I/O，I2C总线、SPI总线、PWM、编码器、UART等接口驱动函数，这都大大缩短了整个系统的开发和调试周期。下图是系统软件的流程图和上位机界面。

图2 .1

程序采用LabVIEW状态机架构，增强了程序的灵活性，方便代码维护。系统上电开机后程序首先执行初始化，包括初始化myRIO上的编码器、设定输出PWM波的频率，初始化mpu6050传感器并设定其工作方式。之后进入状态机循环，第一个状态是“数据采集”，在此状态下myRIO通过I2C通信协议读取mpu6050寄存器内z轴加速度值和y轴的角速度值，同时读取编码器模块的计数值；之后程序进入“PID控制状态”，首先myRIO通过互补滤波算法将加速度计数据与陀螺仪数据融合得到稳定的车体倾角，同时对编码器的计数值做滑动滤波作为平衡车当前的速度，紧接着利用对角度误差和速度误差进行PID控制得到输出即PWM波的占空比，将其输入给PWM模块继而控制电机的转速；最后程序来到“判断停止”状态，读取上位机“停止”控件的值，如果为真则将PWM占空比置零、清空编码计数值，终止整个程序，如果为假程序又会进入“数据采集”状态，完成新一轮的控制过程。本系统的控制周期为5ms，即实现两次控制的时间间隔为5ms。下面将分别介绍几个关键算法的实现。

2.1 基于互补滤波的姿态测量算法

本系统使用的mpu6050六轴传感器可以同时输出加速度值和角速度值，当单独使用加速度计时，车体的倾角可由公式（1）计算：

其中代表单独使用速度计得到的第次的车体与竖直方向的夹角，为加速度计在z轴上采集的第n次数据。由公式（1）得到的姿态角度误差不会随时间积累，低频性能好；但实际车模运行过程中，由于车体本身的摆动所产生的加速度会产生很大的干扰信号，使测量的角度中携带较高频率的噪声，如图2.1 a)所示。

图2 .1

当单独使用陀螺仪的时，myRIO循环采样获取陀螺仪的角速率信息，即每隔一段很短的时间采样一次，采用累加的方法实现积分的功能来计算角度值：

从以上的分析来看，单独使用加速度计或陀螺仪得到的姿态信息并不能准确的反应车体姿态，从而难以让平衡车保持直立状态。但二者在频域上具有互补特性，可采用互补滤波器对这两种传感器数据融合，提高姿态角度测量的精度和动态响应的性能[12]，其主要思想是充分利用加速度计提供的低频角度信号和陀螺仪给出的高频角速度信号，对加速度计和陀螺仪估计出的角度信号和分别通过低通滤波器F1(s)和高通滤波器F2(s)进行滤波，滤除相应干扰信号[13]，最后融合得到较为精确和稳定的车体倾角。其原理框图如图2.2所示：

频域表达式为

图2 .2 互补滤波器的原理框图

2.2 平衡控制算法

ek表示对误差的第k次采样，T为系统的采样周期，Ti、Td分别为PID控制器的积分、微分时间。由于二轮平衡小车的高度不稳定性需要对干扰做出迅速响应，同时积分会存在累积误差，故本设计只引入式（7）实现的PD控制：

车体直立平衡时车体倾角为0°，因此PD控制器的输入偏差

由于本设计的机械结构所限，myRIO控制器只能放在平衡小车顶部，造成车体重心较高，加之电机质量问题使得单纯对角度进行PD控制来实现车体直立平衡的效果不好，所以本系统又引入了速度控制环，通过PI控制器使速度为零进一步增强车体直立平衡的效果。对速度实现PI控制的过程如式：

将（10）、（11）的运算结果Δ uk1、Δ uk2叠加作为控制电机转速的PWM波的占空比，并作用到电机驱动电路上继而控制左右电机实现负反馈控制。

3 系统调试与运行测试

3.1 互补滤波器参数选择

在式（4）中当确定了系统采样周期T，参数τ就决定了式（5）中K1、K2的大小。当τ大于采样周期T时，K1＞K2，加速度传感器测定的角度值比重比陀螺仪的大；反之，陀螺仪的积分起主要的作用[14]。本系统中采样周期T=5ms，经过实际测试τ取0.245s时互补滤波的效果最好，此时式K1=0.98，K2=0.02。本文同时对比了卡尔曼滤波算法，结果曲线如图3.1所示。

采用互补滤波算法相较于卡尔曼滤波算法效果相当，但显然互补滤波算法更加简单，能够大大节省控制器的运算资源。

图3 .1 互补滤波后的车体倾角曲线

3.2 PID参数整定

本设计中存在两个PID控制环，但速度PI控制器的引入仅仅是辅助角度PD控制，增强系统的平衡性能。首先调节角度PD控制器的参数，在实际调试中遵循先比例后微分的过程。调节式（10）中的比例参数让其一直增加，直到车体出现往复的低频抖动，此时引入微分参数使其不断增加[15]，直到车体能够保持平衡但有频率较高的抖动出现，至此便确定了P、D参数的最大值，最终将每个系数乘以0.6得到理想的参数值。

由于机械结构的缺陷，经过测试发现无论怎样调节角度PD控制器的参数，系统始终不能达到一个理想的直立平衡效果，总是产生一定速度的前后摇摆。此时引入速度PI控制器辅助直立平衡的控制，同样遵循先比例后积分的原则，找到合适的Kp2之后可把Ki2的值设置为Ki2=Kp2/200。实验结果表明使用角度PD控制器配合速度PI控制器，本系统能够很好的实现直立平衡，并且具有一定的抗干扰能力。

4 结语

文中首先设计了二轮平衡车的整个硬件系统，之后又讲解了基于LabVIEW 2017开发的软件系统及其工作流程。针对mpu6050的加速度和陀螺仪输出的角度信号的特点，深入的研究了互补滤波算法，从原理以及实现过程上都做了较为详细的讲解。实验数据直观的展示了互补滤波算法的有效性，它能为PID控制器提供稳定、实时的车体倾角信号。为了实现系统的直立平衡，引入了角度PD控制器和速度PI控制器，在选择了合适的参数后，实验结果表明该系统实现了稳定的直立平衡。随着两轮平衡小车的普及，本文设计的简单有效的互补滤波算法和PID控制算法会有更广的应用价值。

[1] 杨世勇，徐莉苹，王培进. 单级倒立摆的PID控制研究[J]. 控制工程，2007（B05）：23-24．YANG Shi-yong， XU Li-ping， WANG Pei-jin. Study on PID Control of a Single Inverted Pendulum System[J]. CONTROL ENGINEERING OF CHINA， 2007（B05）： 23-24．

[2] 薛凡，孙京诰，严怀成. 两轮平衡车的建模与控制研究[J]. 化工自动化及仪表， 2012，39（11）：1450-1497.XUE Fan， SUN Jing-gao， YAN Huan-cheng. Study on Modeling and Control of Two-wheeled Auto-balancing Vehicle[J]. Control and Instruments in Chemical Industry， 2012，39（11）：1450-1497.

[3] 傅忠云，陈秋阳，刘文波. 基于直接转矩控制的两轮自平衡车系统设计[J]. 压电与声光， 2014， 36（6）： 967-971 FU Zhongyun， CHEN Qiuyang， LIU Wenbo. The Design of Two-wheel Self-balanced Vehicle System Based on Direct Torque Control[J].Piezoelectrics & Acoustooptics，2014，36（6）：967-971.

[4] 赵杰，王晓宇，秦勇，等．基于UKF的两轮自平衡机器人姿态最优估计研究[J]．机器人，2006，28（6）：605-609．ZHAO Jie， WANG Xiao-yu， QIN Yong， et al. UKF-based Optimal Attitude Estimation of Two-wheeled Self-balanced.Robots[J]. ROBOT，2006，28（6）：605-609．

[5] 王建群，南金瑞，孙逢春，等. 基于LabVIEW的数据采集系统的实现[J]. 计算机工程与应用， 2003， 39（21）： 122-125.Wang Jianqun， Nan Jinrm， Sun Fengchun， et al. Realization of the Data Acquisition System Based on LabVIEW[J]. COMPUTER ENGINEERING AND APPLICATIONS， 2003，39（21）：122-125.

[6] 杨忠仁， 饶程， 邹建，等. 基于LabVIEW数据采集系统[J]. 重庆大学学报（自然科学版）， 2004， 27（2）： 32-35.YANG Zhong-ren， RAO Cheng， ZOU Jian， et al. Study on DAQ System by Using LabVIEW[J]. JOURNAL OF CHONGQING UNIVERSITY（NATURAL SCIENCE EDITION）， 2004，27（2）：32-35.

[7] 茅颖. 基于myRIO控制器的自平衡车的PID算法实现[J]. 福建电脑，2016，32（12）：107-109.

[8] 谢斌，张超，毛恩荣，等 . 基于 myRIO 的电动拖拉机驱动控制器设计与室内试验 [J]. 农 业 工 程 学 报， 2015， 31（18）： 55-62.Xie Bin， Zhang Chao， Mao Enrong. et al. Motor controller design and indoor experiment for electric tractor based on myRIO[J]. Transactions of the Chinese Society of Agricultural Engineering，2015，31（18）：55-62.

[9] 陈炜峰，席万强，周峰，等. 基于物联网技术的山体滑坡监测及预警系统设计[J]. 电子器件，2014，37（2）：279-282.CHEN Weifeng， XI Wanqiang， ZHOU Feng， et al. Landslide Monitoring and Warning System Based on IOT Technology[J]. Chinese Journal of Electron Devices，2014，37（2）：279-282.

[10] 王建平， 卢杉， 武欢欢. TB6612FNG在直流电机控制设计中的应用[J]. 电子设计工程，2010，18（6）：65-67.WANG Jian-ping， LU Shan， WU Huan-huan. Application of TB6612FNG in DC motor control system[J]. ELECTRONIC DESIGN ENGINEERING， 2010，18（6）：65-67.

[11] 傅忠云， 朱海霞， 孙金秋，等. 基于惯性传感器MPU6050的滤波算法研究[J]. 压电与声光，2015，37（5）：821-829.FU Zhongyun， ZHU Haixia， SUN Jinqiu， et al. Study on Filtering Algorithm Based on Inertial Sensors MPU6050[J]. Piezoelectrics &Acoustooptics， 2015，37（5）：821-829.

[12] 詹长书，邢博坤，朴光宇，等. 两轮自平衡小车的控制策略设计[J]．森林工程，2015，31（4）：73-82．Zhan Changshu， Xing Bokun， Piao Guangyu， et al. Control Strategy of Two-wheeled Self-balancing Car[J]. Forest Engineering， 2015， 31（4）：73-82．

[13] 陈鹏展， 朱年华， 李杰 . 两轮自平衡车姿态检测与平衡控制 [J]. 控制工程， 2017， 24（8）： 1574-1578.CHEN Peng-zhan， ZHU Nian-hua， LI Jie. Attitude Detection and Balance Control of Two-wheeled Self-balancing Vehicles[J]. Control Engineering of China， 2017，24（8）：1574-1578.

[14] 卢锋. 两轮小车姿态检测与平衡控制研究[J]. 工业仪表与自动化装置， 2014，（3）： 26-75.LU Feng. The research of vehicle attitude detection and balance control[J]. Industrial Instrumentation & Automation， 2014，（3）： 26-75.

[15] 佟远， 张莎 . 基于PID双闭环的旋转倒立摆控制系统[J]. 测控技术， 2016， 35（8）： 85-88.TONG Yuan， ZHANG Sha. Rotary Inverted Pendulum Control System Based on PID Double Closed Loop[J]. Measurement & Control Technology， 2016，35（8）： 85-88.