飞控陀螺仪，磁力计，加速计，四元数姿态结算

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

以上仅为本人的理解，仅供参考，如有细节上的错误希望大神指点指点！

飞控陀螺仪，磁力计，加速计，四元数姿态结算相关推荐

3轴/6轴/9轴传感器是什么，加速计/陀螺仪/磁力计又是什么？
欢迎加入Unity业内qq交流群:956187480 qq扫描二维码加群转自:加速计/陀螺仪/磁力计是什么,3轴/6轴/9轴传感器又是什么?_不积跬步,无以至千里!-CSDN博客现在越来越多的设备 ...
陀螺仪、加速计、磁力计等传感器汇总 (转)
转自 http://blog.csdn.net/a345017062/article/details/6459643 陀螺仪就是内部有一个陀螺,它的轴由于陀螺效应始终与初始方向平行,这样就可以通过与初 ...
一阶高通滤波+二阶Mahony滤波的四元数姿态解算
此次实验我使用icm20602进行 icm20602输出有以下特点: 3轴陀螺仪可选量程有±250dps,±500dps,±1000dps,±2000dps.(dps:degrees per seco ...
PX4飞控中利用EKF估计姿态角代码详解
PX4飞控中利用EKF估计姿态角代码详解 PX4飞控中主要用EKF算法来估计飞行器三轴姿态角,具体c文件在px4\Firmware\src\modules\attitude_estimator_ekf ...
四元数姿态解算及多传感器融合详细解析
代码路径ardupolit/modules/PX4Firmware/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp ...
安卓传感器全解：注册、注销传感器、监听传感器，距离传感器、方向传感器、陀螺仪、加速计、磁场、气压传感器
全栈工程师开发手册 (作者:栾鹏) 安卓教程全解安卓传感器全解:注册.注销传感器.监听传感器.距离传感器.方向传感器.陀螺仪.加速计.磁场.气压传感器. 注册.注销.监听传感器 1.自定义传感器监听 ...
AHRS姿态解算说明(加速度+陀螺仪+磁力计原理及原始数据分析)
转载链接:http://www.51hei.com/bbs/dpj-92911-1.html AHRS俗称航姿参考系统,AHRS由加速度计,磁场计,陀螺仪构成,AHRS的真正参考来自于地球的重力场和地 ...
【传感器】IMU (加速度计 + 陀螺仪)PI数据融合以及结算四元数并求解欧拉角
参考文章: 四元数完全解析及资料汇总 mpu6050姿态解算与卡尔曼滤波(1)数学写在开头, 首先我不太想做一个搬运工, 这样没有一点意思, 我会从我的视角(小白)来尝试理解以下问题: 我们从IMU ...
三轴磁力计解算姿态（四元数）
原理根据地磁场向量在水平面上的投影来计算载体的偏航角,类似于加速度计解算姿态,不同在于磁场易受干扰,且只能得到偏航角. 方法假设导航坐标系为东北天,载体坐标系为右前上. 初始载体坐标系和导航坐标系 ...

飞控陀螺仪，磁力计，加速计，四元数姿态结算

飞控陀螺仪，磁力计，加速计，四元数姿态结算相关推荐

最新文章

热门文章