按照一个教学贴写了个融合算法，前几秒不错，可后来明显反应滞后

ShadowWalker

1. 
   
2. #include "Wire.h"
   
3. #include "I2Cdev.h"
   
4. #include "MPU6050.h"
   
5. MPU6050 accelgyro;
   
6. 
   
7. const float Ac_xs = 16384.0;      //重力加速度系数
   
8. const float Pa_xs = 131.0;        //角速度系数
   
9. const float Ra=180/PI ;         //弧度转角度系数
   
10. 
    
11. double  G;                  //重力加速度
    
12. unsigned long T_Now=0,T_Last=0;      //系统当前时间
    
13. 
    
14. int Ac[3];                  //读取加速度xyz
    
15. int Pa[3];                  //读取角速度xyz
    
16. float P[3];     //真实角速度
    
17. float C_Pa[3];              //角速度漂移xyz
    
18. float C_Ac[3];              //加速度计漂移
    
19. 
    
20. float Pa_X_Now,Pa_X_Last,Pa_Y_Now,Pa_Y_Last;     //积分角速度
    
21. float Rg[3];        //过程单位矢量
    
22. float A[3];             //真实惯性合力分量 |A|=1
    
23. float A_X,A_Y;           //x,y与Z轴夹角
    
24. float Gr[3];             //重力分量         |G|=1  算法输出
    
25. float W=10;             //Rg陀螺仪滤波权重
    
26. 
    
27. void setup()
    
28. {
    
29.     Wire.begin();
    
30.     Serial.begin(38400);
    
31.     accelgyro.initialize();
    
32. 
    
33.     for(int a=0;a<3;a++)            //获取角速度偏移
    
34.     {
    
35.         for(int i=0;i<50;i++)
    
36.         {
    
37.             accelgyro.getMotion6(&Ac[0], &Ac[1], &Ac[2], &Pa[0], &Pa[1], &Pa[2]);
    
38.             C_Pa[a]=C_Pa[a]+Pa[a];   
    
39.         }
    
40.         C_Pa[a]=C_Pa[a]/50;  
    
41.     }
    
42.     C_Ac[2]=-0.1;        
    
43. }
    
44. 
    
45. void loop()
    
46. {
    
47.     CalculateGr();
    
48.     printdata();
    
49.     delay(20);
    
50. }
    
51. 
    
52. void printdata ()
    
53. {
    
54. 
    
55.     Serial.print(acos(Gr[0])*Ra);Serial.print(",");
    
56.     Serial.print(acos(A[0])*Ra);Serial.print(",");
    
57.     Serial.println(acos(Rg[0])*Ra);
    
58.   
    
59. }
    
60. 
    
61. void CalculateGr ()           
    
62. {
    
63.     accelgyro.getMotion6(&Ac[0], &Ac[1], &Ac[2], &Pa[0], &Pa[1], &Pa[2]);
    
64.     for(unsigned char a=0;a<3;a++)                //处理角速度漂移
    
65.     {
    
66.         Pa[a]=Pa[a]-C_Pa[a];
    
67.     }
    
68. 
    
69.     for(unsigned char a=0;a<3;a++)                //得到真实加速度和角速度
    
70.     {
    
71.         A[a]=Ac[a]/Ac_xs;           
    
72.         P[a]=Pa[a]/Pa_xs;
    
73.     }
    
74.     A[2]=A[2]-C_Ac[2];              //处理加速度漂移
    
75.     G=sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]);      
    
76.    
    
77. 
    
78. 
    
79.     if(T_Last==0)           //第一次
    
80.     {
    
81.         for(unsigned char a=0;a<3;a++)               
    
82.         {
    
83.             for(unsigned char b=0;b<10;b++)
    
84.             {
    
85.                 accelgyro.getMotion6(&Ac[0], &Ac[1], &Ac[2], &Pa[0], &Pa[1], &Pa[2]);
    
86.                 A[a]=A[a]/Ac_xs;                  
    
87.                 A[a]=A[a]+A[a];
    
88.             }
    
89.             A[a]=A[a]/10.0;
    
90.             Gr[a]=A[a];
    
91.         }
    
92.         A_X=acos(A[0]);
    
93.         A_Y=acos(A[1]);
    
94.         T_Last=micros();
    
95.         Pa_Y_Last=Pa[1];
    
96.         Pa_X_Last=Pa[0];
    
97.     }
    
