OpenVINOTM,给你看得见的未来!>>
电子产品世界 » 论坛首页 » DIY与开源设计 » 电子DIY » 四轴飞行器&角度融合波形(图片+视频)

共31条 1/4 1 2 3 4 跳转至

四轴飞行器&角度融合波形(图片+视频)

菜鸟
2014-11-24 22:33:02    评分
  首先,先贴下我使用的两个姿态解算程序:
AHRSupdate四元数姿态解算:
  1. // Definitions

  2. #define Kp 6.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
  3. #define Ki 0.03f                // integral gain governs rate of convergence of gyroscope biases
  4. #define halfT 0.0014                // half the sample period

  5. //---------------------------------------------------------------------------------------------------
  6. // Variable definitions

  7. float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
  8. float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error

  9. //====================================================================================================
  10. // Function
  11. //====================================================================================================

  12. void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
  13.         float norm;
  14.         float hx, hy, hz, bx, bz;
  15.         float vx, vy, vz, wx, wy, wz;
  16.         float ex, ey, ez;

  17.         // auxiliary variables to reduce number of repeated operations
  18.         float q0q0 = q0*q0;
  19.         float q0q1 = q0*q1;
  20.         float q0q2 = q0*q2;
  21.         float q0q3 = q0*q3;
  22.         float q1q1 = q1*q1;
  23.         float q1q2 = q1*q2;
  24.         float q1q3 = q1*q3;
  25.         float q2q2 = q2*q2;   
  26.         float q2q3 = q2*q3;
  27.         float q3q3 = q3*q3;         
  28.         
  29.         // normalise the measurements
  30.         norm = sqrt(ax*ax + ay*ay + az*az);      
  31.         ax = ax/ norm;
  32.         ay = ay/ norm;
  33.         az = az/ norm;
  34.         norm = sqrt(mx*mx + my*my + mz*mz);         
  35.         mx = mx/ norm;
  36.         my = my/ norm;
  37.         mz = mz/ norm;         
  38.         
  39.         // compute reference direction of flux
  40.         hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
  41.         hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
  42.         hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
  43.         bx = sqrt((hx*hx) + (hy*hy));
  44.         bz = hz;        
  45.         
  46.         // estimated direction of gravity and flux (v and w)
  47.         vx = 2*(q1q3 - q0q2);
  48.         vy = 2*(q0q1 + q2q3);
  49.         vz = q0q0 - q1q1 - q2q2 + q3q3;
  50.         wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
  51.         wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
  52.         wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
  53.         
  54.         // error is sum of cross product between reference direction of fields and direction measured by sensors
  55.         ex = (ay*vz - az*vy) + (my*wz - mz*wy);
  56.         ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
  57.         ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
  58.         
  59.         // integral error scaled integral gain
  60.         exInt = exInt + ex*Ki;
  61.         eyInt = eyInt + ey*Ki;
  62.         ezInt = ezInt + ez*Ki;
  63.         
  64.         // adjusted gyroscope measurements
  65.         gx = gx + Kp*ex + exInt;
  66.         gy = gy + Kp*ey + eyInt;
  67.         gz = gz + Kp*ez + ezInt;
  68.         
  69.         // integrate quaternion rate and normalise
  70.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  71.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  72.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  73.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
  74.         
  75.         // normalise quaternion
  76.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  77.         q0 = q0 / norm;
  78.         q1 = q1 / norm;
  79.         q2 = q2 / norm;
  80.         q3 = q3 / norm;
  81. }
