基于STM32平衡两轮移动机器人的设计与实现

重庆科创职业学院信息与机电工程学院　万　军　吕值敏　文方国



基于STM32平衡两轮移动机器人的设计与实现

重庆科创职业学院信息与机电工程学院万军吕值敏文方国

【摘要】本文设计了一个两轮自平衡小车系统，它以STM32嵌入式微处理器为核心，以MPU6050传感器采集车体姿态，利用卡尔曼（kalman）滤波融合算法获取车体角度和角速度，最后使用PID控制算法获取合适的PWM值，通过控制电机的正、反转来最终达到车体的平衡，得到了较为理想的运行控制效果。两轮自平衡小车系统是一种特殊的轮式移动机器人，它运动灵活，成本低，适合在狭小和危险的空间工作，有着广泛的应用前景。

【关键词】STM32；平衡；机器人；控制

0　引言

自平衡两轮机器人是一个典型的非线性、强耦合、多变量、时变和自然不稳定系统，是近年来研究倒立摆与机器人控制问题的又一研究平台，是检验各种控制理论的理想模型。仿人智能控制理论从分层递阶智能控制系统的最低层(运行控制级)着手，充分应用已有的智能装备技术和控制技术，直接对人的控制经验、技巧和各种直觉推理逻辑，即人体的动觉智能进行测辨、概括和总结，并将其编制成各种简单实用、精度高、能实时运行的控制算法(动觉智能图式)，直接应用于实际控制系统。本文依据动觉智能图式的仿人智能控制理论实现两轮移动机器人的运动控制，对自平衡小车机器人提出了控制思想，实现了机器人自平衡控制，得到了较为理想的平衡和运行控制效果。

1　系统整体设计方案

平衡系统以STM32嵌入式微处理器为核心，以MPU6050传感器采集车体姿态，利用卡尔曼(kalman)滤波融合算法获取车体角度和角速度，最后使用PID控制算法获取合适的PWM值，通过控制电机的正、反转来最终达到车体的平衡，通过人为控制左右红外避障传感器的信号，来实现两轮平衡车的停止、前进、左转、右转等功能，最终实现平衡两轮移动机器人的避障搬运功能[1]。本系统的整体方框图如图1所示。

图1　系统方框图

2　系统硬件设计

2.1系统主控芯片

本系统采用STM32F103作为主控芯片，该芯片采用Cortex-M3为内核，具备72MHz最高工作频率，64K字节的SRAM，2个DMA控制器，支持ADC、定时器、SPI、USB、IIC和UART等外围设备。主控芯片工作及控制电路接线图如图2所示。

图2　STM32F103控制电路图

2.2电机驱动与编码器模块

(1)编码器：本系统采用JGA25-371自带编码器测速码盘马达减速电机，工作电压为6-24VDC。编码器芯片上已集成了脉冲整形触发电路，输出的是矩形波，直接连接芯片PA1、PA0、PB6、PB7引脚，主控芯片STM32F103与编码器JGA25731接线如图2所示。自带编码器电机测速精度高，采用334线码盘即电机每转动一圈就输出334个脉冲。

(2)电机模块：采用双H桥驱动芯片L298N。其电机特点为：内部有4通道逻辑驱动电路或一个两相步进电机，驱动电机的速度可以由控制芯片的使能端直接控制。电机驱动L298N芯片采用5 V(VSS)与12 V(VS)直流供电，ENA和ENB分别用STM32F103主控芯片的PB0/TIM3 CH3和PB1/ADC_ IN9/TIM3_CH4控制，产生的PWM波分别为PWM1和PWM2两路输出，IN1-IN4分别接PA6、PA7、PB0、PB1控制电机转动方向[2]，主控芯片STM32F103与电机驱动芯片L298N连接图如图2所示。

2.3电源电路设计

本系统由于电路模块较多，每个模块所需电压不同，因此电源电路模块需设计多种电源供电，同时本系统进行了电源转换设计，以满足各模块供电需求。主控芯片STM32F103需3.3 V供电，电机驱动模块分别采用5V、12 V电源供电，红外收发检测模块需采用5 V供电，外部电源模块需采用12 V的直流电压供电。

(1)直流电压12V转换成5V电压的电路

采用KA7805芯片实现12 V到5 V的转换。KA7805是常用的稳压芯片，使用方便,管脚较少，易于连接和实现，稳定性高，用很简单的电路便能够将12V直流电压，转换成输出5V的直流电压，电路如图3所示。

(2)直流电压5V转换成3.3V电压的电路

本系统采用LM1117将直流电压5V转换成3.3V电压，具体电路设计如图3所示。LM1117是一款正电压输出的低压降三端线性稳压电路，它有两种类型即输出版本固定电压输出版本和可调电压输出版本。固定输出电压有1.5V、1.8V、2.5V、3.3V、5.0V；可调电压版本输出范围是1.25V-13.8V，电压精度为1%；它内部集成过热保护和限流电路，适用于各类电子产品。

