设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 电子技术分类讨论区 › 智能小车/机器人 › 自己做的双轮自平衡小车
返回列表 发新帖
查看: 6270|回复: 1
打印 上一主题 下一主题
收起左侧

自己做的双轮自平衡小车

[复制链接]
跳转到指定楼层
楼主
ID:113207 发表于 2016-4-10 02:43 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
控制器：ATmega16；8MHz；
加速度传感器：MMA2260；陀螺仪：EWTS82；
传感器的融合：卡尔曼滤波；
马达：EN_2342CR（速比64）+双路12脉冲编码器+CD40106对信号整形；
驱动板芯片：CD4001+IR2111+IRF1404（驱动电流可以很大）；
制作资料在附件压缩包里面，另外这篇文章http://www.51hei.com/bbs/dpj-47882-1.html 供参考

自己做的自平衡小车，基本达到预期效果，图片（拍摄效果不佳）及文件如下，视频请见：
http://v.youku.com/v_show/id_XMTc3ODg0Mjky.html
http://v.youku.com/v_show/id_XMTgxNTAxOTA4.html
http://v.youku.com/v_show/id_XMTgxNTAzMTI4.html
资料供大家参考；


照片 (原文件名:RII_2.jpg)


照片 (原文件名:RII_1.jpg)


照片 (原文件名:RII_3.jpg)


