基于STM32和MPU-6050的两轮自平衡小车系统设计与实现

引言

两轮自平衡小车系统类似于倒立摆系统，具有多变量、非线性、强耦合等特点，是研究各种控制方法的理想平台。两轮自平衡小车系统的控制过程是微控制器对姿态检测传感器和编码器等采集的数据进行分析处理，计算出使系统恢复平衡的实时控制量，从而驱动电机实现系统的动态平衡。

针对小车系统的复杂性，本文提出了将卡尔曼滤波算法和双闭环PID控制算法相结合的方法，既利用卡尔曼滤波算法对MPU-6050传感器采集的倾斜角度和角速度数据进行融合，得到小车平衡姿态的最优估计值，又利用以姿态信息、速度为反馈控制量构成双闭环PID控制算法，再结合采用高性能STM32F103C8T6作为主控制器，从而提高两轮小车系统稳定性和抗干扰能力。

1、系统的硬件设计

系统的硬件电路结构框图如图1所示，其硬件电路主要由主控制器模块、姿态检测模块、直流电机驱动模块、编码器及电源模块等几部分构成。电源模块负责系统各个模块控制电路的电源。主控制器模块是系统的控制核心，用来接收传感器模块采集的数据，对数据进行处理及运算，将控制信号输出给直流电机驱动模块。姿态检测模块实时地对小车角速度及角加速度进行采集。直流电机驱动模块负责将主控制器输出的PWM信号转换为控制信号驱动2个直流电机的转速和方向。编码器负责测量直流电机的速度和方向，将测量数据反馈给主控制器，从而形成闭环控制。

图1  系统的硬件电路结构框图

1．1、电源模块

本系统选用的直流电机工作电压为12V，因此采用三节3．7V可充电电池构成12V直流电源。但STM32主控制器和MPU-6050需要的是3．3V直流电源，因此选用LM2596SDC-DC降压模块得到3．3V电源。

1．2、主控制器模块

本系统选用意法半导体公司的STM32F103C8T6作为主控制器，该控制器是一种基于Cortex-M3内核的32bCPU，最高工作频率为72MHz，片上集成32～512kB的Flash存储器，6～64kB的SＲAM存储器。片上集成了SPI，I2C和USAＲT等通信接口，方便对MPU-6050姿态检测传感器的数据进行采集。含有7个定时器，可输出多路PWM信号使得电机驱动模块获得稳定的PWM波形。

1．3、姿态检测模块

本系统中姿态检测模块选用的是美国InvenSense公司的MPU-6050，MPU-6050为全球首例整合性6轴运动处理器，整合了3轴陀螺仪和3轴加速度计，能够准确的对小车的姿态进行实时检测［4］。该处理器的角速度感测范围为±250、±500、±1000与±2000°/s，加速度感测范围为±2g、±4g、±8g与±16g，加速度感测范围可通过编程来控制［5］。MPU-6050可在不同电压下工作，VDD供电电压为（2．5±0．125）V、（3．0±0．15）V或（3．3±0．165）V，本系统中为MPU-6050提供的是3．3V直流电源。MPU-6050模块的电路如图2所示，本系统中MPU-6050作为从机使用，使用SDA和SCL端口和主机STM32进行通信，MPU-6050检测的数据经过内置的16位AD转换器进行转换，然后将16位数字量通过I2C总线接口发送给主机。

图2  MPU-6050模块的电路

1．4、电机驱动模块

本系统中直流电机驱动模块设计采用ST公司的L298N组成的双H桥电机驱动电路，其电路如图3所示。该模块具有驱动能力强，发热量低，抗干扰能力强等特点。可以同时驱动2台直流电机，当图3中的使能端ENA和ENB使能后，从IN1、IN2输入的PWM信号驱动电机1的转速和方向，从IN3、IN4输入的PWM信号驱动电机2的转速和方向。

图3  直流电机驱动模块的电路

2、系统的软件设计

2．1、系统的总体流程设计

本系统的主程序流程图如图4所示，主要由初始化程序、中断响应程序、传感器数据采集程序、卡尔曼滤波程序、车速及方向检测程序、PID控制程序、直流电机PWM控制程序等几部分组成。

图4  系统的主程序流程图

