一种两轮自平衡小车的设计与实现

熊跃军，史明政，林金勇，方　烨

(长沙大学电子信息与电气工程系，湖南 长沙 410022)



一种两轮自平衡小车的设计与实现

熊跃军，史明政，林金勇，方烨

(长沙大学电子信息与电气工程系，湖南 长沙 410022)

摘要：针对于现在流行的两轮自平衡车，设计了一个与其原理相似的平衡小车控制系统.自平衡小车本身是一个不稳定体，类似于传统的倒立摆与蹊跷板平衡系统.以STM32F103C8T6为主控芯片，利用MPU6050采集小车的姿态数据，通过卡尔曼滤波算法对数据进行处理，最后采用PID控制算法计算电机的PWM值以控制电机的转动，实现小车的平衡.系统采用蓝牙通信技术，通过手机APP实现控制小车的运动.

关键词：STM32；两轮自平衡小车；MPU6050；卡尔曼滤波；PID

早在1987年，日本电信大学教授山藤一雄就提出了两轮自平衡机器人的概念.这个基本的概念就是用数字处理器来侦测平衡的改变，然后以平行的双轮来保持机器的平稳.本世纪初瑞士联邦工业大学的Joe、美国的SegwayN等两轮自平衡机器人相继问世，世界各国越来越多的机器人爱好者和研究者开始关注两轮自平衡机器人.美国发明家狄恩?卡门与他的DEKA研发公司研发出了可以用于载人的两轮自平衡车命名为赛格威，并已投入市场.由于两轮自平衡车有着活动灵便，环境无害等优点，其被广泛应用于各类高规格社会活动，目前该车已用于奥运会以及世博会等大型场合[1].为了提高小车的控制精度与稳定性，本文采用高性能的STM32芯片，模拟两轮直立自平衡电动车的原理，采用蓝牙通信技术来实现对小车的操作.

1系统总体设计方案

本系统采用Cortex-M3内核的STM32处理器为主控芯片，利用MPU6050来采集小车的姿态数据，通过卡尔曼滤波算法获得使小车更稳定的加速度和角速度，之后，利用PID控制算法得到准确的PWM值，通过TB6612直流电机驱动模块有效地控制电机的转速和转向，使得小车达到平衡状态.其次，本设计采用蓝牙无线通信技术，通过手机APP来操作小车的行驶[2].

1.1系统硬件设计方案

系统硬件结构主要由处理器、姿态控制模块，电机、显示、电源以及手机APP组成，系统结构图如图1所示[3].

1.1.1主控芯片

系统采用的主控芯片是STM32F103C8T6单片机，该单片机采用32位Cortex-M3内核，最高工作频率为72MHz.芯片上集成了64KB Flash存储器和20KB SRAM存储器，同时也集成了SPI、USART、I2C等通信接口，12位、16通道的AD转换器，能够产生多路PWM控制电机.

1.1.2车体姿态平衡模块

MPU6050六轴传感器芯片整合了3轴陀螺仪和3轴加速度计，每个轴对应一个16位的AD转换器，分别采集x轴、y轴、z轴的电压值，将其测量的模拟量转换成数字量输出，最后通过I2C总线传送到STM32，但此时得到的值并不是实际的角度和角速度值，还需经过一定的的比例关系进行转换，才能得到实际的角度和角速度值[4].

图1　系统硬件结构框图

图2　小车姿态平衡控制

为了减少陀螺仪的漂移及车体摆动等对小车加速度计的影响，获得准确的车体倾角值，需要对加速度计和陀螺仪的输出值进行融合，系统采用卡尔曼滤波算法进行数据融合[5].卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器)，它能从一系列的不完全包括噪声的测量中，估计动态系统的状态.当两轮自平衡小车产生倾斜时，系统采用PID控制算法，通过整合车体角度、角速度等值，输出PWM信号驱动电机，从而使小车保持动态平衡，其结构框图如图2所示，图中的Kd,kp,ksp,kis是PID控制器的相关参数[6].

1.1.3电机驱动模块