今天做了取消马达转速传感器的测试，利用PWM的值低通滤波后代替转速信号，也可以稳定控制，但在移动距离偏差较大时不能修正，小范围内和利用传感器闭环控制差不多；源文件如下
  1. //MCU：Mega16；晶振：8MHz；
  2. //PWM:4KHz;滤波器频率:100Hz;系统频率：100Hz；10ms;
  3. //赵国霖：10.05.10；
  4. #include
  5. #include
  6. #include

  8. //#define checkbit(var,bit)     (var&(0x01<<(bit)))     /*定义查询位函数*/
  9. //#define setbit(var,bit)     (var|=(0x01<<(bit)))     /*定义置位函数*/
  10. //#define clrbit(var,bit)     (var&=(~(0x01<<(bit)))) /*定义清零位函数*/


  13. //-------------------------------------------------------
  14. //输出端口初始化
  15. void PORT_initial(void)
  16. {
  17.         DDRA=0B00000000;
  18.         PINA=0X00;
  19.         PORTA=0X00;
  20.         
  21.         DDRB=0B00000000;
  22.         PINB=0X00;
  23.         PORTB=0X00;
  24.         
  25.         DDRC=0B00010000;
  26.         PINC=0X00;
  27.         PORTC=0X00;
  28.         
  29.         DDRD=0B11110010;
  30.         PIND=0X00;
  31.         PORTD=0X00;
  32. }


  35. //-------------------------------------------------------
  36. //定时器1初始化
  37. void T1_initial(void)                                       
  38. {
  39.         TCCR1A|=(1<<com1a1)|(1<<wgm10)|(1<<com1b1); t1：8位快速pwm模式、匹配时清0，top时置位
  40.         TCCR1B|=(1<<wgm12)|(1<<cs11); pwm：8分频：8m="" 8="" 256="4KHz;
  41. }


  44. //-------------------------------------------------------
  45. //定时器2初始化
  46. void T2_initial(void)                        //T2：计数至OCR2时产生中断
  47. {
  48.         OCR2=0X4E;                        //T2：计数20ms(0X9C)10ms(0X4E)时产生中断;
  49.         TIMSK|=(1<<ocie2);
  50.         TCCR2|=(1<<wgm21)|(1<<cs22)|(1<<cs21)|(1<<cs20); ctc模式，1024分频
  51. }


  54. //-------------------------------------------------------
  55. //外部中断初始化
  56. void INT_initial(void)
  57. {
  58.         MCUCR|=(1<<isc01)|(1<<isc00)|(1<<isc11)|(1<<isc10); int0、int1上升沿有效
  59.         GICR|=(1<<int0)|(1<<int1); 外部中断使能
  60. }


  63. //-------------------------------------------------------
  64. //串口初始化；
  65. void USART_initial( void )
  66. {        
  67.         UBRRH = 0X00;
  68.         UBRRL = 51;                //f=8MHz;设置波特率:9600:51;19200:25;
  69.         UCSRB = (1<<rxen)|(1<<txen); 接收器与发送器使能；
  70.         UCSRC = (1<<ursel)|(1<<ucsz0)|(1<<ucsz1); 设置帧格式:="" 8="" 个数据位,="" 1="" 个停止位；
  71.         
  72.         UCSRB|=(1<<rxcie); usart接收中断使能
  73. }


  76. //-------------------------------------------------------
  77. //串口发送数据；
  78. void USART_Transmit( unsigned char data )
  79. {
  80.         while ( !( UCSRA & (1<<udre))); 等待发送缓冲器为空；
  81.         UDR = data;                //将数据放入缓冲器，发送数据；
  82. }


  85. //-------------------------------------------------------
  86. //串口接收数据中断，确定数据输出的状态；
  87. #pragma interrupt_handler USART_Receive_Int:12
  88. static char USART_State;
  89. void USART_Receive_Int(void)
  90. {
  91.         USART_State=UDR;//USART_Receive();
  92. }


  95. //-------------------------------------------------------
  96. //计算LH侧轮速：INT0中断；
  97. //-------------------------------------------------------
  98. static int speed_real_LH;
  99. //-------------------------------------------------------
  100. #pragma interrupt_handler SPEEDLHINT_fun:2
  101. void SPEEDLHINT_fun(void)
  102. {
  103.         if (0==(PINB&BIT(0)))
  104.         {
  105.                 speed_real_LH-=1;
  106.         }
  107.         else
  108.         {
  109.                 speed_real_LH+=1;
  110.         }
  111. }


  114. //-------------------------------------------------------
  115. //计算RH侧轮速，：INT1中断；
  116. //同时将轮速信号统一成前进方向了；
  117. //-------------------------------------------------------
  118. static int speed_real_RH;
  119. //-------------------------------------------------------
  120. #pragma interrupt_handler SPEEDRHINT_fun:3
  121. void SPEEDRHINT_fun(void)
  122. {
  123.         if (0==(PINB&BIT(1)))
  124.         {
  125.                 speed_real_RH+=1;
  126.         }
  127.         else
  128.         {
  129.                 speed_real_RH-=1;
  130.         }
  131. }


  134. //-------------------------------------------------------
  135. //ADport采样：10位，采样基准电压Aref
  136. //-------------------------------------------------------
  137. static int AD_data;
  138. //-------------------------------------------------------
  139. int ADport(unsigned char port)
  140. {
  141.         ADMUX=port;
  142.         ADCSRA|=(1<<aden)|(1<<adsc)|(1<<adps1)|(1<<adps0); 采样频率为8分频；
  143.         while(!(ADCSRA&(BIT(ADIF))));
  144.         AD_data=ADCL;
  145.         AD_data+=ADCH*256;
  146.         AD_data-=512;
  147.         return (AD_data);
  148. }


  151. //*
  152. //-------------------------------------------------------
  153. //Kalman滤波，8MHz的处理时间约1.8ms；
  154. //-------------------------------------------------------
  155. static float angle, angle_dot;                 //外部需要引用的变量
  156. //-------------------------------------------------------
  157. static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;
  158.                         //注意：dt的取值为kalman滤波器采样时间;
  159. static float P[2][2] = {
  160.                                                         { 1, 0 },
  161.                                                         { 0, 1 }
  162.                                                 };
  163.         
  164. static float Pdot[4] ={0,0,0,0};

  166. static const char C_0 = 1;

  168. static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
  169. //-------------------------------------------------------
  170. void Kalman_Filter(float angle_m,float gyro_m)                        //gyro_m:gyro_measure
  171. {
  172.         angle+=(gyro_m-q_bias) * dt;
  173.         
  174.         Pdot[0]=Q_angle - P[0][1] - P[1][0];
  175.         Pdot[1]=- P[1][1];
  176.         Pdot[2]=- P[1][1];
  177.         Pdot[3]=Q_gyro;
  178.         
  179.         P[0][0] += Pdot[0] * dt;
  180.         P[0][1] += Pdot[1] * dt;
  181.         P[1][0] += Pdot[2] * dt;
  182.         P[1][1] += Pdot[3] * dt;
  183.         
  184.         
  185.         angle_err = angle_m - angle;
  186.         
  187.         
  188.         PCt_0 = C_0 * P[0][0];
  189.         PCt_1 = C_0 * P[1][0];
  190.         
  191.         E = R_angle + C_0 * PCt_0;
  192.         
  193.         K_0 = PCt_0 / E;
  194.         K_1 = PCt_1 / E;
  195.         
  196.         t_0 = PCt_0;
  197.         t_1 = C_0 * P[0][1];

  199.         P[0][0] -= K_0 * t_0;
  200.         P[0][1] -= K_0 * t_1;
  201.         P[1][0] -= K_1 * t_0;
  202.         P[1][1] -= K_1 * t_1;
  203.         
  204.         
  205.         angle        += K_0 * angle_err;
  206.         q_bias        += K_1 * angle_err;
  207.         angle_dot = gyro_m-q_bias;
  208. }
  209. //*/

  211. /*
  212. //-------------------------------------------------------
  213. //互补滤波
  214. //-------------------------------------------------------
  215. static float angle,angle_dot;                 //外部需要引用的变量
  216. //-------------------------------------------------------        
  217. static float bias_cf;
  218. static const float dt=0.01;
  219. //-------------------------------------------------------
  220. void complement_filter(float angle_m_cf,float gyro_m_cf)
  221. {
  222.         bias_cf*=0.998;                        //陀螺仪零飘低通滤波；500次均值；
  223.         bias_cf+=gyro_m_cf*0.002;
  224.         angle_dot=gyro_m_cf-bias_cf;
  225.         angle=(angle+angle_dot*dt)*0.90+angle_m_cf*0.05;        
  226.         //加速度低通滤波；20次均值；按100次每秒计算，低通5Hz；
  227. }
  228. */        




  233. //-------------------------------------------------------
  234. //AD采样；
  235. //以角度表示；
  236. //加速度计：1.2V=1g=90°；满量程：1.3V~3.7V；
  237. //陀螺仪：0.5V~4.5V=-80°~+80°；满量程5V=200°=256=200°；
  238. //-------------------------------------------------------
  239. static float gyro,acceler;
  240. //-------------------------------------------------------
  241. void AD_calculate(void)
  242. {
  243.         
  244.         acceler=ADport(2)+28;                        //角度校正
  245.         gyro=ADport(3);        
  246.         
  247.         acceler*=0.004069;                //系数换算：2.5/(1.2*512);
  248.         acceler=asin(acceler);
  249.         gyro*=0.00341;                        //角速度系数：(3.14/180)* 100/512=0.01364；        
  250.         
  251.         Kalman_Filter(acceler,gyro);
  252.         //complement_filter(acceler,gyro);
  253. }
  254.         


  257. //-------------------------------------------------------
  258. //PWM输出
  259. //-------------------------------------------------------
  260. void PWM_output (int PWM_LH,int PWM_RH)
  261. {
  262.         if (PWM_LH<0)
  263.         {
  264.                 PORTD|=BIT(6);
  265.                 PWM_LH*=-1;
  266.         }
  267.         else
  268.         {
  269.                 PORTD&=~BIT(6);
  270.         }
  271.         
  272.         if (PWM_LH>252)
  273.         {
  274.                 PWM_LH=252;
  275.         }
  276.         
  277.         
  278.         if (PWM_RH<0)
  279.         {
  280.                 PORTD|=BIT(7);
  281.                 PWM_RH*=-1;
  282.         }
  283.         else
  284.         {
  285.                 PORTD&=~BIT(7);
  286.         }
  287.         
  288.         if (PWM_RH>252)
  289.         {
  290.                 PWM_RH=252;
  291.         }
  292.         
  293.                
  294.         OCR1AH=0;
  295.         OCR1AL=PWM_LH;                        //OC1A输出；
  296.         
  297.         OCR1BH=0;
  298.         OCR1BL=PWM_RH;                        //OC1B输出；
  299.         
  300. }




  305. //-------------------------------------------------------
  306. //计算PWM输出值
  307. //车辆直径：76mm; 12*64pulse/rev; 1m=3216pulses;
  308. //-------------------------------------------------------        
  309. //static int speed_diff,speed_diff_all,speed_diff_adjust;
  310. //static float K_speed_P,K_speed_I;
  311. static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot;
  312. static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;
  313. static float position,position_dot;
  314. static float position_dot_filter;
  315. static float PWM;
  316. static int speed_output_LH,speed_output_RH;
  317. static int Turn_Need,Speed_Need;
  318. //-------------------------------------------------------        
  319. void PWM_calculate(void)        
  320. {
  321.         if ( 0==(~PINA&BIT(1)) )        //左转
  322.         {
  323.                 Turn_Need=-40;
  324.         }
  325.         else if ( 0==(~PINB&BIT(2)) )         //右转
  326.         {
  327.                 Turn_Need=40;
  328.         }
  329.         else        //不转
  330.         {
  331.                 Turn_Need=0;
  332.         }
  333.         
  334.         if ( 0==(~PINC&BIT(0)) )        //前进
  335.         {
  336.                 Speed_Need=-2;
  337.         }
  338.         else if ( 0==(~PINC&BIT(1)) )        //后退
  339.         {
  340.                 Speed_Need=2;
  341.         }
  342.         else        //不动
  343.         {
  344.                 Speed_Need=0;
  345.         }
  346.         
  347.         K_angle_AD=ADport(4)*0.007;
  348.         K_angle_dot_AD=ADport(5)*0.007;
  349.         K_position_AD=ADport(6)*0.007;
  350.         K_position_dot_AD=ADport(7)*0.007;

  352.         position_dot=speed_output_RH*0.04;
  353.         
  354.         position_dot_filter*=0.9;                //车轮速度滤波
  355.         position_dot_filter+=position_dot*0.1;        
  356.         
  357.         position+=position_dot_filter;
  358.         //position+=position_dot;        
  359.         position+=Speed_Need;
  360.         
  361.         if (position<-768)                //防止位置误差过大导致的不稳定
  362.         {
  363.                 position=-768;
  364.         }
  365.         else if  (position>768)
  366.         {
  367.                 position=768;
  368.         }
  369.         
  370.         PWM = K_angle*angle *K_angle_AD + K_angle_dot*angle_dot *K_angle_dot_AD +
  371.                                         K_position*position *K_position_AD + K_position_dot*position_dot_filter *K_position_dot_AD;
  372.         
  373.         
  374.         speed_output_RH = PWM - Turn_Need;
  375.         speed_output_LH = - PWM - Turn_Need ;
  376.         
  377.         /*
  378.         speed_diff=speed_real_RH-speed_real_LH;                        //左右轮速差PI控制；
  379.         speed_diff_all+=speed_diff;
  380.         speed_diff_adjust=(K_speed_P*speed_diff+K_speed_I*speed_diff_all)/2;
  381.         */
  382.                
  383.         PWM_output (speed_output_LH,speed_output_RH);        
  384. }

  386. //-------------------------------------------------------
  387. //定时器2中断处理
  388. //-------------------------------------------------------
  389. static unsigned char temp;
  390. //-------------------------------------------------------
  391. #pragma interrupt_handler T2INT_fun:4
  392. void T2INT_fun(void)                                                
  393. {
  394.         AD_calculate();
  395.         
  396.         PWM_calculate();
  397.         
  398.         if(temp>=4)                        //10ms即中断；每秒计算：100/4=25次；
  399.         {               
  400.                 if (USART_State==0X30)                //ASCII码：0X30代表字符'0'
  401.                 {
  402.                         USART_Transmit(angle*57.3+128);
  403.                         USART_Transmit(angle_dot*57.3+128);
  404.                         USART_Transmit(128);        
  405.                 }
  406.                 else if(USART_State==0X31)                //ASCII码：0X30代表字符'1'
  407.                 {
  408.                         USART_Transmit(speed_output_LH+128);
  409.                         USART_Transmit(speed_output_RH+128);                        
  410.                         USART_Transmit(128);
  411.                 }
  412.                 else if(USART_State==0X32)                //ASCII码：0X30代表字符'2'
  413.                 {
  414.                         USART_Transmit(speed_real_LH+128);
  415.                         USART_Transmit(speed_real_RH+128);
  416.                         USART_Transmit(128);
  417.                 }
  418.                 else if(USART_State==0X33)                //ASCII码：0X30代表字符'3'
  419.                 {
  420.                         USART_Transmit(K_angle+128);
  421.                         USART_Transmit(K_angle_dot+128);
  422.                         USART_Transmit(K_position_dot+128);
  423.                 }                                
  424.                 temp=0;                                
  425.         }
  426.         speed_real_LH=0;
  427.         speed_real_RH=0;        
  428.         temp+=1;        
  429. }



  433. //-------------------------------------------------------
  434. int i,j;
  435. //-------------------------------------------------------
  436. void main(void)
  437. {
  438.         PORT_initial();
  439.         T2_initial();
  440.         INT_initial();
  441.         USART_initial ();
  442.         
  443.         SEI();
  444.         
  445.         K_position=0.8 * 0.209;                //换算系数：(256/10) * (2*pi/(64*12))=0.20944；//256/10:电压换算至PWM，256对应10V；
  446.         K_angle=34 * 25.6;                //换算系数：256/10 =25.6；
  447.         K_position_dot=1.09 * 20.9;                //换算系数：(256/10) * (25*2*pi/(64*12))=20.944；
  448.         K_angle_dot=2 * 25.6;                //换算系数：256/10 =25.6；
  449.         
  450.         for (i=1;i<=500;i++)                //延时启动PWM，等待卡尔曼滤波器稳定
  451.         {
  452.                 for (j=1;j<=300;j++);;
  453.                
  454.         }
  455.         T1_initial();
  456.                
  457.         while(1)
  458.         {
  459.                 ;
  460.         }
  461. }
