第三讲 AHRS姿态解算

 AHRS是自动航向基准系统(Automatic Heading Reference System)的简称。目前，使用四元数来进行AHRS姿态解算的算法被广泛采用于四轴飞行器上。该算法源自英国Bristol大学的Ph.D Sebastian Madgwick，他在2009年开发并发布了该算法。下面我们来对该算法的代码进行详细分析。

我们首先来看IMU部分。IMU是惯性测量装置(Inertial Measurement Unit)的简称，通常包含陀螺仪和加速度计。陀螺仪测量的是角速度，即物体转动的速度，把速度和时间相乘，即可以得到某一时间段内物体转过的角度。加速度计测量的是物体的加速度，我们知道，重力加速度是一个物体受重力作用的情况下所具有的加速度。当物体处于静止状态时，加速度计测量出来的值就等于重力加速度1g, 约等于9.8米每平方秒。重力加速度g的方向总是竖直向下的，通过获得重力加速度在其X轴，Y轴上的分量，我们可以计算出物体相对于水平面的倾斜角度。典型的IMU惯性测量芯片为MPU6050，它被广泛采用在四轴飞行器上。

对于陀螺仪的角速度测量，大家比较好理解，简单来说，相当于一个人绕着一个圆圈行走，假如他的速度是1度没秒，那么通过速度乘以时间，我们就可以知道他距离起点走了多少度。

那么我们如何理解用加速度计来测量倾角呢？一个简单的例子如下: 一个单轴的加速计位于重力水平面上的时候，它在垂直方向上受到的加速度为1g，在水平方向上受到的加速度为0。当我们把它旋转一个角度的时候，就会在水平轴上产生一个加速度分量。通过它们的关系，就可以计算出该单轴加速计的倾角。

 

gravity

在一个三轴的加速度传感器中，可以通过下列的反正切函数运算公式来计算加速度计各个轴的倾角。

gravity_xyz

另外，我们还要注意各传感器的方向。下图是MPU6050和MPU9150的传感器方向定义,其中MPU6050只包含陀螺仪和加速计共六个轴，而MPU9150还包含磁力计，共九个轴。从MPU6050和MPU9150获得的数据可以直接在下面的算法中使用，而不需要做符号变换。

 

imu_dir

现在，让我们回到姿态解算代码的分析。下述代码的核心思想是：通过陀螺仪的积分来获得四轴的旋转角度，然后通过加速度计的比例和积分运算来修正陀螺仪的积分结果。下面代码中的gx，gy，gz分别代表陀螺仪在X轴，Y轴和Z轴三个轴上的分量，ax，ay，az分别代表加速度计在在X轴，Y轴和Z轴三个轴上的分量。

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;

如果加速计各轴的数均是0，那么忽略该加速度数据。否则在加速计数据归一化处理的时候，会导致除以0的错误。

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)

if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

把加速度计的数据进行归一化处理。其中invSqrt是平方根的倒数，使用平方根的倒数而不是直接使用平方根的原因是使得下面的ax，ay，az的运算速度更快。通过归一化处理后，ax，ay，az的数值范围变成-1到+1之间。

// 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);

把上述计算得到的重力差进行积分运算，积分的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。积分系数是Ki，如果Ki参数设置为0，则忽略积分运算。

// 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;

}

把上述计算得到的重力差进行比例运算。比例的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。比例系数为Kp。

// 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;

}

在一个三维的空间内，由于重力加速度的存在，加速度计为我们提供了一个水平位置的绝对参考，但是它无法给我们提供一个方向的参考。这时候，我们首先考虑到的是指南针，磁力计就是这样的一个传感器，它给人们提供了一个正北方向的绝对参考。在四轴中常使用的地磁传感器是HMC5883L。

有了上面IMU惯性测量姿态解算的基础，我们就能很容易理解AHRS姿态解算的代码。它是在IMU惯性测量的基础上加入了地磁姿态的解算。下面代码中的gx，gy，gz分别代表陀螺仪在X轴，Y轴和Z轴三个轴上的分量，ax，ay，az分别代表加速度计在在X轴，Y轴和Z轴三个轴上的分量。mx，my，mz分别代表地磁在在X轴，Y轴和Z轴三个轴上的分量。

void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {

float recipNorm;

float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

float hx, hy, bx, bz;

float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;

float halfex, halfey, halfez;

float qa, qb, qc;

如果地磁传感器各轴的数均是0，那么忽略该地磁数据。否则在地磁数据归一化处理的时候，会导致除以0的错误。

// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)

if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {

MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);

return;

}

如果加速计各轴的数均是0，那么忽略该加速度数据。否则在加速计数据归一化处理的时候，会导致除以0的错误。

// 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;

把地磁的数据进行归一化处理。

// Normalise magnetometer measurement

recipNorm = invSqrt(mx * mx + my * my + mz * mz);

mx *= recipNorm;

my *= recipNorm;

mz *= recipNorm;

预先进行四元数数据运算，以避免重复运算带来的效率问题。

// Auxiliary variables to avoid repeated arithmetic

q0q0 = q0 * q0;

q0q1 = q0 * q1;

q0q2 = q0 * q2;

q0q3 = q0 * q3;

q1q1 = q1 * q1;

q1q2 = q1 * q2;

q1q3 = q1 * q3;

q2q2 = q2 * q2;

q2q3 = q2 * q3;

q3q3 = q3 * q3;

// Reference direction of Earth’s magnetic field

hx = 2.0f * (mx * (0.5f – q2q2 – q3q3) + my * (q1q2 – q0q3) + mz * (q1q3 + q0q2));

hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f – q1q1 – q3q3) + mz * (q2q3 – q0q1));

bx = sqrt(hx * hx + hy * hy);

bz = 2.0f * (mx * (q1q3 – q0q2) + my * (q2q3 + q0q1) + mz * (0.5f – q1q1 – q2q2));

根据当前四元数的姿态值来估算出各重力分量Vx，Vy，Vz和各地磁分量Wx，Wy，Wz。

// Estimated direction of gravity and magnetic field

halfvx = q1q3 – q0q2;

halfvy = q0q1 + q2q3;

halfvz = q0q0 – 0.5f + q3q3;

halfwx = bx * (0.5f – q2q2 – q3q3) + bz * (q1q3 – q0q2);

halfwy = bx * (q1q2 – q0q3) + bz * (q0q1 + q2q3);

halfwz = bx * (q0q2 + q1q3) + bz * (0.5f – q1q1 – q2q2);

使用叉积来计算重力和地磁误差。

// Error is sum of cross product between estimated direction and measured direction of field vectors

halfex = (ay * halfvz – az * halfvy) + (my * halfwz – mz * halfwy);

halfey = (az * halfvx – ax * halfvz) + (mz * halfwx – mx * halfwz);

halfez = (ax * halfvy – ay * halfvx) + (mx * halfwy – my * halfwx);

把上述计算得到的重力和磁力差进行积分运算，

// 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;

}