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

1、项目编号ky2014-131学科分类号（二级）510.10云南师范大学大学生科研训练基金项目中期检查报告书项冃名称： 两轮口平衡小车智能控制研究项目类型：一般项目主持人：赵庆波起止年月：2014年12月结题日期：2015年12月联系电话南师范大学教务处填表说明一、填写项目中期检查报告帖前，请先查阅云南师范大学大学生科研训练 基金管理办法和相关通知。二、项冃中期检查报告书各项内容，必须实事求是，表达要明确严谨，并要求用 打印。对于填写不合耍求、内容含糊不清、字迹潦草者，不予受理。三、项目类型：选填重点项目或一般项目。四、项目类别：选填自然科学或社会科学。五、封面的项口编

2、号与申请书上的编号一致。六、打印格式：（-）纸张为a4大小，双面打印；（二）文中小标题：五号、黑体；（三）栏内正文：五号、宋体。七、填写中期检查报告书一式二份（至少含一份原件），其屮一份提交教务 处实践教学科，另一份留学院存档。课题名称两轮自平衡小车智能控制研究项目类别自然科学项目编号ky2014-131填表时间2015年5月20日主持人赵庆波主要成员赵庆波、余坦藏、聂浩南一、项目进展及取得的成果情况（一）项目进展成果情况本研究从2014年10月份就开始了计划时间范围内的相关学习及制作，到目前为止己经 学习了 stc12c5a60s2单片机，并能够熟练的掌握运用相关的应用软件，同时对双环pid

3、 控制及卡尔曼滤波、涉及的各种传感器、ad转换等知识有了深入的理解，硬件电路也制作完 成了，程序设计编写实现了初步的完成。1. 在指导老师的指导下针对该项目的相关文献查阅和资料的收集与学习；时间：2014年10月至2015年1月。看老师给的相关资料，并利用学校的图巧馆资源查阅文献及相关材料，与老师同学讨论 学习相关知识。通过这一阶段的学习对stc12c5a60s2单片的基础知识有了一定的掌握，单片机技术是软 件和硬件结合的技术，学习了的一些关于单片机开发的软件，还学习了电机张动模块、陀螺 仪加速计、hc-06蓝牙模块等人量的单片机拓展功能实验器材的功能结构。对双环pid控制 及卡尔曼滤波算法都

4、有了全面的掌握。2. 在指导老师的指导下开始着手项目的研发和学习；（1）根据指导老师的指导及指引，进行两轮自平衡智能控制小车的锁件制作；时间：2015年3月 至2015年4月。内容及取得相应的成果：硬件结构基本完成,能够实现直立行走。（2）根据指导老师的指导及指弓i，对两轮自平衡智能控制小车的程序设计编写； 时间：2015年4月 至2015年5月。内容及取得相应的成果：可是实现一部分的功能，但程序设计还不成熟，系统运行不稳定, 仍需改进。（二）阶段性成果情况通过以上比较深入的理论学习，以及在我们着手项目的研发，经过几个月的不断改良，我 们初步实现了两轮自平衡智能控制小车的直立行走。长时间认真的

5、理论知识学习为后期研发 制作打下了坚实的理论基础，两伦口平衡智能控制小年硕件结构都已经完成，口性能良好，程 序设计编写也有了初步的实现，但由于程序设计编写的时间较短，所以还不够成熟，智能控制 两轮小车不能稳定运行，从小还有许多问题，我们会在后期的研究中完善，具体成果如下：1. 两轮自平衡智能控制小车的硬件部分（1）速度检测模块设计两轮自平衡小车的原理是利川地面对车轮的摩擦力抵消车受到的重力，在本系统的控制 环节中有两路闭环控制，即倾角闭环控制以及速度闭环控制。为实现速度的闭环控制，必须 加入速度检测装置实现速度闭环控制中的反馈环节。木系统测速模块采用omron（欧姆龙）公 司500线增屋式旋转

6、编码器。编码器内部为一个屮心有轴的光电码盘，其上有环形通、暗的 刻线，冇光电发射和接收器件读取，获得四组止弦波信号组合成a、b、c、d,每个正弦波相差 90度相位差（相对于一个周波为360度），将c、d信号反向，叠加在a、b两相上，可增强 稳定信号；另每转输出一个z相脉冲以代表零位参考位。山于编码器采用集电极开路输岀， 输出波形为矩形波，因此编码器外围电路较为简单。盂要在信号输出端接入一个上拉电阻， 即可将信号提供给单片机采集数据。(2) 驱动电路采用功率三极管作为功率放大器的输出控制直流电机。线性型驱动的电路结构和原理简 单，加速能力强，采用由达林顿管组成的h型桥式电路，用单片机控制达林顿管