本系统采用额定转速285rpm、自带高精度磁编码器的直流减速电机，精度高，抗干扰能力强.TB6612是一款新型的驱动器件，能够独立控制2个直流电机，每个通道输出可达1.2A的连续驱动电流，启动峰值电流达2A～3.2A，有正传、反转、制动、停止4种电机控制模式.

1.1.4蓝牙通信模块

蓝牙无线模块采用SPP-C蓝牙模块，遵循V2.1+EDR蓝牙规范，采用串口协议，具有成本低、体积小、功耗低、收发灵敏性高等优点.手机图形界面的中间最小圆圈滑置不同的位置时，STM32产生中断并作出相应的处理[7].

1.1.5电源模块

系统包含12V、5V、3.3V电源.采用12V的航模电池，同时也是直流电机的供电来源.电机驱动模块和蓝牙通信模块采用5V的电源，5V电源是通过LM2596稳压得到的.STM32主控芯片的工作电源是3.3V，5V通过AMS1117稳压得到的.

1.2系统软件设计方案

1.2.1系统程序设计总方案

图3　系统程序设计

软件设计的任务是通过卡尔曼滤波算法得到准确的角速度与角度，在通过PID控制算法精确的控制小车的正反转.系统程序设计流程图如图3所示.

1.2.2卡尔曼滤波算法

卡尔曼滤波器是一个“最优化自回归数据处理算法”.对于解决大部分的问题，它是最优，效率最高甚至是最有用的.其广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等.近年来更被应用于计算机图像处理，例如，面部识别，图像分割，图像边缘检测等方面.在小车控制系统中，卡尔曼滤波算法是用来得到小车的角速度和加速度[8].程序如下.

void Kalman_Filter(float Accel,float Gyro)

