MPU6050四元数解算求助

knightsky

我最近在学习MPU6050姿态解算，原始数据通过四元数解算后的数据一直在正常值上下震荡，成正弦波动。我对程序中的Kp，Ki值进行调整，不管怎么调，一直是震荡的。请教大家，这是什么原因造成的？郁闷的很。

1. 
   
2. #include "Wire.h"
   
3. #include "I2Cdev.h"
   
4. #include "MPU6050.h"
   
5. 
   
6. MPU6050 accelgyro;
   
7. 
   
8. int16_t ax, ay, az;
   
9. int16_t gx, gy, gz;
   
10. float accx,accy,accz;
    
11. float gyrox,gyroy,gyroz;
    
12. float axo=0,ayo=0,azo=0;  //加速度计偏移量
    
13. float gxo=0,gyo=0,gzo=0;  //陀螺仪偏移量
    
14. float pi=3.1415926;
    
15. float AcceRatio=16384.0;
    
16. float GyroRatio=131.0;
    
17. float Rad=180.0/pi;
    
18. 
    
19. //
    
20. //
    
21. #define Kp 0.025f//10.0f
    
22. #define Ki 0.0f//0.008f
    
23. #define halfT 0.001f
    
24. 
    
25. float q0=1,q1=0,q2=0,q3=0;
    
26. float exInt=0,eyInt=0,ezInt=0;
    
27. 
    
28. float Angle_roll,Angle_pitch,Angle_yaw;
    
29. 
    
30. void AngleUpdate(float ax,float ay,float az,float gx,float gy,float gz)
    
31. {
    
32.     float norm;
    
33.     float vx,vy,vz;
    
34.     float ex,ey,ez;
    
35.    
    
36.     if (ax*ay*az==0)
    
37.         return;
    
38.    
    
39.     norm=sqrt(ax*ax+ay*ay+az*az);
    
40.     ax=ax/norm;
    
41.     ay=ay/norm;
    
42.     az=az/norm;
    
43.    
    
44.     vx=2*(q1*q3-q0*q2);
    
45.     vy=2*(q0*q1-q3*q2);
    
46.     vz=q0*q0-q1*q1-q2*q2+q3*q3;
    
47.    
    
48.     ex=ay*vz-az*vy;
    
49.     ey=az*vx-ax*vz;
    
50.     ez=ax*vy-ay*vx;
    
51.    
    
52.     exInt=exInt+ex*Ki;
    
53.     eyInt=eyInt+ey*Ki;
    
54.     ezInt=ezInt+ez*Ki;
    
55.    
    
56.     gx=gx+Kp*ex+exInt;
    
57.     gy=gy+Kp*ey+eyInt;
    
58.     gz=gz+Kp*ez+ezInt;
    
59.    
    
60.     q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT;
    
61.     q1=q1+(q0*gx+q2*gz-q3*gy)*halfT;
    
62.     q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;
    
63.     q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;
    
64.    
    
65.     norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
    
66.     q0=q0/norm;
    
67.     q1=q1/norm;
    
68.     q2=q2/norm;
    
69.     q3=q3/norm;
    
70.    
    
71.     Angle_roll=atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+1)*Rad;
    
72.     Angle_pitch=asin(-2*q1*q3+2*q0*q2)*Rad;
    
73.     Angle_yaw=atan2(2*q1*q2+2*q0*q3,-2*q2*q2-2*q3*q3+1)*Rad;
    
74. }
    
75. 
    
76. void setup()
    
77. {
    
78.     Wire.begin();
    
79.     Serial.begin(9600);
    
80.    
    
81.     accelgyro.initialize();
    
82.    
    
83.     unsigned short times=400;
    
84.     for(int i=0;i<times;i++)
    
85.     {
    
86.         accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
    
87.         axo += ax; ayo += ay; azo += az;      //采样和
    
88.         gxo += gx; gyo += gy; gzo += gz;
    
89.     }
    
90.     axo /= times; ayo /= times; azo /= times; //计算加速度计偏移
    
91.     gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移
    
92.    
    
93.    Serial.println("Angle_roll   ,   Angle_pitch   ,   Angle_yaw");
    
94.    Serial.print(axo);Serial.print("  ,   ");Serial.print(gxo);
    
95. }
    
96. 
    
97. void loop()
    
98. {
    
99.     accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
    
100.     accx=(ax-axo)/AcceRatio;
     
101.     accy=(ay-ayo)/AcceRatio;
     
102.     accz=(az-azo)/AcceRatio;
     
103.     gyrox=(ax-gxo)/GyroRatio;
     
104.     gyroy=(ay-gyo)/GyroRatio;
     
105.     gyroz=(az-gzo)/GyroRatio;
     
106.     //Serial.print(accx);Serial.print(",");Serial.print(accy);Serial.print(",");Serial.println(accz);
     
107.     AngleUpdate(accx,accy,accz,gyrox,gyroy,gyroz);
     
108.     //Kalman_Filter(Angle_roll,gyrox);
     
109.     Serial.print(Angle_roll);Serial.print(",");Serial.print(Angle_pitch);Serial.print(",");Serial.println(Angle_yaw);
     
110.     delay(100);
     
111. }

复制代码

迷你强

数学算法这块一直啃不下来。帮顶

knightsky

一起学习，等高手帮我指导一下来自: Android客户端

五哥U五哥

LZ解决了么，第44行那里我看了网上好多都是+，LZ那里用的是减号，我改了也没用。。。波动很有规律， 就是不能正常工作。

詹波波

请问楼主问题解决了么

詹波波

楼主　我知道你的程序哪里错了　那里应该是ＧＸ－ＧＸ偏移量

knightsky

詹波波 发表于 2015-11-13 13:56
楼主　我知道你的程序哪里错了　那里应该是ＧＸ－ＧＸ偏移量

竟然没发现。。。谢谢你，你调出来了吗，我放下好久，今天才又拿起来玩玩，参数不好调