双轮自平衡小车结题

1、m册moz憊*血-*“迟s“#w皿瘁卅WIMr皿铝傑电子信息科学与技术专业科研训练论文电子信息科学与技术专业科研训练论文 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*/后验估计误差协方差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

2、;/输出值（后验估计）的微分=角速度/彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、/倾角计算(卡尔曼融合)voidAngle_Calcu(void)/加速度范围为2g时，换算关系：16384LSB/g角度较小时，x=sinx得到角度(弧度),deg=rad*180/3.14因为x=sinx，故乘以1.3适当放大Accel_x=GetData(ACCEL_XOUT_H);/读取X轴加速度Angle_ax=(Acce

3、l_x-1100)/16384;/去除零点偏移,计算得到角度(弧度)Angle_ax=Angle_ax*1.2*180/3.14;/弧度转换为度,/角速度范围为2000deg/s时，换算关系：16.4LSB/(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);/卡尔曼滤波计算倾角/*/互

4、补滤波/补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与/陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度/0.5为放大倍数，可调节补偿度；0.01为系统周期10msAngle=Angle+(Angle_ax-Angle)*0.5+Gyro_y)*0.01);*/彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、/电机转速和位移值计算/彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、

5、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、voidPsn_Calcu(void)speed_r_l=(speed_mr+speed_ml)*0.5;speed*=0.7;/车轮速度滤波speed+=speed_r_l*0.3;position+=speed;/积分得到位移position+=speed_need;if(position6000)position=6000;staticfloatcodeKp=9.0;/PID参数staticfloatcodeKd=2.6;/P

6、ID参数staticfloatcodeKpn=0.01;/PID参数staticfloatcodeKsp=2.0;/PID参数/彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、/电机PWM值计算/彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、

7、彳、voidPWM_Calcu(void)if(Angle40)if(Angle40)CCAP0H=0;CCAP1H=0;/角度过大，关闭电机return;PWMreturn;PWM=Kp*Angle+Kd*Gyro_y;PWM+=Kpn*position+Ksp*speed;PWM_R=PWM+turn_need;PWM_L=PWM-turn_need;/PID:角速度和角度/PID:速度和位置PWM_Motor(PWM_L,PWM_R);/彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、

8、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、/手机蓝牙遥控/彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、voidrun(void)speed_need=-80;/前进/*=*/彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、

9、彳、彳、彳、彳、彳、/main/彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、voidmain()delaynms(500);/上电延时Init_PWM();/PWM初始化Init_Timer0();/初始化定时器0，作为PWM时钟源Init_Timer1();/初始化定时器1Init_Interr();/中断初始化Init_Motor();/电机控制初始化Init_BRT();/串口初始化（独立波特率）InitM

10、PU6050();/初始化MPU6050delaynms(500);while(1)run()/*=*/voidINT_L(void)interrupt0/*timer1中断/*timer1中断彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、voidTimer1_Update(void)interrupt3voidTimer1_Update(void)interrupt3TL1=0 xE5;/设置定时初值10MSTH1=0 xBE;/STC_ISP();Angle_Calcu();Psn_Calcu();PWM_Calcu();speed_mr=sp

11、eed_ml=0;/程序下载/倾角计算/电机位移计算/计算PWM值/*右电机中断/*右电机中断彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、彳、if(SPDL=1)speed_ml+;/左电机前进elsespeed_ml-;/左电机后退LED=LED;/*左电机中断*voidINT_R(void)interrupt2/右电机前进/右电机前进/右电机后退if(SPDR=1)speed_mr+;elsespeed_mr-;LED=LED;六、实验结果与分析经过三周的努力，从开始的选题分组，然后再到开题报告，电路板焊接，机构设计与调试，硬件电路调试，软件电路

12、调试，再到最后的综合调试，我们最终取得了以下实验结果完成了一个结构稳定的小车硬件平台搭建。完成了一个可以控制小车进行平衡调整的。设计了一款可以基本站立的双轮自平衡小车。小车图片小车调试图片七、参与人心得从这次科研训练中，学到了很多知识，不单单加强了自己的动手能力，也学会了单片机的应用，需要将各个模块吃透才能将其运用自如，动脑是前提，动手是硬道理，在动手制作过程中总结经验，跟队友和老师一起克服各种困难。在此过程中我知道，一个项目的完成不只是靠一个人就能成功的，需要团队的同心协力，迎难而上，共同克服各种困难。在训练的过程中，在硬件平台搭建起来后，发现小车一直不能平衡的行走，于是乎我和队友每天都在研

13、究程序，通过改参数，发现还是不能平衡，结果发现重心不稳，不在小车的中心，于是又重新搭建硬件平台，把重心放低，放在小车的中心。经过这个过程，发现还是不能成功。再后来在老师的指导下，通过调PID参数，把小车的速度跟角度调到合适的位置，最后终于完成了。在完成的那一刻发现这次项目远非想象的那么简单，真正通过自己动手去做才会发现问题和如何坚决问题。在这次项目制作过程中我获得最重的东西是相互学习，弥补自己的不足，一起讨论中得到不同的意见，从中找到合适的见解，讨论一点一滴的知识并分析这些知识的作用，让大家知道要学的知识很多，决不能取得一点成绩就沾沾自喜，应更加勤奋学习。纸上得来终觉浅，绝知此事要躬行。最后感

14、谢老师们的指导让我顺利完成了这次科研训练。八、参考文献【1】郭天祥.51单片机C语言教程，电子工业出版社,2011【2】王元峰.戴旭辉.AltiumDesigner10电路设计标准教程.科学出版社,2012【3】谭浩强.C+程序设计清华大学出版社,2011【4】邱关愿.电路,高等教育出版社,2011【5】陈永蒲.用万用表检测电子元器件,电子工业出版社,2008【6】同济大学数学系.高等数学,高等教育出版社,2011【7】王正茂，阎治安，崔新艺等电机学西安交通大学出版社2000：3248【8】张晓华系统建模与仿真清华大学出版社2006230232【9】祝永刚，徐正扬惯性测量系统的理论与应用测绘出

15、版社，1989【10】杨耕，罗应立电机与运动控制系统清华大学出版社2006【11】许江宁，边少峰，殷立吴陀螺原理国防工业出版社，2005【12】袁泽睿两轮自平衡机器人控制算法的研究哈尔滨工业大学硕士论文【13】FelixGrasser,AldoDAr，riSgilovioColombi，AlfredCRuferJOE：AMobileInvertedPendulumIEEETransactionsOnIndustrialElectronics，v0149,No1，February2002【14】PaulE.SandinRobotMechanismsandMechanicalDevicesIllustrated，TheMcGrawHillcompanies【15】BillurBarshanandHughKDurrantWhyteInertialNavigationSystemsforM0bileRobotsIEEETransactionsonRoboticsandautomation，v0111，No3，June1995【16】AlessioSalernoJorgeAngelesOntheNonlinearContro