找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

帖子
查看: 8388|回復(fù): 4
收起左側(cè)

平衡車卡爾曼濾波源碼,6軸MPU6050+互補(bǔ)濾波

  [復(fù)制鏈接]
ID:321526 發(fā)表于 2018-5-3 19:45 | 顯示全部樓層 |閱讀模式
卡爾曼濾波單片機(jī)源程序如下:
  1.                                                                   
  2. /***********************************************************************
  3. // 兩輪自平衡車最終版控制程序（6軸MPU6050+互補(bǔ)濾波+PWM電機(jī)）
  4. // 單片機(jī)STC12C5A60S2
  5. // 晶振：20M
  6. ***********************************************************************/

  8. #include <REG52.H>       
  9. #include <math.h>     
  10. #include <stdio.h>   
  11. #include <INTRINS.H>

  13. typedef unsigned char  uchar;
  14. typedef unsigned short ushort;
  15. typedef unsigned int   uint;

  17. //******功能模塊頭文件*******

  19. #include "DELAY.H"                    //延時(shí)頭文件
  20. #include "STC_ISP.H"            //程序燒錄頭文件
  21. #include "SET_SERIAL.H"                //串口頭文件
  22. #include "SET_PWM.H"                //PWM頭文件
  23. #include "MOTOR.H"                        //電機(jī)控制頭文件
  24. #include "MPU6050.H"                //MPU6050頭文件



  28. //******角度參數(shù)************

  30. float Gyro_y;        //Y軸陀螺儀數(shù)據(jù)暫存
  31. float Angle_gy;      //由角速度計(jì)算的傾斜角度
  32. float Accel_x;             //X軸加速度值暫存
  33. float Angle_ax;      //由加速度計(jì)算的傾斜角度
  34. float Angle;         //小車最終傾斜角度
  35. uchar value;                 //角度正負(fù)極性標(biāo)記       

  37. //******PWM參數(shù)*************

  39. int   speed_mr;                 //右電機(jī)轉(zhuǎn)速
  40. int   speed_ml;                 //左電機(jī)轉(zhuǎn)速
  41. int   PWM_R;         //右輪PWM值計(jì)算
  42. int   PWM_L;         //左輪PWM值計(jì)算
  43. float PWM;           //綜合PWM計(jì)算
  44. float PWMI;                         //PWM積分值

  46. //******電機(jī)參數(shù)*************

  48. float speed_r_l;        //電機(jī)轉(zhuǎn)速
  49. float speed;        //電機(jī)轉(zhuǎn)速濾波
  50. float position;            //位移

  52. //******藍(lán)牙遙控參數(shù)*************
  53. uchar remote_char;
  54. char  turn_need;
  55. char  speed_need;

  57. //*********************************************************
  58. //定時(shí)器100Hz數(shù)據(jù)更新中斷
  59. //*********************************************************

  61. void Init_Timer1(void)        //10毫秒@20MHz,100Hz刷新頻率
  62. {
  63.         AUXR &= 0xBF;                //定時(shí)器時(shí)鐘12T模式
  64.         TMOD &= 0x0F;                //設(shè)置定時(shí)器模式
  65.         TMOD |= 0x10;                //設(shè)置定時(shí)器模式
  66.         TL1 = 0xE5;                    //設(shè)置定時(shí)初值
  67.         TH1 = 0xBE;                    //設(shè)置定時(shí)初值
  68.         TF1 = 0;                    //清除TF1標(biāo)志
  69.         TR1 = 1;                    //定時(shí)器1開始計(jì)時(shí)
  70. }



  74. //*********************************************************
  75. //中斷控制初始化
  76. //*********************************************************

  78. void Init_Interr(void)         
  79. {
  80.         EA = 1;     //開總中斷
  81.     EX0 = 1;    //開外部中斷INT0
  82.     EX1 = 1;    //開外部中斷INT1
  83.     IT0 = 1;    //下降沿觸發(fā)
  84.     IT1 = 1;    //下降沿觸發(fā)
  85.         ET1 = 1;    //開定時(shí)器1中斷
  86. }



  90. //******卡爾曼參數(shù)************
  91.                
  92. float code Q_angle=0.001;  
  93. float code Q_gyro=0.003;
  94. float code R_angle=0.5;
  95. float code dt=0.01;                          //dt為kalman濾波器采樣時(shí)間;
  96. char  code C_0 = 1;
  97. float xdata Q_bias, Angle_err;
  98. float xdata PCt_0, PCt_1, E;
  99. float xdata K_0, K_1, t_0, t_1;
  100. float xdata Pdot[4] ={0,0,0,0};
  101. float xdata PP[2][2] = { { 1, 0 },{ 0, 1 } };


  104. //*********************************************************
  105. // 卡爾曼濾波
  106. //*********************************************************

  108. //Kalman濾波，20MHz的處理時(shí)間約0.77ms；

  110. void Kalman_Filter(float Accel,float Gyro)               
  111. {
  112.         Angle+=(Gyro - Q_bias) * dt; //先驗(yàn)估計(jì)

  114.        
  115.         Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗(yàn)估計(jì)誤差協(xié)方差的微分

  117.         Pdot[1]=- PP[1][1];
  118.         Pdot[2]=- PP[1][1];
  119.         Pdot[3]=Q_gyro;
  120.        
  121.         PP[0][0] += Pdot[0] * dt;   // Pk-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分
  122.         PP[0][1] += Pdot[1] * dt;   // =先驗(yàn)估計(jì)誤差協(xié)方差
  123.         PP[1][0] += Pdot[2] * dt;
  124.         PP[1][1] += Pdot[3] * dt;
  125.                
  126.         Angle_err = Accel - Angle;        //zk-先驗(yàn)估計(jì)
  127.        
  128.         PCt_0 = C_0 * PP[0][0];
  129.         PCt_1 = C_0 * PP[1][0];
  130.        
  131.         E = R_angle + C_0 * PCt_0;
  132.        
  133.         K_0 = PCt_0 / E;
  134.         K_1 = PCt_1 / E;
  135.        
  136.         t_0 = PCt_0;
  137.         t_1 = C_0 * PP[0][1];

  139.         PP[0][0] -= K_0 * t_0;                 //后驗(yàn)估計(jì)誤差協(xié)方差
  140.         PP[0][1] -= K_0 * t_1;
  141.         PP[1][0] -= K_1 * t_0;
  142.         PP[1][1] -= K_1 * t_1;
  143.                
  144.         Angle        += K_0 * Angle_err;         //后驗(yàn)估計(jì)
  145.         Q_bias        += K_1 * Angle_err;         //后驗(yàn)估計(jì)
  146.         Gyro_y   = Gyro - Q_bias;         //輸出值(后驗(yàn)估計(jì))的微分=角速度

  148. }



  152. //*********************************************************
  153. // 傾角計(jì)算（卡爾曼融合）
  154. //*********************************************************

  156. void Angle_Calcu(void)         
  157. {
  158.         //------加速度--------------------------

  160.         //范圍為2g時(shí)，換算關(guān)系：16384 LSB/g
  161.         //角度較小時(shí)，x=sinx得到角度（弧度）, deg = rad*180/3.14
  162.         //因?yàn)閤>=sinx,故乘以1.3適當(dāng)放大

  164.         Accel_x  = GetData(ACCEL_XOUT_H);          //讀取X軸加速度
  165.         Angle_ax = (Accel_x - 1100) /16384;   //去除零點(diǎn)偏移,計(jì)算得到角度（弧度）
  166.         Angle_ax = Angle_ax*1.2*180/3.14;     //弧度轉(zhuǎn)換為度,


  169.     //-------角速度-------------------------

  171.         //范圍為2000deg/s時(shí)，換算關(guān)系：16.4 LSB/(deg/s)

  173.         Gyro_y = GetData(GYRO_YOUT_H);              //靜止時(shí)角速度Y軸輸出為-30左右
  174.         Gyro_y = -(Gyro_y + 30)/16.4;         //去除零點(diǎn)偏移，計(jì)算角速度值,負(fù)號為方向處理
  175.         //Angle_gy = Angle_gy + Gyro_y*0.01;  //角速度積分得到傾斜角度.       

  177.        
  178.         //-------卡爾曼濾波融合-----------------------

  180.         Kalman_Filter(Angle_ax,Gyro_y);       //卡爾曼濾波計(jì)算傾角


  183.         /*//-------互補(bǔ)濾波-----------------------

  185.         //補(bǔ)償原理是取當(dāng)前傾角和加速度獲得傾角差值進(jìn)行放大，然后與
  186.     //陀螺儀角速度疊加后再積分，從而使傾角最跟蹤為加速度獲得的角度
  187.         //0.5為放大倍數(shù)，可調(diào)節(jié)補(bǔ)償度；0.01為系統(tǒng)周期10ms       
  188.                
  189.         Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/
  190.                                                                                                                           
  191. }  



  195. //*********************************************************
  196. //電機(jī)轉(zhuǎn)速和位移值計(jì)算
  197. //*********************************************************

  199. void Psn_Calcu(void)         
  200. {
  201.        
  202.         speed_r_l =(speed_mr + speed_ml)*0.5;
  203.         speed *= 0.7;                                  //車輪速度濾波
  204.         speed += speed_r_l*0.3;       
  205.         position += speed;                          //積分得到位移
  206.         position += speed_need;
  207.         if(position<-6000) position = -6000;
  208.         if(position> 6000) position =  6000;

  210.          
  211. }


  214. static float code Kp  = 9.0;       //PID參數(shù)
  215. static float code Kd  = 2.6;            //PID參數(shù)
  216. static float code Kpn = 0.01;      //PID參數(shù)
  217. static float code Ksp = 2.0;            //PID參數(shù)

  219. //*********************************************************
  220. //電機(jī)PWM值計(jì)算
  221. //*********************************************************

  223. void PWM_Calcu(void)         
  224. {
  225.    
  226.         if(Angle<-40||Angle>40)               //角度過大，關(guān)閉電機(jī)
  227.         {  
  228.           CCAP0H = 0;
  229.       CCAP1H = 0;
  230.           return;
  231.         }
  232.         PWM  = Kp*Angle + Kd*Gyro_y;          //PID：角速度和角度
  233.         PWM += Kpn*position + Ksp*speed;      //PID：速度和位置
  234.         PWM_R = PWM + turn_need;
  235.         PWM_L = PWM - turn_need;
  236.         PWM_Motor(PWM_L,PWM_R);
  237.          
  238. }




  243. //*********************************************************
  244. //手機(jī)藍(lán)牙遙控
  245. //*********************************************************

  247. void Bluetooth_Remote(void)         
  248. {

  250.         remote_char = receive_char();                                   //接收藍(lán)牙串口數(shù)據(jù)

  252.         if(remote_char ==0x02) speed_need = -80;           //前進(jìn)
  253.         else if(remote_char ==0x01) speed_need = 80;   //后退
  254.              else speed_need = 0;                                           //不動(dòng)

  256.     if(remote_char ==0x03) turn_need = 15;                   //左轉(zhuǎn)
  257.         else if(remote_char ==0x04) turn_need = -15;   //右轉(zhuǎn)
  258.              else turn_need = 0;                                           //不轉(zhuǎn)
  259.          
  260. }


  263. /*=================================================================================*/

  265. //*********************************************************
  266. //main
  267. //*********************************************************
  268. void main()
  269. {

  271.         delaynms(500);           //上電延時(shí)
  272.         Init_PWM();               //PWM初始化
  273.     Init_Timer0();     //初始化定時(shí)器0，作為PWM時(shí)鐘源
  274.         Init_Timer1();     //初始化定時(shí)器1
  275.         Init_Interr();     //中斷初始化
  276.         Init_Motor();           //電機(jī)控制初始化
  277.         Init_BRT();                   //串口初始化（獨(dú)立波特率）
  278.         InitMPU6050();     //初始化MPU6050
  279.         delaynms(500);           

  281.         while(1)
  282.         {
  283.           
  284.          Bluetooth_Remote();

  286.         }
  287. }


  290. /*=================================================================================*/

  292. //********timer1中斷***********************

  294. void Timer1_Update(void) interrupt 3
  295. {
  296.   
  297.    TL1 = 0xE5;                    //設(shè)置定時(shí)初值10MS
  298.    TH1 = 0xBE;
  299.    
  300.    //STC_ISP();                    //程序下載
  301.    Angle_Calcu();                  //傾角計(jì)算
  302.    Psn_Calcu();                    //電機(jī)位移計(jì)算
  303.    PWM_Calcu();                    //計(jì)算PWM值
  304.    
  305.    speed_mr = speed_ml = 0;         

  307. }


  310. //********右電機(jī)中斷***********************

  312. void INT_L(void) interrupt 0
  313. {

  315.    if(SPDL == 1)  { speed_ml++; }                 //左電機(jī)前進(jìn)
  316.    else                      { speed_ml--; }                 //左電機(jī)后退
  317.    LED = ~LED;

  319. }


  322. //********左電機(jī)中斷***********************

  324. ……………………

  326. …………限于本文篇幅 余下代碼請從51黑下載附件…………
復(fù)制代碼

所有資料51hei提供下載:
MPU6050_Kalman_PWM_remote.rar (56.26 KB, 下載次數(shù): 256)


回復(fù)

使用道具 舉報(bào)

ID:471390 發(fā)表于 2019-5-24 09:26 | 顯示全部樓層
強(qiáng)，剛好需要
回復(fù)

使用道具 舉報(bào)

ID:471390 發(fā)表于 2019-5-24 09:26 | 顯示全部樓層
剛好需要，謝謝大佬
回復(fù)

使用道具 舉報(bào)

ID:622782 發(fā)表于 2021-6-12 06:39 | 顯示全部樓層
太棒了，正需要這個(gè)。頂禮膜拜中~
回復(fù)

使用道具 舉報(bào)

ID:81918 發(fā)表于 2024-1-6 14:00 | 顯示全部樓層

太棒了，正需要這個(gè)。頂禮膜拜中~
回復(fù)

使用道具 舉報(bào)

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規(guī)則

小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表