四元数与欧拉角的相互转换

RPY角与Z-Y-X欧拉角

描述机体坐标系{Body}相对于参考坐标系{Earth}的姿态有两种方式。第一种是绕固定（参考）坐标轴旋转：
　　(外在旋转）
　　假设开始两个坐标系重合，先将机体坐标系{Body}绕参考坐标系{Earth}的X轴旋转γ，然后绕{Earth}的Y轴旋转 β，最后绕{Earth}的Z轴旋转 α，就能旋转到当前姿态。可以称其为 X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。
由于是绕固定坐标系旋转，则旋转矩阵为
（其中，cα=cos⁡(α),sα=sin⁡(α)cα = \cos(α), sα = \sin(α)cα=cos(α),sα=sin(α)；以此类推）
RXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=[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γ]\begin{aligned} R_{XYZ}(\gamma,\beta,\alpha)&=R_Z(\alpha)R_Y(\beta)R_X(\gamma)\\&=\begin{bmatrix} c\alpha*c\beta & c\alpha*s\beta*s\gamma-s\alpha*c\gamma & c\alpha*s\beta*c\gamma+s\alpha*s\gamma\\ s\alpha*c\beta & s\alpha*s\beta*s\gamma+c\alpha*c\gamma & s\alpha*s\beta*c\gamma-c\alpha*s\gamma\\ -s\beta& c\beta*s\gamma & c\beta*c\gamma \end{bmatrix} \end{aligned} RXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=⎣⎡cα∗cβsα∗cβ−sβcα∗sβ∗sγ−sα∗cγsα∗sβ∗sγ+cα∗cγcβ∗sγcα∗sβ∗cγ+sα∗sγsα∗sβ∗cγ−cα∗sγcβ∗cγ⎦⎤
另一种姿态描述方式是绕自身坐标轴旋转：
（内在旋转）
假设开始两个坐标系重合，先将机体坐标系{Body}绕自身的Z轴旋转α，然后绕Y轴旋转β，最后绕X轴旋转γ，就能旋转到当前姿态。称其为Z-Y-X欧拉角，由于是绕自身坐标轴进行旋转，则旋转矩阵为：
RZ′Y′X′(α,β,γ)=RZ(α)RY(β)RX(γ)=[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γ]\begin{aligned} R_{Z'Y'X'}(\alpha,\beta,\gamma)&=R_Z(\alpha)R_Y(\beta)R_X(\gamma)\\&=\begin{bmatrix} c\alpha*c\beta & c\alpha*s\beta*s\gamma-s\alpha *c\gamma & c\alpha*s\beta*c\gamma+s\alpha*s\gamma\\ s\alpha*c\beta & s\alpha*s\beta *s\gamma+c\alpha*c\gamma & s\alpha*s\beta*c\gamma-c\alpha*s\gamma\\ -s\beta& c\beta*s\gamma & c\beta*c\gamma \end{bmatrix} \end{aligned} RZ′Y′X′(α,β,γ)=RZ(α)RY(β)RX(γ)=⎣⎡cα∗cβsα∗cβ−sβcα∗sβ∗sγ−sα∗cγsα∗sβ∗sγ+cα∗cγcβ∗sγcα∗sβ∗cγ+sα∗sγsα∗sβ∗cγ−cα∗sγcβ∗cγ⎦⎤

可以发现：
这两种描述方式得到的旋转矩阵是一样的，即绕固定坐标轴X-Y-Z旋转(γ,β,α)(\gamma,\beta,\alpha)(γ,β,α)和绕自身坐标轴Z-Y-X旋转(α,β,γ)(\alpha,\beta,\gamma)(α,β,γ)的最终结果一样，只是描述的方法有差别而已。

Axis-Angle与四元数

绕坐标轴的多次旋转可以等效为绕某一转轴（向量）旋转一定的角度。假设：等效旋转轴方向向量为
　　K⃗=[kx,ky,kz]T\vec{K}=[k_x,k_y,k_z]^T 　　　　K=[kx,ky,kz]T　　
等效旋转角为θ\thetaθ，则四元数可表示为：
q=(x,y,z,w)q=(x,y,z,w) q=(x,y,z,w)
其中，
x=kx⋅sinθ2y=ky⋅sinθ2z=kz⋅sinθ2w=cosθ2\begin{aligned} x &= k_x \cdot sin \frac{\theta}{2}\\ y &= k_y \cdot sin \frac{\theta}{2}\\ z &= k_z \cdot sin \frac{\theta}{2}\\ w &= cos \frac{\theta}{2} \end{aligned} xyzw=kx⋅sin2θ=ky⋅sin2θ=kz⋅sin2θ=cos2θ
且有
x2+y2+z2+w2=1x^2+y^2+z^2+w^2=1x2+y2+z2+w2=1
即：四元数存储了旋转轴和旋转角的信息，它能方便的描述刚体绕任意轴的旋转。　　
四元数转换为旋转矩阵：
R=[1−2y2−2z22(xy−zw)2(xz+yw)2(xy+zw)1−2x2−2z22(yz−xw)2(xz−yw)2(yz+xw)1−2x2−2y2]R=\begin{bmatrix} 1-2y^2-2z^2 & 2(xy-zw) & 2(xz+yw)\\ 2(xy+zw) & 1-2x^2-2z^2 & 2(yz-xw)\\ 2(xz-yw)& 2(yz+xw) & 1-2x^2-2y^2 \end{bmatrix} R=⎣⎡1−2y2−2z22(xy+zw)2(xz−yw)2(xy−zw)1−2x2−2z22(yz+xw)2(xz+yw)2(yz−xw)1−2x2−2y2⎦⎤
假设已知旋转矩阵为：
R=[r11r12r13r21r22r23r31r32r33]R=\begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{bmatrix} R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
则对应的四元数为：
x=+r32−r234wy=−r31−r134wz=+r21−r124ww=+121+r11+r22+r33\begin{aligned} x &= +\frac{r_{32} - r_{23}}{4w} \\ y &= -\frac{r_{31}-r_{13}}{4w} \\ z &=+ \frac{r_{21}-r_{12}}{4w} \\ w &= +\frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}} \end{aligned} xyzw=+4wr32−r23=−4wr31−r13=+4wr21−r12=+211+r11+r22+r33

