两轮自平衡小车智能控制研究中期检查报告书.doc

两轮自平衡小车智能控制研究中期检查报告书

项目编号
ky2014-131
学科分类号（二级） 510.10

云南师范大学大学生科研训练基金项目

中期检查报告书

项目名称: 两轮自平衡小车智能控制研究
项目类型: 一般项目
主 持 人: 赵庆波
起止年月： 2014年12月
结题日期： 2015年12月
联系电话: [phone]

云南师范大学教务处

填 表 说 明

一、填写《项目中期检查报告书》前，请先查阅《云南师范大学大学生科研训练基金管理办法》和相关通知。
二、项目中期检查报告书各项内容，必须实事求是，表达要明确严谨，并要求用打印。对于填写不合要求、内容含糊不清、字迹潦草者，不予受理。
三、项目类型：选填重点项目或一般项目。
四、项目类别：选填自然科学或社会科学。
五、封面的项目编号与申请书上的编号一致。
六、打印格式：
（一）纸张为A4大小，双面打印；
（二）文中小标题：五号、黑体；
（三）栏内正文：五号、宋体。
七、填写《中期检查报告书》一式二份（至少含一份原件），其中一份提交教务处实践教学科，另一份留学院存档。

课题名称
两轮自平衡小车智能控制研究
项目类别
自然科学
项目编号
ky2014-131
填表时间
2015年5月20日
主 持 人
赵庆波
主要成员
赵庆波、余坦藏、聂浩南
一、项目进展及取得的成果情况
（一）项目进展成果情况
本研究从2014年10月份就开始了计划时间范围内的相关学习及制作，到目前为止已经学习了STC12C5A60S2单片机，并且能够熟练的掌握运用相关的应用软件，同时对双环PID控制及卡尔曼滤波、涉及的各种传感器、AD转换等知识有了深入的理解，硬件电路也制作完成了，程序设计编写实现了初步的完成。
1. 在指导老师的指导下针对该项目的相关文献查阅和资料的收集与学习；
时间:2014年 10月至2015年1月。
看老师给的相关资料，并利用学校的图书馆资源查阅文献及相关材料，与老师同学讨论学习相关知识。
通过这一阶段的学习对STC12C5A60S2单片的基础知识有了一定的掌握，单片机技术是软件和硬件结合的技术，学习了的一些关于单片机开发的软件，还学习了电机驱动模块、陀螺仪加速计、HC-06蓝牙模块等大量的单片机拓展功能实验器材的功能结构。对双环PID控制及卡尔曼滤波算法都有了全面的掌握。
2.在指导老师的指导下开始着手项目的研发和学习；
(1).根据指导老师的指导及指引，进行两轮自平衡智能控制小车的硬件制作；
时间：2015年3月 至 2015年4月。
内容及取得相应的成果：硬件结构基本完成,能够实现直立行走。
(2) 根据指导老师的指导及指引，对两轮自平衡智能控制小车的程序设计编写；
时间：2015年4月 至 2015年5月。
内容及取得相应的成果：可是实现一部分的功能,但程序设计还不成熟,系统运行不稳定,仍需改进。

