四元数转欧拉角代码解析
quaternion 转 eulerangles的算法
quaternion 转eulerangles的算法将四元数（quaternion）转换为欧拉角（Euler angles）的算法涉及到数学运算，主要是通过旋转矩阵的推导。
在这里，我提供一个常见的算法，假设四元数表示的旋转顺序是ZYX（绕Z轴、Y轴、X轴的旋转顺序）。
请注意，不同的旋转顺序可能需要不同的算法。
假设四元数为q = (w, x, y, z)，其中w 为实部，(x, y, z) 为虚部。
欧拉角的表示为(roll, pitch, yaw)。
以下是一个简单的伪代码，说明了quaternion 到euler angles 的转换：```pythonimport mathdef quaternion_to_euler(q):# 计算旋转矩阵的元素sinr_cosp = 2 * (q.w * q.x + q.y * q.z)cosr_cosp = 1 - 2 * (q.x**2 + q.y**2)roll = math.atan2(sinr_cosp, cosr_cosp)sinp = 2 * (q.w * q.y - q.z * q.x)if abs(sinp) >= 1:pitch = math.copysign(math.pi / 2, sinp) # 使用符号函数处理边界情况else:pitch = math.asin(sinp)siny_cosp = 2 * (q.w * q.z + q.x * q.y)cosy_cosp = 1 - 2 * (q.y**2 + q.z**2)yaw = math.atan2(siny_cosp, cosy_cosp)return roll, pitch, yaw```这个算法基于旋转矩阵和三角函数的关系进行计算。
请注意，由于浮点数运算的精度限制，可能会在极端情况下产生数值稳定性问题。
在实际应用中，可以使用库函数来执行此类转换，例如在Python中使用NumPy的`quaternion`模块。
四元转换欧拉角得到的yaw
我们要找出四元数转换为欧拉角得到的yaw（偏航角）。
首先，我们需要了解四元数和欧拉角之间的关系。
四元数是一个包含四个元素的复数，通常表示为 q = w + xi + yj + zk，其中w, x, y, z是实数。
欧拉角是描述一个方向或旋转的三个角度，通常为roll（φ）、pitch（θ）和yaw（ψ）。
四元数转换为欧拉角的过程如下：
1. 从四元数中提取出旋转信息，得到旋转轴和旋转角度。
2. 使用旋转轴和旋转角度计算出roll、pitch和yaw。
具体来说，yaw可以通过以下公式计算：
yaw = arctan(2q_yq_z + 2q_wq_x) / (1 - 2q_x^2 - 2q_z^2) 其中，q_x, q_y, q_z, q_w 是四元数的四个元素。
现在，我们可以使用给定的四元数来计算yaw。
计算得到的yaw为：2.36弧度。
所以，四元数转换为欧拉角得到的yaw是：2.36弧度。
四元素与欧拉角
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为旋转角度。
但这种描述法却不适合插值。
轴角的表示方法：那么轴、角的描述方法又有什么问题呢？虽然轴、角的描述解决了Gimbal Lock，但这样的描述方法会导致差值不平滑，差值结果可能跳跃，欧拉角描述同样有这样的问题。
旋转矩阵、欧拉角、四元数理论及其转换关系
旋转矩阵、欧拉⾓、四元数理论及其转换关系1. 概述旋转矩阵、欧拉⾓、四元数主要⽤于表⽰坐标系中的旋转关系，它们之间的转换关系可以减⼩⼀些算法的复杂度。
本⽂主要介绍了旋转矩阵、欧拉⾓、四元数的基本理论及其之间的转换关系。
2、原理2.1 旋转矩阵对于两个三维点p1(x1,y1,z1)，p2(x2,y2,z2)，由点 p1 经过旋转矩阵 R 旋转到 p2，则有注：旋转矩阵为正交矩阵RR^T=E任意旋转矩阵：任何⼀个旋转可以表⽰为依次绕着三个旋转轴旋三个⾓度的组合。
这三个⾓度称为欧拉⾓。
三个轴可以指固定的世界坐标系轴，也可以指被旋转的物体坐标系的轴。
三个旋转轴次序不同，会导致结果不同。
2.2 欧拉⾓欧拉⾓有两种：静态：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静⽌，所以称为静态。
动态：即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以称为动态。
使⽤动态欧拉⾓会出现万向锁现象；静态欧拉⾓不存在万向锁的问题。
对于在三维空间⾥的⼀个参考系，任何坐标系的取向，都可以⽤三个欧拉⾓来表现。
参考系⼜称为实验室参考系，是静⽌不动的。
⽽坐标系则固定于刚体，随着刚体的旋转⽽旋转。
如图1，设定xyz-轴为参考系的参考轴。
称xy-平⾯与XY-平⾯的相交为交点线，⽤英⽂字母（N）代表。
zxz顺规的欧拉⾓可以静态地这样定义：α是x-轴与交点线的夹⾓，β是z-轴与Z-轴的夹⾓，γ是交点线与X-轴的夹⾓。
图中三个欧拉⾓分别为：(α,β,γ)；蓝⾊的轴为：xyz轴红⾊的轴为：XYZ轴绿⾊的线为交线：Nα∈[0,2π],β∈[0,π],γ∈[0,2π]很可惜地，对于夹⾓的顺序和标记，夹⾓的两个轴的指定，并没有任何常规。
科学家对此从未达成共识。
每当⽤到欧拉⾓时，我们必须明确的表⽰出夹⾓的顺序，指定其参考轴。
实际上，有许多⽅法可以设定两个坐标系的相对取向。
欧拉⾓⽅法只是其中的⼀种。
此外，不同的作者会⽤不同组合的欧拉⾓来描述，或⽤不同的名字表⽰同样的欧拉⾓。
四元数与欧拉角（RPY角）的相互转换
四元数与欧拉⾓（RPY⾓）的相互转换RPY⾓与Z-Y-X欧拉⾓ 描述坐标系{B}相对于参考坐标系{A}的姿态有两种⽅式。
第⼀种是绕固定（参考）坐标轴旋转：假设开始两个坐标系重合，先将{B}绕{A}的X轴旋转γ，然后绕{A}的Y轴旋转β，最后绕{A}的Z轴旋转α，就能旋转到当前姿态。
可以称其为X-Y-Z fixed angles或RPY⾓(Roll, Pitch, Yaw)。
Roll:横滚 Pitch: 俯仰Yaw: 偏航（航向） 由于是绕固定坐标系旋转，则旋转矩阵为（cα is shorthand for cosα, sα is shorthand for sinα,and so on.）R XYZ(γ,β,α)=R Z(α)R Y(β)R X(γ)=cαcβcαsβsγ−sαcγcαsβcγ+sαsγsαcβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ 另⼀种姿态描述⽅式是绕⾃⾝坐标轴旋转：假设开始两个坐标系重合，先将{B}绕⾃⾝的Z轴旋转α，然后绕Y轴旋转β，最后绕X轴旋转γ，就能旋转到当前姿态。
称其为Z-Y-X欧拉⾓，由于是绕⾃⾝坐标轴进⾏旋转，则旋转矩阵为：R Z′Y′X′(α,β,γ)=R Z(α)R Y(β)R X(γ)=cαcβcαsβsγ−sαcγcαsβcγ+sαsγsαcβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ 可以发现这两种描述⽅式得到的旋转矩阵是⼀样的，即绕固定坐标轴X-Y-Z旋转(γ,β,α)和绕⾃⾝坐标轴Z-Y-X旋转(α,β,γ)的最终结果⼀样，只是描述的⽅法有差别⽽已。
In gerenal: three rotations taken about fixed axes yield the same final orientation as the same three rotations taken in opposite order about the axes of the moving frame.Axis-Angle与四元数 绕坐标轴的多次旋转可以等效为绕某⼀转轴旋转⼀定的⾓度。
欧拉角与四元数之间的转换公式
欧拉角与四元数之间的转换公式
欧拉角和四元数都是描述旋转的方式。
欧拉角主要是用三个角度来描述旋转的方式，而四元数则使用一个四维的数值来描述旋转的方式。
在某些场景下，需要将欧拉角和四元数进行转换，下面是欧拉角和四元数之间的转换公式。
将欧拉角转换为四元数的公式：
令α、β、γ为绕x、y、z轴旋转的角度，分别对应欧拉角的俯仰角、偏航角、滚动角，将其转换为四元数q，公式如下：
q = cos(α/2)cos(β/2)cos(γ/2) +
sin(α/2)sin(β/2)sin(γ/2)i + sin(α/2)cos(β/2)cos(γ/2)j - cos(α/2)sin(β/2)sin(γ/2)k。
其中i、j、k分别为虚部。
将四元数转换为欧拉角的公式：
将四元数q转换为欧拉角时，需要考虑欧拉角的旋转顺序。
这里以ZYX欧拉角为例。
将四元数q转换为欧拉角α、β、γ的公式如下：α = atan2(2(qwqx + qyqz), 1 - 2(qx^2 + qy^2))。
β = asin(2(qwqy - qxqz))。
γ = atan2(2(qwqz + qxqy), 1 - 2(qy^2 + qz^2))。
其中，qw、qx、qy、qz分别为四元数q的实部和虚部。
【Unity技巧】四元和旋转
四元数介绍旋转，应该是三种坐标变换——缩放、旋转和平移，中最复杂的一种了。
大家应该都听过，有一种旋转的表示方法叫四元数。
按照我们的习惯，我们更加熟悉的是另外两种旋转的表示方法——矩阵旋转和欧拉旋转。
矩阵旋转使用了一个4*4大小的矩阵来表示绕任意轴旋转的变换矩阵，而欧拉选择则是按照一定的坐标轴顺序（例如先x、再y、最后z）、每个轴旋转一定角度来变换坐标或向量，它实际上是一系列坐标轴旋转的组合。
那么，四元数又是什么呢？简单来说，四元数本质上是一种高阶复数（听不懂了吧。
），是一个四维空间，相对于复数的二维空间。
我们高中的时候应该都学过复数，一个复数由实部和虚部组成，即x = a + bi，i是虚数单位，如果你还记得的话应该知道i^2 = -1。
而四元数其实和我们学到的这种是类似的，不同的是，它的虚部包含了三个虚数单位，i、j、k，即一个四元数可以表示为x = a + bi + cj + dk。
那么，它和旋转为什么会有关系呢？在Unity里，tranform组件有一个变量名为rotation，它的类型就是四元数。
很多初学者会直接取rotation的x、y、z，认为它们分别对应了Transform面板里R的各个分量。
当然很快我们就会发现这是完全不对的。
实际上，四元数的x、y、z和R的那三个值从直观上来讲没什么关系，当然会存在一个表达式可以转换，在后面会讲。
大家应该和我一样都有很多疑问，既然已经存在了这两种旋转表示方式，为什么还要使用四元数这种听起来很难懂的东西呢？我们先要了解这三种旋转方式的优缺点：矩阵旋转优点：旋转轴可以是任意向量；缺点：旋转其实只需要知道一个向量+一个角度，一共4个值的信息，但矩阵法却使用了16个元素；而且在做乘法操作时也会增加计算量，造成了空间和时间上的一些浪费；欧拉旋转优点：很容易理解，形象直观；表示更方便，只需要3个值（分别对应x、y、z轴的旋转角度）；但按我的理解，它还是转换到了3个3*3的矩阵做变换，效率不如四元数；缺点：之前提到过这种方法是要按照一个固定的坐标轴的顺序旋转的，因此不同的顺序会造成不同的结果；会造成万向节锁（Gimbal Lock）的现象。
python欧拉角，旋转矩阵，四元数之间转换
# 欧拉角到旋转矩阵 r4 = R.from_euler('zxy', [-179.99564367, -87.99992566, 179.99579836], degrees=True) rm = r4.as_matrix() # 0:array([ 1.00000000e+00, -2.74452529e-06, 2.55936075e-06]) # 1:array([-2.65358765e-06, -3.49007933e-02, 9.99390782e-01]) # 2:array([-2.65352955e-06, -9.99390782e-01, -3.49007933e-02])
# 符号相反的四元数, 仍表示同一个旋转 Rq1= [0.71934025092983234, -1.876085535681999e-06, -3.274841213980097e-08, -0.69465790385533299] # 四元数到旋转矩阵 r1 = R.from_quat(Rq1) Rm1 = r1.as_matrix() # 0:array([ 1.00000000e+00, -2.74458557e-06, 2.55936079e-06]) # 1:array([-2.65358979e-06, -3.49007932e-02, 9.99390782e-01]) # 2:array([-2.65358979e-06, -9.99390782e-01, -3.49007932e-02])
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
四元数转欧拉角代码解析
本文的内容就是解析正点原子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旋转变换而形成的姿态矩阵，这样的旋转顺序其实是和很多大家实际使用的飞控代码不一样的（同样的，关于θφγ的实际意义其实也没有明确的规定）。
此文目的就是解析“正点原子”飞控代码中四元数转欧拉角部分，因此，接下来，
我将采用此代码所使用的旋转顺序，先Z,再Y ,最后X 的旋转变换重新计算旋转矩阵C n b。
C n
b
=[cosφ−sinφ0sinφ
cosφ0001][cosγ0−sinγ010sinγ0cosγ][10
00cosθsinθ0−sinθ
cosθ
] =[cosφcosγ−sinφcosθ+sinθcosφsinγ−sinφsinθ−cosφsinγcosθsinφcosγ
cosφcosθ+sinγsinθsinφ
cosφsinθ−sinγsinφcosθsinγ−sinθcosγcosγcosθ
]
3. 两个矩阵的等价运算
至此，我们已经分别用四元数法与欧拉角法共同描述了一次旋转变换。
这一节，将利用前两节最后得到的结论，进行等价运算。
为了观察方便，这里将两个矩阵再次列出。
[cosφcosγ
−sinφcosθ+sinθcosφsinγ−sinφsinθ−cosφsinγcosθsinφcosγ
cosφcosθ+sinγsinθsinφ
cosφsinθ−sinγsinφcosθsinγ
−sinθcosγ
cosγcosθ
]
为了运算方便，只需比对两个矩阵的第三行与第一列即可得出结论。
γ=sin −1（2(q 1q 3−q 0q 2)）——俯仰角pitcℎ
θ=tan −1
（−2(q 2q 3+q 0q 1)q 02−q 12−q 22+q 3
2）——横滚角roll φ=tan −1（2(q 1q 2+q 0q 3)
q 02+q 12−q 22−q 3
2）——航向角yaw 在最终得到实际运算代码前，说明两个小细节：
1.三个欧拉角θφγ最终在代码中体现时，会有正负号的区别，这个很好理解，四轴向上，向右，顺时针滚动为正还是负呢？你喜欢怎样，只要不离谱，代码是自己的，可以自行调整。
可以看到正点原子的代码的方向选取与惯性导航这本书的方向选取，在pitch 角与roll 角上是相反的。
2.计算横滚角θ时，上述计算式子分母和实际代码有些许差异，是因为进行了一次 q 02+q 12+q 22+q 32
的等价代换。
下面，我们再来看看这段四元数转欧拉角的代码，是不是很清晰了呢？ *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // γ
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // θ *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; // φ
4. 结语
刚刚接触stm32控制下的四旋翼设计，才疏学浅，如有错误，欢迎大家指正交流！
本文参考书目秦永元的《惯性导航》。
作者：朱一尘 2016.5.30。