四元数姿态解析x

这个程序得到了四元数，怎么进一步计算姿态YAW，ROLL，PITCH？
//----------------------------------------------------------------------------------------------------
// Header files
#include "AHRS.h"
#include <math.h>
//----------------------------------------------------------------------------------------------------
// Definitions
#define Kp 2.0f // 比例增益支配收敛率accelerometer/magnetometer
#define Ki 0.005f // 积分增益执政速率陀螺仪的衔接gyroscopeases
#define halfT 0.5f // 采样周期的一半
//---------------------------------------------------------------------------------------------------
// Variable definitions
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计
方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
//============================================================================= =======================
// Function
//============================================================================= =======================
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
// 辅助变量，以减少重复操作数
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
// 测量正常化
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
// 计算参考磁通方向
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2); hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1); hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2); bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
//估计方向的重力和磁通（V和W）
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
// 错误是跨产品的总和之间的参考方向的领域和方向测量传感器
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
// 积分误差比例积分增益
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// 调整后的陀螺仪测量
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// 整合四元数率和正常化
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// 正常化四元
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
//============================================================================= =======================
// END OF CODE
//============================================================================= =======================
一种常见的四轴飞行器姿态解算方法分析
作者：让四轴飞时间：2014-06-04来源：电子产品世界
全国各地已经陆续开放低空管制，北京也将在2015年全面开放低空领域，这对低空飞行器将是一个十分重大的好消息!低空飞行器也将迎来一个新的发展春天。

实际上，近年四轴飞行器发展相当迅速，国内的航拍水平越来越高，顺丰及亚马逊已在尝试将无人机用于快递行业。

越来越多的人开始关注并研究四轴飞行器。

本文引用地址：/article/247809.htm
本文将分析一种常见的四轴飞行器姿态解算方法，Mahony的互补滤波法。

此法简单有效，希望能给学习四轴飞行器的朋友们带来帮助。

关于姿态解算和滤波的理论知识，推荐秦永元的两本书，一是《惯性导航》，目前已出到第二版了;二是《卡尔曼滤波与组合导航原理》。

程序中的理论基础，可在书中寻找。

同时欢迎到论坛发帖交流：/forum/368/1
下面开始进入正题：
先定义Kp，Ki，以及halfT 。

Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度
halfT ，姿态解算时间的一半。

此处解算姿态速度为500HZ，因此halfT 为0.001
#define Kp 2.0f
#define Ki 0.002f
#define halfT 0.001f
初始化四元数
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
定义姿态解算误差的积分
float exInt = 0, eyInt = 0, ezInt = 0;
以下为姿态解算函数。

参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒;
参数ax，ay，az分别对应三个轴的加速度原始数据
由于加速度的噪声较大，此处应采用滤波后的数据
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
将加速度的原始数据，归一化，得到单位加速度
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
把四元数换算成“方向余弦矩阵”中的第三列的三个元素。

根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。

所以这里的vx、vy、vz，其实就是当前的机体坐标参照系上，换算出来的重力单位向量。

(用表示机体姿态的四元数进行换算)
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
这里说明一点，加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。

陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。

在机体坐标参照系上，加速度计测出来的重力向量是ax、ay、az;陀螺积分后的姿态来推算出的重力向量是vx、vy、vz;它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。

向量间的误差，可以用向量积(也叫外积、叉乘)来表示，ex、ey、ez就是两个重力向量的叉积。

这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。

由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。

叉乘是数学基础，百度百科里有详细解释。

ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
将叉乘误差进行积分
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
用叉乘误差来做PI修正陀螺零偏，通过调节Kp，Ki两个参数，可以控制加速度计修正陀螺仪积分姿态的速度
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
四元数微分方程，没啥好说的了，看上面推荐的书吧，都是理论的东西，自个琢磨琢磨实在琢磨不明白，那就把指定的参数传进这个函数，再得到相应的四元数，最后转化成欧拉角即可了。

不过建议还是把理论弄清楚一点。

q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
四元数单位化
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
姿态解算后，就得到了表示姿态的四元数。

但四元数不够直观，一般将其转化为欧拉角。

转化时根据旋转轴的次序不同，公式也不同。

以下给出其中一种公式：
/
读完程序，深刻的意识到了理论基础的重要性。

Mahony的互补滤波函数，确实很巧妙，利用叉乘误差来修正四轴的姿态，姿态解算速度越快，则解算的精度越高。

在许多国内开源程序中，也是用到了这种方法。

在解四元数微分方程时，该程序用到了一阶毕卡解法。

同样可用于解四元数微分方程的还有龙格-库塔法，由于篇幅有限，此处就不介绍龙格-库塔法了，有兴趣的网友请自行查阅相关资料。