7、使z工作在 占空比可调的开关状态下，梢确调整电动机转速。这种电路山于工作在管子的饱和截止模式 下，效率非常高，ii型桥式电路保证了简单的实现转速和方向的控制，电了管的开关速度很 快，稳定性也极强，是一种广泛采用的pwm调速技术。现市而上很多这种芯片，我选了 lm298n, l298n是sgs公司的产站，内部包含4通道逻辑驱动电路。是一种二相和四相电机的专用驱 动器，即内含二个h桥的高电压大电流双全桥式驱动器，接收标准ttl逻辑电平信号，可驱 动46v、2a以下的电机。由于电力电子器件只工作在开关状态，主电路损耗较小，装置效率 较高。根据以上考虑，以及本设计屮受控电机的容量和直流电机转速的发展方

8、向，本设计采 用了 h型单极型对逆pwm变换器件lm298n进行电机调速。具体原理图如下：图1电机驱动电路原理图(3) cpu微控制器cpu微控制器采用stc12c5a60s2单片机，微控制器作为整个系统的核心部分，主要负责 对传感器采集的信号进行实时处理，但其计算量较大，普通的单片机不能满足使用要求。本 系统选用了高速stc12c5a60s2单片机作为微处理器，具有丰富的i/o接口、有双串口。其功 耗低、超强抗干扰能力、指令代码完全兼容传统单片机8051,能够满足系统的设计盂求，在 单片机上增加复位电路、时钟电路等外围电路构成单片机最小系统，让单片机工作起來，然 后通过编写程序対单片机进行控

9、制，进而单片机对传感器采集信号进行处理。在程序设计编 写方面i 分容易，r成木较低，也口j以很好的实现木设计。具体stc12c5a60s2单片机原理图 如下：im 12345678910homj<t5x2vccri3ljrc2 ik6djj 卜rxd2«27出化6 他5c3<t>2.34*2.2<p2.ilfcaier2ox2 c8lh)3gndgndc6i mltxb滞vddf-in1112i：14ind nm_k rts n vdd.232 rxd r11n gnd vdd dsr n ix7d.n crs_n slfld n he.cik hldatao

10、sc2 osci ptl. uxt cnd.pll vim) pll. id vkmx- ixi moi*gsi) vdd reset gxi3v3 vdd.3v3 1>m dp5y2 33pi2mpi2303hx24互22211915171615rfcwgnb>c9-| gn!)tvddr2rcx2 2727r4333p图2 stc12c5a60s2单片机原理图（4）陀螺仪传感器模块陀螺仪可以用来测量物体的旋转角速度。木设计选用村田公司出品的enc-03系列的加速 度传感辭。它利用了旋转朋标系中的物体会受到科里奥利力的原理，在器件中利用压电陶瓷 做成振动单元。当旋转器件时会改变振

11、动频率从而反映出物体旋转的角速度。在车模上安装 角速度传感器，可以测最车模倾斜角速度，再对倾斜角速度进行积分就得到了车模的倾如。 由于陀螺仪输出的是车模的角速度，不会受到车体运动的影响，因此该信号屮噪声很小。车 模的角度又是通过对角速度积分而得，这可进一步平滑信号，从而使得角度信号更加稳定。 因此车模控制所盂要的角度和角速度可以使用陀螺仪所得到的信号。由于从陀螺仪角速度获 得角度信息，需要经过积分运算。如果角速度信号存在微小的偏差和漂移，经过积分运算z 后，变化形成积累误差。这个误差会随着时间延长逐步增加，最终导致电路饱和，无法形成 正确的角度信号。为了消除积分漂移，我们引入姿态测量的方法。硬

12、件两轮口平衡小车实物图如下图：图3两轮口平衡小午实物图2. 程序的设计与编写在自平衡小车未做控制时，不论其车身向前或向后倾斜，两轮都处于静止状态，这时其 车身前后摆动与其车轮转动是相互独立的；当其开始控制时，其车身的状态变化使小车有静 止、前进（前倾）、后退（后仰）三种运动的方式，在正确的控制策略下，小车能够保持自身的 平衡。这三种运动方式与控制策略如图所示（1）前倾（2）静止（3）后仰图4小车三种运动方式（1）前倾状态：即车身重心靠前，车身会向前倾斜，则驱动车伦向前滚动，以保持小车 平衡。（2）静止状态：即车身重心位于电机轴心线的正上方，则小千将保持动态平衡静止状态, 不需要做任何控制。（3

