基于MEMS传感器的两轮平衡小车设计

李详鹏 陈春 周子文 施娜

摘  要：介绍一种基于MEMS加速度传感器的两轮平衡小车设计。以STM32F103RBT6 ARM微处理器为主控制器，以一片TB661

2FNG驱动小车两轮，采用MPU-6050 MEMS加速度传感器做为小车运动姿态信息感知的两轮自平衡系统，通过直立角度环和速度环串级PID实现了小车的动平衡和静平衡控制。

关键词：平衡小车;MEMS传感器;串级控制

中图分类号：TP273 文献标志码：A 文章编号：2095-2945（2019）22-0027-03

Abstract： This paper introduces the design of a two-wheel balanced trolley based on MEMS accelerometer. The STM32F103RBT6 ARM microprocessor is used as the main controller， a piece of TB6612FNG is used to drive the two wheels of the car， and the MPU-6050 MEMS accelerometer is used as the two-wheel self-balanced system for the perception of the motion and attitude information of the car. The dynamic balance and static balance control of the car are realized by vertical angle loop and velocity loop cascade PID.

Keywords： balance trolley; MEMS sensor; cascade control

引言

两轮自平衡小车是一个集动态决策和规划、环境感知、行为控制和执行等多种功能于一体的综合复杂系统，其关键是在解决自平衡的同时，还能够适应在各种环境下的控制任務。通过运用外加速度传感器、超声波传感器、倾角传感器、防碰撞开关等，可以实现小车的跟踪、路径规划和自主避障等多种复杂的功能。还可以把GPS和惯性导航设备配备到小车上实现组合导航。所以，两轮自平衡小车其实是一个实现起来相对简单又复杂系统，受到世界各个国家的重视，具有较高的学术研究意义。

1 硬件系统设计

1.1 硬件系统总体框架

自平衡两轮小车控制系统的总体硬件框图如图1所示：

整个硬件控制系统以ST意法半导体的ARM微处理器STM32F103RBT6为核心主控芯片，通过IIC接口实时获取MPU-6050 MEMS运动传感器的姿态数据，通过卡尔曼滤波算法以及姿态解耦算法获得小车在X、Y、Z三个方向的姿态以及加速度信息，两路转速编码信号实时获取左右两个减速电机的转速和转向反馈信息，主控芯片通过位置环和速度环的PID输出PWM控制信号，驱动电机驱动模块芯片TB6612FNG接收到PWM信号之后再完成对电机的驱动控制，最终实现平衡小车的动平衡和静平衡以及动态运动控制。其中，蓝牙模块主要实现小车运动状态的远程设定与控制。

1.2 姿态检测传感器及其电路设计

平衡小车姿态检测主要是完成角度的测量和角速度的测量，这就需要用到加速度计和陀螺仪，在实际使用中，同时使用陀螺仪和加速度计会产生轴间差的问题，为了避免这个问题，这里选择了Invensense公司的MPU6050模块，它首创的整合性6轴运动处理组件，是全球首例，其中包含三轴角速度，三轴加速度，厂家提供了一个信号处理驱动库（DMP驱动），用户借此能够方便高效地解决数据融合的难题。MPU6050传感器外围电路如图2所示。MPU6050 拥有一个中断系统，并可根据用户需求进行编程设定中断输出间隔，INT管脚可以产生中断信号。

1.3 TB6612FNG电机驱动电路设计

为保持平衡小车处于动态平衡的状态，就要在控制系统中做到根据小车姿态不断地变换电机的正反转，这就要求电机驱动电路拥有大电流输出使电机快速切换运行状态的能力，从而防止在工作过程中出现堵转的现象。东芝半导体公司TB6612FNG具有两路电机驱动输出通道，每通道能输出最高1.2A的连续驱动电流，启动峰值电流达2A/3.2A（连续脉冲/单脉冲）;4种电机控制模式：正转/反转/制动/停止;PWM支持频率高达100kHz;驱动不需要外加散热片，而且外围电路比较简单，只需要外接电源滤波电容就能直接驱动电机，大大简化了硬件设计难度。图3所示是电机驱动电路。

