EEPW论坛

  首先，先贴下我使用的两个姿态解算程序：
AHRSupdate四元数姿态解算：

1. // Definitions
   
2. 
   
3. #define Kp 6.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
   
4. #define Ki 0.03f                // integral gain governs rate of convergence of gyroscope biases
   
5. #define halfT 0.0014                // half the sample period
   
6. 
   
7. //---------------------------------------------------------------------------------------------------
   
8. // Variable definitions
   
9. 
   
10. float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
    
11. float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error
    
12. 
    
13. //====================================================================================================
    
14. // Function
    
15. //====================================================================================================
    
16. 
    
17. void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
    
18.         float norm;
    
19.         float hx, hy, hz, bx, bz;
    
20.         float vx, vy, vz, wx, wy, wz;
    
21.         float ex, ey, ez;
    
22. 
    
23.         // auxiliary variables to reduce number of repeated operations
    
24.         float q0q0 = q0*q0;
    
25.         float q0q1 = q0*q1;
    
26.         float q0q2 = q0*q2;
    
27.         float q0q3 = q0*q3;
    
28.         float q1q1 = q1*q1;
    
29.         float q1q2 = q1*q2;
    
30.         float q1q3 = q1*q3;
    
31.         float q2q2 = q2*q2;   
    
32.         float q2q3 = q2*q3;
    
33.         float q3q3 = q3*q3;         
    
34.         
    
35.         // normalise the measurements
    
36.         norm = sqrt(ax*ax + ay*ay + az*az);      
    
37.         ax = ax/ norm;
    
38.         ay = ay/ norm;
    
39.         az = az/ norm;
    
40.         norm = sqrt(mx*mx + my*my + mz*mz);         
    
41.         mx = mx/ norm;
    
42.         my = my/ norm;
    
43.         mz = mz/ norm;         
    
44.         
    
45.         // compute reference direction of flux
    
46.         hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
    
47.         hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
    
48.         hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
    
49.         bx = sqrt((hx*hx) + (hy*hy));
    
50.         bz = hz;        
    
51.         
    
52.         // estimated direction of gravity and flux (v and w)
    
53.         vx = 2*(q1q3 - q0q2);
    
54.         vy = 2*(q0q1 + q2q3);
    
55.         vz = q0q0 - q1q1 - q2q2 + q3q3;
    
56.         wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
    
57.         wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
    
58.         wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
    
59.         
    
60.         // error is sum of cross product between reference direction of fields and direction measured by sensors
    
61.         ex = (ay*vz - az*vy) + (my*wz - mz*wy);
    
62.         ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
    
63.         ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
    
64.         
    
65.         // integral error scaled integral gain
    
66.         exInt = exInt + ex*Ki;
    
67.         eyInt = eyInt + ey*Ki;
    
68.         ezInt = ezInt + ez*Ki;
    
69.         
    
70.         // adjusted gyroscope measurements
    
71.         gx = gx + Kp*ex + exInt;
    
72.         gy = gy + Kp*ey + eyInt;
    
73.         gz = gz + Kp*ez + ezInt;
    
74.         
    
75.         // integrate quaternion rate and normalise
    
76.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    
77.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    
78.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    
79.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
    
80.         
    
81.         // normalise quaternion
    
82.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    
83.         q0 = q0 / norm;
    
84.         q1 = q1 / norm;
    
85.         q2 = q2 / norm;
    
86.         q3 = q3 / norm;
    
87. }

复制代码 卡尔曼滤波：

1. //============================================================================
   
2. //                 卡尔曼滤波参数
   
3. float Q_angle=0.001,
   
4.       Q_gyro=0.005,
   
5.       R_angle=0.5,
   
6.       dt=0.0055,//注意：dt的取值为kalman滤波器采样时间;
   
7.       angle,
   
8.       angle_dot;   
   
9. 
   
10. float P[2][2] = {            { 1, 0 },
    
11.                              { 0, 1 }
    
12.                  };                  //alpha_k
    
13.    
    
14. float Pdot[4] ={0,0,0,0};        //Pdot = (u_k-bias)
    
15. const char C_0 = 1;       //H = [1 0]矩阵，用于提取
    
16. float   q_bias ,           //陀螺仪常值偏差bias
    
17.         angle_err,        //观测值减去估计值
    
18.         PCt_0, PCt_1,     //计算K的中间步骤，分子 PCT = PH_T
    
19.         E,                //计算K的中间步骤，分母 E =(HPH_T + R)
    
20.         K_0, K_1,         //K为卡尔曼增益,K = P(K),矩阵
    
21.         t_0, t_1;         //更新alpha的中间步骤  
    
22. 
    
23. 
    
24. 
    
25. 
    
