IMU数据融合之Mahony算法应用
关键词：Mahony，IMU，九轴，数据融合，滤波
经过前面几篇博客的努力，目前我们已经能够使用上位机获取并显示MPU9250的实时测量值，基于此本篇博客将介绍本着实用的原则介绍Mahony算法，对IMU的测量数据进行融合以减小噪声获得姿态信息。
Mahony算法源码
#define sampleFreq 512.0f // sample frequency in Hz
#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef (2.0f * 0.0f) // 2 * integral gain
//---------------------------------------------------------------------------------------------------
// Variable definitions
volatile float twoKp = twoKpDef; // 2 * proportional gain (Kp)
volatile float twoKi = twoKiDef; // 2 * integral gain (Ki)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
//---------------------------------------------------------------------------------------------------
// Function declarations
float invSqrt(float x);
void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q1 * q3 - q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 - 0.5f + q3 * q3;
// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
Mahony算法使用注意点
加速度计、陀螺仪输出符号。MPU9250加速度计和陀螺仪各轴的正方向如图1所示。在测量的过程中，我们发现MPU9250加速度计三轴的输出值与图1所示方向相反，陀螺仪三轴输出值与图1所示方向一致。因此，在使用之前我们需要更改加速度输出数据的符号。
加速度计与陀螺仪的输出单位。加速度计输出单位为g，陀螺仪的输出单位为deg/s，在Mahony算法中首先对加速度值进行了归一化，故加速度计的输出单位不会对算法本身产生影响。而Mahony算法中，关于角度使用的是弧度制，我们在使用陀螺仪的输出数据之前，需要进行转换。
Kp和Ki参数调节。Kp与Ki参数的调节应遵循PID参数整定原则，这里不再详述。建议先将Ki参数设置为零，调节Kp参数至略微过调，然后再调节Ki参数。
图1 加速度计与陀螺仪各轴正方向示意图
Mahony算法粗略分析
刚体旋转动力学方程
其中
1.测量加速度a和角速度d
2.归一化加速度a
3.获取预测重力矢量
4.计算误差向量e
5.计算I增量
6.计算K增量并加上I增量
7. 融合计算结果
8.归一化四元数q
9.重复步骤一
至此，在【001】IMU相关嵌入式开发与应用项目简介 中提及的第一个迭代完成，在接下来的第二个迭代中，将对技术细节进行深入的分析与完善。
联系作者
