MPU-6050的姿态识别（只差z轴了）

ggko123

我作为一直在论坛关注着6050的使用问题，几乎刨遍了所有有关的帖子，终于找到了一个合适的算法，就是所谓的一阶互补，
http://www.geek-workshop.com/forum.php?mod=viewthread&tid=1549
根据贴里的程序，我完成了x，y轴的程序，效果不错，能输出比较准确的角度

1. #include "Wire.h"
   
2. #include "i2cdev.h"
   
3. #include "MPU6050.h"
   
4. 
   
5. MPU6050 accelgyro;
   
6. int16_t ax, ay, az;
   
7. int16_t gx, gy, gz;
   
8. 
   
9. #define Gry_offset_X 45//陀螺仪X轴的静态飘移。
   
10. #define Gry_offset_Y 271
    
11. #define Gyr_Gain -0.00763//由陀螺仪X轴读数转换为角速度值，=1/131，向前方向与X轴为反方向，故乘以-1
    
12. 
    
13. #define ACC_Gain 0.000061//由读数转换为加速度值，=1/16384
    
14. #define pi 3.1415926
    
15. #define K_x 0.715//一阶互补滤波权重取值，不知由来，可更改。
    
16. #define k_Y 1.3
    
17. 
    
18. unsigned long preTime = 0;
    
19. float angle_X = 0;
    
20. float angle_Y = 0;
    
21. 
    
22. void setup()
    
23. {
    
24.   Wire.begin();
    
25.   Serial.begin(9600);
    
26.   accelgyro.initialize();//MPU6050初始化。
    
27. }
    
28. 
    
29. void loop()
    
30. {
    
31.   
    
32.   unsigned long now = millis();//当前时间(ms)
    
33.   float dt = (now - preTime) / 500.0;//微分时间(s)
    
34.   preTime = now;//上一次采样时间(ms)
    
35. 
    
36.   
    
37.   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
    
38.   
    
39.   float Y_Accelerometer = ay * ACC_Gain;//转换为向前的加速度(g)，为负值
    
40.   float Z_Accelerometer = az * ACC_Gain;//转换为向下的加速度(g)
    
41.   float X_Accelerometer = ax * ACC_Gain;
    
42.   float angleA_X= atan(Y_Accelerometer/ Z_Accelerometer)* (-180)/ pi;//加速度仪，反正切获得弧度值，乘以180度/pi
    
43.                                                                    //获得角度值，乘以-1得正
    
44.   float angleA_Y= atan(X_Accelerometer/Z_Accelerometer)*(-180)/pi;
    
45.                                                                   
    
46.   float gx_revised = gx + Gry_offset_X;//陀螺仪x轴静态时修正后的角速度读数，向前为负值
    
47.   float gy_revised = gy + Gry_offset_Y;
    
48.   float omega_X= Gyr_Gain* gx_revised;//陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
    
49.   float omega_Y= Gyr_Gain* gy_revised;
    
50.   float angle_dt_X = omega_X * dt;//角度的单次积分值
    
51.   float angle_dt_Y = omega_Y * dt;
    
52. 
    
53.   angle_X+= (Gyr_Gain * (gx + Gry_offset_X)) * dt;
    
54.   angle_Y+= (Gyr_Gain * (gy + Gry_offset_Y)) * dt;
    
55. 
    
56. 
    
57.   float A_X= K_x/ (K_x+ dt);
    
58.   //陀螺仪的权值
    
59.   float A_Y= k_Y/(k_Y+dt);
    
60.   angle_X= A_X* (angle_X+ omega_X* dt)+ (1-A_X)* angleA_X;//一阶互补滤波后的输出角度(o)
    
61.   angle_Y= A_Y*(angle_Y+omega_Y*dt)+(1-A_Y)*angleA_Y;
    
62.   delay(10);
    
63.   
    
64.     Serial.println(angle_X);
    
65.     Serila.println(angle_Y);
    
66.     }
    

复制代码

但是到了z轴就卡住了，用同样的算法，z轴只能输出角度变化，不能输出准确的角度。

1. #include "Wire.h"
   
2. #include "i2cdev.h"
   
3. #include "MPU6050.h"
   
4. 
   
5. MPU6050 accelgyro;
   
6. int16_t ax, ay, az;
   
7. int16_t gx, gy, gz;
   
8. 
   
9. #define Gry_offset 38//z轴偏移量
   
