飞控陀螺仪，磁力计，加速计，四元数姿态结算
MPU6050主要包含陀螺仪和加速度计。陀螺仪主要测量角速度，即可以测出某一时间段物体转过的角度。加速度计测量的是物体的加速度，重力加速度即物体受重力作用的情况下具有的加速度，物体静止时，加速度计测出来的加速度等于重力加速度g，约等于9.8米每平方秒，重力加速度总是竖直向下，通过计算重力加速度在X,Y的分量，则可以计算出物体相对于水平面的倾斜角度。
我们先来看看如何用欧拉角描述旋转(坐标变换)：
绕Z轴旋转即滚转角，绕Y轴旋转即偏航角，绕X旋转即俯仰角。
这里需要注意绕X,Y,Z轴方向旋转的先后顺序不一样，得出余弦矩阵的顺序（也就是公式）也不一样。并且在一组旋转里X,Y,Z不可交换(Euler旋转)，所以有先后顺序之分（程序中）。旋转变换有12种表示方式，分别为：XYZ、XZY、XYX、XZX、YXZ、YZX、YXY、YZY、ZXY、ZYX、ZXZ、ZYZ。这里还有一点需要注意，那就是"Gimbal Lock"。图中（ZYX，以下均为此顺序）的绕Z旋转会导致YX轴变化，但是Y影响X不会影响Z，X不会影响ZY。因此，如果旋转Y±90°，那么Z轴方向不变，但是会改变X轴方向，导致YX轴同向。此时保持Y值不变，那么改变Z或者X的效果相同。
Gimbal Lock总结就是，其源自Euler旋转原理，此原理旋转变量不可交换，所以有先后之分，所以可以改变后两个轴而第一轴方向不变，所以产生轴共线，即Gimbal Lock。
上图中得出旋转后的欧拉角公式，但是无人机的姿态结算中不能用欧拉角公式计算，一方面是因为欧拉角微分方程中包含了大量的三角运算，这给实时结算带来了一定的困难。而且当俯仰角为90度时方程式会出现神奇的“Gimbal Lock”。所以欧拉角方法只适合水平姿态变化不大的情况，而不适用于飞行器的姿态确定。
下面对四元数姿态进行结算：
- 重力加速度归一化。即将加速度计的三维向量（ax,ay,az）转换为单位向量，因为是单位矢量到参考性的投影，所以要把加速度计数据单位化。归一化只是改变这三个向量的长度，方向不改变，也就是只改变了相同的倍数，只是为了与单位四元数对应。ax,ay,az是机体坐标参照系上，加速度测出来的重力向量。
- 四元数换成方向余弦中的第三行的三个元素。将当前姿态重力在三个轴的分量分离出来，把四元数换算成方向余弦中的第三行的三个元素(vx,vy,vz)。惯性测量器件测量的都是关于b系的值。vx,vy,vz是陀螺仪的值积分后的姿态来推算出的重力向量。
- 向量叉积得出姿态误差。将ax,ay,az和vx,vy,vz对应的进行向量叉积（向量外积、叉乘）。分别得出ex,ey,ez。这个叉积向量仍然是位于机体坐标系，并和积分误差成正比，正好矫正陀螺。
- 对误差进行积分。将ax,ay,az和vx,vy,vz的误差进行积分消除误差。
- 进行滤波。陀螺仪测的值不断的进行更新，相应的积分误差也不断的修正，最后将积分误差反馈给陀螺仪，修正陀螺仪的值。
- 四元数微分方程。通过一阶龙格库塔法更新四元数。
- 四元数归一化。对四元数的单位化，单位化的四元数可以表示一个完整的旋转，只有单位四元数才可以表示旋转，这就是四元数表示旋转的约束条件。
- 四元数转欧拉角。
程序如下：
-
float Kp =
0.4f;
// 比例增益
-
float Ki =
0.001f;
// 积分增益
-
float exInt =
0.f;
// 重力在X轴上的分量
-
float eyInt =
0.0f;
-
float ezInt =
0.0f;
-
-
//四元数
-
static
float q0 =
1.0f;
-
static
float q1 =
0.0f;
-
static
float q2 =
0.0f;
-
static
float q3 =
0.0f;
-
-
float Rot_matrix[
3][
3];
-
Vector3f angle1;
-
-
void IMU_Updata(Vector3f acc,Vector3f gyro,Attitude *state,float dt)
-
{
-
-
float norm;
-
float ex,ey,ez;
-
float halfT = dt/
2.0f;
-
//重力加速度在X,Y,Z方向的分量
-
static
float verxZ,veryZ,verzZ;
-
angle1.x =
0.0f;
-
angle1.y =
0.0f;
-
float q0q0,q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3;
-
-
//1、获取原始值
-
//角速度转弧速度
-
gyro.x = gyro.x * DEG2RAD;
-
gyro.y = gyro.y * DEG2RAD;
-
gyro.z = gyro.z * DEG2RAD;
-
-
//2、加速度计量化
-
if((acc.x!=
0.0f)||(acc.y!=
0.0f)||(acc.z!=
0.0f))
-
{
-
norm = sqrtf3(acc.x , acc.y , acc.z);
-
acc.x = acc.x /norm ;
-
acc.y = acc.y /norm ;
-
acc.z = acc.z /norm ;
-
-
-
//3、叉乘
-
ex = (acc.y * verzZ) - (veryZ * acc.z);
-
ey = (acc.x * verzZ) - (verxZ * acc.z);
-
ez = (acc.x * veryZ) - (verxZ * acc.y);
-
-
//4、积分误差滤波
-
gyro.x += ex * Kp + exInt;
-
gyro.y += ey * Kp + eyInt;
-
gyro.z += ez * Kp + ezInt;
-
-
}
-
-
float q0_last =q0;
-
float q1_last =q1;
-
float q2_last =q2;
-
float q3_last =q3;
-
-
//解四元微分方程
-
q0 += (-q1_last*gyro.x - q2_last * gyro.y - q3_last *gyro.z)*halfT;
-
q1 += ( q0_last*gyro.x + q3_last * gyro.y - q2_last *gyro.z)*halfT;
-
q2 += (-q3_last*gyro.x + q0_last * gyro.y + q1_last *gyro.z)*halfT;
-
q3 += ( q2_last*gyro.x - q1_last * gyro.y + q0_last *gyro.z)*halfT;
-
-
norm = sqrtf4(q0,q1,q2,q3);
-
q0 = q0/norm;
-
q1 = q1/norm;
-
q2 = q2/norm;
-
q3 = q3/norm;
-
-
-
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;
-
-
verxZ =
2.0f * (q1q3 - q0q2);
-
veryZ =
2.0f * (q2q3 + q0q1);
-
verzZ = q0q0 - q1q1 - q2q2 + q3q3;
-
-
Rot_matrix[
0][
0] = q0q0 + q1q1 - q2q2 - q3q3;
-
Rot_matrix[
0][
1] =
2.0f * (q1q2 - q0q3);
-
Rot_matrix[
0][
2] =
2.0f * (q1q3 + q0q2);
-
Rot_matrix[
1][
0] =
2.0f * (q1q2 + q0q3);
-
Rot_matrix[
1][
1] = q0q0 - q1q1 + q2q2 - q3q3;
-
Rot_matrix[
1][
2] =
2.0f * (q2q3 - q0q1);
-
Rot_matrix[
2][
0] =
2.0f * (q1q3 - q0q2);
-
Rot_matrix[
2][
1] =
2.0f * (q2q3 + q0q1);
-
Rot_matrix[
2][
2] = q0q0 - q1q1 - q2q2 + q3q3;
-
-
//四元数转换为欧拉角输出
-
state->pitch =
asin(Rot_matrix[
1][
2]);
-
state->roll =
atan2(-Rot_matrix[
0][
2],Rot_matrix[
2][
2]);
-
state->yaw =
atan2(-Rot_matrix[
1][
0], Rot_matrix[
1][
1]);
-
-
}
运算宏定义如下：
-
#define MM2M 0.001f
-
#define DEG2RAD 0.017453292519943295769236907684886f //角度转弧度
-
#define RAD2DEG 57.295779513082320876798154814105f //弧度转角度
-
-
#define sq(a) ((a) * (a)) //平方、
-
#define sqrtf4(a,b,c,d) __sqrtf(sq(a) + sq(b) + sq(c) + sq(d))
-
#define sqrtf3(a,b,c) __sqrtf(sq(a) + sq(b) + sq(c))
-
#define sqrtf2(a,b) __sqrtf(sq(a) + sq(b))
在加速度计矫正的基础上+磁力计：
在三维空间中，根据重力加速度，加速度为我们提供一个水平位置的参考，但是无法获得方向的参考，这时就需要磁力计，它能给人们提供一个正北方向的绝对参考。
在上述姿态结算的基础上进行磁力计矫正：
- 把磁力计的数据进行归一化处理（进行量化）。
- 根据当前四元数姿态值估算各重力分量vx,vy,vz，再根据vx,vy,vz预估磁场的方向wx,wy,wz。
- 再根据wx,wy,wz对磁力计测出的值进行误差矫正。
- 把加速度计和磁力计修正后的陀螺仪数据整合到四元数中。
- 最后进行角度运算。
-
float KpDef =
0.5f;
-
float KiDef =
0.025f;
-
-
float q0 =
1.0f,q1 =
0.0f,q2 =
0.0f,q3 =
0.0f;
-
-
float integralFBx =
0.0f,integralFBy =
0.0f,integralFBz =
0.0f;
-
-
static
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
-
-
static
float Rot_matrix[
3][
3];
//方向余弦矩阵
-
-
//使用加速度传感器测量重力向量，使用磁力计测量方向
-
void AHRS_Uptate(Vector3f acc,Vector3f gyro,Vector3i mag,Attitude *state,float dt)
-
{
-
-
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;
-
//大地坐标系(e) --> 机体坐标系(b)
-
Rot_matrix[
0][
0] = q0q0 + q1q1 - q2q2 - q3q3;
-
Rot_matrix[
0][
1] =
2.f * (q1q2 + q0q3);
-
Rot_matrix[
0][
2] =
2.f * (q1q3 - q0q2);
-
Rot_matrix[
1][
0] =
2.f * (q1q2 - q0q3);
-
Rot_matrix[
1][
1] = q0q0 - q1q1 + q2q2 - q3q3;
-
Rot_matrix[
1][
2] =
2.f * (q2q3 + q0q1);
-
Rot_matrix[
2][
0] =
2.f * (q1q3 + q0q2);
-
Rot_matrix[
2][
1] =
2.f * (q2q3 - q0q1);
-
Rot_matrix[
2][
2] = q0q0 - q1q1 - q2q2 + q3q3;
-
-
float ex =
0.0f, ey =
0.0f, ez =
0.0f;
-
float norm;
-
-
if((mag.x !=
0) || (mag.y !=
0) || (mag.z !=
0))
-
{
-
//磁力计量化
-
norm = sqrtf3(mag.x,mag.y,mag.z);
-
mag.x /= norm;
-
mag.y /= norm;
-
mag.z /= norm;
-
-
//地球磁场的参考方向
-
float hx = Rot_matrix[
0][
0] * mag.x + Rot_matrix[
1][
0] * mag.y + Rot_matrix[
2][
0] * mag.z ;
//计算出x轴的方向
-
float hy = Rot_matrix[
0][
1] * mag.x + Rot_matrix[
1][
1] * mag.y + Rot_matrix[
2][
1] * mag.z ;
//计算出Y轴的方向
-
float by = sqrtf2(hx,hy);
//预估磁场的方向
-
float bz = Rot_matrix[
0][
2] * mag.x + Rot_matrix[
1][
2] * mag.y + Rot_matrix[
2][
2] * mag.z ;
//计算出Z轴方向
-
-
//估计磁场的方向
-
float wx = Rot_matrix[
0][
1] * by + Rot_matrix[
0][
2] * bz;
-
float wy = Rot_matrix[
1][
1] * by + Rot_matrix[
1][
2] * bz;
-
float wz = Rot_matrix[
2][
1] * by + Rot_matrix[
2][
2] * bz;
-
-
//误差是估计方向和场矢量测量方向的乘积
-
ex = mag.y * wz - mag.z * wy;
-
ey = mag.z * wx - mag.x * wz;
-
ez = mag.x * wz - mag.z * wx;
-
-
if((acc.x !=
0)||(acc.y !=
0)||(acc.z !=
0))
-
{
-
//加速度值量化
-
norm = sqrtf3(acc.x,acc.y,acc.z);
-
acc.x /= norm;
-
acc.y /= norm;
-
acc.z /= norm;
-
-
//根据当前四元数的姿态值估算出各重力分量Vx,Vy,Vz
-
float vx = Rot_matrix[
0][
2];
-
float vy = Rot_matrix[
1][
2];
-
float vz = Rot_matrix[
2][
2];
-
-
//使用叉积来计算重力误差
-
ex += acc.y * vz - acc.z * vy;
-
ey += acc.z * vx - acc.x * vz;
-
ez += acc.x * vy - acc.y * vx;
-
}
-
-
//只有当从加速度计或磁力计收集有效数据时才应用反馈
-
if(ex !=
0.0f && ey !=
0.0f && ez !=
0.0f)
-
{
-
//把上述计算得到的重力和磁力差进行积分运算
-
if(KiDef >
0.0f)
-
{
-
integralFBx += KiDef * ex * dt;
-
integralFBy += KiDef * ey * dt;
-
integralFBz += KiDef * ez * dt;
-
-
gyro.x += integralFBx;
-
gyro.y += integralFBy;
-
gyro.z += integralFBz;
-
}
-
else
-
{
-
integralFBx =
0.0f;
-
integralFBy =
0.0f;
-
integralFBz =
0.0f;
-
}
-
//把上述计算得到的重力差和磁力差进行比例运算
-
-
gyro.x += KpDef * ex;
-
gyro.y += KpDef * ey;
-
gyro.z += KpDef * ez;
-
}
-
float dq0 =
0.5f*(-q1 * gyro.x - q2 * gyro.y - q3 * gyro.z);
-
float dq1 =
0.5f*( q0 * gyro.x + q2 * gyro.z - q3 * gyro.y);
-
float dq2 =
0.5f*( q0 * gyro.y - q1 * gyro.z + q3 * gyro.x);
-
float dq3 =
0.5f*( q0 * gyro.z + q1 * gyro.y - q2 * gyro.x);
-
-
q0 += dt * dq0;
-
q1 += dt * dq1;
-
q2 += dt * dq2;
-
q3 += dt * dq3;
-
-
norm = sqrtf4(q0, q1, q2, q3);
-
q0 /= norm;
-
q1 /= norm;
-
q2 /= norm;
-
q3 /= norm;
-
-
Rot_matrix[
0][
0] = q0q0 + q1q1 - q2q2 - q3q3;
-
Rot_matrix[
0][
1] =
2.f * (q1q2 + q0q3);
-
Rot_matrix[
0][
2] =
2.f * (q1q3 - q0q2);
-
Rot_matrix[
1][
0] =
2.f * (q1q2 - q0q3);
-
Rot_matrix[
1][
1] = q0q0 - q1q1 + q2q2 - q3q3;
-
Rot_matrix[
1][
2] =
2.f * (q2q3 + q0q1);
-
Rot_matrix[
2][
0] =
2.f * (q1q3 + q0q2);
-
Rot_matrix[
2][
1] =
2.f * (q2q3 - q0q1);
-
Rot_matrix[
2][
2] = q0q0 - q1q1 - q2q2 + q3q3;
-
-
//四元数转换为欧拉角输出
-
state->pitch =
asin(Rot_matrix[
1][
2]);
-
state->roll =
atan2(-Rot_matrix[
0][
2],Rot_matrix[
2][
2]);
-
state->yaw =
atan2(-Rot_matrix[
1][
0], Rot_matrix[
1][
1]);
-
}
-
}
运算宏定义如下：
-
#define MM2M 0.001f
-
#define DEG2RAD 0.017453292519943295769236907684886f // 角度转弧度
-
#define RAD2DEG 57.295779513082320876798154814105f // 弧度转角度
-
-
#define sq(a) ((a) * (a)) //平方、
-
#define sqrtf4(a,b,c,d) __sqrtf(sq(a) + sq(b) + sq(c) + sq(d))
-
#define sqrtf3(a,b,c) __sqrtf(sq(a) + sq(b) + sq(c))
-
#define sqrtf2(a,b) __sqrtf(sq(a) + sq(b))
以上仅为本人的理解，仅供参考，如有细节上的错误希望大神指点指点！