四元数解算姿态完全解析及资料汇总

《四元数解算姿态完全解析及资料汇总》由会员上传分享，免费在线阅读，更多相关内容在应用文档-天天文库。

1、四元数完全解析及资料汇总本文原帖出自匿名四轴论坛，附件里的资源请到匿名论坛下载：http://www.anobbs.com/forum.php感谢匿名的开源分享，感谢群友的热心帮助。说什么四元数完全解析其实都是前辈们的解析，小弟真心是一个搬砖的，搬得不好希望大神们给以批评和指正，在此谢过了。因为本人是小菜鸟一枚，对，最菜的那种菜鸟······所以对四元数求解姿态角这么一个在大神眼里简单的算法，小弟我还是费了很大劲才稍微理解了那么一点点，小弟搬砖整理时也是基于小弟的理解和智商的，有些太基础，有些可能错了，大牛们发现了再骂过我后希望能够给与指正

2、哈。好，废话到此为止，开始说主体。四元数和姿态角怎么说呢？先得给和我一样的小菜鸟们理一理思路，小鸟我在此画了一个“思维导图”（我承认我画的丑），四元数解算姿态首先分为两部分理解：第一部分先理解什么是四元数，四元数与姿态角间的关系；第二部分要理解怎么由惯性单元测出的加速度和角速度求出四元数，再由四元数求出欧拉角。图1渣渣思维导图在讲解什么是四元数时，小弟的思维是顺着说的，先由四元数的定义说起，说到四元数与姿态角间的关系。但在讲解姿态解算时，小弟的思维是逆向的，就是反推回来的，从欧拉角一步步反推回到惯性器件的测量数据，这样逆向说是因为便于理解，

3、因为实际在工程应用时和理论推导有很大差别。实际应用时正确的求解顺序应该为图1中序号顺序，即1->2->3->…….但在笔者讲解姿态求解时思路是如图2的。图2逆向讲解思路大家在看四元数时最好结合着代码一块看，小弟看的是匿名四轴的代码，感觉写的非常好也非常清晰，粘出来大家一块观摩。红色部分是核心代码，总共分为八个步骤，和图1中的八个步骤是一一对应的。讲解介绍时也是和代码对比起来讲解的。代码可以去匿名官网上下载，都是开源的，不是小弟的，所以小弟不方便加在附件中。//四元数更新姿态#defineKp2.0f//加速度权重，越大则向加速度测量值收敛越

4、快#defineKi0.001f//误差积分增益voidANO_IMU::Quaternion_CF(Vector3fgyro,Vector3facc,floatdeltaT){Vector3fV_gravity,V_error,V_error_I;//1.重力加速度归一化acc.normalize();//2.提取四元数的等效余弦矩阵中的重力分量Q.vector_gravity(V_gravity);//3.向量叉积得出姿态误差V_error=acc%V_gravity;//4.对误差进行积分V_error_I+=V_error*Ki;/

5、/5.互补滤波，姿态误差补偿到角速度上，修正角速度积分漂移Gyro+=V_error*Kp+V_error_I;//6.一阶龙格库塔法更新四元数Q.Runge_Kutta_1st(Gyro,deltaT);//7.四元数归一化Q.normalize();//8.四元数转欧拉角Q.to_euler(&angle.x,&angle.y,&angle.z);}好的，下面搬砖开始！。。。。。。。。嘿咻嘿咻！！！！一．什么是四元数？关于四元数的定义摘自秦永元的《惯性导航》，里面有非常好的讲解，大家可以直接看绪论和第九章就可以。下面我粘贴了部分原文，粘

6、贴的比较多比较详细，应为本人比较笨还爱较真，所以按本人的风格就要详尽一点，大牛们都可以自动忽略。四元数定义、表达方式及运算方法——摘自《惯性导航》-秦永元P289-292好，关于四元数定义就搬这么多，其他的大家去附件下载《惯性导航》的pdf自己看吧。下面开始搬四元数与姿态解算关系的。。。。。。嘿咻嘿咻~~~~二、四元数与姿态阵间的关系从上面我们知道了四元数的定义，可这四个数和我们要求的三个姿态角有什么关系呢？下面是详细的推导，同样摘自《惯性导航》-秦永元P292-297。四元数与姿态阵间的关系——摘自《惯性导航》-秦永元P292-297呃，

7、粘了这么多其实就是为了想知道推导过程小伙伴好理解，真正有用的就是（9.2.34）这个公式。▲这个公式把四元数转换成了方向余弦矩阵中的几个元素，再用这几个元素转换为欧拉角。就求解除了姿态！先从四元数q0~q3转成方向余弦矩阵：xbybzb=q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02-q12+q22-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02-q12-q22+q32xnynzn再从方向余弦矩阵转换为欧拉角：θ=sin-1（T32）——俯仰

8、角pitchθ=tan-1（-T31T33）——横滚角rollθ=tan-1（T12T22）——航向角yaw好的，公式原理都讲清楚了，下面来看一下匿名的代码：//四元数转欧拉角，