复制代码

OK：其实控制很简单，利用定时器产生10ms中断，每次中断进行如下工作：
1：AD采样加速度传感器和陀螺仪，然后卡尔曼滤波得出角度与角速度；（滤波模块借鉴老外的）（互补滤波效果感觉不是很好）；
2：计算车轮的速度，积分得出位置；
3：利用PD算法得出PWM值=K1*angle + K2*angle_dot + K3*speed + K4 * position；
4：前进后退给定参考速度计算即可；
注：
1：增量式轮速传感器A相中断，读取B相电位判断前进还是后退，在10ms的时间内累加，计算车轮的速度；
2：由于10ms累加的轮速信号不多，直接计算车体会发抖，所以增加了低通滤波，解决问题；
3：我在外面增加4个电位器可以手动调节4个K值，方便调试；
以上；

附件的文件夹里面有matlab建模的资料，可以求出所需的4个K的值，（参考NXT的），大家可自行调试K值看看对车辆的影响；

回复hahahagg
环路控制怎么这么慢啊?
振荡很久才稳定...
-----------------------------------------------------------------------

其实我也在思考，可能原因：
1：对速度做了滤波导致相位延迟；
2：倾斜角速度和车轮速度的K值不够大；
经实践：K值增大后，振荡减小；:)

回复format
敢问楼主的车轮和电机哪里买的？
价格？
-----------------------------------------------------------------------