四元数与欧拉角的相互转换

定义两个四元数：
q=a+u⃗=a+bi+cj+dkp=t+t⃗=t+xi+yj+zk\begin{aligned} q &= a+\vec{u} = a+bi+cj+dk \\ p&= t+\vec{t} = t+xi+yj+zk \end{aligned} qp=a+u=a+bi+cj+dk=t+t=t+xi+yj+zk
其中，u⃗\vec{u}u表示矢量<b,c,d><b,c,d><b,c,d>；而v⃗\vec{v}v表示矢量<x,y,z><x,y,z><x,y,z>。

四元数加法：

跟复数、向量和矩阵一样，两个四元数之和需要将不同的元素加起来。
p+q=a+t+u⃗+v⃗=(a+t)+(b+x)i+(c+y)j+(d+z)kp+q = a + t + \vec{u} + \vec{v} = (a+t) + (b+x)i+(c+y)j + (d+z)kp+q=a+t+u+v=(a+t)+(b+x)i+(c+y)j+(d+z)k
加法遵循实数和复数的所有交换律和结合律。

四元数乘法：

四元数的乘法的意义类似于矩阵的乘法，可以表示旋转的合成。当有多次旋转操作时，使用四元数可以获得更高的计算效率。
pq=at−u⃗⋅v⃗+av⃗+tu⃗+u⃗×v⃗pq=(at−bx−cy−dz)+(ax+bt+cz−dy)i+(ay−bz+ct+dx)j+(az+dt−cx+by)kpq = at - \vec{u} \cdot \vec{v} + a\vec{v}+t\vec{u}+\vec{u} \times \vec{v}\\ pq=(at-bx-cy-dz)+(ax+bt+cz-dy)i+(ay-bz+ct+dx)j+(az+dt-cx+by)kpq=at−u⋅v+av+tu+u×vpq=(at−bx−cy−dz)+(ax+bt+cz−dy)i+(ay−bz+ct+dx)j+(az+dt−cx+by)k
由于四元数乘法的非可换性，pqpqpq并不等于qpqpqp，qpqpqp乘积的向量部分是：
pq=at−u⃗⋅v⃗+av⃗+tu⃗−u⃗×v⃗pq = at - \vec{u} \cdot \vec{v} + a\vec{v}+t\vec{u}-\vec{u} \times \vec{v} pq=at−u⋅v+av+tu−u×v
那么将Z-Y-X欧拉角（或RPY角：绕固定坐标系的X-Y-Z依次旋转α,β,γ\alpha,\beta,\gammaα,β,γ角）转换为四元数：
q=[cos⁡γ200sin⁡γ2][cos⁡β20sin⁡β20][cos⁡α2sin⁡α200]=[cos⁡α2cos⁡β2cos⁡γ2+sin⁡α2sin⁡β2sin⁡γ2sin⁡α2cos⁡β2cos⁡γ2−cos⁡α2sin⁡β2sin⁡γ2cos⁡α2sin⁡β2cos⁡γ2+sin⁡α2cos⁡β2sin⁡γ2cos⁡α2cos⁡β2sin⁡γ2−sin⁡α2sin⁡β2cos⁡γ2]\begin{aligned} q&=\begin{bmatrix}\cos\frac{\gamma}{2}\\ 0\\ 0\\ \sin\frac{\gamma}{2}\end{bmatrix} \begin{bmatrix}\cos\frac{\beta}{2}\\ 0\\ \sin\frac{\beta}{2}\\ 0\end{bmatrix} \begin{bmatrix}\cos\frac{\alpha}{2}\\ \sin \frac{\alpha}{2}\\ 0\\ 0\end{bmatrix}\\&=\begin{bmatrix} \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \sin\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}-\cos\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \cos\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2} \\ \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}-\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2} \end{bmatrix} \end{aligned} q=⎣⎢⎢⎡cos2γ00sin2γ⎦⎥⎥⎤⎣⎢⎢⎡cos2β0sin2β0⎦⎥⎥⎤⎣⎢⎢⎡cos2αsin2α00⎦⎥⎥⎤=⎣⎢⎢⎡cos2αcos2βcos2γ+sin2αsin2βsin2γsin2αcos2βcos2γ−cos2αsin2βsin2γcos2αsin2βcos2γ+sin2αcos2βsin2γcos2αcos2βsin2γ−sin2αsin2βcos2γ⎦⎥⎥⎤
根据上面的公式可以求出逆解，即由四元数q=(q0,q1,q2,q3)q=(q_0,q_1,q_2,q_3)q=(q0,q1,q2,q3)或q=(w,x,y,z)q=(w,x,y,z)q=(w,x,y,z)到欧拉角(α,β,γ)(\alpha,\beta,\gamma)(α,β,γ)的转换为：
[αβγ]=[arctan⁡2(q0q1+q2q3)1−2(q12+q22)arcsin⁡(2(q0q2−q1q3))arctan⁡2(q0q3+q1q2)1−2(q22+q32)]\begin{bmatrix}\alpha\\ \beta\\ \gamma\end{bmatrix} = \begin{bmatrix} \arctan\frac{2(q_0q_1+q_2q_3)}{1-2(q_1^2+q_2^2)}\\ \arcsin(2(q_0q_2-q_1q_3)) \\ \arctan\frac{2(q_0q_3+q_1q_2)}{1-2(q_2^2+q_3^2)} \end{bmatrix} ⎣⎡αβγ⎦⎤=⎣⎢⎡arctan1−2(q12+q22)2(q0q1+q2q3)arcsin(2(q0q2−q1q3))arctan1−2(q22+q32)2(q0q3+q1q2)⎦⎥⎤
对于tan⁡(θ)=y/x\tan(\theta) = y / xtan(θ)=y/x ：
θ=arctan⁡(y/x)\theta = \arctan(y / x)θ=arctan(y/x)，求出的θ取值范围是[−π/2,π/2][-\pi/2, \pi/2][−π/2,π/2]；
θ=arctan⁡2(y,x)\theta = \arctan2(y, x)θ=arctan2(y,x)，求出的θ取值范围是[−π,π][-\pi, \pi][−π,π]；。
当 (x,y)(x, y)(x,y) 在第一象限, +0<θ<π/2+0 < \theta < \pi/2+0<θ<π/2
当 (x,y)(x, y)(x,y) 在第二象限, +π/2<θ<π+\pi/2 < \theta < \pi+π/2<θ<π
当 (x,y)(x, y)(x,y) 在第三象限, −π<θ<−π/2-\pi < \theta < -\pi/2−π<θ<−π/2
当 (x,y)(x, y)(x,y) 在第四象限, −π/2<θ<0-\pi/2 < \theta < 0−π/2<θ<0

