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

a372775236

用的是如下算法：

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;   
float exInt = 0, eyInt = 0, ezInt = 0;
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
  float norm;
  float vx, vy, vz;
  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);  
  ax = ax /norm;
  ay = ay / norm;
  az = az / norm;

vx = 2*(q1q3 - q0q2);   
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;

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;

  gx = gx + Kp*ex + exInt;
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt;

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;

  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.004f 的时候；
输出的四元数会在一段范围内波动，在范围之间由慢到快，再到慢地跳动；
2.当设定Kp 50f 时；
输出的四元数会比较稳定，看上去正常，把MPU6050转动的时候，他需要一个时间去变化到某个值。
我想请教各路大神，Kp Ki的意义是啥呢？Kp的值设定这么大，肯定有问题，那为啥小值得时候不能良好的输出呢？求解答啊！
5.2.rar (57.6 KB, 下载次数: 737)

2016-1-18 14:19 上传

点击文件名下载附件
源码

xkwy

PID算法？

小李飞刀

楼主大人 ，能解释下算法么？

小李飞刀

你的“MPU6050转动的时候，他需要一个时间去变化到某个值”可能是你的程序在单片机上跑不起来，你用的是什么板子

正点原子

帮顶

fengshu426

顶一个

fanleung

楼主问题解决了吗

wangda123yuan

原函数代码，不知道对你有没有帮助。

周亚龙

我前段时间用的也是这个算法，滞后太严重。各种找资料，搞了一个星期没弄好，我现在用DMP的了