图3　电源电路

2.4车体姿态控制设计

本系统车体姿态硬件控制设计采用MPU6050模块实现，如图2所示。MPU6050的角速度全格感测范围为±250、±500、±1000与±2000°/sec (dps)，可准确追踪快速与慢速动作，并且，可程式控制的加速器全格感测范围为±2g、±4g、±8g与±16g[3]。MPU6050可工作在不同电压下，供电电压为2.5V±5%、3.0V±5%或3.3V ±5%。逻辑接口VVDIO供电为1.8V± 5%，本系统中MPU-6000 (6050)与芯片PB10、PB11相连，主控芯片STM32与MPU6050传感器接线图如图2所示，车体姿态平衡控制结构框图如图4所示。

图4　平衡控制结构框图

3　系统软件设计

3.1系统软件

系统软件设计主要包括系统初始化、MPU6050数据采集、Kalman数据滤波数据融合、倾角PID控制算法、电机PWM输出等，系统软件流程图如图5所示。

3.2Kalman滤波融合算法

Kalman卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器，它能够从一系列的不完全包含噪声的测量中，估计动态系统的状态。基本思想是采用信号与噪声的状态空间模型，利用前一时刻的估计值和现时刻的观测值来更新对状态变量的估计，求出现在时刻的估计值实质，由量测值重构系统的状态向量。它以“预测—实测—修正”的顺序递推，根据量测值来消除随机干扰，再现系统的状态[4]。

图5　系统软件设计流程

程序段如下所示：

void Kalman_Filter(float Angle_Kal,float Gyro_Kal)

{

........

g_fAngle += (Gyro_Kal - q_bias) * dt;//先验估计

Pdot[0] = Q_angle - P[0][1] - P[1][0]; // Pk-’ 先验估计误差协方差的微分

Pdot[1] =- P[1][1];

Pdot[2] =- P[1][1];

Pdot[3] = Q_gyro;

P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差

P[0][1] += Pdot[1] * dt;

P[1][0] += Pdot[2] * dt;

P[1][1] += Pdot[3] * dt;

angle_err = Angle_Kal - g_fAngle;//zk-先验估计

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;//Kk

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = P[0][1];//C_0 *

P[0][0] -= K_0 * t_0;//后验估计误差协方差

P[0][1] -= K_0 * t_1;

P[1][0] -= K_1 * t_0;

P[1][1] -= K_1 * t_1;

g_fAngle+= K_0 * angle_err;//后验估计

q_bias += K_1 * angle_err; //后验估计

g_fAngle_Dot = Gyro_Kal - q_bias;//输出值(后验估计)的微分 = 角速度

}

3.3电机控制算法

在本系统中，电机的控制算法起到了十分关键的作用，采用PID控制算法来控制电机，因为PID获取的输入是红外传感器的角度和角速度值的误差值，并不是绝对数值，而是一个变量，因此采取增量PID算法。通过增量PID算法，每次输出是电机PWM占空比的值[5]。如：

u(k) = u(k-1) + u

先将Ti设置为无穷大，Td设置为0，调节Kp，然后再调节Ti，最后是Td。

PID控制器的输入输出关系：

其中e(t)是实际值与设定值的偏差。

Kp Ki Kd 是系数

比如这里我们假设车子直立的角度为0(设定值)，实际车子向前倾斜了10°(实际值)，偏差就是10°

4　结论

本系统以STM32为主控芯片，实现对两轮平衡移动机器人的控制。系统利用STM32集成开发环境IAR EWARM 5.3编写各模块程序，包含卡尔曼滤波函数，速度位置闭环函数，陀螺仪加速度采集函数，避障模块和红外循迹模块，并通过主控制程序将各模块链接，实现两轮移动机器人的前进、后退、左转、右转等自平衡动作。整个系统硬件通过PROTEL DXP绘制原理图，并组装调试成实物。自平衡小车运动系统能与实际应用相结合，实现物品的分拣、配料、搬运等智能控制，具有普遍的实用价值和现实意义。

参考文献

[1]杨莘.基于STM32的两轮自平衡遥控小车[J].数控技术应用,2014,5.

[2]董杰.基于STM32的智能循迹往返小车设计[J].电子设计工程,2013,12.

[3]王建明.四旋翼飞行器设计[J].军民两用技术与产品,2014,7.

[4]秦永元.卡尔曼滤波与组合导航原理[M].西北工业大学出版社,2012,6.

[5]刘金琨.先进PID控制MATLAB仿真[M].电子工业出版社,2011,3.

万军（1978—），男，四川广安人，硕士，讲师，重庆科创职业学院电信教研室教师，主要从事电子信息工程教学研究。

作者简介：