注意：

将四元数转换为欧拉角需要注意，欧拉角有 3×2×2=123\times2\times2=123×2×2=12 种旋转次序，而上面推导的公式是按照Z-Y-X顺序进行的，所以有时会在网上看到不同的转换公式（因为对应着不同的旋转次序），在使用时一定要注意旋转次序是什么。比如ADAMS软件里就默认Body 3-1-3次序，即Z-X-Z欧拉角，而VREP中则按照X-Y-Z欧拉角旋转。

讨论：欧拉角奇异问题

下面看一种特殊的情况（参考Maths - Conversion Quaternion to Euler）：假设一架飞机绕Y轴旋转了90°（俯仰角pitch=90），机头垂直向上，此时如何计算航向角和横滚角？

这时会发生自由度丢失的情况，即Yaw和Roll会变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题：

arcsin⁡(2(q0q2−q1q3))\arcsin(2(q_0q_2−q_1q_3))arcsin(2(q0q2−q1q3))的定义域为[−1,1][−1,1][−1,1]，因此(q0q2−q1q3)∈[−0.5,0.5](q_0q_2−q_1q_3)∈[−0.5,0.5](q0q2−q1q3)∈[−0.5,0.5]，当q0q2−q1q3=0.5q_0q_2−q_1q_3=0.5q0q2−q1q3=0.5时（在程序中浮点数不能直接进行等于判断，要使用合理的阈值），俯仰角β\betaβ为90°，将其带入正向公式计算出四元数(q0,q1,q2,q3)(q_0,q_1,q_2,q_3)(q0,q1,q2,q3)，然后可以发现逆向公式中arctan⁡2\arctan2arctan2函数中的参数全部为000，即出现了00\frac{0}{0}00的情况！无法计算。
β=π/2β=π/2β=π/2时，sin⁡β2=cos⁡β2=0.707\sin\frac{β}{2}=\cos\frac{β}{2}=0.707sin2β=cos2β=0.707，将其带入公式中有
q=[wxyz][0.707(cos⁡α2cos⁡γ2+sin⁡α2sin⁡γ2)0.707(sin⁡α2cos⁡γ2−cos⁡α2sin⁡γ2)0.707(cos⁡α2cos⁡γ2+sin⁡α2sin⁡γ2)0.707(cos⁡α2sin⁡γ2−sin⁡α2cos⁡γ2)]=[0.707cos⁡α−γ20.707sin⁡α−γ20.707cos⁡α−γ20.707sin⁡α−γ2]\begin{aligned} q&=\begin{bmatrix}w\\ x\\ y\\ z\end{bmatrix} \begin{bmatrix} 0.707(\cos\frac{\alpha}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\sin\frac{\gamma}{2})\\ 0.707(\sin\frac{\alpha}{2}\cos\frac{\gamma}{2}-\cos\frac{\alpha}{2}\sin\frac{\gamma}{2})\\ 0.707(\cos\frac{\alpha}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\sin\frac{\gamma}{2})\\ 0.707(\cos\frac{\alpha}{2}\sin\frac{\gamma}{2}-\sin\frac{\alpha}{2}\cos\frac{\gamma}{2}) \end{bmatrix}\\&= \begin{bmatrix} 0.707\cos\frac{\alpha-\gamma}{2}\\ 0.707\sin\frac{\alpha-\gamma}{2}\\ 0.707\cos\frac{\alpha-\gamma}{2}\\ 0.707\sin\frac{\alpha-\gamma}{2} \end{bmatrix}\end{aligned} q=⎣⎢⎢⎡wxyz⎦⎥⎥⎤⎣⎢⎢⎡0.707(cos2αcos2γ+sin2αsin2γ)0.707(sin2αcos2γ−cos2αsin2γ)0.707(cos2αcos2γ+sin2αsin2γ)0.707(cos2αsin2γ−sin2αcos2γ)⎦⎥⎥⎤=⎣⎢⎢⎡0.707cos2α−γ0.707sin2α−γ0.707cos2α−γ0.707sin2α−γ⎦⎥⎥⎤
则xw=zy=tanα−γ2，\frac{x}{w}=\frac{z}{y}=tan\frac{\alpha-\gamma}{2}，wx=yz=tan2α−γ，于是有α−γ=2⋅atan2(x,w)\alpha-\gamma=2\cdot atan2(x,w)α−γ=2⋅atan2(x,w)
一般地有令α=0\alpha=0α=0，这时γ=−2⋅arctan⁡2(x,w)\gamma=-2\cdot \arctan2(x,w)γ=−2⋅arctan2(x,w)。
可以验证，当四元数为(x,y,z,w)=(−0.271,0.653,0.271,0.653)时(x,y,z,w) = (-0.271,0.653,0.271,0.653)时(x,y,z,w)=(−0.271,0.653,0.271,0.653)时，计算出Z-Y-X欧拉角为：α=0°,β=90°,γ=45°\alpha=0°,\beta=90°,\gamma=45°α=0°,β=90°,γ=45°。
当俯仰角为−90°-90°−90°，即机头竖直向下时的情况也与之类似，可以推导出奇异姿态时的计算公式。