(二）阶段性成果情况
通过以上比较深入的理论学习，以及在我们着手项目的研发，经过几个月的不断改良，我们初步实现了两轮自平衡智能控制小车的直立行走。长时间认真的理论知识学习为后期研发制作打下了坚实的理论基础,两轮自平衡智能控制小车硬件结构都已经完成,且性能良好,程序设计编写也有了初步的实现,但由于程序设计编写的时间较短,所以还不够成熟,智能控制两轮小车不能稳定运行,从中还有许多问题,我们会在后期的研究中完善,具体成果如下：
1． 两轮自平衡智能控制小车的硬件部分
（1） 速度检测模块设计
两轮自平衡小车的原理是利用地面对车轮的摩擦力抵消车受到的重力，在本系统的控制环节中有两路闭环控制，即倾角闭环控制以及速度闭环控制。为实现速度的闭环控制，必须加入速度检测装置实现速度闭环控制中的反馈环节。本系统测速模块采用OMRON(欧姆龙)公司500线增量式旋转编码器。编码器内部为一个中心有轴的光电码盘，其上有环形通、暗的刻线，有光电发射和接收器件读取,获得四组正弦波信号组合成A、B、C、D,每个正弦波相差90度相位差（相对于一个周波为360度），将C、D信号反向，叠加在A、B两相上，可增强稳定信号；另每转输出一个Z相脉冲以代表零位参考位。由于编码器采用集电极开路输出，输出波形为矩形波，因此编码器外围电路较为简单。需要在信号输出端接入一个上拉电阻，即可将信号提供给单片机采集数据。

（2） 驱动电路
采用功率三极管作为功率放大器的输出控制直流电机。线性型驱动的电路结构和原理简单，加速能力强，采用由达林顿管组成的H型桥式电路，用单片机控制达林顿管使之工作在占空比可调的开关状态下，精确调整电动机转速。这种电路由于工作在管子的饱和截止模式下，效率非常高，H型桥式电路保证了简单的实现转速和方向的控制，电子管的开关速度很快，稳定性也极强，是一种广泛采用的PWM调速技术。现市面上很多这种芯片，我选了LM298N，L298N是SGS公司的产品，内部包含4通道逻辑驱动电路。是一种二相和四相电机的专用驱动器，即内含二个H桥的高电压大电流双全桥式驱动器，接收标准TTL逻辑电平信号，可驱动46V、2A以下的电机。 由于电力电子器件只工作在开关状态，主电路损耗较小，装置效率较高。根据以上考虑，以及本设计中受控电机的容量和直流电机转速的发展方向，本设计采用了H型单极型可逆PWM变换器件LM298N进行电机调速。具体原理图如下：

图1 电机驱动电路原理图
（3） CPU微控制器
CPU微控制器采用STC12C5A60S2单片机，微控制器作为整个系统的核心部分，主要负责对传感器采集的信号进行实时处理，但其计算量较大，普通的单片机不能满足使用要求。本系统选用了高速STC12C5A60S2单片机作为微处理器，具有丰富的I/O接口、有双串口。其功耗低、超强抗干扰能力、指令代码完全兼容传统单片机8051，能够满足系统的设计需求，在单片机上增加复位电路、时钟电路等外围电路构成单片机最小系统，让单片机工作起来，然后通过编写程序对单片机进行控制，进而单片机对传感器采集信号进行处理。在程序设计编写方面十分容易，且成本较低，也可以很好的实现本设计。具体STC12C5A60S2单片机原理图如下：

图2 STC12C5A60S2单片机原理图
（4） 陀螺仪传感器模块
陀螺仪可以用来测量物体的旋转角速度。本设计选用村田公司出品的ENC-03系列的加速度传感器。它利用了旋转坐标系中的物体会受到科里奥利力的原理，在器件中利用压电陶瓷做成振动单元。当旋转器件时会改变振动频率从而反映出物体旋转的角速度。在车模上安装角速度传感器，可以测量车模倾斜角速度，再对倾斜角速度进行积分就得到了车模的倾角。由于陀螺仪输出的是车模的角速度，不会受到车体运动的影响，因此该信号中噪声很小。车模的角度又是通过对角速度积分而得，这可进一步平滑信号，从而使得角度信号更加稳定。因此车模控制所需要的角度和角速度可以使用陀螺仪所得到的信号。由于从陀螺仪角速度获得角度信息，需要经过积分运算。如果角速度信号存在微小的偏差和漂移，经过积分运算之后，变化形成积累误差。这个误差会随着时间延长逐步增加，最终导致电路饱和，无法形成正确的角度信号。为了消除积分漂移，我们引入姿态测量的方法。
硬件两轮自平衡小车实物图如下图：

图3 两轮自平衡小车实物图
2． 程序的设计与编写
在自平衡小车未做控制时，不论其车身向前或向后倾斜，两轮都处于静止状态，这时其车身前后摆动与其车轮转动是相互独立的；当其开始控制时，其车身的状态变化使小车有静止、前进(前倾)、后退(后仰)三种运动的方式，在正确的控制策略下，小车能够保持自身的平衡。这三种运动方式与控制策略如图所示

（1）前倾 （2）静止 （3）后仰
图4 小车三种运动方式
（1） 前倾状态：即车身重心靠前，车身会向前倾斜，则驱动车轮向前滚动，以保持小车平衡。
（2） 静止状态：即车身重心位于电机轴心线的正上方，则小车将保持动态平衡静止状态，不需要做任何控制。
（3） 后仰状态：即车身重心靠后，车身会向后倾斜，则驱动车轮向后滚动，以保持小车平衡。
因此，两轮自平衡小车平衡控制的基本思想是：当测量倾斜角度的传感器检测到车体产生倾斜时，控制系统会根据测得的倾角产生一个相应的力矩，通过控制电机驱动两个车轮朝车身要倒下的方向运动，以保持小车自身的动态平衡。
主要程序清单:
#include <reg51.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include<intrins.h>
#include<STC12C5A60S2.h>
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define PWR_MGMT_1 0x6B
#define WHO_AM_I 0x75
#define SlaveAddress 0xD0
typedef unsigned char uchar;
typedef unsigned short ushort;
typedef unsigned int uint;
sbit in1=P1^1;
sbit in2=P1^2;
sbit in3=P1^5;
sbit in4=P1^6;
sbit SCL=P2^7;
sbit SDA=P2^6;
void delay(unsigned int k);
void InitMPU6050();
void Delay5us();
void I2C_Start();
void I2C_Stop();
void I2C_SendACK(bit ack);
bit I2C_RecvACK();
void I2C_SendByte(uchar dat);
uchar I2C_RecvByte();
void I2C_ReadPage();
void I2C_WritePage();
int GetData(uchar REG_Address);
int PWM(int a,int b);
void PWM_calculate( void);
int temp;
uchar Single_ReadI2C(uchar REG_Address);
float Adjust_up1,Adjust_up2;
unsigned int PWM_Duty1,PWM_Duty2;
//******角度参数************
float Gyro_y; //Y轴陀螺仪数据暂存
float Angle_gy; //由角速度计算的倾斜角度
float Accel_x; //X轴加速度值暂存
float Angle_ax; //由加速度计算的倾斜角度
float Angle; //小车最终倾斜角度
float Angle1;
uchar value; //角度正负极性标记
//******卡尔曼参数************
float code Q_angle=0.001;
float code Q_gyro=0.003;
float code R_angle=0.5;
float code dt=0.01; //dt为kalman滤波器采样时间;
char code C_0 = 1;
float xdata Q_bias, Angle_err;
float xdata PCt_0, PCt_1, E;
float xdata K_0, K_1, t_0, t_1;
float xdata Pdot[4] ={0,0,0,0};
float xdata PP[2][2] = { { 1, 0 },{ 0, 1 } };
void Single_WriteI2C(uchar REG_Address,uchar REG_data);
//*********************************************************
// 卡尔曼滤波
//*********************************************************
//在程序中利用Angle+=(Gyro - Q_bias) * dt计算出陀螺仪积分出的角度，其中Q_bias是陀螺仪偏差。
//此时利用陀螺仪积分求出的Angle相当于系统的估计值，得到系统的观测方程；而加速度计检测的角
//度Accel相当于系统中的测量值，得到系统状态方程。
//程序中Q_angle和Q_gyro分别表示系统对加速度计及陀螺仪的信任度。
//根据Pdot = A*P + P*A' + Q_angle计算出先验估计协方差的微分，
//用于将当前估计值进行线性化处理。其中A为雅克比矩阵。
//随后计算系统预测角度的协方差矩阵P。计算估计值Accel与预测值Angle间的误差Angle_err。
//计算卡尔曼增益K_0,K_1，K_0用于最优估计值，K_1用于计算最优估计值的偏差并更新协方差矩阵P。
//通过卡尔曼增益计算出最优估计值Angle及预测值偏差Q_bias，此时得到最优角度值Angle及角速度值。
//Kalman滤波，20MHz的处理时间约0.77ms；
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[0][1] -= K_0 * t_1;
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; //后验估计
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}
//*******************************
//*******卡尔曼融合**************
//*******************************
Angle_Calcu(void)
{
//------加速度--------------------------
//范围为2g时，换算关系：16384 LSB/g
//角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14
//因为x>=sinx,故乘以1.3适当放大
Accel_x = GetData(ACCEL_XOUT_H); //读取X轴加速度
if(Accel_x>0) value=1;
else value=0;
Angle_ax = (Accel_x - 1100) /16384; //去除零点偏移,计算得到角度（弧度）
Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度,

//-------角速度-------------------------
//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
Gyro_y = GetData(GYRO_YOUT_H); //静止时角速度Y轴输出为-30左右
Gyro_y = -(Gyro_y + 30)/16.4; //去除零点偏移，计算角速度值,负号为方向处理
Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度.
//-------卡尔曼滤波融合-----------------------
Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
/*-------互补滤波-----------------------*/
//补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与
//陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度
//0.5为放大倍数，可调节补偿度；0.01为系统周期10ms
Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);
}
int PWM(int a,int b)
{
CCAP0H=(65536-a)/256;
CCAP0L=(65536-a)%256;
CCAPM0=0x42; //PCA模块0工作在8位PWM模式
CCAP1H=(65536-b)/256;
CCAP1L=(65536-b)%256;
CCAPM1=0x42;
CR=1; //启动PCA定时器
}
void PWM_calculate( void)
{
// Adjust_up1 =Gyro_y*2;
Adjust_up1=0;
Adjust_up2 = Angle*200;
if( Angle>0) //加速度向前
{
in1=0;
in2=1;
in3=0;
in4=1;
if(Gyro_y>0)
{
PWM_Duty1 =(int)Adjust_up2 -(int)Adjust_up1;
}
if(Gyro_y<=0)
{
PWM_Duty1 =(int)Adjust_up1 + (int)Adjust_up2;
}
}
else if(Angle<0)//加速度向后
{
in1=1;
in2=0;
in3=1;
in4=0;
if(Gyro_y<=0)
{
PWM_Duty1 =0 - (int)Adjust_up2 - (int)Adjust_up1;
}
if(Gyro_y>0)
{
PWM_Duty1 =0 - (int)Adjust_up2 + (int)Adjust_up1;
}
}
}
void delay(unsigned int k)
{
unsigned int i,j;
for(i=0;i<k;i++)
{
for(j=0;j<121;j++);
}
}
void I2C_Start()
{
SDA = 1;
SCL = 1;
Delay5us();
SDA = 0;
Delay5us();
SCL = 0;
}
void I2C_SendACK(bit ack)
{
SDA = ack;
SCL = 1;
Delay5us();
SCL = 0;
Delay5us();
}
bit I2C_RecvACK()
{
SCL = 1;
Delay5us();
CY = SDA;
SCL = 0;
Delay5us();
return CY;
}
void I2C_SendByte(uchar dat)
{
uchar i;
for (i=0; i<8; i++)
{
dat <<= 1;
SDA = CY;
SCL = 1;
Delay5us();
SCL = 0;
Delay5us();
}
I2C_RecvACK();
}
uchar I2C_RecvByte()
{
uchar i;
uchar dat = 0;
SDA = 1;
for (i=0; i<8; i++)
{
dat <<= 1;
SCL = 1;
Delay5us();
dat |= SDA;
SCL = 0;
Delay5us();
}
return dat;
}
void Single_WriteI2C(uchar REG_Address,uchar REG_data)
{
I2C_Start();
I2C_SendByte(SlaveAddress);
I2C_SendByte(REG_Address);
I2C_SendByte(REG_data);
I2C_Stop();
}
uchar Single_ReadI2C(uchar REG_Address)
{
uchar REG_data;
I2C_Start();
I2C_SendByte(SlaveAddress);
I2C_SendByte(REG_Address);
I2C_Start();
I2C_SendByte(SlaveAddress+1);
REG_data=I2C_RecvByte();
I2C_SendACK(1);
I2C_Stop();
return REG_data;
}
void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00);
Single_WriteI2C(SMPLRT_DIV, 0x07);
Single_WriteI2C(CONFIG, 0x06);
Single_WriteI2C(GYRO_CONFIG, 0x18);
Single_WriteI2C(ACCEL_CONFIG, 0x01);
}
int GetData(uchar REG_Address)
{
char H,L;
H=Single_ReadI2C(REG_Address);
L=Single_ReadI2C(REG_Address+1);
return (H<<8)+L;
}
void main()
{
CCON=0x00; //初始化PCA控制寄存器
CMOD=0x02;
CL=0x00;
CH=0x00;
InitMPU6050();
delay(150);
while(1)
{
Angle_Calcu();
PWM_calculate();
PWM(PWM_Duty1+38000,PWM_Duty1+38000);
// PWM(37000,37000);
}
}

以上程序是我们在指导老师的耐心指导下不断地解决各种错误，能够基本实现智能控制两轮自平衡小车直立行走的代码。但是上面程序还有许多漏洞,实际应用是仍然不稳定，小车左右摆动太大，速度控制不好。
二、存在的问题和困难
1. 对项目研发过程中所需的知识面不够及所需具备的技术不够成熟；
2. 目前对于还是学生的我们来说没有更多的资金去购买更多更好的相关书籍和学习，这是一大问题和困难；
3. 以我们现在所掌握的基础对程序设计以及实现还存在很多的不足，所设计出来的程序虽然能运行和实现相应的功能，但程序有很多的不足，如：程序以及方法比较繁琐，容易出错，对内存空间的使用存在浪费空间等问题，还有程序的运行效率不够好等等。
4. 以上问题我们会在指导老师的指导下努力改善，使得程序更完善。
三、有何建议和要求
希望学校在资金方面能够多给一些资助，让我们可以获得更多的信息资源和实验器材。

四、学院意见

主管领导签名（盖章）： 年 月 日

27