13、）后仰状态：即车身重心靠后，车身会向后倾斜，则驱动车轮向后滚动，以保持小车 平衡。因此，两轮自平衡小车平衡控制的基木思想是：当测量倾斜饬度的传感器检测到车体产 牛倾斜时，控制系统会根据测得的倾角产生一个相应的力矩，通过控制电机驱动两个车轮朝 车身要倒下的方向运动，以保持小车自身的动态平衡。主要程序清单：include <reg51. h>#include <math. h>ttinclude <stdio. h>#inelude <string. h>nclude<intrins.h>#include<stc12c5a60s2.

14、 h> ttdefine smplrt_div 0x19 #define config oxla #define gyro_conftg oxlb #define accel_conf1g oxlc ttdefine accel_xout_h 0x3b #define accel_xout_l 0x3c #define accel yout h 0x3d #define accel_yout_l 0x3e #define accel_zoutj1 0x3f #define accel_zout_l 0x40 #define temp_out_h 0x41 ttdefine temp ou

15、t l 0x42 #define gyro xout h 0x43define gyro_xout_l 0x44 define gyro_yout_h 0x45 define gyro yout l 0x46 #define gyro_zout_h 0x47 #define gyro_zout_l 0x48 ttdefine pwr_mgmt_1 0x6b define who_am_i 0x75 ttdefine slaveaddress oxdo typedef unsigned char uchar; typedef unsigned short ushort; typedef unsi

16、gned int uint; sbit inl=pl 1;sbit in2二p2;sbit in3二p5;sbit in4=pl飞；sbit scl二p2"7;sbit sda=p2飞；void delay (unsignod int k);void tnitmpu6050()；void delay5us();void i2c_start();void 12c_stop();void i2c sendack(bit ack);bit t2c_recvack();void t2c_sendbyte(uchar dat); uchar i2c_recvbyte();void 12c_re

