四元数姿态解析x

1、这个程序得到了四元数，怎么进一步计算姿态YAW,ROLL,PITCH?/Headerfiles#include"AHRS.h"#include<math.h>/Definitions#defineKp2.0f/比例增益支配收敛率accelerometer/magnetometer#defineKi0.005f/积分增益执政速率陀螺仪的衔接gyroscopeases#definehalfT0.5f/采样周期的一半/Variabledefinitionsfloatq0=1,q1=0,q2=0,q3=0;/四元数的元素，代表估计方向floatexInt=0,eyInt

2、=0,ezInt=0;/按比例缩小积分误差/=/Function/=voidAHRSupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatmx,floatmy,floatmz)floatnorm;floathx,hy,hz,bx,bz;floatvx,vy,vz,wx,wy,wz;floatex,ey,ez;/辅助变量，以减少重复操作数floatq0q0=q0*q0;floatq0q1=q0*q1;floatq0q2=q0*q2;floatq0q3=q0*q3;floatq1q1=q1*q1;floatq1q2=q1*q2;fl

3、oatq1q3=q1*q3;floatq2q2=q2*q2;floatq2q3=q2*q3;floatq3q3=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)+

4、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);/错误是

5、跨产品的总和之间的参考方向的领域和方向测量传感器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*

6、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;/=/ENDOFCODE/=一种常见的四轴飞行器姿态解算方法分析作者：让四轴飞时间：2014-06-04来源：电子产品世界全国各地已经陆续开放低空管制，北京也将在2015年全面开放低空领域，这对低空飞行器将是一个十分重大的好消息!低空飞行器也将迎来一个新的发

7、展春天。实际上，近年四轴飞行器发展相当迅速，国内的航拍水平越来越高，顺丰及亚马逊已在尝试将无人机用于快递行业。越来越多的人开始关注并研究四轴飞行器。本文引用地址：本文将分析一种常见的四轴飞行器姿态解算方法，Mahony的互补滤波法。此法简单有效，希望能给学习四轴飞行器的朋友们带来帮助。关于姿态解算和滤波的理论知识，推荐秦永元的两本书，一是惯性导航，目前已出到第二版了;二是卡尔曼滤波与组合导航原理。程序中的理论基础，可在书中寻找。同时欢迎到论坛发帖交流：下面开始进入正题：先定义Kp，Ki，以及halfT。Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度halfT，姿态解算时间的一半。此处解算姿态

8、速度为500HZ，因此halfT为0.001#defineKp2.0f#defineKi0.002f#definehalfT0.001f初始化四元数floatq0=1,q1=0,q2=0,q3=0;定义姿态解算误差的积分floatexInt=0,eyInt=0,ezInt=0;以下为姿态解算函数。参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒;参数ax，ay，az分别对应三个轴的加速度原始数据由于加速度的噪声较大，此处应采用滤波后的数据voidIMUupdate(floatgx,floatgy,floatgz,floatax,floatay,floataz)floatnorm;fl

9、oatvx,vy,vz;floatex,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

10、;这里说明一点，加速度计由于噪声比较大，而且在飞行过程中，受机体振动影响比陀螺仪明显，短时间内的可靠性不高。陀螺仪噪声小，但是由于积分是离散的，长时间的积分会出现漂移的情况，因此需要将用加速度计求得的姿态来矫正陀螺仪积分姿态的漂移。在机体坐标参照系上，加速度计测出来的重力向量是ax、ay、az;陀螺积分后的姿态来推算出的重力向量是vx、vy、vz;它们之间的误差向量，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。向量间的误差，可以用向量积(也叫外积、叉乘)来表示，ex、ey、ez就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小

11、与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。叉乘是数学基础，百度百科里有详细解释。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;四元数微分

12、方程，没啥好说的了，看上面推荐的书吧，都是理论的东西，自个琢磨琢磨实在琢磨不明白，那就把指定的参数传进这个函数，再得到相应的四元数，最后转化成欧拉角即可了。不过建议还是把理论弄清楚一点。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;姿态解算后，就得到了表示姿态的四元数。但四元数不够直观，一般将其转化为欧拉角。转化时根据旋转轴的次序不同，公式也不同