{

angle+=(Gyro - Q_bias) * dt; //先验估计

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

Pdot[1]=-PP[1][1];

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

Pdot[3]=Q_gyro;

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

PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差

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

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

Angle_err = Accel - angle;//zk-先验估计

PCt_0 = C_0 * PP[0][0];

PCt_1 = C_0 * PP[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = C_0 * PP[0][1];

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

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

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

Angle+= K_0 * Angle_err; //后验估计

Q_bias+= K_1 * Angle_err; //后验估计

angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度

}

1.2.3PID控制算法

对于小车的平衡控制算法，可以从以下三个方面去调整：直立环、速度环、转向环[9].

(1)直立环程序

int balance(float Angle,float Gyro)// 比例微分PD控制器

{

float Bias,kp=300,kd=1;

int balance;

Bias=Angle-1;//求出平衡的角度中值和机械中值

balance=kp*Bias+Gyro*kd;//计算平衡控制的电机PWM

return balance;

}

(2)速度环程序

int velocity(int encoder_left,int encoder_right)// 比例积分PI控制器

{

static float Velocity,Encoder_Least,Encoder,Movement;

static float Encoder_Integral;

float kp=80,ki=0.40;

if(1==Flag_Qian)Movement=-90/Flag_sudu;

else if(1==Flag_Hou)Movement=90/Flag_sudu;

elseMovement=0;

Encoder_Least=(Encoder_Left+Encoder_Right)-0;//获得最新速度偏差

Encoder *= 0.7;//一阶低通滤波器

Encoder += Encoder_Least*0.3;//一阶低通滤波器

Encoder_Integral +=Encoder;//积分得到位移和时间

Encoder_Integral=Encoder_Integral-Movement;

if(Encoder_Integral>10000)Encoder_Integral=10000;//积分限幅

if(Encoder_Integral<-10000)Encoder_Integral=-10000;//积分限幅

Velocity=Encoder*kp+Encoder_Integral*ki;//速度控制

if(Turn_Off(Angle_Balance,Voltage)==1) Encoder_Integral=0;//电机关闭清除数据

return Velocity;

}

(3)转向环程序

int turn(int encoder_left,int encoder_right,float gyro)// 比例P控制器

{

static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;

float Turn_Amplitude=88/Flag_sudu,Kp=42,Kd=0;

if(1==Flag_Left||1==Flag_Right) //调整小车的起始速度，增加小车的适应性

{

if(++Turn_Count==1)

Encoder_temp=myabs(encoder_left+encoder_right);

Turn_Convert=50/Encoder_temp;

if(Turn_Convert<0.6)Turn_Convert=0.6;

if(Turn_Convert>3)Turn_Convert=3;

}

Else

{

Turn_Convert=0.9;

Turn_Count=0;

Encoder_temp=0;

}

if(1==Flag_Left)Turn_Target-=Turn_Convert;

else if(1==Flag_Right)Turn_Target+=Turn_Convert;

else Turn_Target=0;

if(Turn_Target>Turn_Amplitude)Turn_Target=Turn_Amplitude;if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;

if(Flag_Qian==1||Flag_Hou==1)Kd=1;

else Kd=0;//模糊PID控制

Turn=-Turn_Target*Kp -gyro*Kd;

return Turn;

}

2结束语

本系统介绍了一种以STM32F103C8T6微处理器、采用卡尔曼滤波算法和PID控制算法的两轮自平衡小车控制系统.实验测试表明，在一定的干扰的情况下，如斜坡、车顶放置一定量的物体，小车仍能保持平衡，小车在手机APP通过蓝牙设备发出的指令下可实现前进、倒退、转弯、停止等动作，该过程相对无遥控情况下更加稳定.测试结果表明，利用卡尔曼滤波算法能有效的避免陀螺仪的漂移及车体摆动等对小车加速度计的影响，准确计算出姿态倾角，结合PID控制算法，从而实现小车的平衡，并且具有自动恢复平衡的功能，适用于机器人排爆，狭小空间工作.系统实物如图4.

图4　系统实物

参考文献：

[1]Wan E A, Ronell V D M. The unscented Kalman filter for nonlinear estimation[A]. Proceedings of the 2000 Symposium on Adaptive Systems for Signal Processing, Communications and Control[C]. New York,2000:153-158.

[2]卢秀和,力嗨铭.两轮自平衡小车姿态平衡控制系统的研究[J].数控技术,2013，(2):9-12.

[3]王良成, 杨志民, 胡聪聪,等. 两轮自平衡小车的设计与实现[J]. 实验室科学,2012,(6):52-55.

[4]赖义汉,王凯.基于MPU6050的双轮平衡车控制系统设计[J].河南工程学院学报(自然科学版),2014，(1):53-57.

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

[6]冯智勇,曾瀚,张力,等.基于陀螺仪及加速度计信号融合的姿态角度测量[J].西南师范大学学报,2011,(4):137-41.

[7]杨莘,刘海涛.基于STM32的两轮自平衡遥控小车[J].数字技术与开发,2014,(5):151-152.

[8]任国华,唐昌进,蒋刚.一种基于Kalman 滤波的双轮机器人姿态控制算法[J].机械设计与制造,2013,(2):39-44.

[9]毛欢,张彬彬.采用改进PID 算法的双轮自平衡小车研制[J].工业控制计算机,2014,(1):55-56.

(作者本人校对)

Design and Realization of a Two-wheel Self-balanced Vehicle

XIONG Yuejun, SHI Mingzheng, LIN Jinyong, FANG Ye

(Department of Electronic Information and Electrical Engineering, Changsha University，Changsha Hunan 410022, China)

Abstract：Aiming at the popular two-wheel self-balanced vehicle, this article designed a similar vehicle. It is an unstable system which is similar to traditional inverted pendulum balancing system. In this system, STM32F103C8T6 is selected as the microcontroller, and MPU6050 is used in attitude data acquisition. Kalman filtering algorithm is used to process the data, and PWM results for motor control are obtained by using PID algorithm. Ultimately, bluetooth communication and mobile APP technologies are used in the movement control of this vehicle .

Key Words：STM32; two-wheel self-balanced vehicle; MPU6050; Kalman filter; PID

中图分类号:TP273

文献标识码：A

文章编号：1008-4681(2016)02-0039-04

作者简介：熊跃军(1978— )，男，湖南汨罗人，长沙大学电子信息与电气工程系讲师，硕士.研究方向：嵌入式系统.

收稿日期：2015-12-09