【007】IMU数据融合之Mahony算法应用

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相关嵌入式开发与应用项目简介 中提及的第一个迭代完成，在接下来的第二个迭代中，将对技术细节进行深入的分析与完善。

联系作者