26. //============================================================================
    
27. //函数名称：Kalman_Filter
    
28. //功能概要：卡尔曼滤波
    
29. 
    
30. void Kalman_Filter(float angle_m,float gyro_m)          //gyro_m:gyro_measure
    
31. {
    
32. 
    
33.   
    
34. 
    
35. 
    
36. //        卡尔曼滤波开始
    
37.    
    
38.     angle+=(gyro_m-q_bias) * dt;//先验估计
    
39. 
    
40. 
    
41.     Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
    
42.     Pdot[1]=- P[1][1];
    
43.     Pdot[2]=- P[1][1];
    
44.     Pdot[3]=Q_gyro;                      //Pdot = (u_k-bias)
    
45.    
    
46.     P[0][0] += Pdot[0] * dt;        // Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
    
47.     P[0][1] += Pdot[1] * dt;
    
48.     P[1][0] += Pdot[2] * dt;
    
49.     P[1][1] += Pdot[3] * dt;        //alpha_k = alpha_k-1 + [(u_k-bias)*dt]
    
50.    
    
51.    
    
52.     angle_err = angle_m - angle;    //zk-先验估计 观测值减去估计值
    
53.    
    
54.    
    
55.     PCt_0 = C_0 * P[0][0];
    
56.     PCt_1 = C_0 * P[1][0];          //计算K的中间步骤，分子 PCT = PH^T
    
57.    
    
58.     E = R_angle + C_0 * PCt_0;      //计算K的中间步骤，分母 E =(HPH^T + R)
    
59.    
    
60.     K_0 = PCt_0 / E;        //K =   PH^T / (HPH^T + R)
    
61.     K_1 = PCt_1 / E;
    
62.    
    
63.     t_0 = PCt_0;            //更新alpha的中间步骤
    
64.     t_1 = C_0 * P[0][1];
    
65. 
    
66.     P[0][0] -= K_0 * t_0;   //后验估计误差协方差
    
67.     P[0][1] -= K_0 * t_1;
    
68.     P[1][0] -= K_1 * t_0;   //alpha_k+1 = alpha_k - KH*alpha_k
    
69.     P[1][1] -= K_1 * t_1;
    
70.    
    
71.    
    
72.     angle+= K_0 * angle_err;//后验估计   更新估计值作为最终结果
    
73.     q_bias+= K_1 * angle_err;//后验估计   更新bias的值
    
74.     angle_dot=gyro_m-q_bias;//输出值（后验估计）的微分 = 角速度
    
75.    
    
76. 
    
77. //                 卡尔曼滤波结束
    
78. 
    
79. 
    
80. 
    
81. }

复制代码 如上， 我使用了以上的两个函数分别进行实验进行姿态解算，卡尔曼滤波成功使用，已经在去年的飞思卡尔华南赛区直立摄像头组跑完全程，直立上是妥妥的。由于快毕业了，比赛结束后没有休息，继续做下了四轴，两个月后也成功飞行~
  可是飞行的时候发现遥控起来特别吃力，很容易在空中荡秋千，超难维持稳定，一开始以为是PID调试问题，后面发现不是。这几天使用了MP6050的 DMP功能，昨晚再次移植成功，四轴非常稳定飞行，达到了几乎快悬停的效果，尽管在狭窄的走廊也能操控。这时候我意识到了问题所在，是角度融合有问题！！
  以下是我用以上四元数的函数解算出来的角度波形



如上波形，第一图左边的波形相对还是比较完美的，原地直立是没有问题的。但是四轴是在空中飞行的，只要运动，就很容 易叠加上运动的加速度，这时候加速度计和陀螺仪融合出来的角度就出错了。看第一图的右边，这是我在角度维持不变，进行平移加速突然停止的动作而呈现的波 形，很明显看到运动的加速度成分的存在。
  我在做实验的时候虽然手拿会有波动，但不至于产生二三十度的波动，这能很明显的发现是由加速度产生的。那么问题就来了？！！！
  同样的姿态解算，这个四元数和卡尔曼已经是网上被移植很多的程序，不过大家的四轴好像都飞行得很好，难道就没有出现遥控难度难的荡秋千现象么？

  不知道在这论坛里是否有人尝试去解决这个问题或者发现这个问题的呢？你们是如何去解决的？

下面是调试视频：

http://v.youku.com/v_show/id_XODM0MjAxNTE2.html

在DMP飞行的情况下，我把手机绑上去了。。。。

没想到遥控异常，方向不听使唤，然后就从三楼追下去了，手机屏幕碎了，金属支架也折断了，电路板也莫名冒烟了。。。。

补充上坠机的视频http://v.youku.com/v_show/id_XODM2MTY4NzA4.htmlhttp://