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

项目编号ky-131学科分类号（二级） 510.10 云南师范大学大学生科研训练基金项目中期检验汇报书项目名称: 两轮自平衡小车智能控制研究 项目类型: 通常项目 主 持 人: 赵庆波 起止年月： 12月 结题日期： 12月 联络电话: 云南师范大学教务处填 表 说 明一、填写项目中期检验汇报书前，请先查阅云南师范大学大学生科研训练基金管理措施和相关通知。二、项目中期检验汇报书各项内容，必需实事求是，表示要明确严谨，并要求用打印。对于填写不合要求、内容含糊不清、字迹潦草者，不予受理。三、项目类型：选填关键项目或通常项目。四、项目类别：选填自然科学或社会科学。五、封面项目编号和申请书上编号一致。六、打印格式：（一）纸张为A4大小，双面打印；（二）文中小标题：五号、黑体；（三）栏内正文：五号、宋体。七、填写中期检验汇报书一式二份（最少含一份原件），其中一份提交教务处实践教学科，另一份留学院存档。课题名称两轮自平衡小车智能控制研究项目类别自然科学项目编号ky-131填表时间5月20日主 持 人赵庆波关键组员赵庆波、余坦藏、聂浩南一、项目进展及取得结果情况（一）项目进展结果情况本研究从10月份就开始了计划时间范围内相关学习及制作，到现在为止已经学习了STC12C5A60S2单片机，而且能够熟练掌握利用相关应用软件，同时对双环PID控制及卡尔曼滤波、包含多种传感器、AD转换等知识有了深入了解，硬件电路也制作完成了，程序设计编写实现了初步完成。1. 在指导老师指导下针对该项目标相关文件查阅和资料搜集和学习；时间: 10月至1月。看老师给相关资料，并利用学校图书馆资源查阅文件及相关材料，和老师同学讨论学习相关知识。经过这一阶段学习对STC12C5A60S2单片基础知识有了一定掌握，单片机技术是软件和硬件结合技术，学习了部分相关单片机开发软件，还学习了电机驱动模块、陀螺仪加速计、HC-06蓝牙模块等大量单片机拓展功效试验器材功效结构。对双环PID控制及卡尔曼滤波算法全部有了全方面掌握。2.在指导老师指导下开始着手项目标研发和学习；(1).依据指导老师指导及指导，进行两轮自平衡智能控制小车硬件制作；时间：3月 至 4月。 内容及取得对应结果：硬件结构基础完成,能够实现直立行走。(2) 依据指导老师指导及指导，对两轮自平衡智能控制小车程序设计编写；时间：4月 至 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 #include #include #include #include#include#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 0xD0typedef unsigned char uchar;typedef unsigned short ushort;typedef unsigned int uint;sbit in1=P11;sbit in2=P12;sbit in3=P15;sbit in4=P16;sbit SCL=P27;sbit SDA=P26;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 Pdot4 =0,0,0,0;float xdata PP22 = 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; /先验估量Pdot0=Q_angle - PP01 - PP10; / Pk-先验估量误差协方差微分Pdot1=- PP11;Pdot2=- PP11; Pdot3=Q_gyro; PP00 += Pdot0 * dt; / Pk-先验估量误差协方差微分积分 PP01 += Pdot1 * dt; / =先验估量误差协方差 PP10 += Pdot2 * dt; PP11 += Pdot3 * dt; Angle_err = Accel - Angle; /zk-先验估量 PCt_0 = C_0 * PP00; PCt_1 = C_0 * PP10; 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 * PP01;PP00 -= K_0 * t_0; /后验估量误差协方差 PP01 -= K_0 * t_1; PP10 -= K_1 * t_0; PP11 -= 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_x0) value=1;else value=0; Angle_ax = (Accel_x - 1100) /16384; /去除零点偏移,计算得到角度（弧度） Angle_ax = Angle_ax*1.2*180/3.14; /弧度转换为度, /-角速度-/范围为deg/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( Angle0) /加速度向前 in1=0; in2=1; in3=0; in4=1; if(Gyro_y0) PWM_Duty1 =(int)Adjust_up2 -(int)Adjust_up1; if(Gyro_y=0) PWM_Duty1 =(int)Adjust_up1 + (int)Adjust_up2; else if(Angle0)/加速度向后 in1=1; in2=0; in3=1; in4=0; if(Gyro_y0) PWM_Duty1 =0 - (int)Adjust_up2 + (int)Adjust_up1; void delay(unsigned int k)unsigned int i,j;for(i=0;ik;i+) for(j=0;j121;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; i8; 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; i8; 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 (H8)+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. 以上问题我们会在指导老师指导下努力改善，使得程序更完善。三、有何提议和要求期望学校在资金方面能够多给部分资助，让我们能够取得更多信息资源和试验器材。四、学院意见主管领导署名（盖章）： 年 月 日