四元数转欧拉角代码解析
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])
四旋翼飞行器 四元数和欧拉角的关系 与 姿态解算
四旋翼飞行器四元数和欧拉角的关系与姿态解算下载提示：该文档是本店铺精心编制而成的，希望大家下载后，能够帮助大家解决实际问题。
文档下载后可定制修改，请根据实际需要进行调整和使用，谢谢！本店铺为大家提供各种类型的实用资料，如教育随笔、日记赏析、句子摘抄、古诗大全、经典美文、话题作文、工作总结、词语解析、文案摘录、其他资料等等，想了解不同资料格式和写法，敬请关注！Download tips: This document is carefully compiled by this editor. I hope that after you download it, it can help you solve practical problems. The document can be customized and modified after downloading, please adjust and use it according to actual needs, thank you! In addition, this shop provides you with various types of practical materials, such as educational essays, diary appreciation, sentence excerpts, ancient poems, classic articles, topic composition, work summary, word parsing, copy excerpts, other materials and so on, want to know different data formats and writing methods, please pay attention!四旋翼飞行器：四元数与欧拉角的关系与姿态解算1. 引言四旋翼飞行器是一种常见的无人机类型，其姿态（即飞行器的方向）通常由欧拉角或四元数来描述。
四元数与欧拉角间的转换
四元数与欧拉角之间的转换早在1843年哈米尔顿就提出了四元数的基本概念[138]，但当时仅停留在理论概念的讨论上。
20世纪以来，随着航天、航空工业的发展，四元数得到了广泛地实际应用[139-142]。
四元数的定义为[143]4q ⎛⎫≡ ⎪⎝⎭Q q（0-1）式中：123(,,)sin 2T q q q α≡=Q e （0-2）4cos2q α= （0-3）其中，e 是沿旋转轴的单位向量，α是旋转角度。
四元数满足约束条件1T =q q（0-4）四元数与姿态矩阵具有如下关系：224334222212341234132422221234123423142222132423141234()22[]()()2()2()2()2()2()2()T T q I q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q ⨯=-+-⨯=Ξψ⎛⎫--++-⎪=--+-++ ⎪⎪+---++⎝⎭A Q QQ Q q q q（0-5）其中，33⨯I 是33⨯单位矩阵，433[]()T q ⨯+⨯⎛⎫Ξ≡ ⎪-⎝⎭I Q q Q（0-6）433[]()T q ⨯-⨯⎛⎫ψ≡ ⎪-⎝⎭I Q q Q（0-7）3231210[]00q q q q q q -⎛⎫ ⎪⨯=- ⎪ ⎪-⎝⎭Q （0-8）地面坐标系Axyz 和弹体坐标系O 1x 1y 1z 1的关系可以用下式来表示=b Ar（0-9）其中，b 、r 分别为某向量在弹体坐标系和地面坐标系中的表示形式，A 的表达形式见错误！未找到引用源。
，具体为cos cos sin cos sin sin cos cos sin sin cos cos sin sin cos cos sin sin cos sin sin cos cos sin sin sin sin cos cos ϑψϑϑψϑψγψγϑγϑψγψγϑψγψγϑγϑψγψγ-⎛⎫⎪=-++ ⎪ ⎪+--+⎝⎭A（0-10）比较式（0-5）和式（0-10），得到姿态角的四元数表示131324222211123412123432231422222212342()arctan arctan arcsin arcsin 2()2()arctan arctan q q q q q q q q q q q q q q q q q q q q ψθφ⎧-=-=-⎪--+⎪⎪==+⎨⎪-⎪=-=-⎪-+-+⎩q q q q q A A A A A （0-11）也可以用姿态角来表示四元数q ，即(cossinj)(cossinj)(cos sin j)222222ψψθθφφ=+⊗+⊗+q（0-12）其中，符号⊗表示四元数乘法，其定义如下[144-145]41234123(i j k)(i j k)A A A A B B B B ⊗=+++⊗+++A B44123114322341232143B A A A A B A A A A A A A A B A A A A B ---⎛⎫⎛⎫ ⎪ ⎪- ⎪⎪= ⎪⎪- ⎪ ⎪ ⎪-⎝⎭⎝⎭44112233144132232431421334211243()()i ()j ()kA B A B A B A B A B A B A B A B A B A B A B A B A B A B A B A B =---++-++++-+-++（0-13）其中，4123i j k A A A A =+++A 和4123i j k B B B B +++B =均为四元数。
四元数转化成欧拉角
四元数转化成欧拉角笔者：奋斗 修改时间：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） 即为初始俯仰角和横滚角，通过加速度计得到载体坐标系下的加速度即可将其解出，偏航角可以通过电子罗盘求出。
（2）四元数法（通过处理单位采样时间内的角增量(mpu的陀螺仪得到的就是角增量)，为了避免噪声的微分放大，应该直接用角增量）void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az){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;if(ax*ay*az==0)return;norm = sqrt(ax*ax + ay*ay + az*az); //acc数据归一化ax = ax /norm;ay = ay / norm;az = az / norm;// estimated direction of gravity and flux (v and w)vx = 2*(q1q3 - q0q2); //四元素中xyzvy = 2*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3 ;// error is sum of cross product between reference direction of fields and direction measured by sensorsex = (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;// adjusted gyroscope measurementsgx = gx + Kp*ex + exInt; //将误差PI 后补偿到陀螺仪，即补偿零点漂移 gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt; //这里的gz 由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减// integrate quaternion rate and normalise //四元素的微分方程 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;// normalise quaternionnorm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;//Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yawQ_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitchQ_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll}下面对上面的程序逐条解释。
在matlab和pythontf中的旋转变换（四元数、欧拉角、旋转矩阵等）
在matlab 和pythontf 中的旋转变换（四元数、欧拉⾓、旋转矩阵等）⽬录1. 基本的认识空间中的坐标变换包括平移和旋转。
平移变换较为简单，只需要加上⼀个位置⽮量即可。
旋转变换常见的有三种表⽰⽅式：旋转矩阵、欧拉⾓、四元数。
注：由于博主本⼈知识有限以及篇幅的缘故，博⽂⼗分简略，阅读本篇博客前需要⼀定的知识基础，有问题欢迎⼀起交流讨论。
2. 变换矩阵根据《机器⼈学导论》，我们⽤平移算⼦A P BO 和旋转算⼦A B R 进⾏变换矩阵的计算。
A P14×1=AB RA PBO014×4B P 14×11. 平移算⼦A P BO平移将空间中的⼀个点沿着⼀个已知的⽮量⽅向移动⼀定距离。
A PBO=q x q yq z 3×12. 旋转算⼦A B R通常是绕坐标系的x 、y 、z 轴旋转⼀定⾓度的旋转算⼦，遵循右⼿旋转法则。
例如：绕z 轴旋转θ⾓度的算⼦：A B R=cos θ−sin θ0sin θcos θ13×3具体原理的话可以参考原书，也可以参考，。
3. 欧拉⾓欧拉⾓⽤三个数描述从⼀个坐标系到另⼀个坐标系的变换，每个数分别是绕某⼀个坐标轴转动的⾓度。
⽹上对于欧拉⾓对应坐标轴的名称不是很统⼀，这⾥参考，所指的欧拉⾓是偏航、俯仰和滚转（yaw, pitch and roll ），分别对应z,y,x 轴的转⾓。
通常的欧拉⾓是intrinsic rotations ，⼜称内在旋转，动态旋转等，每次旋转参照的坐标系是旋转后的新坐标系，也可以理解为刚体坐标系。
⽽还有⼀种欧拉⾓是extrinsic rotations ，⼜称固定⾓，外在旋转，静态旋转等，每次旋转参照的坐标系是原始的固定坐标系，两者存在⼀⼀对应的关系，以下摘⾃维基百科：三个基本旋转可以围绕原始坐标系的轴发⽣，它保持不动（extrinsic rotations ，外在旋转），或者围绕旋转坐标系的轴，在每次基本旋转后改变其⽅向（intrinsic rotations ，内在旋转）。
欧拉角与四元数
四元数与旋转一．四元组基础Q(x,y,z,w)，其中x，y，z用来确定旋转轴，w为旋转的角度Q=w+xi+yj+zk，i，j，k为三个虚轴的单位分量I*j=kJ*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-<v1,v2>,w1*v2+w2*v1+v1xv2)( w1+x1i+y1j+z1k)*( w2+x2i+y2j+z2k)=w1*w2-x1*x2-y1*y2-z1*z2+(W1*x2+x1*w2+y1*z2-z1-y2)i+(y1*w2+w1*y2+z1*x2-x1*z2)j+(w1*z2+z1*w2+x1*y2-y1*x2)k对于其中的轴部分，假如v1//v2，则有v1 x v2=0（平行向量的叉乘结果为0）2．四元组的点乘，点乘积为数值：Q1.*Q2=w1*w2+<v1,v2>=w1*w2+x1*x2+y1*y2+z1*z2;3．数乘s为一实数，q为四元组，则有sq=qs4．共轭p=(w,v)，则p*=(w,-v)(pq)*=q*p*N(q)=w2+x2+y2+z2q-1=q*/N(q)--------------- 显然可得qq-1=(1,0)二．使用四元数旋转向量假如有一表示向量的四元组q=(w,v)，对其应用旋转量p后的结果为：q’=pqp-1=(w,v’)从上可以看出，计算的结果q’的实部和q的实部是相等的，并且有N(v)=N(v’)如果N(q)=1，则可以令q=(cosa,usina)，u也为一个单位向量，则q’是q绕u旋转2a个弧度的结果假如S(q)表示q的实部，则有2S(q)=q+q*2S(pqp-1)= pqp-1+( pqp-1)*=pqp*+(pqp*)*=pqp*+pq*p*=p(q+q*)p*=2S(q)（这里由于p是单位四元数，所以有p-1等于p*）欧拉角到四元数的转换定义pitch, yaw, roll分别为绕X轴、Y轴、Z轴的旋转弧度float p = pitch * PIOVER180 / 2.0;float y = yaw * PIOVER180 / 2.0;float r = roll * PIOVER180 / 2.0;float sinp = sin(p);float siny = sin(y);float sinr = sin(r);float cosp = cos(p);float cosy = cos(y);float cosr = cos(r);this->x = sinr * cosp * cosy - cosr * sinp * siny;this->y = cosr * sinp * cosy + sinr * cosp * siny;this->z = cosr * cosp * siny - sinr * sinp * cosy;this->w = cosr * cosp * cosy + sinr * sinp * siny;normalise();三．使用matlab进行相关计算计算两个向量v1和v2之间的旋转量四元数p，使得v1应用p后到达v2假如v1转到v2的旋转轴为v，旋转角为theta，则q=[v*cos(theta/2) sin(theta/2)] Matlab代码：function q=vector2q(v1,v2)%..normalize....len1=sqrt(v1*v1');len2=sqrt(v2*v2');v1=v1/len1;v2=v2/len2;angle=v1*v2';axis=cross(v1,v2);alen=sqrt(axis*axis');axis=axis/alen;t=acos(angle);t=t/2;q(1)=axis(1)*sin(t);q(2)=axis(2)*sin(t);q(3)=axis(3)*sin(t);q(4)=cos(t);end计算出了q之后，可以获得对应的旋转矩阵，旋转矩阵的计算Matlab里面的矩阵是以列为主顺序的function r=q2rot(q)w=q(4);x=q(1);y=q(2);z=q(3);r=zeros(3,3);r(1,1)=1-2*y*y-2*z*z;r(1,2)=2*x*y+2*w*z;r(1,3)=2*x*z-2*w*y;r(2,1)=2*x*y-2*w*z;r(2,2)=1-2*x*x-2*z*z;r(2,3)=2*z*y+2*w*x;r(3,1)=2*x*z+2*w*y;r(3,2)=2*y*z-2*w*x;r(3,3)=1-2*x*x-2*y*y;r=r';end同时，也可以根据四元数来计算欧拉角function R=q2euler(q)w=q(4);x=q(1);y=q(2);z=q(3);t11=2*(w*x+y*z);t12=1-2*(x*x+y*y);R(1)=atan2(t11,t12);t2=2*(w*y-z*x);R(2)=asin(t2);t31=2*(w*z+x*y);t32=1-2*(y*y+z*z);R(3)=atan2(t31,t32);end计算出来的欧拉角rx，ry，rz，分别为绕X轴、Y轴和Z轴的旋转角，假如有：Rotq=q2rot(q)R=q2euler(q)[rotx roty rotz]=Rotation(R)可以发现Rotq==rotz*roty*rotx从这里可以看出，上面使用四元数这样计算出来的旋转矩阵的旋转顺序分别是X轴、Y轴和Z轴的ra=pi/4;qz=[0 0 -sin(ra) cos(ra)] %绕z旋转-90度qy=[0 sin(ra) 0 cos(ra) ] %绕y旋转90度qyz=qmult(qy,qz)r=q2euler(qyz)上面的r得出的结果为r = -1.5708 0.0000 -1.5708也就是说其几何意义变成先绕X轴旋转-90度，再绕Z轴旋转-90度，而根据qy和qz的相乘我们实际进行的操作却是先绕Z轴旋转-90度，再绕Y轴旋转90度，但是结果却是这两种操作等价，这说明由四元数到欧拉角可以有多个解两个四元数，假如它们的方向是相反的，用它们作用于向量得到的新向量的值仍然相等q1=[0.024666 -0.023954 0.504727 0.862594];arm=[-8.881719 6.037597 -2.36776];q2=-q1;rot1=q2rot(q1);rot2=q2rot(q2);v1=rot1*arm'v2=rot2*arm'上面计算出来的v1等于v2四元数的余弦值为它们的内积假如余弦值小于0，则需要将其中的一个取反，因为上面我们知道一个四元数和它的反方向的四元数对一个向量起相同的作用四元数的相乘，代表旋转的累积pq=p*q;rotp=q2rot(p);rotq=q2rot(q);rotpq=q2rot(pq);rotmul=rotp*rotq;这里rotpq与rotmul相等四．OGRE中Quaternion类的几个函数1.四元数到旋转向量void Quaternion::ToRotationMatrix (Matrix3& kRot) const1 - 2*qy2 - 2*qz22*qx*qy - 2*qz*qw2*qx*qz + 2*qy*qw2*qx*qy + 2*qz*qw 1 - 2*qx2 - 2*qz22*qy*qz - 2*qx*qw2*qx*qz - 2*qy*qw2*qy*qz + 2*qx*qw 1 - 2*qx2 - 2*qy22.旋转量到四元数根据1中的表格，有：4 *(1-qx2-qy2-qz2) = 1 + m00 + m11 + m22又qw2=1-qx2-qy2-qz2，可得4 *qw2= 1 + m00 + m11 + m22这里解qw必须保证1 + m00 + m11 + m22>=0，如果不是的话，就构造其他的等式来计算，OGRE中分成两种情况，一种是m00 + m11 + m22>=0，就可以直接先解出qw，否则的采用另外的等式计算3．Local axisVector3 xAixs(void) const;取得旋转矩阵的第一列，旋转矩阵和一个向量相乘的话，第一列的数据均和向量的x分量相乘Vecotr3 yAxis(void) const;取得旋转矩阵的第二列，旋转矩阵和一个向量相乘的话，第二列的数据均和向量的y分量相乘Vecotr3 zAxis(void) const;取得旋转矩阵的第三列，旋转矩阵和一个向量相乘的话，第三列的数据均和向量的z分量相乘。
刚体在三维空间的旋转（关于旋转矩阵、DCM、旋转向量、四元数、欧拉角）
刚体在三维空间的旋转（关于旋转矩阵、DCM、旋转向量、四元数、欧拉角）最近学习了一些关于三维空间旋转相关的知识，借此梳理一下备忘。
三维空间的旋转(3D Rotation)是一个很神奇的东东：如果对某个刚体在三维空间进行任意次的旋转，只要旋转中心保持不变，无论多少次的旋转都可以用绕三维空间中某一个轴的一次旋转来表示。
表示三维空间的旋转有多种互相等价的方式，常见的有旋转矩阵、DCM、旋转向量、四元数、欧拉角等。
本篇文章主要梳理一下这些表示方式及相互转换的方法。
1. 欧拉角(Euler Angle)最直观的表示方式是绕刚体自身的X、Y、Z三个轴分别进行旋转某个角度，这就是所谓的欧拉角(Euler Angle)表示方式。
Euler Angle需要注意的是，欧拉角的表示方式里，yaw、pitch、roll的顺序对旋转的结果是有影响的。
给定一组欧拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的顺序旋转和按照yaw-roll-pitch的顺序旋转，最终刚体的朝向是不同的！换言之，若刚体需要按照两种不同的旋转顺序旋转到相同的朝向，所需要的欧拉角角度值则是不同的！另外需要注意的是，在欧拉角的表示方式里，三个旋转轴一般是随着刚体在运动，即wikipedia中所谓的intrinsic rotation，见下图动画所示(图来自wikipedia)。
相对应的另一种表示方式是，三个旋转轴是固定的，不随刚体旋转而旋转，即extrinsic rotation，这种表示方式在计算机视觉中不是很常用。
欧拉角的表示方式比较直观，但是有几个缺点：(1) 欧拉角的表示方式不唯一。
给定某个起始朝向和目标朝向，即使给定yaw、pitch、roll的顺序，也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。
比如，同样的yaw-pitch-roll顺序，(0,90,0)和(90,90,90)会将刚体转到相同的位置。
threejs 欧拉角和四元数 转换
threejs 欧拉角和四元数转换【知识】三维图形编程中的欧拉角与四元数转换1. 引言在三维图形编程中，欧拉角和四元数是两种常见的旋转表示方法。
它们可以在计算机图形学、游戏开发等领域中用于描述物体的旋转。
本文将介绍欧拉角和四元数的概念和转换方法，以及它们在Three.js中的应用。
2. 欧拉角的定义和转换欧拉角是一种常用的旋转表示方法，它由三个角度组成，分别表示物体绕x、y、z轴旋转的角度。
在Three.js中，欧拉角可以通过Euler 类进行表示和转换。
2.1 欧拉角的定义在Three.js中，欧拉角可以使用以下方式定义：```var euler = new THREE.Euler(x, y, z, order);```其中，x、y、z是绕x、y、z轴旋转的角度，order是旋转的顺序。
常见的旋转顺序有"XYZ"、"YXZ"、"ZXY"等。
2.2 欧拉角的转换在Three.js中，欧拉角可以和旋转矩阵、四元数进行相互转换。
下面是欧拉角和旋转矩阵、四元数的转换方法：- 欧拉角转换为旋转矩阵：```var euler = new THREE.Euler(x, y, z, order);var matrix = newTHREE.Matrix4().makeRotationFromEuler(euler);```- 旋转矩阵转换为欧拉角：```var matrix = new THREE.Matrix4();var euler = new THREE.Euler().setFromRotationMatrix(matrix, order);```- 欧拉角转换为四元数：```var euler = new THREE.Euler(x, y, z, order);var quaternion = new THREE.Quaternion().setFromEuler(euler); ```- 四元数转换为欧拉角：```var quaternion = new THREE.Quaternion();var euler = new THREE.Euler().setFromQuaternion(quaternion, order);```3. 四元数的定义和转换四元数是一种用于表示旋转的数学工具，它由一个实部和三个虚部组成。
四元数转换成欧拉角易语言
四元数转换成欧拉角易语言在易语言中，可以编写一个函数来将四元数转换为欧拉角。
以下是一个简单的示例代码：// 四元数结构体typedef struct {float w, x, y, z;} Quaternion;// 欧拉角结构体typedef struct {float pitch, yaw, roll;} EulerAngle;// 四元数转换为欧拉角函数void QuaternionToEulerAngles(Quaternion quat, EulerAngle *angles) {// 计算方向向量float sinr_cosp = 2 * (quat.w * quat.x + quat.y * quat.z);float cosr_cosp = 1 - 2 * (quat.x * quat.x + quat.y * quat.y);angles->roll = atan2(sinr_cosp, cosr_cosp);float sinp = 2 * (quat.w * quat.y - quat.z * quat.x);if (fabs(sinp) >= 1)angles->pitch = copysign(M_PI / 2, sinp); // 使用符号函数处理边界情况elseangles->pitch = asin(sinp);float siny_cosp = 2 * (quat.w * quat.z + quat.x * quat.y);float cosy_cosp = 1 - 2 * (quat.y * quat.y + quat.z * quat.z);angles->yaw = atan2(siny_cosp, cosy_cosp);}使用这个函数，你可以将一个四元数转换为对应的欧拉角。
你需要提供一个Quaternion结构体表示四元数，以及一个EulerAngle结构体来保存转换后的欧拉角。
四元数转欧拉角奇异值问题
四元数转欧拉角奇异值问题四元数和欧拉角作为常见的表示方法，在计算机图形学和机器人领域中广泛应用。
在四元数表示角度时，存在着一些奇异值问题，会使得欧拉角的转换结果出现异常，本文将对此进行详细的阐述。
一、四元数简介四元数是一种扩充了实数的数学结构，它包括一个实数部分和三个虚数部分，可以表示旋转和剪切等变换。
四元数形式上可以写成 $q = w + xi + yj + zk$，其中 $w, x, y, z$ 都是实数，$i, j, k$ 是三个虚数单位，它们的平方分别等于-1。
四元数通过旋转的方式来表示角度。
例如，单位四元数可以表示3D中的转动180度绕轴 $(1,1,1)$，则可以写成：$$q = [\cos(\theta/2),\sin(\theta/2)\frac{1}{\sqrt{3}},\sin(\theta/2)\frac{1}{\sqrt{3}},\sin(\theta/2)\frac{1}{\sqrt{3}}]$$其中，$\theta$ 是旋转的角度。
二、欧拉角简介欧拉角是一种表示旋转的方式，它通过描述旋转的三个角度来确定旋转的方向。
欧拉角可以通过任何航向、俯仰、翻转（yaw-pitch-roll）的组合来表示。
例如，将 3D 物体绕 X 轴旋转30度，然后绕 Y 轴旋转45度，再绕 Z 轴旋转 60 度，这个过程可以用以下的欧拉角表示：$$\alpha = 30^\circ,\beta = 45^\circ,\gamma = 60^\circ$$当然，在欧拉角的表示中也存在固有的问题，例如欧拉角的万向锁问题等。
但是，在本文中，我们主要探讨的问题是四元数转为欧拉角时可能会遇到的问题。
三、四元数转换为欧拉角在将四元数转换为欧拉角时，我们可以首先计算对应的旋转矩阵，然后通过该矩阵计算欧拉角。
假设四元数为$q = [w, x, y, z]$，则相应的旋转矩阵可以表示为：$$\left[\begin{matrix} 1 - 2(y^2+z^2)& 2(xy-wz)& 2(xz+wy)\\ 2(xy+wz)& 1-2(x^2+z^2)& 2(yz-wx)\\ 2(xz-wy)& 2(yz+wx)& 1 - 2(x^2+y^2)\end{matrix}\right]$$根据四元数到旋转矩阵的转换方法，我们可以通过这个矩阵计算出欧拉角，例如：$$\alpha = \tan^{-1}(\frac{-m_{23}}{m_{33}})$$$$\beta = \sin^{-1}(m_{13})$$$$\gamma = \tan^{-1}(\frac{-m_{12}}{m_{11}})$$其中 $m_{ij}$ 表示旋转矩阵中的第 $i$ 行第$j$ 列的元素。
- 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。