17、adpage (); void i2c writepage();int getdata(uchar reg_address); int pwm(int a, int b);void pwm_calculate( void);int temp;uchar single readl2c(uchar reg address); float adjust_upl, adjust_up2;unsigned int pwm_dutyl, pwm_duty2;/* * 角度参数* *float gyro_y;float angle gy; float accelx;float angle_ax;float

18、angle;/y轴陀螺仪数据暂存/由角速度计算的倾斜角度 /x轴加速度值暂存/ rti加速度计算的倾斜角度/小车最终倾斜角度/角度正负极性标记float anglel;uchar value;float code q_angle=0. 001; float code q gyro二0. 003;float code r_angle二0.5;charfloat float float float float voidcode c 0 = 1;xdataxdataxdataxdataxdatafloat code dt=0. 01; /dt 为 kalman 滤波器釆样时间;q_bias, ang

19、leerr;pct_0, pct_l, e;k_0, k_l, t_0, t_l;pdot 4 =0,0, 0,0;pp 2 = 1, 0 , 0, 1 ;si ng1e_wri tet2c(uchar reg address, uchar reg data);/卡尔曼滤波f fk|0"0"0"0"0" kj>/ /<tn彳j彳、ts zs彳、彳j7 彳j彳、z t%/在程序中利用angle+= (gyro - q bias) * dt计算出陀螺仪积分出的角度,其中q_bias 是陀螺仪偏差。/此时利川陀螺仪积分求出的angle相当

20、于系统的估计值，得到系统的观测方程；而加速度 计检测的角度accel相当于系统中的测量值，得到系统状态方程。/程序屮q.angle和q_gyro分别表示系统对加速度计及陀螺仪的信任度。/根据pdot = a*p + p*a' + q_angle计算出先验估计协方差的微分,/用于将当前佔计值进行线性化处理。其中a为雅克比矩阵。/随后计算系统预测角度的协方差矩阵p。计算估计值accel与预测值angle间的谋差angle_erro/计算卡尔曼增益k 0, k _l, k.0用于最优估计值，k_1用于计算最优估计值的偏差并更新协 方差矩阵po/通过卡尔曼增益计算出授优佔计值angle及预测值

21、偏差q_bi as，此时得到最优角度值angl e 及角速度值。/kalman滤波，20mhz的处理时间约0. 77ms； void kalman filter(float accel, float gyro)angle+= (gyro-q_bias) * dt; /先验估计pdot0=q_angle - pp0l - ppl0; / pk-先验估计误差协方差的微分pdotl=- pp11;pdot2二- pp11;/ pk-先验佔计谋差协方差微分的积分 /二先验估计谋差协方差/zk-先验估计pdot3=q_gyro;pp00+=pdot0*dt;pp0l+二pdotl*dt;ppl0+=pd

22、ot2*dt;pp11+=pdot3*dt;angle_err = accel - angle;pct_o = c_0*pp00;pct_1 = c_0*ppl0;e =r_angle+c_0 * pct_0;k 0=pct_0/e;k_1=pct_1/e;t_0=pct_0；“t l=c o*pp0i；pp00 -二 k_0 * t_0;ppoi-=ppl0-=ppll-=angle += k/后验估计误差协方差k_0k 1k 10t_l; t 0;t_l;angle_err; angle_err; q_bias;q_bias +二 k_1gyro_y = gyro/ /* * *卡尔曼融合

23、 *f f>1k1 a szz sxf ft% t%后验估计/ /后验估计/输岀俏(后验估计)的微分二角速度angle_calcu(void)/加速度/范围为2g时,换算关系:16384 lsb/g/角度较小时,x=sinx得到角度(弧度),deg = rad*180/3. 14/因为x>二sinx,故乘以1. 3适当放大accel x = get data (accel xol't h) ;/读取 x 轴加速度辻(accel_x>0) value二1;else value二0;angle_ax = (accel_x - 1100) /16384;/去除零点偏移，计算

24、得到角度(弧度)angle_ax = angle_ax*l. 2*180/3. 14;/弧度转换为度，/角速度/范围为 2000deg/s 时,换算关系：16.4 lsb/(deg/s)gyro_y = getdata(gyr0_y0ut_h) ;/静止时角速度y轴输出为-30左右gyro_y = -(gyro_y + 30)/16. 4;/去除零点偏移，计算角速度值，负号为方向处理angle_gy = angle_gy + gyro_y*0. 01;/角速度积分得到倾斜用度./卡尔曼滤波融合kalman_f订ter (angle_a.x, gyro_y) ;/卡尔曼滤波计算倾角/*互补滤波*

25、/补偿原理是取当前倾角和加速度获得倾角差值进行放人，然后与/陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度/0.5为放人倍数，可调节补偿度；0.01为系统周期10msangle = angle + (angle_axangle)*0. 5 + gyro_y)*001);int pwm(int a, int b)ccap0h=(65536-a)/256;ccap0l=(65536-a)%256;ccapho二0x42;/ka模块0工作在8位pwm模式ccap1h二(65536-b)/256;ccap1l二(65536-b)%256;ccapml=0x42;cr二1;/启动pca定时

26、器void pwm_calculate( void) / adjust_upl =gyro_y*2;adjust_up1=0;adjust_up2 = angle*200;if ( angle>0) /加速度向询inl=0;in2=l;in3=0;in4=l;if(gyro y>0)pwm_dutyl =(int)adjust_up2 -(int)adjust_upl;if(gyro_y<=0)pwmdutyl =(int)adjustupl + (int)adjust_up2;else if(angle<0)/力ii速度向后inl=l;in2=0;in3=l;in4=

27、0;if(gyro y<=0)pwm_dutyl =0 - (int)adjust_up2 - (int)adjust_upl; if(gyro_y>0)pwmdutyl =0 - (int)adjust_up2 + (int)adjustupl;void delay(unsignod 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 t2c_sendack(b

28、it 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 ();12c recvack ();)uchar t2c recvbyte()uchar i;uc

29、har dat = 0;sda = 1;for (i=0; i<8; i+)dat «= 1;scl = 1;delay5us ();dat |= sda;scl = 0;delay5us ();return dat;void single_writel2c(uchar reg_address, uchar reg_data) i2c_start ();i2c sendbyte(slaveaddress);t2c_sendbyte(regaddress);12c_sendbyte(reg_data);i2c_stop();uchar single readl2c(uchar reg address)uchar reg data;i2c_start ();i2c_sendbyte(slaveaddress);i2c sendbyte(reg address);i2c_start();i2c_sendbyte(sla