mpu6050加速度角速度融合为四元数计算函数的说明

与arduino mpu6050 四元数相关基本能找到这样的源码。

至于它参数是怎么填，似乎有些迷惑，下面说说我的理解。

ax,ay,az是加速度，

一般的计算是a=acc/AcceRatio;

gx,gy,gz 是角速度，单位是弧度/秒,这个一定要注意。

g=gyro/GyroRatio;//此时单位为度/秒。

g=g/180*3.14;//这样才能带入函数进行计算

在应用时有时候会考虑静态漂移的误差，所以在程序初始一般会取n个角速度gyro求和再做平均，得到平均数 g0。

所以g就是这样计算的

g=(gyro-g0)/GyroRatio;//此时单位为度/秒。

g=g/180*3.14;//这样才能带入函数进行计算

应该注意a的计算和g的计算不能混淆，加速度a不能与平均值做差。

调节的参数有halfT Kp Ki

#define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.2f  // integral gain governs rate of convergence of gyroscope biases

#define  halfT 0.05f                 // half the sample period

halfT 应该根据实际采样花费的时间作调整，是半个周期的时间值

定义一个timer记录时间

在loop可计算每次执行时的时间间隔dt

  dt = (micros() - timer) / 1000000; // Calculate delta time

  timer = micros();

其中取值

  halfT = dt / 2.0;

 void IMUupdate(double gx, double gy, double gz, double ax, double ay, double az)
{
  double norm;
  double vx, vy, vz;
  double ex, ey, ez;

  // normalise the measurements
  norm = sqrt(ax * ax + ay * ay + az * az);
  ax = ax / norm;
  ay = ay / norm;
  az = az / norm;

  // estimated direction of gravity
  vx = 2.0 * (q1 * q3 - q0 * q2);
  vy = 2.0 * (q0 * q1 + q2 * q3);
  vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

  // error is sum of cross product between reference direction of field and direction measured by sensor
  ex = (ay * vz - az * vy);
  ey = (az * vx - ax * vz);
  ez = (ax * vy - ay * vx);

  // integral error scaled integral gain
  exInt = exInt + ex * Ki;
  eyInt = eyInt + ey * Ki;
  ezInt = ezInt + ez * Ki;

  // adjusted gyroscope measurements
  gx = gx + Kp * ex + exInt;
  gy = gy + Kp * ey + eyInt;
  gz = gz + Kp * ez + ezInt;
  
  // 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.当前加速计值转成描述重力的向量，与前一次四元数姿态表达的重力向量做差值运算（向量间的差别，用叉乘表达），计算他们直接的误差。

2.误差做积分（使用ki参数）

3.再把一定比例的当前误差（使用kp参数），和积分后的误差补到陀螺仪的角速度上。

4.通过得到的角速度，积分到四元数中（使用halfT参数），让四元数的姿态更新。

如此循环。

q0~q3为四元数 

四元数转rpy欧拉角公式（Rad=180/3.14转化单位为度）

    Angle_roll=atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+1)*Rad;
    Angle_pitch=asin(-2*q1*q3+2*q0*q2)*Rad;
    Angle_yaw=atan2(2*q1*q2+2*q0*q3,-2*q2*q2-2*q3*q3+1)*Rad;