基于AVR单片机: 两轮自平衡智能车
来源:互联网 发布:上瘾网络剧秒拍视频 编辑:程序博客网 时间:2024/04/29 09:03
寒假无所事事,决定做一个两轮平衡车来玩玩.个人觉得两轮自平衡车是集合所有算法的精髓.它就像蛋炒饭,最简单也是最困难.他可以用简单的算法实现,当然也可以用最复杂的控制理论来实现. 本质上来说它就是个倒立摆.
个人原创,转载请注明原文出处:
http://www.embbnux.com/2014/02/21/avr_mcu_balance_car/
所需原件:
AVR单片机 : 采用之前的ATmega128开发板
传感器: ENC03陀螺仪 mma8451加速度计
电机驱动: L298n
总体思路:
使用陀螺仪测角速度,使用加速度计测角度,使用测得的这两个值进行传感器融合,获取准确的角度.由得到的角度来控制车子两个轮子的转向,转速等参数.进而实现两轮车的平衡. 传感器融合使用卡尔曼滤波算法,不得不大赞一下卡尔曼同学,给了个这么perfect的算法,传说当年挑战者号就是用的这个算法.
角度的测量:
enc03陀螺仪: 采用的这个陀螺仪是模拟量的,输出电压需要用ATmega128进行adc转换成数字量,测得电压,再转换成角速度:
ISR(ADC_vect){ if(adc_done_flag==0){ adc_read(adc_channel); if(adc_channel==1) { adc_channel=0; } else{ adc_channel++; adc_mid_tmp0[cou]=adc_value[0];//中值滤波 cou++; if(cou==3){ cou=0; if(adc_mid_tmp0[0]>adc_mid_tmp0[1]){ mid_tmp = adc_mid_tmp0[0]; adc_mid_tmp0[0] =adc_mid_tmp0[1]; adc_mid_tmp0[1] =mid_tmp; } if(adc_mid_tmp0[2]>adc_mid_tmp0[1]) adc_mid[0]=adc_mid_tmp0[1] ; else if (adc_mid_tmp0[2]>adc_mid_tmp0[0]) adc_mid[0]=adc_mid_tmp0[2] ; else adc_mid[0]=adc_mid_tmp0[0] ; } } adc_set_channel(adc_channel); adc_done_flag =1;}else adc_done_flag = 0;//ADCSRA|=(1<<ADSC); }
float gyro_enc03_w(unsigned int adc_value,float adc_bias){ float w_value; //把adc采集的值转化为电压 w_value = (float)(adc_value)*2540/1024; //mV w_value = w_value-adc_bias; //把电压转化为角速度 w_value = -w_value; w_value = w_value/0.67; return w_value;}
mma8451加速度计: mma8451使用的I2C接口,不过没事,128上面就有一个了,这里我使用硬件I2C来实现,代码较多这里给出关键代码:
unsigned char i2c_mma8451_Write(unsigned char Address,unsigned char Wdata){ unsigned char i; Start();//I2C启动 Wait(); if(TestAck()!=START) return 1;//ACK Write8Bit(MMA845x_IIC_ADDRESS);//写I2C 8451从器件地址和写方式 Wait(); if(TestAck()!=MT_SLA_ACK) return 1;//ACK Write8Bit(Address);//写x地址 Wait(); if(TestAck()!=MT_DATA_ACK) return 1;//ACK Write8Bit(Wdata);//写入设备ID及读信 Wait(); if(TestAck()!=MT_DATA_ACK) return 1;//ACK Stop();//I2C停止 for(i=0;i<250;i++); return 0;}//mma8451初始化//初始化为指定模式void mma845x_init(){ i2c_init(); i2c_mma8451_Write(CTRL_REG1,ASLP_RATE_20MS+DATA_RATE_5MS); i2c_mma8451_Write(XYZ_DATA_CFG_REG, FULL_SCALE_2G); //2G i2c_mma8451_Write(CTRL_REG1, (ACTIVE_MASK+ASLP_RATE_20MS+DATA_RATE_5MS)&(~FREAD_MASK)); //激活状态 14bit}//unsigned char i2c_mma8451_Read(unsigned char Address){ unsigned char temp; Start();//I2C启动 Wait(); if (TestAck()!=START) return 0;//ACK Write8Bit(MMA845x_IIC_ADDRESS);//写入设备ID及写信号 Wait(); if (TestAck()!=MT_SLA_ACK) return 0;//ACK //// Write8Bit(Address);//写X地址 Wait(); if (TestAck()!=MT_DATA_ACK) return 0; //?//data1=TestAck(); Start();//I2C重新启动 Wait(); if (TestAck()!=RE_START) return 0; Write8Bit(MMA845x_IIC_ADDRESS+1);//写I2C从器件地址和读方式 Wait(); if(TestAck()!=MR_SLA_ACK) return 0;//ACK Twi();//启动主I2C读方式 Wait(); if(TestAck()!=MR_DATA_NOACK) return 0;//ACK temp=TWDR;//读取I2C接收数据 Stop();//I2C停止 return temp;}//求出加速度原始对应数值unsigned int get_mma8451_data(unsigned char Address){ unsigned char x; unsigned int wx; unsigned char address_LSB; x = i2c_mma8451_Read(Address); if(Address==OUT_Z_MSB_REG) address_LSB= OUT_Z_LSB_REG; else if(Address==OUT_Y_MSB_REG) address_LSB= OUT_Y_LSB_REG; else address_LSB= OUT_X_LSB_REG; wx = ((i2c_mma8451_Read(address_LSB))|x<<8); return wx;}
使用卡尔曼滤波融合两个测量量:(参考自原来在阿莫论坛很流行的算法)
float angle, angle_dot; //外部需要引用的变量//-------------------------------------------------------float Q_angle=0.001, Q_gyro=0.003, R_angle=5.0, dt=0.01;//注意:dt的取值为kalman滤波器采样时间;float P[2][2] = { { 1, 0 }, { 0, 1 }}; float Pdot[4] ={0,0,0,0}; const char C_0 = 1; float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure{ angle+=(gyro_m-q_bias) * dt;//先验估计 Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分 Pdot[1]=- P[1][1]; Pdot[2]=- P[1][1]; Pdot[3]=Q_gyro; P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差 P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; angle_err = angle_m - angle;//zk-先验估计 PCt_0 = C_0 * P[0][0]; PCt_1 = C_0 * P[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E;//Kk K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * P[0][1]; P[0][0] -= K_0 * t_0;//后验估计误差协方差 P[0][1] -= K_0 * t_1; P[1][0] -= K_1 * t_0; P[1][1] -= K_1 * t_1; angle += K_0 * angle_err;//后验估计 q_bias += K_1 * angle_err;//后验估计 angle_dot = gyro_m-q_bias;//输出值(后验估计)的微分 = 角速度}
卡尔曼算法是要在定时器里面不断的更新,才能发挥作用.dt就是定时器的时间,这里我采用10ms.
主要算法在定时器中断里面实现:
定时器里面完成对角速度,角度的测量,卡尔曼更新,以及对电机的控制.
//定时器0 初始化void timer0_init(void){ //设置计数开始的初始值 TCNT0 = 176 ; // 208>>6ms 176>>10ms //设置分频 TCCR0 = (1<<CS02)|(1<<CS00)|(1<<CS01); //1024分频 47次6ms 62.5次8ms //设置中断屏蔽寄存器 TIMSK = (1<<TOIE0); //设置溢出使能中断}//定时器0溢出中断ISR(TIMER0_OVF_vect){ //设置计数开始的初始值 TCNT0 = 176 ; //10ms mma8451_data = get_mma8451_data(OUT_Z_MSB_REG); acc = mma8451_i2c_to_angel(mma8451_data); acc_tmp=acc; gyro = gyro_enc03_w(adc_mid[0],adc_bias); gyro_tmp = gyro; //卡尔曼 Kalman_Filter(acc,gyro); //控制车速if (angle>0) { car_back(); if(angle>15) car_speed(0x2f,0x2f);else car_speed(0x4f,0x4f);}else if(angle<0){ car_forward(); if(angle<-15) car_speed(0x2f,0x2f); else car_speed(0x4f,0x4f);} }
对于车速的控制,这里只采用简单的开环控制,之后会采用更复制的DIP控制或者模糊控制等算法,日后持续更新,敬请关注本博客,
0 0
- 基于AVR单片机: 两轮自平衡智能车
- 基于AVR单片机: 两轮自平衡智能车(2)-PID控制器
- 基于STC单片机的两轮平衡车设计
- 基于PID调节的两轮自平衡小车的循迹控制
- 两轮自平衡小车总结
- 基于AVR单片机的电机调速
- 基于单片机的智能密码锁
- 光电平衡智能车
- 自平衡智能机器人开发笔记
- 基于AVR单片机的多任务嵌入式Internet系统设计
- 基于AVR单片机的通用USB接口模块设计
- 基于AVR (ATmega16)单片机SPI接口计时器的设计
- 基于MC9S12DGl28单片机的智能寻迹车设计
- 基于51单片机的智能小车
- 二轮自平衡车
- AVR单片机开发笔记
- avr单片机开发入门
- avr单片机1602驱动
- JAVA完全控制Oracle中BLOB、CLOB说明
- asp.net web应用程序后台弹出提示框代码
- Linux常用命令英文全称与汉语解释
- 面试 需要注意三种公司
- spring框架学习(四)自动装配
- 基于AVR单片机: 两轮自平衡智能车
- 循环移位
- oracle中exp,imp的使用详解
- oracle 创建表空间
- C++学习[1] 字符串
- Json的规格非常简单
- 豪鹫闲谈:IBM x3650 m4服务器安装centos6.4系统
- Import Data from Excel to SQL Server
- 数据类型和Json格式