基于STM32的两轮自平衡小车控制系统设计

季鹏飞，朱 燕，程传统，杜 晓

(盐城师范学院物理科学与电子技术学院，江苏盐城 224002)

两轮自平衡小车是一种自然不稳定体，其动力学系统具有多变量、非线性、强耦合等特性，是研究各种控制方法的理想平台。两轮小车的自平衡控制过程是利用微控制器对系统各种状态参数进行实时分析，使系统在水平方向或垂直方向上的位移和角度的偏移量被控制在允许的范围以内，使之在静态平衡点附近保持平衡，或者在一定范围内在运动过程中使小车保持平衡。因此小车控制的关键是姿态倾角、角速度及车轮加速度测量的准确性和实时性，这直接决定小车的控制精度及稳定性。针对这一问题，本文采用高性能单片机STM32F103C8T6作控制器，通过安装在车轮轴上的光栅码盘等实现小车位移、速度及运动方向的测量，用数字运动传感器MPU－6050实现车体倾角及倾角角速度的测量，利用卡尔曼滤波算法进行数据融合，精确地计算出小车的实时姿态信息，再利用PID控制算法实现小车的自平衡控制［1］。

1 动力学模型

两轮小车受力分析如图1所示。图中小车质量为m，车轮半径为r、车速为v，车体自身水平作用力为H、车轮受到地面摩擦力为Hf，θ为倾角，车轮对车轴的等效转动惯量为J，车体对车轮吊销转动惯量为I，车轮转速为ω，电机电磁转矩为Tm，车体质心位置坐标(x，y)，车体质量为ma，车轮对车身扭矩为T。在倾角θ较小的情况下，分析可得小车动力学方程可简化为［2］

图1 小车受力分析方程

式(1)为两轮自平衡小车车体倾角与角加速度的微分。文献指出在小车质量一定的情况下，地面摩擦系数越大、电机扭矩越高，小车自平衡可控角度范围越大，越容易控制小车的平衡［3］。

2 卡尔曼滤波数据融合算法

车体倾角测量由陀螺仪和加速度传感器实现，单独使用陀螺仪或加速度传感器都无法完成倾角的测量。一方面，陀螺仪能够测量车体倾斜的角速度，对角速度积分得到车体倾角，但在实际应用中陀螺仪输出的信号有偏差或漂移，经过积分运算后会形成随时间增加而增大的积累误差，导致无法得到有效的倾角信号;另一方面，小车运动中车身会不断摆动，导致加速度传感器输出的信号中有很大的干扰，虽然通过数据平滑滤波可有效降低干扰，但滤波后的信号已无法实时反映车体倾角的变化，因此仅用加速度传感器也无法实现倾角测量。陀螺仪和加速度传感器输出的信号中含有高斯白噪声，使用卡尔曼滤波算法将陀螺仪和加速度传感器输出的数据进行优化处理，补偿加速度传感器的动态误差及陀螺仪的漂移误差，可得到准确的车体倾角。

2.1 卡尔曼滤波器一般形式

卡尔曼滤波算法是一种求最优估计值的递推算法，是利用前一状态的估计值和当前的测量值来更新对状态变量的预测和估计，最终可得到当前状态的最优估计值。对本系统而言，将陀螺仪输出的角速度作为预测值的控制量，加速度传感器输出的角度作为测量值，对车体当前倾角进行最优估计。记是 k时刻对状态的先验估计，是k时刻得到观测值后对状态的后验估计，是先验估计协方差，Pk为后验估计协方差，Kk为卡尔曼增益，则卡尔曼滤波方程如式(2)～式(6)所示［3］。其中式(2)～式(3)为预测方程;式(4)～式(6)为校正方程。其中矩阵A、B是系统结构确定的矩阵，Q、R分别是系统噪声和观测噪声的协方差矩阵，H是测量系统的参数

2.2 加速度传感器和陀螺仪数据融合算法