四元数与欧拉角的相互转换相关推荐

四元数和欧拉角的相互转换
四元数转欧拉角&欧拉角转四元数程序:四元数转欧拉角&欧拉角转四元数程序. ? 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 ...
unity 四元数和欧拉角的相互转换
四元数和欧拉角相互转换 //四元数转化成欧拉角 Vector3 v3=transform.rotation.eulerAngles; //欧拉角转换成四元数 Quaternion rotation = ...
unity中的四元数，欧拉角，方向向量之间的相互转换方法。
四元数: 到欧拉角:quaternion.eulerAngles 到方向向量:(quaternion * Vector3.forward).normalized 欧拉角: 到四元数:Quaternio ...
旋转矩阵与欧拉角的相互转换及代码
这篇博客将会分享旋转矩阵和欧拉角的相互转换. 三维旋转矩阵有三个自由度,旋转能够使用多种方法表示(旋转矩阵,欧拉角,四元数,轴角,李群与李代数),比如一个3x3的矩阵,比如四元数,甚至可以将旋转表示为 ...
ROS中四元数、欧拉角、旋转矩阵等格式转换
未完- ROS接收到odometry格式消息: nav_msgs::Odometry pos_msg 具有: pos_msg.pose.pose.orientation.x; // xyzw pos_ ...
eigen 编译_头条 | 使用eigen实现四元数、欧拉角、旋转矩阵、旋转向量间的转换...
点击上方蓝字,关注本公众号,获得更多资源上一篇文章介绍了四元数.欧拉角.旋转矩阵.轴角如何相互转换,本篇文章介绍如何用eigen来实现. 旋转向量 1,初始化旋转向量:旋转角为alpha,旋转轴为(x ...
四元数，欧拉角，旋转矩阵相互转换
#include <TransForms3d/TransForms.h>/*---------------------------------------角度弧度转换----------- ...
【飞控理论】【惯性导航基础】二维平面的旋转如何用代数表示？三维平面的旋转如何用代数表示？什么是四元数？四元数、欧拉角、方向余弦之间有什么关系？
上一篇欧拉角由于欧拉角在描述三维空间物体旋转问题时存在万向节死锁问题(详情戳这里),所以引入四元数概念. 目录 1.二维平面的旋转 2.三维平面的旋转(什么是四元数) 3.<捷联惯性导航> ...
Matlab ——旋转矩阵，四元数，欧拉角之间的转换
最近要用这方面的东西,整理,记录,分享一下基于Matlab现有函数下的内容 Matlab --旋转矩阵,四元数,欧拉角之间的转换旋转矩阵 dcm R 四元数 quat q = [q0 q1 q2 ...
【Unity3D 灵巧小知识点】☀️ | Unity 四元数、欧拉角与方向向量之间转换
Unity 小科普老规矩,先介绍一下 Unity 的科普小知识: Unity是实时3D互动内容创作和运营平台 . 包括游戏开发.美术.建筑.汽车设计.影视在内的所有创作者,借助 Unity 将创意 ...

四元数与欧拉角的相互转换

RPY角与Z-Y-X欧拉角

Axis-Angle与四元数

四元数与欧拉角的相互转换

四元数加法：

四元数乘法：

注意：

讨论：欧拉角奇异问题

四元数与欧拉角的相互转换相关推荐

最新文章

热门文章