这些小活动你都参加了吗?快来围观一下吧!>>
电子产品世界 » 论坛首页 » DIY与开源设计 » 电子DIY » 关于MPU6050得出加速度角速度后输出四元数的值

共3条 1/1 1 跳转至

关于MPU6050得出加速度角速度后输出四元数的值

菜鸟
2016-01-13 19:08:56     打赏

用的是如下算法:


float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
  float norm;
//  float hx, hy, hz, bx, bz;
  float vx, vy, vz;// wx, wy, wz;
  float ex, ey, ez;


  // ÏÈ°ÑÕâЩÓõõ½µÄÖµËãºÃ
  float q0q0 = q0*q0;
  float q0q1 = q0*q1;
  float q0q2 = q0*q2;
  float q0q3 = q0*q3;
  float q1q1 = q1*q1;
  float q1q2 = q1*q2;
  float q1q3 = q1*q3;
  float q2q2 = q2*q2;
  float q2q3 = q2*q3;
  float q3q3 = q3*q3;
 
 if(ax*ay*az==0)
   return;
  
  norm = sqrt(ax*ax + ay*ay + az*az);       //accÊý¾Ý¹éÒ»»¯
  ax = ax /norm;
  ay = ay / norm;
  az = az / norm;


  // estimated direction of gravity and flux (v and w)              ¹À¼ÆÖØÁ¦·½ÏòºÍÁ÷Á¿/±äǨ
  vx = 2*(q1q3 - q0q2);            //ËÄÔªËØÖÐxyzµÄ±íʾ
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3 ;


  // error is sum of cross product between reference direction of fields and direction measured by sensors
  ex = (ay*vz - az*vy) ;                                 //ÏòÁ¿Íâ»ýÔÚÏà¼õµÃµ½²î·Ö¾ÍÊÇÎó²î
  ey = (az*vx - ax*vz) ;
  ez = (ax*vy - ay*vx) ;


  exInt = exInt + ex * Ki;          //¶ÔÎó²î½øÐлý·Ö
  eyInt = eyInt + ey * Ki;
  ezInt = ezInt + ez * Ki;


  // adjusted gyroscope measurements
  gx = gx + Kp*ex + exInt;               //½«Îó²îPIºó²¹³¥µ½ÍÓÂÝÒÇ£¬¼´²¹³¥ÁãµãƯÒÆ
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt;              //ÕâÀïµÄgzÓÉÓÚûÓй۲âÕß½øÐнÃÕý»á²úÉúƯÒÆ£¬±íÏÖ³öÀ´µÄ¾ÍÊÇ»ý·Ö×ÔÔö»ò×Ô¼õ


  // integrate quaternion rate and normalise         //ËÄÔªËصÄ΢·Ö·½³Ì
  q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;


  // normalise quaternion
  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;


 
}

1.当设定Kp 1.5f,Ki 0.01f,halfT 0.001f 的时候;

输出的四元数会在一段范围内波动,在范围之间由慢到快,再到慢地跳动;

2.当设定Kp 50f 时;

输出的四元数会比较稳定,看上去正常,把MPU6050转动的时候,他需要一个时间去变化到某个值。

我想请教各路大神,Kp Ki的意义是啥呢?还有halfT的值也不知道怎么去确定的,因为我也不知道姿态解算速度是如何获得的?最后就是,Kp的值设定这么大,肯定有问题,那为啥小值得时候不能良好的输出呢?求解答啊!

这是源码,求解答!

5.1.rar




助工
2016-01-14 10:09:52     打赏
2楼
表示看不懂呀呀呀

菜鸟
2016-06-24 11:52:28     打赏
3楼
感謝分享!感謝分享!

共3条 1/1 1 跳转至

回复

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