[学习IMU]（MEMS 三轴加速计、三轴陀螺仪、三轴磁力计）6轴IMU+磁力计，9轴传感器讲解

1、三轴加速度计

（1）测量比力

三轴加速度计是一种惯性传感器，能够测量物体的比力，即去掉重力后的整体加速度或者单位质量上作用的非引力。当加速度计保持静止时，加速度计能够感知重力加速度，而整体加速度为零。在自由落体运动中，整体加速度就是重力加速度，但加速度计内部处于失重状态，而此时三轴加速度计输出为零。

（2）测量角度

三轴加速度计的原理能够用来测量角度。直观地，如图所示，弹簧压缩量由加速度计与地面的角度决定。比力能够通过弹簧压缩长度来测量。因此在没有外力作用的情况下，加速度计能够精确地测量俯仰角和滚转角，且没有累积误差。

MEMS三轴加速度计是采用压阻式、压电式和电容式工作原理，产生的比力（压力或者位移）分别正比于电阻、电压和电容的变化。这些变化可以通过相应的放大和滤波电路进行采集。该传感器的缺点是受振动影响较大。

介于其测量角度的工作原理三轴加速度计无法测量偏航角

可测量俯仰角和横滚角

2、三轴陀螺仪

作用：用于无人机中的角速度及对角速度积分后角度的计算

原理：理解三轴陀螺仪的原理首先要知道科里奥利力

科里奥利力

当一个质点相对于惯性系做直线运动时，因为质点自身惯性，它相对于旋转体系，其轨迹是一条曲线。立足于旋转体系，我们认为有一个力驱使质点运动轨迹形成曲线。科氏力就是对这种偏移的一种描述，表示为

即本来直线的运动当放在一个旋转体系中直线轨迹会发生偏移，而实际上并直线运动的问题并未受到力的作用，设立这样一个虚拟的力称为科里奥利力。

由此我们在陀螺仪中，选用两块物体，他们处于不断的运动中，并令他们运动的相位相差-180度，即两个质量块运动速度方向相反，而大小相同。它们产生的科氏力相反，从而压迫两块对应的电容板移动，产生电容差分变化。电容的变化正比于旋转角速度。由电容即可得到旋转角度变化。

3、三轴磁力计

磁力计能提供装置在XYZ各轴所承受磁场的数据，接着相关数据会汇入微控制器的运算法，以提供磁北极相关的航向角，利用这些信息可侦测地理方位。

磁力仪是采用三个互相垂直的磁阻传感器，每个轴向上的传感器检测在该方向上的地磁场强度。

上图为一种采用具有晶体结构的合金材料。它们对外界的磁场很敏感，磁场的强弱变化会导致磁阻传感器电阻值发生变化。

另外三轴磁力计还可以采用洛伦兹力原理，电流流过磁场产生力，从而驱动电容等变化。

https://guoqing.blog.csdn.net/article/details/104598337

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

把加速度计的数据进行归一化处理。
// 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);

上文详细描述了由Madgwick提出四元数AHRS姿态解算和IMU姿态解算方法，：该文章出自圆点博士无人机www.bspilot.com