大话多旋翼飞行器--欧拉角与四元数
四元数转欧拉角代码解析
四元数转欧拉角代码解析 本文的内容就是解析正点原子MPU6050的mpu_dmp_get_data()函数中，三个欧拉角的由来，即如何将MPU6050输出的四元数转化为姿态解算所需要的欧拉角。 *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw 其实上述三个公式的核心就是将一次的姿态变换分别用四元数矩阵和欧拉角矩阵表示出来，由于这两个矩阵是等价的即对应元素都相等，通过简单的对比运算就可以得到上述的三个公式。 因此，我将从1.四元数矩阵的得到；2.欧拉角矩阵的得到；3.两个矩阵的等价运算三个部分进行说明。 1.四元数矩阵的得到 三重矢量计算公式： AX(BXC)=B(A·C)-C(A·B) 这个公式很好记，右边部分就是BACK-CAB(后面的出租车)
2.欧拉角矩阵的得到 q02+q12+q22+q32=1 从9.2.33到9.2.34的化简，其实就是利用 进行化简，把1去掉即可。 将右侧的矩阵乘开，可得到一个3x1矩阵， 与左边3x1矩阵对应元素相等，这个相等的 关系，就是上个框框中求出的三个等式。 各轴上的单位1，就是图1.2.2矩阵任意 行与列各个元素的平方和为1。
到这里，用欧拉角表示描述一次旋转变换已经结束了。然而，上述的姿态矩阵C n b仅仅是《惯性导航》这本书先Z,再X,最后Y旋转变换而形成的姿态矩阵，这样的旋转顺序其实是和很多大家实际使用的飞控代码不一样的（同样的，关于θφγ的实际意义其实也没有明确的规定）。此文目的就是解析“正点原子”飞控代码中四元数转欧拉角部分，因此，接下来，
欧拉角与四元数
四元数与旋转 一．四元组基础 Q(x,y,z,w)，其中x，y，z用来确定旋转轴，w为旋转的角度 Q=w+xi+yj+zk，i，j，k为三个虚轴的单位分量 I*j=k J*k=i; K*i=j; 叉乘： c=a × b= | i j k| |a1 b1 c1| |a2 b2 c2| =(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1) c也为一个向量，且c的长度为|a||b|sin(theta)，垂直于a和b所在的平面，方向由右手法则来判定，用右手的四指先表示向量a的方向，然后手指朝着手心的方向摆动到向量b的方向，大拇指所指的方向就是向量c的方向 1．四元组相乘： Q1=w1+x1i+y1j+z1k=(w1,v1) Q2=w2+x2i+y2j+z2k=(w2,v2) Q1*Q2=(w1*w2-
Unity旋转(四元数)
在Unity 3D中，实现物体旋转有多种方式，如旋转矩阵、欧拉角和四元数等[1]。旋转需要两个基本参量轴和角，物体从一个方位旋转到另一个方位可以采用多次改变轴和角的方式，依次旋转。其中，有一种旋转方式是只绕一个轴旋转一次就能达到指定方位，且旋转角度在﹣180°～180°之间，称 这样的旋转方式为最短旋转。 任意指定两个方位，要找出其中的最短旋转并不是一件容易的事。本文将给出最短旋转的数学描述以及在Unity 3D中实现最短旋转的方法。 最短旋转的数学描述 刚体的运动包括平动和转动。描述刚体的空间位置，用三维空间坐标点（x,y,z）表示，在Unity 3D中有3个基本坐标系，分别是世界坐标系、惯性坐标系与本地坐标系。相应的，描述刚体的旋转状态，即方位，是本地坐标系与惯性坐标系所形成的角度变化，采用欧拉角来描述。由于本文不涉及平移，因此为方便讨论，将惯性坐标系和世界坐标系重合。 众所周知，“两点之间线段最短”，同样两个方位之间也存在类似的关系，即最短旋转，两点之间的距离用两点位置之差来描述。相应的，两个方位之间的最短旋转用两个方位的四元数之比来描述。 如果方位a的四元数为q1，方位b的四元数为q2，刚体从方位a旋转到方位b的最短旋转的四元数为q，则q = q2÷q1。 设四元数q的4个分量分别是(x,y,z,w)，该四元数隐含了旋转轴向量n和旋转角d，设轴向量n 的3个分量为(nx,ny,nz)。一般将轴n和角d写成“轴角对”的形式，即(n,d)=(nx,ny,nz,d )。四元数q=(x,y,z,w)与轴角对(n,d)=(nx,ny,nz,d )之间的关系为： q = (x,y,z,w) = (nx*sin(d/2),ny*sin(d/2),nz*sin(d/2),cos(d/2)) 在Unity 3D中，改变欧拉角和改变四元数是两种基本的旋转方式，Unity 3D提供了Lerp和Slerp 两种插值函数，在两个方位之间进行采样插值。对欧拉角的Lerp插值，很难实现最短旋转，而四元数Slerp函数插值则非常容易实现最短旋转，下面通过例子来进行验证。 在Unity3D中改变欧拉角实现旋转 在Unity 3D中，制作一个空物体，命名为a，该物体位于世界坐标系的原点位置，在其下面放置一个立方体Cube和一个小球Sphere，调整立方体和小球的大小和位置，如下图所示，让小球位于立方体的一个角点。
旋转矩阵、欧拉角、四元数
旋转矩阵、欧拉角、四元数比较 旋转矩阵、欧拉角、四元数主要用于: 向量的旋转、坐标系之间的转换、角位移计算、方位的平滑插值计算 各方法比较 任务/性质旋转矩阵欧拉角四元数 在坐标系间(物体和惯性)旋转点能不能(必须转换到矩 阵) 不能(必须转换到矩 阵) 连接或增量旋转能,但经常比四元数 慢,小心矩阵蠕变的情 况 不能能,比矩阵快 插值基本上不能能,但可能遭遇万向锁 或其他问题Slerp提供了平滑插值 易用程度难易难 在内存或文件中存储9个数3个数4个数 对给定方位的表达方式是否唯一是不是,对同一方位有无 数多种方法 不是,有两种方法,它 们互相为互 可能导致非法矩阵蠕变任意三个数都能构成 合法的欧拉角可能会出现误差积累,从而产生非法的四元数 不同的方位表示方法适用于不同的情况。下面是我们对合理选择格式的一些建议: l 欧拉角最容易使用。当需要为世界中的物体指定方位时,欧拉角能大大的简化人机交互, 包括直接的键盘输入方位、在代码中指定方位(如为渲染设定摄像机)、在调试中测试。这个优点不应该被忽视,不要以”优化”为名义而牺牲易用性,除非你去顶这种优化的确有效果。 2如果需要在坐标系之间转换响亮,那么就选择矩阵形式。当然,这并不意味着你就不能用其他格式来保存方位,并在需要的时候转换到矩阵格式。另一种方法是用欧拉角作为方位的”主拷贝”但同时维护一个旋转矩阵,当欧拉角发生改变时矩阵也要同时进行更新。
3 当需要大量保存方位数据(如:动画)时,就使用欧拉角或四元数。欧 拉角将少占用25%的内存,但它在转换到矩阵时要稍微慢一些。如果动画数据需要嵌套坐标系之间的连接,四元数可能是最好的选择。 4 平滑的插值只能用四元数完成。如果你用其他形式,也可以先转换 到四元数然后再插值,插值完毕后再转换回原来的形式。
学习四元数笔记
复数是由实数加上虚数单位i 组成，其中 i^2 = -1 \,。 相似地，四元数都是由实数加上三个元素i、j、k 组成，而且它们有如下的关系： i^2 = j^2 = k^2 = ijk = -1 \, 每个四元数都是1、i、j 和k 的线性组合，即是四元数一般可表示为a + bi + cj + dk \,。四元数不像实数或复数那样，它的乘法是不可交换的，看乘数表 四元数的优点是： 表达式无奇点（和例如欧拉角之类的表示相比） 比矩阵更简炼（也更快速） 单位四元数的对可以表示四维空间中的一个转动。 以矩陣表示四元數[编辑] 有兩種方法能以矩陣表示四元數，並以矩陣之加法、乘法應用於四元數之加法、乘法。 第一種是以二階複數矩陣表示。若h = a + bi + cj + dk 則它的複數形式為： 這種表示法有如下優點： 所有複數(c = d = 0) 就相應於一個實矩陣。 四元數的絕對值的平方就等於矩陣的行列式。 四元數的共軛值就等於矩陣的共軛轉置。 對於單位四元數(|h| = 1) 而言，這種表示方式給了四維球体和SU(2)之間的一個同型，而後者對於量子力學中的自旋的研究十分重要。（請另見泡利矩陣） 第二種則是以四階實數矩陣表示： 其中四元數的共軛等於矩陣的轉置。
（转载）四元数入门(2012-02-14 00:52:24)转载▼ 标签：computer graphic quaternion 四元数it 分类：学习 （转载）四元数入门 --------------------------------------------------------------------- https://www.wendangku.net/doc/3517804301.html,/showthread.asp?threadid=73511 4元数宝典 这是国内找不到的超好文章。（为什么大陆的4元数文章很垃圾呢？） （翻译中。。。奉献给大家~~） 70秒即懂，能使用，用四元数，4元数，阔特尼恩，Quaternion旋转 (C) 中田亨（独立行政法人产业技术综合研究所数字人类研究中心研究员博士（工学）） 2003年11月25日 ★这个页面的对象读者 想把三次元的旋转，用CG等定量地处理的人 使用欧拉角(Euler Angles)的话，不懂得其道理的人 卡尔丹角和欧拉角(Cardan Angles)不能区别的人 对吉恩瓦尔洛克很困惑的人 但是，对数学之类麻烦的事情很讨厌的人 想要实例程序的人 没有时间的人 ★旋转篇： 我将说明使用了四元数（si yuan shu, quaternion）的旋转的操作步骤
基于四元数方法的姿态解算
基于四元数方法的姿态解算方法分析 摘要:载体的姿态解算算法是实现捷联式惯性导航系统精确导航的核心技术之一。分析了欧拉法、方向余弦法、四元数法求解姿态矩阵的优缺点，采用四元数法与方向余弦法两种解算方法分别计算载体姿态，两种方法的计算结果之差与理论真值比较以得到解算的相对误差，从而验证了四元数法的正确性和有效性。最后，指出提高采样频率和采用高阶计算算法能进一步减小姿态解算误差。数字化仿真与转台试验结果表明，本文提出的载体姿态解算法具有良好的实时性。 1引言 捷联惯导是一种自主式的导航方法。该方法将陀螺仪和加速度计直接安装在载体上，省掉机电式导航平台，利用计算机软件建立一个“数学平台”来代替机电平台实体[1]。由于其结构简单且抗干扰能力强，目前已成为航空航天、航海、机器人、智能交通等领域的研究热点之一。 姿态解算是捷联式惯性导航系统的关键技术，通过姿态矩阵可以得到载体的姿态和导航参数计算需要的数据，是捷联式惯导算法中的重要工作。载体的姿态和航向体现了载体坐标系与导航坐标系之间的方位关系，确定两个坐标系之间的方位关系需要借助矩阵法和力学中的刚体定点运动的位移定理。通过矩阵法推导方向余弦表，而刚体定点运动的位移定理表明，定点运动刚体的任何有限位移都可以绕过定点的某一轴经过一次转动来实现。目前描述动坐标相对参考坐标系方位关系的方法有多种，可简单地将其分为3类，即三参数法、四参数法和九参数法「1-2]。三参数法也叫欧拉角法，四参数法通常指四元数法，九参数法称作方向余弦法。欧拉角法由于不能用于全姿态飞行运载体上而难以广泛用于工程实践，且实时计算困难。方向余弦法避免了欧拉法的“奇点”现象，但方程的计算量大，工作效率低。随着飞行运载体导航控制系统的迅速发展和数字计算机在运动控制中的应用，控制系统要求导航计算环节能更加合理地描述载体的刚体空间运动，四元数法的研究得到了广泛重视。本文全面分析了3种解算方法的特点，通过对比四参法与九参法的计算结果以验证四元数法的正确性和有效性，基于数值仿真和转台实验相结合的分析方法得到进一步减少姿态解算误差的有效途径，为捷联式惯性导航技术的工程实践提供参考。（就是这部分内容需要程序解算，不会搞） 2姿态矩阵的计算方法 由于载体的姿态方位角速率较大，所以针对姿态矩阵的实时计算提出了更高的要求。通常假定捷联系统“数学平台”模拟地理坐标系，即导航坐标系;而确定载体的姿态矩阵即为研究载体坐标系(6)和导航坐标系(E)的空间转动关系，一般用载体坐标系相对导航坐标系的三次转动角确定，习惯上俯仰角和偏航角用B和必表示，滚转角用Y表示。目前主要的研究方法为:欧拉法、方向余弦法与四元数法。图1为捷联式惯性导航原理图。
欧拉角
欧拉角 科技名词定义 中文名称：欧拉角 英文名称：Euler angles 定义：构件在三维空间中的有限转动，可依次用三个相对转角表示，即进动角、章动角和自旋角，这三个转角统称为欧拉角。 所属学科：机械工程（一级学科）；机构学（二级学科）；机构运动学（三级学科） 本内容由全国科学技术名词审定委员会审定公布 欧拉角 用来确定定点转动刚体位置的3个一组独立角参量，由章动角θ、旋进角（即进动角）ψ和自转角j组成，为欧拉首先提出而得名。 目录
它们有多种取法，下面是常见的一种。如图所示，由定点O作出固定坐标系Oxyz和固连于刚体的动坐标系Ox′y′z′。以轴Oz和Oz′为基本轴，其垂直面Oxy和Ox′y′为基本平面。由轴Oz 欧拉角 量到Oz′的角θ称章动角。平面zOz′的垂线ON称节线，它又是基本平面Ox′y′和Oxy的交线。在右手坐标系中，由ON的正端看，角θ应按逆时针方向计量。由固定轴Ox量到节线ON的角ψ称旋进角；由节线ON量到动轴Ox′的角j称自转角。由轴Oz和Oz′正端看，角ψ和j也都按逆时针方向计量。若令Ox′y′z′的初始位置与Oxyz重合，经过相继绕Oz、ON和Oz′的三次转动后，刚体将转到图示的任意位置。如果刚体绕通过定点O的某一轴线以角速度ω转动，而ω在动坐标系Ox′y′z′上的投影为ωx′、ωy′、ωz′，则它们可用欧拉角及其微商表示如下：ωx′＝sinθsinj＋cosj，ωy′＝ sinθcosj－sinj，ωz′＝cosθ＋。如果已知ψ、θ、j和时间的关系，则可用上式计算ω在动坐标轴上的3个分量；反之，如已知任一瞬时t的ω各个分量，也可利用上式求出ψ、θ、j和时间t的关系，因而也就决定了刚体的运动。上式通常被称为欧拉运动学方程。 原理 欧拉角 Eulerian angles 用来唯一地确定定点转动刚体位置的三个一组独立角参量[1]，由章动角θ、进动角ψ和自转角嗞组成，为L.欧拉首先提出，故得名。对于任何一个参考系，一个刚体的取向，是依照顺序，从这参考系，做三个欧拉角的旋转而设定的。所以，刚体的取向可以用三个基本旋转矩阵来决定。换句话说，任何关于刚体旋转的旋转矩阵是由三个基本旋转矩阵复合而成的。它们有多种取法，下面是常见的一种。 欧拉运动学方程
四元数矩阵转化
//公式都是网上搜罗的，下面这些经过简单的测试，确认可用。 //ps: x,y,z,w 分别是四元素的四个值。稍微修改下就可以用。 // 由旋转矩阵创建四元数 inline CQuaternion(const_Matrix4& m) { float tr, s, q[4]; int i, j, k; int nxt[3] = {1, 2, 0 }; // 计算矩阵轨迹 tr = m._11 + m._22 + m._33; // 检查矩阵轨迹是正还是负 if(tr>0.0f) { s = sqrt(tr + 1.0f); this->w = s / 2.0f; s = 0.5f / s; this->x = (m._23 - m._32) * s; this->y = (m._31 - m._13) * s; this->z = (m._12 - m._21) * s; } else { // 轨迹是负 // 寻找m11 m22 m33中的最大分量 i = 0; if(m.m[1][1]>m.m[0][0]) i = 1; if(m.m[2][2]>m.m[i][i]) i = 2; j = nxt[i]; k = nxt[j]; s = sqrt((m.m[i][i] - (m.m[j][j] + m.m[k][k])) + 1.0f); q[i] = s * 0.5f; if( s!= 0.0f) s = 0.5f / s; q[3] = (m.m[j][k] - m.m[k][j]) * s; q[j] = (m.m[i][j] - m.m[j][i]) * s; q[k] = (m.m[i][k] - m.m[k][i]) * s; this->x = q[0]; this->y = q[1]; this->z = q[2]; this->w = q[3]; }
四元数,欧拉角,矩阵的相互转换
四元数，欧拉角，矩阵的相互转换 网上太多的将转换的了，翻来覆去转载没有意义。。奉上源码，TC下直接编译即可～～在附上编译好了的exe可以直接下载运行～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～不华丽的分割～～以下是源码～～～～～～～～～～～～～～～～～～～～～～ /* 输入欧拉角，能看到四元数，以及再转换回去成欧拉角Yaw范围（-180~180）Pitch范围（-90~90）Roll范围（-180~180）*/ #include "stdio.h"#include "math.h"#include "conio.h"main(){float theta_z , theta_y ,theta_x ;float cos_z_2;float cos_y_2;float cos_x_2;float sin_z_2;float sin_y_2;float sin_x_2;float Pitch;float Roll;float Yaw;float Q[4];float T[3][3];do{printf("/nYaw = ");scanf("%f",&theta_z);printf("/nPitch = ");scanf("%f",&theta_y);printf("/nRoll = ");scanf("%f",&theta_x);theta_z = theta_z*3.1416/180;theta_y = theta_y*3.1416/180;theta_x = theta_x*3.1416/180;cos_z_2 = cos(0.5*theta_z);cos_y_2
四元素与欧拉角
1.欧拉角 在四元数出现之前先看下欧拉角： 对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。为了后面的角度不混乱，我们要先区分参考系和坐标系的概念。 参考系即为大地参考系，是静止不动的。而坐标系则固定于四轴飞行器，随着四轴飞行器的旋转而旋转。 按照右图所示。设定xyz-轴为四轴上的参考轴，XYZ-轴则是大地的参考轴。右图即为四轴相对地面进行了一定旋转，xy-平面与XY-平面的相交线为交点线，用英文字母（N）代表。我们可以这样定义欧拉角： α是x-轴与交点线的夹角 β是z-轴与Z-轴的夹角 γ是交点线与X-轴的夹角 这样我们就可以用三个欧拉角：(α，β，γ)其取值为0-360来描述四轴飞行器相对于大地的参考系的姿态角度了。 三个欧拉角：(α，β，γ)。蓝色的轴是xyz-轴，红色的轴是XYZ-坐标轴。绿色的线是交点线(N) 。 2.轴角 欧拉角使用roll，pitch，yaw来表示这些分量的旋转值。需要注意的是，这里的旋转是针对大地参考系说的，这意味着第一次的旋转不会影响第二、三次的转轴，简单的说，三角度系统无法表现任意轴的旋转，只要一开始旋转，物体本身就失去了任意轴的自主性，这也就导致了万向节锁（Gimbal Lock）的问题。 什么是Gimbal Lock？ 正如前面所说，因为欧拉描述中针对x,y,z的旋转描述是世界坐标系下的值，所以当任意一轴旋转90°的时候会导致该轴同其他轴重合，此时旋转被重合的轴可能没有任何效果，这就是Gimbal Lock，还有一种是轴角的描述方法，这种方法比欧拉描述要好，它避免了Gimbal Lock，它使用一个3维向量表示转轴和一个角度分量表示绕此转轴的旋转角度，即(x,y,z,angle)，一般表示为(x,y,z,w)或者(v,w)。(x,y,z)为旋转轴，w为旋转角度。但这种描述法却不适合插值。
四元数与欧拉角之间的转换
四元数与欧拉角之间的转换 在3D图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。本文主要归纳了两种表达方式的转换，计算公式采用3D笛卡尔坐标系： 图1 3D Cartesian coordinate System (from wikipedia) 定义分别为绕Z轴、Y轴、X轴的旋转角度，如果用Tait-Bryan angle表示，分别为Yaw、Pitch、Roll。 图2 Tait-Bryan angles (from wikipedia) 一、四元数的定义 通过旋转轴和绕该轴旋转的角度可以构造一个四元数：
其中是绕旋转轴旋转的角度，为旋转轴在x,y,z方向的分量（由此确定了旋转轴）。 二、欧拉角到四元数的转换 三、四元数到欧拉角的转换 arctan和arcsin的结果是，这并不能覆盖所有朝向(对于角的取值范围已经满足)，因此需要用atan2来代替arctan。 四、在其他坐标系下使用 在其他坐标系下，需根据坐标轴的定义，调整一下以上公式。如在Direct3D中，笛卡尔坐标系的X轴变为Z轴，Y轴变为X轴，Z轴变为Y轴（无需考虑方向）。
五、示例代码 https://www.wendangku.net/doc/3517804301.html,/Files/heath/Euler2Quaternion.rar Demo渲染两个模型，左边使用欧拉角，右边使用四元数，方向键Up、Left、Right旋转模型。 参考文献： [1] https://www.wendangku.net/doc/3517804301.html,/wiki/Conversion_between_quaternions_and _Euler_angles [2] Ken Shoemake, Animating Rotation with Quaternion Curves, 1985
四元数法VS旋转矩阵法的性能比较
探讨：物体绕任意向量的旋转-四元数法VS.旋转矩阵法的性能比较 3D空间中的旋转可用旋转矩阵、欧拉角或四元数等形式来表示，他们不过都是数学工具，其中在绕任意向量的旋转方面，旋转矩阵和四元数两种工具用的较多，欧拉角由于存在万向节死锁等问题，使用存在限制。 （本文假设坐标系为左手坐标系中，旋转方向为顺时针。） 所求问题： 给定任意单位轴q(q1,q2,q3)(向量),求向量p(x,y,z)(或点p)饶q旋转theta角度的变换后的新向量p'(或点p')： 1.用四元数工具： ------------------------------------------------------------------------- 结论：构造四元数变换p'= q*p*q-1，（p,q是由向量p,q扩展成的四元数）。那么，p'转换至对应的向量(或点)就是变换后的新向量p'(或点p')。 其中，p',q,p,q-1均为四元数。q由向量q扩展,为q=(cos(theta/2),sin(theta/2)*q)，p由向量p扩展,为p=(0,x,y,z),q-1为q的逆，因为q为单位四元数，所以 q-1=q*=(cos(theta/2),-sin(theta/2)*q)。 ------------------------------------------------------------------------- （这个结论的证明过程可以在网上找到。这里略去。） 下面看其时间复杂度： 首先有个三角函数的计算时间，这个可以预先计算好，花费时间不计。考虑n个四元数相乘需进行4*4*(n-1)=16*(n-1)次乘法，15*(n-1)次加法，因为加法化费时间较少，这里仅考虑乘法。这里涉及到三个四元数的乘法，设一次乘法的时间为T,故花费16*2=32T 2.旋转矩阵工具： ------------------------------------------------------------------------- 结论：构造旋转矩阵变换Trot,则变换后的新向量p'(或点p')为p'= p*Trot 其中，p'（x',y',z',1）,p(x,y,z,1)为向量p'，p的4D齐次坐标表示，Trot = |t*q1*q1 + c, t*q1*q2 + s*q3, t*q1*q3 - s*q2, 0| |t*xq1*q2 - s*q3, t*q2*q2 + c, t*q2*q3 + s*q1, 0| |t*q1*q3 + s*q2, t*q2*q3 - s*q1, t*q3*q3 + c, 0| |0, 0, 0, 1| c=cos(theta), s=sin(theta),t=1-c. ------------------------------------------------------------------------- （这个结论的证明过程可以在网上找到。这里略去。） 下面看其时间复杂度： 三角函数的计算时间不计。矩阵本身的元素乘法主要是计算t*x和s*x之类，需进行12+3=15次乘法。两个矩阵相乘的需进行n*n*n次乘法,这里n=4，所以花费4*4*4=64次乘法时间,
四元数转化成欧拉角
四元数转化成欧拉角 笔者：奋斗 修改时间：2015.11.23 一． 姿态解算（以匿名版程序为例） 首先，程序中一般用了两种求解姿态的方法，一种为欧拉角法，一种为四元数法。由四元数法将MPU6050的值融合成四元，再由欧拉角法把四元转换成欧拉角送入串级PID 控制器。 （1）欧拉角法静止状态，或者总加速度只是稍微大于g 时，由加计算出的值比较准确。使用欧拉角表示姿态，令Φ,θ和Φ代表ZYX 欧拉角，分别称为偏航角、俯仰角和横滚角。载体坐标系下的 加 速 度(a x B,a y B ,a z B )和参考坐标系下的加速度(a x N, a y N, a z N)之间的关系可表示为(1)。其中 c 和 s 分别代表 cos 和 sin 。 axB,ayB,azB 就是mpu 读出来的三个值。 这个矩阵就是三个旋转矩阵相乘得到的，因为矩阵的乘法可以表示旋转。 axB c c c s s axN ayB c s s s c c c s s s s c ayN azB s s c s c s c c s s c c azN θψθψθφψφθψφψφθψφθφψφθψ φψφθψφθ-????????????=-++????????????+-+?????? (1) 飞行器处于静止状态，此时参考系下的加速度等于重力加速度，即 00xN yN zN a a g a ????????=????????????（2） 把（2）代入（1）可以解得 arctg θ=（3） yB zB a arctg a φ??= ??? （4） 即为初始俯仰角和横滚角，通过加速度计得到载体坐标系下的加速度即可将其解出，偏航角可以通过电子罗盘求出。
MPU6050 四元数 欧拉角 程序
#include "sys.h" #include "usart.h" #include "delay.h" #include "chinese.h" #include "lcd.h" #include "IIC.h" #include "inv_mpu.h" #include "inv_mpu_dmp_motion_driver.h" #include"upload.h" #include"math.h" static signed char gyro_orientation[9] = {-1, 0, 0, 0,-1, 0, 0, 0, 1}; #define q30 1073741824.0f float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; char num[50]; float Pitch,Roll,Yaw; unsigned long sensor_timestamp; short gyro[3], accel[3], sensors; unsigned char more; long quat[4]; int main(void) { // u16 i; int result; Stm32_Clock_Init(9);//系统时钟设置 delay_init(72); //延时初始化 uart_init(72,9600); //串口1初始化 LCD_Init(); POINT_COLOR=RED; i2cInit(); // result = mpu_init(); // if(!result) { // PrintChar("mpu initialization complete......\n "); //mpu_set_sensor // if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); // PrintChar("mpu_set_sensor complete ......\n");
四元数姿态解析x
这个程序得到了四元数，怎么进一步计算姿态YAW，ROLL，PITCH？ //---------------------------------------------------------------------------------------------------- // Header files #include "AHRS.h" #include
Unity欧拉角与四元数的关系
Unity 3D中欧拉角与四元数关系 在Unity 3D游戏引擎中，物体的方位是用四元数的形式来存储的，但界面上给用户呈现的却是欧拉角。用户设置的欧拉角会先转化为四元数，最终转化为新的欧拉角，而每一个欧拉角都可以用两个四元数来表示，这样就会使问题变得非常复杂。这里给出了欧拉角的两种约定和欧拉角与四元数的数学表达式，通过对欧拉角和四元数在Unity3D游戏引擎中的实例应用的分析研究，将两者之间的转换关系做出了详细的说明。 在Unity3D 游戏引擎中，用方位来表征物体的旋转姿态，这是一个状态变量，方位可以采用旋转矩阵、欧拉角和四元数三种方法来描述。在引擎中采用四元数来定义方位，但四元数不直观，设置困难，因此一般给用户呈现的是欧拉角。由于欧拉角可以任意设置，理论上同一个方位可以用无数个欧拉角来表示，也可以用两个四元数来表示。但Unity 3D 引擎只用一个四元数来定义方位，这就造成了四元数与欧拉角一对多(1:n)的问题。采用四元数定义物体方位又带来另外一个问题，就是同一个方位可以用两个四元数来表示，这又形成了1:2 的问题。因此同一个方位对应两个四元数和无数个欧拉角。用户为物体设置好的欧拉角会转化为系统内部的四元数，由系统内部的四元数又可以转化成一个新的欧拉角。因此转换过程是：用户设置欧拉角→系统内部四元数→新欧拉角。 虽然是同一个方位，但用户设置的欧拉角和系统生成的欧拉角在数据表现上并不相同，这会给学习Unity 3D 游戏引擎带来巨大障碍，这里从欧拉角和四元数的数学定义和转换公式入手，详细阐明其在Unity 3D 中的基本原理，并通过具体实例讲解了两者的应用过程。 欧拉角的概念 欧拉角是用来定义方位的一种表示，它有两种约定分别是“heading-pitch-bank”约定和“roll -pitch-yaw”约定。“heading-pitch-bank”约定是首先把物体坐标系和惯性坐标系对齐，物体旋转顺序为：y → x →z。heading 是绕竖轴方向旋转，在Unity 3D 中y 轴竖直向上，因此heading 是绕物体坐标系的y 轴旋转。pitch是绕物体坐标系的x 轴旋转，bank 是绕物体坐标系的z 轴旋转，如下图所示。
捷联系统的四元数法姿态算法
捷联系统的四元数法姿态算法 算法输入：物体的初始姿态，3轴陀螺仪不同时刻的Yaw，Pitch，Roll的角速度；算法输出：物体的当前姿态。 具体算法： 1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A 2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(Yaw,Pitch,Roll)的变化量，命名为欧拉角b 3. b是Tait-Bryan angle定义的欧拉角，将其转为四元数B 4. A=A×B，做四元数乘法，即可得到当前姿态对应的新的四元数A 5.重复2～4部，即可连续更新姿态 6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a，就可以以直观的形式查看当前姿态 核心算法1，欧拉角转四元数 void Quaternion::FromEulerAngle(const EulerAngle &ea) { float fCosHRoll = cos(ea.fRoll * .5f); float fSinHRoll = sin(ea.fRoll * .5f); float fCosHPitch = cos(ea.fPitch * .5f); float fSinHPitch = sin(ea.fPitch * .5f); float fCosHYaw = cos(ea.fYaw * .5f); float fSinHYaw = sin(ea.fYaw * .5f); w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw; x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw; y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw; z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw; } 核心算法2，四元数转欧拉角 EulerAngle Quaternion::ToEulerAngle() const { EulerAngle ea; ea.fRoll = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x)); ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f)); ea.fYaw = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y)); return ea; }
用四元数法的捷联惯性导航姿态解算程序
用四元数法的捷联惯性导航姿态解算程序 close all; clear all; %重力产生的加速度矢量 g=9.80142;%单位9.8m/s^2 G=[0,0,-g]'; %****************************读入数据 %**********读入陀螺仪的数据 gyro_x=load('gyrox.txt'); gyro_y=load('gyroy.txt'); gyro_z=load('gyroz.txt'); %****************读入加速度计的数据************** %acc_rate=3/1024; acc_x =load('acceleratex.txt'); acc_y =load('acceleratey.txt'); acc_z=load('acceleratez.txt'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %加速度数字转换为模拟电压 data_acc=[acc_x;acc_y;acc_z]; data_acc=data_acc/1024*3 %将数据转换为相应的加速度值 % %********************************************************* %加速度计三个轴向的零点电压 %zero_ax=? %zero_ay=? %zero_az=? %加速度计三个轴向的电压/加速度比值
%rate_ax=? %单位是m/s^2/V %rate_ay=? %rate_az=? %acc_x=acc_x*acc_rate; %acc_y=acc_y*acc_rate; %acc_z=acc_z*acc_rate; aver_acc_x=mean(acc_x) aver_acc_y=mean(acc_y) aver_acc_z=mean(acc_z) %采样时间 dtime=0.01; tm=0:dtime:0.01* (size(gyro_x,2)-1); %个数num n_point=size(gyro_x,2); %图1 figure plot(tm,data_acc(1,:),'-',tm,data_acc(2,:),'.',tm,data_acc(3,:),'-.'); title('加速度计的采样曲线'); legend('x_ACC','Y_ACC','Z_ACC'); xlabel('Time / (10ms)'); ylabel('Accelerate/ (m/s'')'); grid on; %plot(tm,acc_x,'-',tm,acc_y,'.',tm,acc_z,'-.'); %title('加速度的计的采样曲线'): %对采样曲线进行低通滤波 a=[1,2,4,2,1]; %gyro_x=filter(a/sum(a),1,gyro_x); %gyro_y=filter(a/sum(a),1,gyro_y); %gyro_z=filter(a/sum(a),1,gyro_z);
四元数
[1]四元数是什么鬼?她和欧拉角又是什么暧昧关系? [2]陀螺仪和加速度计的读数和四元数怎么建立关系? [3]欧拉角的"gimal lock"?她和欧拉角不合适表示姿态有什么关系? [4]姿态解算的方法:互补滤波,卡尔曼滤波,梯度下降,DCM都是啥? [5]如何真正通过代码实现姿态结算? 下面逐个讲讲 [1]四元数是什么鬼?她和欧拉角又是什么暧昧关系? 四元数,百度搜一下一大片,可是我就是不理解到底和姿态有什么关系? 可以把四元数理解为旋转轴+旋转角的描述方法的优化版.用一个3维向量表示转轴(暂时理解为四元数的x,y,z,表示的向量),一个角度表示绕此转轴的旋转角度(暂时理解为四元数的w). 下面举例子说明下，有点长,配了些图片方便理解,跟着文章一步步来,就能理解 假设你一个人在野外，饿了几天饥肠辘辘。突然有只野鸡飞出来，一头撞在了你身边的大树上，挂了。于是你感谢上苍的恩赐。。。有点扯远了。之后你生起了火要烤了那只野鸡。可是怎么烤呢？你找了根木棒，削尖了想插进野鸡里，首先为了转起来顺畅棒子要穿过鸡的重心。那么问题来了。这个棒要从哪个角度插进去呢？，为了方便描述，我们把鸡按照活着时候的样子摆好，并且鸡头对着正东方向。我们把重心当做坐标原点，重心到鸡头方向当
做x正方向（正东），重心到左翅膀方向为y正方向（正北），重心到鸡背方向为z正方向。这个样子就是我们的初始姿态（和世界坐标系重合）。如下图所示：绿色箭头为y轴正方向，红色箭头为x轴正方向，蓝色为z轴正方向。（之前写的帖子有错误，现已改正，详见9楼的回复）大的坐标为世界坐标系。绿色指向正北，红色指向正东，蓝色指向天空。小的坐标为野鸡的姿态，红色是鸡头，绿色是左翅膀方向，蓝色是鸡背方向。本文只需要关注两个坐标系之间的旋转变化即可，请无视两个坐标系之间的距离变化。 不过一般我们都喜欢从菊花方向插入，那这个方向单位化后就是（-1,0,0）。然后我们把棒子和鸡放上烤架，准备转起来烤鸡。那么问题又来了，往哪个方向转呢？如果从鸡屁股的方向看，你按逆时针方向把鸡转动了90度。现在鸡背朝北，鸡头仍旧朝东。如下图所示：大的坐标（世界坐标）不变。小坐标系和上图相比绕着红轴的负方向旋转了90度。