宣丽萍;李艳丽
【摘 要】采用PID算法、卡尔曼滤波算法,实现一个基于STM32F103RCT6两轮自平衡小车系统.运用MPU6050模块来获取数据信息,进而弥补加速度计的动态误差以及陀螺仪的漂移误差,获得更精确的倾角值,然后经STM32芯片处理输出适当指令,控制电机的运行,并通过蓝牙模块与手机蓝牙APP的通信,实现小车循迹.实验表明:小车通信可靠,保持平稳,且能实时控制小车的运动姿态,达到预期目的. 【期刊名称】《黑龙江工程学院学报（自然科学版）》
【年(卷),期】2018(032)004
【总页数】5页(P6-10)
【关键词】两轮自平衡小车;卡尔曼滤波算法;MPU6050模块;PID算法
【作 者】宣丽萍;李艳丽
【作者单位】黑龙江科技大学 电气与控制工程学院 ,黑龙江 哈尔滨 150022;黑龙江科技大学 电气与控制工程学院 ,黑龙江 哈尔滨 150022
固态发酵罐【正文语种】中 文
【中图分类】TP273
近年来，两轮自平衡小车的研究、发展逐步加快，在很多领域都有应用，同时伴随其执行任务的复杂性，对其适应环境的能力要求逐渐提高。比如，在空间狭窄的地方，交通堵塞的情形，或者弯道较大的场所，就小车如何在较短的时间处理所获取的信息，并执行相应的任务，已成为研究的热点。两轮自平衡小车概念的提出，顺应了日前研究的热点，可很好地解决上述复杂环境下小车的运动，加之无线遥控技术，可远距离控制两轮自平衡小车的运动姿态，未来在军用、民用领域有广泛的发展空间。
1 系统整体方案设计
本设计以STM32处理器为核心，采用MPU6050模块获取小车的运动信息，运用Kalman滤波算法得到小车的运行数据、角度值与角速度值。然后选用PID算法得到实时的PWM波形，
达到对小车的控制以保持小车的平衡，通过手机蓝牙APP与小车通信，控制小车实现前进、左右转、后退以及停止等运动姿态。系统的整体设计方案如图1所示。
图1 小车硬件总体结构
2 系统硬件设计
2.1 主控芯片
使用STM32F103RCT6处理器芯片，该芯片以Cortex-M3为内核，最高工作频率为72 MHz，48K字节的SRAM，其板载资源有DMA、ADC、定时器、IIC以及UART等外围设备，可产生多路PWM用以控制电机。
2.2 直流电机驱动电路设计
电机驱动芯片选用ST公司生产的一种电机，其驱动芯片型号为L298N。主要参数：工作电压的最大值46 V，输出电流瞬间峰值3 A，持续工作电流2 A。拥有2个H桥高电压、大电流的桥式驱动器，用以驱动电机；控制使用标准逻辑电平信号；拥有2个控制端，即在没有输
入信号干扰的场景下许可或禁止器件运转，有1个逻辑电源输入端，可使逻辑电路在低电压情形运行；也可外接1个测试电阻，将变化量传达到控制电路。其原理如图2所示。
图2 L298N模块原理
2.3 电源模块
光纤调整架采用三节3.7 V大容量电池供电，用以提供电机的驱动电压，电源变换电路采用稳压芯片LM2940把电池组电压稳定在+5 V，然后再经稳压芯片AMS1117-3.3把电压稳定在+3.3 V。为更好地降低电磁干扰，得到精度更高的数据信息，采用两路+3.3 V电源分别给STM32F103和MPU6050供电，然后把多路输入源各自接地，可达到提高系统稳定性的目的以及抗干扰水平。其原理如图3所示。
图3 电源变换原理
2.4 车体姿态控制模块
选用InvenSense公司发行的一种6轴运动的整合性器件MPU6050，即3轴陀螺仪传感器以
及3轴加速度传感器，能在外围连接磁力传感器，并运用本身的数字运动处理器以及IIC接口，把所测得的模拟量转换为数字量，最终传达至主控芯片，但要获取实际的角度值以及角速度值，仍需一个转换关系，才能得到二者的实际值。
为得到更确切的车体倾角值，降低陀螺仪的漂移、车体摆动等对小车加速度计的干扰，需要对陀螺仪与加速度计的输出值进行融合，本设计将采用Kalman滤波算法进行数据融合。该算法属于一类效率很高的递归滤波器，可从一连串复杂的噪声测试中，估算出动态系统的姿态。当小车失去平衡时，该PID算法就会起作用，处理小车车体的角度与角速度值，产生实时的PWM波形，用以驱动电机来让小车的车体维持平衡，小车姿态平衡控制如图4所示，其中，kd，kp，ksp，kis为PID控制器参数。其控制框图如图4所示。
图4 小车姿态平衡控制
2.5 蓝牙模块
蓝牙无线模块选用的型号为HC-05，选用UART协议，其优点为价格低、尺寸小、能耗低、发送接收灵敏等。HC-05有两种工作模式：自动连接工作模式与命令响应工作模式。前者
又分为3个形式：主(Master)、从(Slave)、回环(Loopback)。当模块工作在自动连接模式，它能依照预先设置好的步骤进行数据传输，当模块工作在命令响应工作模式，用户能向该模块传送任一AT指令，为模块设置所需的执行参数或者指令。该模块工作状态的合理切换，取决于其外部引脚的输入电平情况。
2.6 循迹传感器模块
循迹传感器模块可让小车依照预先设置的任意路线运动，该模块工作电压为3～5 V，可发射某一频率的光线，运动过程中会对所走的路线做出反应，若所走的路线为白，该光线能反射回来且能被该模块获取，然后经该模块处理会产生高电平，若所走线路为黑线，则能吸收该光线不能被该模块获取，此时经该模块处理会产生低电平，所以它能依照所测电平实时调整运动姿态，让小车的两轮产生差速，用以保持预定的路线运行。
3 系统软件设计
汽车投影
该系统软件主要有系统初始化、MPU6050模块信息采集、卡尔曼滤波算法、PID算法以及PWM的产生等功能函数的设计，其控制流程如图5所示。
图5 系统程序设计流程
3.1 卡尔曼滤波融合算法
Kalman滤波的思路为：利用信号、噪声的状态空间模型，以最小均方误差为最优估计准则。根据前一时刻的估算值与此刻的观测值实时替换状态变量的值，得出此刻状态变量的估算值。依据建立的系统方程与观测方程，然后对所需处理的信号，作出适合的最小均方差估计。跟互补滤波比较，该算法处理起来虽复杂，但它有更好的性能。对于大多数问题的求解，其最优、效率又高甚至最有用的特点尽显。所以本设计的数据融合算法选用的是卡尔曼滤波算法。卡尔曼滤波方程如下所示：
X(k|k-1)=AX(k-1|k-1)+BU(k)，
无人机吊舱
(1)
P(k|k-1)=AP(k-1|k-1)+AT+Q，
(2)
Kg(k)=P(k|k-1)HT/(HP(k|k-1)HT+R)，
(3)
X(k|k)=X(k|k-1)+Kg(k)(Z(k)-
HX(k|k-1))，
(4)智能筷子
P(k|k)=(I-Kg(k)H)AP(k|k-1).
(5)
式(1)为状态预测方程，X(k|k-1)为k时刻的估算值，为2维列向量，系统中为k时刻倾角的估计值，Q为k时刻陀螺仪的位偏差)，A为2维方阵，(d t为采样时间)，X(k-1|k-1)为k-1时刻的最优估计值，B为2维列向量，(gyro为陀螺仪k-1时刻测得的角速度值)。
式(2)为系统的协方差方程，Q为向量的协方差矩阵。
式(3)为卡尔曼增益方程，Kg(k)为2维列向量，为角度测量噪声值，程序中定义R-angle=0.5.
式(4)为状态估计方程，Z(k)=accel为加速度计的角度测量值。
式(5)为估计均方误差方差，
其程序设计如下：
void kalman_filter(float angle_m, float gyro_m, float *angle_f, float *angle_d)
{
angle += (gyro_m- q_bias) * dt;
Pdot[0]=Q_angle-P[0][1]-P[1][0];
Pdot[1]=-P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q-gyro;
P[0][0]+=Pdot[0]*dt;
P[0][1]+=Pdot[1]*dt;
P[1][0]+=Pdot[2]*dt;
P[1][1]+=Pdot[3]*dt;
angle_err = angle_m - angle;
PCt_0=C_0*P[0][0];
PCt_1=C_0*P[1][0];
E=R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;ip电话系统
t_0 = PCt_0;