10. #define Gyr_Gain -0.00763
    
11. 
    
12. #define ACC_Gain 0.000061
    
13. #define pi 3.1415926
    
14. #define K 1.75//这个比例常数可以改的
    
15. 
    
16. unsigned long preTime = 0;
    
17. float angleG = 0;
    
18. 
    
19. void setup()
    
20. {
    
21.   Wire.begin();
    
22.   Serial.begin(9600);
    
23.   accelgyro.initialize();
    
24. 
    
25. 
    
26. void loop()
    
27. {
    
28.   
    
29.   unsigned long now = millis();
    
30.   float dt = (now - preTime) / 400.0;
    
31.   preTime = now;
    
32. 
    
33.   
    
34.   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
35.   
    
36.   float Y_Accelerometer = ay * ACC_Gain;
    
37.   float X_Accelerometer = ax * ACC_Gain;
    
38.   float angleA= atan(Y_Accelerometer/ X_Accelerometer)* (-180)/ pi;
    
39.                                                                   
    
40.                                                                   
    
41.   float gz_revised = gz + Gry_offset;
    
42.   float omega= Gyr_Gain* gz_revised;
    
43.   float angle_dt = omega * dt;/
    
44. 
    
45.   
    
46.   angleG+= (Gyr_Gain * (gz + Gry_offset)) * dt;
    
47. 
    
48. 
    
49.   float A= K/ (K+ dt);
    
50.   angleG= A* (angleG+ omega* dt)+ (1-A)* angleA ;
    
51.   
    
52.   delay(10);
    
53. 
    
54.     Serial.println(angleG);
    
55.     }
    

复制代码

如图，有四次波动，代表四次旋转，波形上升是顺时针转动，下降是逆时针转动，但都会回到零点附近，不能停在具体偏转角度上。是算法问题，还是6050的设计问题？

我觉得离成功不远了，只有读出z轴角度，就能编写一个processing上的姿态识别程序,到时一定会写一篇详细过程，希望诸位大牛帮忙!

三水

弱弱的说。。我的z轴是配合地磁传感器换算的。。⊙﹏⊙

cai_mouse

Z轴陀螺数据是没有办法使用z轴的加速度进行修正的

ggko123

能解释一下吗？如果我只用这个做四轴够用吗？

cai_mouse

Z轴陀螺数据只能用罗盘数据进行修正，你做四轴的，只对z轴的数据进行PID修正控制就行了

ggko123

原来是这样啊，谢谢了，我之前还以为一个6050就能测出各轴的旋转量

at97

你是做两轮小车的吗？我想问问，你怎么控制电机的。能说说吗？

ggko123

at97 发表于 2012-10-2 15:35
你是做两轮小车的吗？我想问问，你怎么控制电机的。能说说吗？

还没打算做小车，以后可能会尝试，目前都在做姿态识别程序，你可以看上面那个地址，程序很不错的

smallsmallxx

好程序留个名

smallsmallxx

还有问问楼主那个示波程序是怎么弄的，我是超级新手，请教一下

lph1987

为什么你的微分时间一个除以500  一个除以400呢  不是都应该是除以1000吗？？

心之永恒

能否把Z轴的代码也贴出来学习一下呢？

颠覆理论

楼主问一下，我的MPU6050陀螺仪静态值为零，不知道你的为什么有静态值，还是我的有问题

颠覆理论

C:\Users\Administrator\Desktop\杂项

1.          X_Acc = A_X * ACC_Gain;
   
2.      Y_Acc = A_Y * ACC_Gain;//转换为向前的加速度(g)，为负值
   
3.      Z_Acc = A_Z * ACC_Gain;//转换为向下的加速度(g)
   
4.          X_Gyro=G_X*Gyr_Gain;
   
5.          Y_Gyro=G_Y*Gyr_Gain;
   
6.          Z_Gyro=G_Z*Gyr_Gain;
   
7.         angleA_X=atan2(X_Acc,Z_Acc)*(-180)/pi;
   
8. //        angleA_Y=atan2(X_Acc,Z_Acc)*(-180)/pi;
   
9. 
   
10.         Kalman_Filter(angleA_X,Y_Gyro);

复制代码

滤波出来如下

颠覆理论

卡尔曼滤波之后响应比较慢，，对你的代码里与哦几个不太明白，那个 ACC_Gain和Gyr_Gain，是怎么得来的，是什么意思