VM1，VM2，VM3直接用12V电池供电，模块的引脚3、4、9、10、18接地，当 STBY接高电平时模块正常工作。A路电机的正极和负极分别接到AO1和AO2。然后通过 PWMA、AIN2、AIN1控制电机。如表1所示TB6612FNG控制的真值表。

2 软件设计

系统软件设计：

（1）主程序软件设计

主程序主要实现的工作是各个变量的定义，各模块的初始化，中断的初始化等部分，程序的流程图如图4所示。从主函数开始，首先完成对系统各模块的初始化设定，开启MPU6050传感器5ms定时输出中断，再进入主循环的函数。

（2）控制软件设计

如图5所示是控制函数的主要工作流程。为主函数提供50ms的精准延时，5ms读取一次陀螺仪和加速度计的值，这样的采样频率可以改善卡尔曼滤波和互补滤波的效果。

平衡小车调试直立控制使用的是PD（比例微分）控制器，如图6所示是直立调试的流程。平衡小车调试速度控制使用的是PI（比例积分）控制器，如图7所示是实现速度调试流程。

平衡小車获取角度使用的算法有三种：四元数、互补滤波和卡尔曼滤波。平衡小车的卡尔曼滤波器设计关键要素是选取状态向量，平衡小车的车体倾斜角度和车体的倾斜角速度是倒数关系，所以状态向量的元素可以选择为车体的角速度（Q_gyro）、车体的倾角（Q_angle）和选取加速度计来估计陀螺仪的零位偏（Q_bias），卡尔曼滤波采样时间为dt=0.005，观查噪声协方差矩阵（R_angle）。

平衡小车的调试主要分为三个环节：直立控制调试、速度控制调转向控制调试。

a.平衡小车直立控制的调试使用的是PD（比例微分）控制器。根据直立控制的调试过程，得到如表2，表3所示的结论。

可以看出P、D参数的最大值是kp=500，kd=1.7。根据实践经验，对每个参数乘以0.6，取整数部分得到我们最终需要的理想参数 kp=300，kd=1。

b.平衡小车速度控制的调试中使用的是PI（比例积分）控制器。根据速度控制的调试过程，得到如表4所示的结论。

从表4中我们能看到速度控制P、I参数的理想值是kp=80，ki=0.4。

c.平衡小车转向控制调试中使用的是P（比例）控制器，根据转向控制的调试过程，得到如表5所示的结论。

根据表5中的数据，我们可以确定转向控制 P 参数的理想值是kp=1。综上所述，PID参数的调试就到达了设计要求，平衡小车能稳定的做自平衡运动。

3 结束语

平衡小车与倒立摆系统很相似，是一种多变量、非线性、强耦合、参数不确定的复杂系统，它体积小、简单的结构、灵活的运动性能，非常适合在狭小的空间内工作，因此成为检验各种控制方法处理能力的理想装置。通过对小车软硬件系统分析，研究平衡小车在自平衡基础上的其他性能，例如在特殊环境下的导航、拐角的转弯等问题，对解决在工业生产和社会生活中的许多问题有很大的帮助。

参考文献：

[1]王晓宇.两轮自平衡机器人的研究[D].哈尔滨工业大学，2007.

[2]闻双云.两轮自平衡小车控制算法的研究与优化[D].吉林大学，2017.

[3]王泉.两轮自平衡机器人自主移动系统设计和应用[D].武汉科技大学，2018.

[4]范淇元，覃羡烘，李洪毅.基于STM32的双轮平衡机器人的控制系统设计[J].自动化与仪表，2018（03）：20-21+41.

[5]潘二伟.基于STM32的两轮自平衡车设计与实现[D].黑龙江大学，2018.