电机在淘宝上买二手的，大几十元一个，因为考虑到减速器，而且带传感器；
其实后面实践得知可以不要位置传感器，这样选择电机的空间就很大了，买便宜的；
建议：
1：电机扭矩要大；说白了，功率要大；转动惯量大似乎的更稳定，所以不用空心杯的也许更好；
2：减速比可以大一点，用较大的轮子；
3：重心越靠下，抗扰性越强；但是可能会影响稳定性；

只是删除了RS232通讯和多余的注释（包括互补滤波），效果和原来的一样，这个是没有速度传感器的版本；片内8M振荡器: 无速度传感器的精简版程序.rar (2.34 KB, 下载次数: 18)

电路图： protel99格式的电路图_563431QMA9X5.rar (74.88 KB, 下载次数: 15)

全部制作资料： 平衡小车制作资料ProjectRII.rar (1.44 MB, 下载次数: 23)

</aden)|(1<<adsc)|(1<<adps1)|(1<</ursel)|(1<<ucsz0)|(1<</rxen)|(1<</int0)|(1<</isc01)|(1<<isc00)|(1<<isc11)|(1<</wgm21)|(1<<cs22)|(1<<cs21)|(1<</ocie2);
</wgm12)|(1<</com1a1)|(1<<wgm10)|(1<
平衡

评分

参与人数 1黑币 +12 收起 理由
YJGG + 12 很给力！

查看全部评分

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏4 分享淘帖 顶顶 踩踩

相关帖子
回复

使用道具 举报

沙发
ID:113207 发表于 2016-4-10 02:58 | 只看该作者
常用计算方法有两种：互补滤波和卡尔曼滤波；都可以输出校正后的角度与角速度；我采用的是卡尔曼滤波，考虑到单片机的运算能力，是经过精简的，当然这些都是老外做的，关于卡尔曼滤波，我找了很多资料，较好的如下：
Kalman Filter Tutorial for Balancing Robot.pdf (76.47 KB, 下载次数: 15) kalman_intro_chinese.pdf (539.84 KB, 下载次数: 10)
互补滤波如下：点击此处下载 filter.rar (1.22 MB, 下载次数: 12)
我觉得卡尔曼的效果很好，但是初期有较大波动，要经过一段时间才能稳定；
http://www.docin.com/p-52169676.html
这个地方的可供参考；

回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表