复制代码 卡尔曼滤波:
  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. float P[2][2] = {            { 1, 0 },
  10.                              { 0, 1 }
  11.                  };                  //alpha_k
  12.    
  13. float Pdot[4] ={0,0,0,0};        //Pdot = (u_k-bias)
  14. const char C_0 = 1;       //H = [1 0]矩阵,用于提取
  15. float   q_bias ,           //陀螺仪常值偏差bias
  16.         angle_err,        //观测值减去估计值
  17.         PCt_0, PCt_1,     //计算K的中间步骤,分子 PCT = PH_T
  18.         E,                //计算K的中间步骤,分母 E =(HPH_T + R)
  19.         K_0, K_1,         //K为卡尔曼增益,K = P(K),矩阵
  20.         t_0, t_1;         //更新alpha的中间步骤  




  21. //============================================================================
  22. //函数名称:Kalman_Filter
  23. //功能概要:卡尔曼滤波

  24. void Kalman_Filter(float angle_m,float gyro_m)          //gyro_m:gyro_measure
  25. {

  26.   


  27. //        卡尔曼滤波开始
  28.    
  29.     angle+=(gyro_m-q_bias) * dt;//先验估计


  30.     Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
  31.     Pdot[1]=- P[1][1];
  32.     Pdot[2]=- P[1][1];
  33.     Pdot[3]=Q_gyro;                      //Pdot = (u_k-bias)
  34.    
  35.     P[0][0] += Pdot[0] * dt;        // Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
  36.     P[0][1] += Pdot[1] * dt;
  37.     P[1][0] += Pdot[2] * dt;
  38.     P[1][1] += Pdot[3] * dt;        //alpha_k = alpha_k-1 + [(u_k-bias)*dt]
  39.    
  40.    
  41.     angle_err = angle_m - angle;    //zk-先验估计 观测值减去估计值
  42.    
  43.    
  44.     PCt_0 = C_0 * P[0][0];
  45.     PCt_1 = C_0 * P[1][0];          //计算K的中间步骤,分子 PCT = PH^T
  46.    
  47.     E = R_angle + C_0 * PCt_0;      //计算K的中间步骤,分母 E =(HPH^T + R)
  48.    
  49.     K_0 = PCt_0 / E;        //K =   PH^T / (HPH^T + R)
  50.     K_1 = PCt_1 / E;
  51.    
  52.     t_0 = PCt_0;            //更新alpha的中间步骤
  53.     t_1 = C_0 * P[0][1];

  54.     P[0][0] -= K_0 * t_0;   //后验估计误差协方差
  55.     P[0][1] -= K_0 * t_1;
  56.     P[1][0] -= K_1 * t_0;   //alpha_k+1 = alpha_k - KH*alpha_k
  57.     P[1][1] -= K_1 * t_1;
  58.    
  59.    
  60.     angle+= K_0 * angle_err;//后验估计   更新估计值作为最终结果
  61.     q_bias+= K_1 * angle_err;//后验估计   更新bias的值
  62.     angle_dot=gyro_m-q_bias;//输出值(后验估计)的微分 = 角速度
  63.    

  64. //                 卡尔曼滤波结束



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


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

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

下面是调试视频:

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

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

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

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




关键词: 四轴     角度     波形     四元数     卡尔曼滤波    

菜鸟
2014-11-24 22:42:01    评分
2楼

这是我的灰机


菜鸟
2014-11-24 22:43:47    评分
3楼
有部分BUG的主控板

院士
2014-11-25 08:58:48    评分
4楼
真棒~~~

工程师
2014-11-25 09:43:26    评分
5楼
荡秋千,首先姿态解算要正确,这是前提,这个过不了,控制也会好不了……,主要原因为PID没有调好。

菜鸟
2014-11-25 11:19:27    评分
6楼
Pid还是可以的,我使用dmp融合的角度去控制飞行还是很稳定的,融合出来的波形感觉也是正确的。觉得是姿态解算的动态特性不好。你能否从波形看出什么么?

高工
2014-11-26 00:20:20    评分
7楼

估计滤波没做好吧?


菜鸟
2014-11-26 12:33:20    评分
8楼
怎么去判断滤波的正误呢?我只是通过快速摆动重复操作,看波形是否为矩形波而没有过冲现象即可。

高工
2014-11-27 00:42:34    评分
9楼
你对加速度做滤波了吗?最简单的做一下滑动均值滤波,效果可能会好点

菜鸟
2014-11-27 12:03:13    评分
10楼
嗯嗯,有的,我也开了个buffer,进行最近二十次数据进行平均处理,每两毫秒就进行采样,并解算,控制。

共31条 1/4 1 2 3 4 跳转至

回复

匿名不能发帖!请先 [ 登陆 注册 ]