记θk为车体k时刻的倾角，陀螺仪k时刻测得的角速度记为，加速度传感器k时刻测量到的车体倾角为θakccel，为k时刻角速度是k时刻角速度误差，采样时间间隔为 d t，则预测方程为式(7)～式(8)，更新方程为式(9)～式(11)［4］

3 系统设计

3.1 系统硬件设计

两轮自平衡小车硬件电路主要由微控制器、测量模块、直流电机驱动模块、蓝牙模块等构成。其中测量模块包括传感器MPU－6050、光栅编码器两部分。MPU－6050用于测量车体倾角，同时也完成加速度测量;安装在左右两个车轮轴上的增量型光栅码盘经光栅编码器处理后，分别输出两路AB相脉冲信号，经单片机处理后可得到每个车轮的转速及方向;蓝牙模块接收手机或电脑发出的信号，控制小车的运动。

小车的硬件系统结构如图2所示。选用意法半导体公司的STM32F103C8T6作为控制器，该控制器是一种基于Cortex M3内核的高性能MCU，运行时钟最高为72 MHz，运算速度高达 1.25 DMIPS/MHz，片上64 kB Flash ROM、20 kB RAM，内含7个定时器，可输出多路PWM信号又可配置成增量型编码器接口［5］，尤其适合于车轮测速传感器输出的AB相脉冲信号的处理。片上集成的I2C总线模块，方便了对传感器MPU－6050的控制。STM32F103C8T6完成传感器MPU－6050输出的加速度信号和陀螺仪信号的采集;对加速度信号和陀螺仪信号进行数据融合，计算出车体实时倾角;处理两个车轮上光栅编码器输出的两路AB相信号，得到小车运动速度及方向信息;利用数字PID算法输出PWM信号控制电机驱动模块，实现小车姿态调整从而实现小车的自平衡控制。

图3为单片机及传感器接口电路，P1、P2分别接左右车轮的光栅传感器检测电路，检测后的AB相脉冲分别从PA0PA1、PA6PA7送入单片机;PA9PA10用以与蓝牙模块进行串行通信;PA5为电池电压检测输入;PA2PA3、PB0PB1分别为左右车轮的控制信号;PB6、PB7分别是左右车轮的PWM调速控制信号;PB10PB11与传感器MPU－6050相连，实现加速度和倾角的测量。

图2 系统硬件结构框图

图3 单片机及传感器接口电路

图4为传感器MPU－6050电路。MPU－6050是世界上首款9轴运动传感器，它将3轴MEMS陀螺仪、3轴MEMS加速度计和一个可扩展的运动数字处理器(DPM)集成在一个芯片上，其内部对陀螺仪和加速度计分别使用3个16位ADC，检测输出的16位数字量通过I2C总线接口输出，为实现精确跟踪快速和慢速运动，陀螺仪和加速度传感器的测量范围都可设定［6］。

图5为直流电机驱动电路。TB6612FNG是一款新型驱动器件，能独立控制2个直流电机，具有集成度高、输出驱动能力强等特点，每通道输出最高1.2 A的连续驱动电流，启动峰值电流达2 A/3.2 A，有正转/反转/制动/停止4种电机控制模式，PWM支持频率高达100 kHz［7］，在集成化、小型化的电机控制系统中，是一种理想的电机驱动器件。

图4 MPU－6050电路

图5 电机驱动电路

3.2 系统软件设计

软件主要由传感器数据采集程序、卡尔曼滤波程序、车速及运动方向检测程序、PID控制程序、直流电机PWM控制程序、蓝牙串口模块数据处理程序等组成。STM32F103C8T6内含PWM模块及I2C总线接口模块，因此直流电机PWM控制和传感器MPU－6050数据采集通过读写单片机内部的寄存器就可实现，将单片机内的定时器配置成增量型编码器接口，车速及运动方向的检测也很容易实现，不需要CPU过多干预。因此，卡尔曼滤波算法及PID控制算法的实现是软件编程的关键。