系统启动后，首先进行初始化，然后执行中断程序，判断5ms时间是否到，若时间到，则读取MPU6050传感器采集的角度和角速度的数据，然后利用卡尔曼滤波算法对读取的数据进行融合得到最优估计值。同样采用中断的方式读取编码器的脉冲数从而得到直流电机的转速以及方向。然后利用增量式PID控制算法分别对小车的角度和角速度、转速进行PID控制，调节PID参数即可改变PWM占空比，利用改变占空比的方法即可改变电机的速度，从而实现对电机速度和方向的控制。

2．2、系统初始化程序

系统初始化程序主要包括：系统时钟初始化、NVIC中断初始化、定时器外部中断初始化、通信（I2C）初始化、测速模块初始化、MPU-6050初始化和电机PWM初始化等。

STM32的工作频率为72MHz或36MHz，本系统对时钟频率的要求很苛刻，如果频率过低，系统就会工作在不稳定状态下，系统产生的误差会变大，又因系统使用的滤波方式是卡尔曼滤波，因此在时钟初始化程序中需将时钟频率设置为72MHz。通信（I2C）初始化主要设置串口波特率为9600b/s，需设置中断允许标志位和模块的工作模式。

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

2．3．1、卡尔曼滤波算法

陀螺仪动态性能较好，可以提供瞬间的动态角度变化，但存在输出信号有偏差或漂移，经过积分后形成逐渐增大的累积误差，不适合单独进行长时间的动态角度测量。加速度计静态性能较好，可以提供较为准确的静态角度值，但受重力加速度影响比较大，不适宜测量动态系统的角度值。由于自平衡小车系统需要在动静态情况下均能准确的测量出小车的倾角，因此需要对陀螺仪和加速度计的采集数据进行数据融合得到最优值。本系统采用卡尔曼滤波算法对陀螺仪和加速度计的数据进行融合。

卡尔曼滤波算法是以最小均方误差为最佳估计准则的递归数据处理算法，利用前一时刻的估计值和当前时刻的测量值来更新对状态变量的估计，最终求出当前时刻的最优估计值。

2．3．2、卡尔曼滤波器设计

卡尔曼滤波方程如下所示：

2．4、PID控制算法

控制算法的编写是小车平衡控制的核心问题，也是软件编写和程序调试中的关键技术。本系统中采用了常用的增量式PID控制算法。PID控制的原理是根据系统的设定值和测量值之间的偏差，对偏差进行比例、积分、微分计算出控制量。系统采用双闭环PID控制算法对小车系统进行自平衡控制，其结构框图如图5所示。

图5  系统双闭环PID控制算法框图

2．5、电机控制策略

本小车系统使用的电机是小型12V直流电机，其转速是由施加给电机的电压大小来决定，转向是通过改变施加给电机电源的正负极性。电机驱动模块输出两路PWM信号，使用过程中通过给其中一路占空比为零，另外一路不为零来控制电机的正反转。

3、系统调试

程序下载后对小车系统各硬件模块部分分别进行测试，观察小车动作情况。若测试均能正常通过，则表明系统硬件电路工作正常。调试时，检测到小车的角度为30．90439°，小车电机转速为－52．09581。

在小车自平衡过程中，若给小车一个外力来破坏小车的平衡，则小车要能迅速响应这个变化，立即运动去减小倾角的变化，建立一个稳定平衡状态。开始调试时，能响应，但波动较大，建立平衡的时间较长。通过修正PID参数，最终能使小车快速达到平衡状态。

小车平衡时效果图如图6所示。系统通电后，小车完全实现自平衡的时间为12s，在普通水磨石地面上，平衡时倾角范围约在±10o以内。若给予一定外力干扰时，小车在20s内能够自动调节实现自平衡状态。

图6  小车平衡时效果图

4、结语

本文设计了一个两轮自平衡小车系统，该系统基于STM32F103C8T6微处理器、采用卡尔曼滤波算法和双闭环PID控制算法对小车进行自平衡控制。实验测试结果证明，采用卡尔曼滤波算法能够有效地消除陀螺仪和加速度计在测量过程中的噪声干扰和随机漂移误差，准确地计算出小车的姿态倾角。采用高性能的微处理器，使得卡尔曼滤波算法和PID控制算法能够快速完成，提高了数据采集和控制的实时性，提高了系统的稳定性和抗干扰能力。