98.     if(T_Last>0)
    
99.     {           
    
100.         Pa_X_Now=P[0];
     
101.         Pa_Y_Now=P[1];
     
102.         T_Now=micros();
     
103.         A_X=A_X+(Pa_Y_Now+Pa_Y_Last)*(T_Now-T_Last)/2000000.0;          //XY轴角度，从陀螺仪积分
     
104.         A_Y=A_Y+(Pa_X_Now-Pa_X_Last)*(T_Now-T_Last)/2000000.0;
     
105.         if(abs(G-1)>0.05)
     
106.         {
     
107.             Rg[0]=sq(sin(A_Y))/sqrt(1+sq((cos(A_X)))*sq(tan(A_Y)));
     
108.             Rg[1]=sq(sin(A_X))/sqrt(1+sq((cos(A_Y)))*sq(tan(A_X)));
     
109.             Rg[2]=sqrt(1-sq(Rg[0])-sq(Rg[1]));
     
110.         }
     
111.         
     
112.         if(abs(G-1)>=0.03)          //校正
     
113.         {
     
114.             for(unsigned char a=0;a<3;a++)               
     
115.             {
     
116.                 A[a]=A[a]/G;
     
117.             }
     
118.             G=sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]);   //此时G化为1
     
119. 
     
120.             for(unsigned char v=0;v<3;v++)               
     
121.             {
     
122.                 Rg[v]=A[v]/G;
     
123.             }
     
124.             A_X=acos(A[0]);
     
125.             A_Y=acos(A[1]);
     
126.         }
     
127.         
     
128.         for(unsigned char b=0;b<3;b++)          //互补滤波
     
129.         {
     
130.             Gr[b]=(A[b]+Rg[b]*W)/(W+1);
     
131.         }
     
132.         G=sqrt(sq(Gr[0])+sq(Gr[1])+sq(Gr[2]));
     
133.         for(unsigned char b=0;b<3;b++)      //再次单位化 重力矢量
     
134.         {
     
135.             Gr[b]=Gr[b]/G;
     
136.         }
     
137.         T_Last=micros();
     
138.         Pa_X_Last=P[0];
     
139.         Pa_Y_Last=P[1];
     
140.     }
     
141. }
     

复制代码

参考的是http://www.geek-workshop.com/forum.php?mod=viewthread&tid=1695
真实效果：
前十来秒还行，时间一长就明显反应滞后。
各位帮忙看看怎么弄。

拾瑞

你是要得到加速度的重力分量?真看不懂,我怎么看出来好象在做"水平校准"........楼下的高手继续吧

ShadowWalker

拾瑞 发表于 2013-8-13 08:32
你是要得到加速度的重力分量?真看不懂,我怎么看出来好象在做"水平校准"........楼下的高手继续吧

嗯，得到重力的分量，就能根据余弦关系得到当前传感器的倾角。

学慧放弃

参考的什么资料？？？？

ShadowWalker

学慧放弃 发表于 2013-8-13 12:07
参考的什么资料？？？？

http://www.geek-workshop.com/for ... thread&tid=1695

拾瑞

ShadowWalker 发表于 2013-8-13 11:24
嗯，得到重力的分量，就能根据余弦关系得到当前传感器的倾角。

我看一下，你的这种算法，是以加速度计为主的！我们通常所用的互补滤波，是以陀螺为主，加速度计辅助！（短时间内角度和角速度我们相信陀螺，加速计出的角度只是用来消除陀螺的积分误差）

ShadowWalker

拾瑞 发表于 2013-8-13 14:01
我看一下，你的这种算法，是以加速度计为主的！我们通常所用的互补滤波，是以陀螺为主，加速度计辅助！（ ...

嗯，你说得对，我也知道111行应该是[pre lang="arduino" line="111"]if(abs(G-1)<=0.03)          //校正[/code]但那样貌似情况更不好了，郁闷
你手里有现成的测试好用的算法吗，发一个我学习学习

学慧放弃

ShadowWalker 发表于 2013-8-13 12:09
http://www.geek-workshop.com/forum.php?mod=viewthread&tid=1695

那个资料我早就打印了一份，还是外国资料牛，国内简直弱爆了