根据式(7)～式(11)卡尔曼滤波数据融合过程分成以下步骤:(1)设定数据更新频率。具体编程时由定时器定时产生所需要的时间间隔d t。(2)设定参数。主要是矩阵Q的确定，可取般由实验确定 q1、q2、q3，式(11)中矩阵 P^k取单位矩阵。(3)计算更新后的协方差矩阵。(4)将陀螺仪的更新后的协方差融入到加速度计的倾角误差估算。(5)求加速计新的协方差矩阵。(6)计算倾角误差、角速度误差、输出角速度。

PID控制的原理是根据系统的被调量实测值与设定值之间的偏差，利用偏差的比例、积分、微分3个环节的组合计算出控制量，程序设计中采用PD组合控制方式，利用卡尔曼滤波器输出的车体倾角及角速度计算电机PWM信号的占空比，同时利用小车位移及小车速度计算PWM信号占空比增量值，如式(13)、式(14)所示

4 系统调试

两轮自平衡小车系统中变量多且相互耦合，因此调试过程比较繁琐，主要包括(1)确定零偏。关闭电机驱动，使车体垂直于水平桌面并保持静止，读取若干组MPU－6050中各通道的偏移量到PC机中，求取陀螺仪的零偏值及加速度传感器z轴最小值。(2)静止倒立小车确定加速度传感器z轴最大值，从而可确定加速度传感器z轴输出值变化范围，进而确定加速度计的比例因子。(3)陀螺仪比例因子的确定。确定的目的是使由陀螺仪积分得到的车体倾角值与加速度计计算到的值一致。(4)PID控制中按照先比例后微分的步骤确定比例因子和微分因子。

5 结束语

介绍了一种基于微控制器STM32F103C8T6、采用卡尔曼数据融合算法的两轮自平衡小车的设计。实验测试表明，文中设计的小车在普通水磨石地面上自平衡控制范围约±15°，在外加一定干扰的情况下小车仍能保持平衡，小车在蓝牙设备发出的指令下可实现前进、倒退、转弯、停止等动作，控制过程稳定、可靠。测试结果表明，利用卡尔曼滤波算法能有效消除陀螺仪漂移及车体摆动等对加速度计的影响，准确计算出姿态倾角，结合数字PID控制算法可有效实现小车的自平衡控制;采用高性能微控制器，使得卡尔曼滤波算法及PID控制算法得以快速完成，小车的状态信息采集实时性好，提高了自平衡小车稳定性和抗干扰能力。

［1］魏文，段晨东，高精隆，等.基于数据融合的两轮自平衡小车控制系统设计［J］.电子技术应用，2013，39(5):79 －81.

［2］冯智勇，曾瀚，张力，等.基于陀螺仪及加速度计信号融合的姿态角度测量［J］.西南师范大学学报:自然科学版，2011(4):137－141.

［3］程刚，屈胜利，刘学超.两轮自平衡小车可控角度的推导研究［J］.机床与液压，2009，37(5):115－116.

［4］Rodrigo da Silva Guerra.Kalman filter tutorial for balancing robot［EB/OL］.(2014 －01 －28)［2014 －04－06］http://wenku.baidu.com/view/7cfe5fef102de2bd960588cb.html.

［5］STMicroelectronics Group of Companies.STM32F103Cx8 datasheet［EB/OL］.(2011 －08 －28)［2014 －04 －06］http://ishare.iask.sina.com.cn/f/17966465.html.

［6］InvenSense Inc.MPU －6050 datasheet［EB/OL］.(2013 －08 －31)［2014 －04 －21］http://ishare.iask.sina.com.cn/f/36111609.html.

［7］Toshiba Corporation.TB6612FNG motor driver datasheet［M］.Tokyo:Toshiba Corporation，2011.