旋转矩阵、欧拉角、四元数理论及其转换关系

author@jason_ql（lql0716）
http://blog.csdn.net/lql0716

1、概述

旋转矩阵、欧拉角、四元数主要用于表示坐标系中的旋转关系，它们之间的转换关系可以减小一些算法的复杂度。

本文主要介绍了旋转矩阵、欧拉角、四元数的基本理论及其之间的转换关系。

2、原理

2.1 旋转矩阵

对于两个三维点 p1(x1,y1,z1)p1(x1,y1,z1)p_1(x_1, y_1, z_1)，p2(x2,y2,z2)p2(x2,y2,z2)p_2(x_2, y_2, z_2)，由点 p1p1p_1 经过旋转矩阵 RRR 旋转到 p2" role="presentation">p2p2p_2，则有：

R=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥R=[r11r12r13r21r22r23r31r32r33]

R = \left[ \begin{matrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{matrix} \right]

⎡⎣⎢x2y2z2⎤⎦⎥=R⎡⎣⎢x1y1z1⎤⎦⎥[x2y2z2]=R[x1y1z1]

\left[ \begin{matrix} x_2 \\ y_2 \\ z_2 \end{matrix} \right] = R \left[ \begin{matrix} x_1 \\ y_1 \\ z_1 \end{matrix} \right]

注：旋转矩阵为正交矩阵RRT=ERRT=ERR^T=E

绕x轴旋转：

Rx(θ)=⎡⎣⎢1000cosθsinθ0−sinθcosθ⎤⎦⎥Rx(θ)=[1000cosθ−sinθ0sinθcosθ]

R_x(\theta) = \left[ \begin{matrix} 1 & 0 & 0\\ 0 & cos\theta & -sin\theta\\ 0 & sin\theta & cos\theta \end{matrix} \right]

绕y轴旋转：

Ry(θ)=⎡⎣⎢cosθ0−sinθ010sinθ0cosθ⎤⎦⎥Ry(θ)=[cosθ0sinθ010−sinθ0cosθ]

R_y(\theta) = \left[ \begin{matrix} cos\theta & 0 & sin\theta\\ 0 & 1 & 0\\ -sin\theta & 0 & cos\theta \end{matrix} \right]

绕z轴旋转：

Rz(θ)=⎡⎣⎢cosθsinθ0−sinθcosθ0001⎤⎦⎥Rz(θ)=[cosθ−sinθ0sinθcosθ0001]

R_z(\theta) = \left[ \begin{matrix} cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0 & 0 & 1 \end{matrix} \right]

任意旋转矩阵：
任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。
三个轴可以指固定的世界坐标系轴，也可以指被旋转的物体坐标系的轴。三个旋转轴次序不同，会导致结果不同。

Rx(−θ1)Ry(−θ2)Rz(θ)Ry(θ2)Rx(θ1)Rx(−θ1)Ry(−θ2)Rz(θ)Ry(θ2)Rx(θ1)

R_x(-\theta_1) R_y(-\theta_2) R_z(\theta) R_y(\theta_2) R_x(\theta_1)

2.2 欧拉角

欧拉角有两种：
静态：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静止，所以称为静态。
动态：即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以称为动态。
使用动态欧拉角会出现万向锁现象；静态欧拉角不存在万向锁的问题。
对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，是静止不动的。而坐标系则固定于刚体，随着刚体的旋转而旋转。
如图1，设定xyz-轴为参考系的参考轴。称xy-平面与XY-平面的相交为交点线，用英文字母（N）代表。zxz顺规的欧拉角可以静态地这样定义：

αα\alpha 是x-轴与交点线的夹角，
ββ{\displaystyle \beta } 是z-轴与Z-轴的夹角，
γγ{\displaystyle \gamma } 是交点线与X-轴的夹角。
图1：

图中三个欧拉角分别为：(α,β,γ)(α,β,γ)(\alpha, \beta, \gamma)；
蓝色的轴为：xyz轴xyz轴xyz轴
红色的轴为：XYZXYZXYZ轴
绿色的线为交线：NNN
α∈[0,2π],β∈[0,π],γ∈[0,2π]" role="presentation">α∈[0,2π],β∈[0,π],γ∈[0,2π]α∈[0,2π],β∈[0,π],γ∈[0,2π]\alpha \in [0, 2\pi], \beta \in [0, \pi], \gamma \in [0, 2\pi]
很可惜地，对于夹角的顺序和标记，夹角的两个轴的指定，并没有任何常规。科学家对此从未达成共识。每当用到欧拉角时，我们必须明确的表示出夹角的顺序，指定其参考轴。
实际上，有许多方法可以设定两个坐标系的相对取向。欧拉角方法只是其中的一种。此外，不同的作者会用不同组合的欧拉角来描述，或用不同的名字表示同样的欧拉角。因此，使用欧拉角前，必须先做好明确的定义。
欧拉角的基本思想是将角位移分解为绕三个互相垂直轴的三个旋转组成的序列。所以，欧拉旋转的三个角，可以对应于三个旋转矩阵。
Yaw（偏航）：欧拉角向量的y轴
Pitch（俯仰）：欧拉角向量的x轴
Roll（翻滚）：欧拉角向量的z轴
Unity3D中，欧拉旋转是按照ZYX的顺序旋转的。（不同的旋转顺序最终得到的结果是不一样的，要引起重视。给定一组欧拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的顺序旋转和按照yaw-roll-pitch的顺序旋转，最终刚体的朝向是不同的！换言之，若刚体需要按照两种不同的旋转顺序旋转到相同的朝向，所需要的欧拉角角度值则是不同的！）
欧拉角的缺点：

1、欧拉角的表示方式不唯一。给定某个起始朝向和目标朝向，即使给定yaw、pitch、roll的顺序，也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。这其实主要是由于万向锁(Gimbal Lock)引起的；

2、欧拉角的插值比较难；

3、计算旋转变换时，一般需要转换成旋转矩阵，这时候需要计算很多sin, cos，计算量较大；

2.2.1 由欧拉角求旋转矩阵

设三个轴x，y，z的欧拉角分别为θx,θy,θzθx,θy,θz\theta_x, \theta_y, \theta_z，正弦值、余弦值分别为sx,cx,sy,cy,sz,czsx,cx,sy,cy,sz,czs_x, c_x, s_y, c_y, s_z, c_z那么旋转矩阵为：

R(θx,θy,θz)=Rz(θz)Ry(θy)Rx(θx)=⎡⎣⎢cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czszcxcy⎤⎦⎥R(θx,θy,θz)=Rz(θz)Ry(θy)Rx(θx)=[cyczczsxsy−cxszsxsz+cxczsycyszcxcz+sxsyszcxsysz−czsz−sycysxcxcy]

R(\theta_x,\theta_y,\theta_z)=R_z(\theta_z)R_y(\theta_y)R_x(\theta_x) = \left[ \begin{matrix} c_y c_z & c_z s_x s_y - c_x s_z & s_x s_z + c_x c_z s_y\\ c_y s_z & c_x c_z + s_x s_y s_z & c_x s_y s_z - c_z s_z\\ -s_y & c_y s_x & c_x c_y \end{matrix} \right]

2.2.2 由旋转矩阵求欧拉角

R=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥=⎡⎣⎢cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czszcxcy⎤⎦⎥R=[r11r12r13r21r22r23r31r32r33]=[cyczczsxsy−cxszsxsz+cxczsycyszcxcz+sxsyszcxsysz−czsz−sycysxcxcy]

R = \left[ \begin{matrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{matrix} \right] = \left[ \begin{matrix} c_y c_z & c_z s_x s_y - c_x s_z & s_x s_z + c_x c_z s_y\\ c_y s_z & c_x c_z + s_x s_y s_z & c_x s_y s_z - c_z s_z\\ -s_y & c_y s_x & c_x c_y \end{matrix} \right]

解方程可得：

θx=atan2(r32,r33)θx=atan2(r32,r33)

\theta_x = atan2(r_{32}, r_{33})

θy=atan2(−r31,r232+r233−−−−−−−√)θy=atan2(−r31,r322+r332)

\theta_y = atan2(-r_{31}, \sqrt{r_{32}^2+r_{33}^2})

θz=atan2(r21,r11)θz=atan2(r21,r11)

\theta_z = atan2(r_{21}, r_{11})

注：atan2()为C++中的函数，atan2(y，x) 求的是y/x的反正切，其返回值为[-pi,+pi]之间的一个数。

2.3 四元数

四元数（Quaternions）是由爱尔兰数学家哈密顿（William Rowan Hamilton）在1843年提出。

三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示，即所谓的Axis-Angle表示方法。这种表示方法里，Axis可用一个三维向量(x,y,z)(x,y,z)(x,y,z)来表示，θθ\theta可以用一个角度值来表示，直观来讲，一个四维向量(θ,x,y,z)(θ,x,y,z)(\theta,x,y,z)就可以表示出三维空间任意的旋转。注意，这里的三维向量(x,y,z)(x,y,z)(x,y,z)只是用来表示axis的方向朝向，因此更紧凑的表示方式是用一个单位向量来表示方向axis，而用该三维向量的长度来表示角度值θθ\theta。这样以来，可以用一个三维向量(θ∗x,θ∗y,θ∗z)(θ∗x,θ∗y,θ∗z)(\theta*x, \theta*y, \theta*z)就可以表示出三维空间任意的旋转，前提是其中(x,y,z)(x,y,z)(x,y,z)是单位向量。这就是旋转向量(Rotation Vector)的表示方式，OpenCV里大量使用的就是这种表示方法来表示旋转(见OpenCV相机标定部分的rvec)。

单位向量(x,y,z)(x,y,z)(x,y,z)旋转θθ\theta角度后的四元数：

(cosθ2,x∗sinθ2,y∗sinθ2,z∗sinθ2)(cosθ2,x∗sinθ2,y∗sinθ2,z∗sinθ2)

(cos \frac{\theta}{2}, x*sin \frac{\theta}{2}, y*sin \frac{\theta}{2}, z*sin \frac{\theta}{2})
对于三维坐标的旋转，可以通过四元数乘法直接操作，与旋转矩阵操作可以等价，但是表示方式更加紧凑，计算量也可以小一些。

2.3.1 四元数基本概念

四元数的复数定义：
q=q0+q1i+q2j+q3k=[s,v]q=q0+q1i+q2j+q3k=[s,v]

\mathbf{q} = q_0 + q_1 i + q_2 j + q_3 k = [s, \mathbf{v}]
- 其中 q0,q1,q2,q3q0,q1,q2,q3q_0,q_1,q_2,q_3均为实数, s=q0,v=[q1,q2,q3]s=q0,v=[q1,q2,q3]s=q_0,\mathbf{v}=[q_1, q_2, q_3]，i2=j2=k2=−1i2=j2=k2=−1i^2=j^2=k^2=-1
- 对于 i,j,ki,j,ki,j,k 本身的几何意义可以理解为一种旋转，其中 iii 代表 x" role="presentation">xxx 轴与 yyy 轴相交平面中 x" role="presentation">xxx 轴正向向 yyy 轴正向的旋转， j" role="presentation">jjj 旋转代表 zzz 轴与 x" role="presentation">xxx 轴相交平面中 zzz 轴正向向 x" role="presentation">xxx 轴正向的旋转， kkk 旋转代表 y" role="presentation">yyy 轴与 zzz 轴相交平面中 y" role="presentation">yyy 轴正向向 zzz 轴正向的旋转， −i,−j,−k" role="presentation">−i,−j,−k−i,−j,−k-i,-j,-k 分别代表 i,j,ki,j,ki,j,k 的反向旋转。
四元数的模

|q|=q20+q21+q22+q23−−−−−−−−−−−−−−√|q|=q02+q12+q22+q32

|q|=\sqrt{q_0^2 + q_1^2 + q_2^2 +q_3^2}
四元数的优点：
- 非奇异表达（和欧拉角之类的表示相比）
- 比矩阵更紧凑（更快速）
- 单位四元数的对可以表示四维空间中的一个旋转
四元数与群

所有单位四元数的集合组成一个三维球S3和在乘法下的一个群（李群）。S3是行列式为1的实正交3x3正交矩阵的群SO(3,R)的双面覆盖，因为每两个单位四元数通过上述关系对应于一个转动。群S3和SU(2)同构，SU(2)是行列式为1的复酉2x2矩阵的群。

设A={q=q0+q1i+q2j+q3k|q0,q1,q2,q3均为实数}A={q=q0+q1i+q2j+q3k|q0,q1,q2,q3均为实数}A=\{\mathbf{q}=q_0 + q_1 i + q_2 j + q_3 k | q_0,q_1,q_2,q_3 均为实数\}，则AAA是一个环，并且是一个格，A" role="presentation">AAA中存在24个四元数，而它们是施莱夫利符号为{3,4,3}{3,4,3}\{3,4,3\}的正二十四胞体的顶点。
四元数的运算：
p1=[s1,v1]=(a1b1c1d1)Tp1=[s1,v1]=(a1b1c1d1)T

\mathbf{p}_1 = [s_1, \mathbf{v}_1]=\begin{pmatrix} a_1 & b_1 & c_1 & d_1 \end{pmatrix}^T

p2=[s2,v2]=(a2b2c2d2)Tp2=[s2,v2]=(a2b2c2d2)T

\mathbf{p}_2 = [s_2, \mathbf{v}_2]=\begin{pmatrix} a_2 & b_2 & c_2 & d_2 \end{pmatrix}^T
- 四元数的加法
p1+p2=s1+s2+v1+v2=(a1+a2)+(b1+b2)i+(c1+c2)j+(d1+d2)kp1+p2=s1+s2+v1+v2=(a1+a2)+(b1+b2)i+(c1+c2)j+(d1+d2)k

\mathbf{p}_1 + \mathbf{p}_2 = s_1 + s_2 + \mathbf{v_1} + \mathbf{v_2} \\= (a_1 + a_2) + (b_1 + b_2)i + (c_1 + c_2)j + (d_1 + d_2)k
- 四元数的乘法
p1p2=s1s2−v1v2+s1v2+s2v1+v1×v2p1p2=s1s2−v1v2+s1v2+s2v1+v1×v2

\mathbf{p}_1 \mathbf{p}_2 =s_1s_2 - \mathbf{v_1 v_2} + s_1 \mathbf{v_2} + s_2 \mathbf{v_1} + \mathbf{v_1 \times v_2}

p1p2=⎛⎝⎜⎜⎜a1a2−b1b2−c1c2−d1d2a1b2+b1a2+c1d2−d1c2a1c2−b1d2+c1a2+d1b2a1d2+b1c2−c1b2+d1a2⎞⎠⎟⎟⎟p1p2=(a1a2−b1b2−c1c2−d1d2a1b2+b1a2+c1d2−d1c2a1c2−b1d2+c1a2+d1b2a1d2+b1c2−c1b2+d1a2)

\mathbf{p}_1 \mathbf{p}_2 = \begin{pmatrix} a_1 a_2 - b_1 b_2 - c_1 c_2 - d_1 d_2 \\ a_1 b_2 + b_1 a_2 + c_1 d_2 - d_1 c_2 \\ a_1 c_2 - b_1 d_2 + c_1 a_2 + d_1 b_2 \\ a_1 d_2 + b_1 c_2 - c_1 b_2 + d_1 a_2 \end{pmatrix}

注：四元数不满足交换律，故 p1p2≠p2p1p1p2≠p2p1\mathbf{p}_1 \mathbf{p}_2 \neq \mathbf{p}_2 \mathbf{p}_1，四元数的乘法不等同于外积。
- 四元数的点积（内积）
  点积也叫欧几里得内积，四元数的点积等同于一个四维向量的点积。点积的值是p1,p2p1,p2\mathbf{p_1} , \mathbf{p_2}中相应元素的数值的乘积的和。
p1⋅p2=s1s2+v1⋅v2=a1a2+b1b2+c1c2+d1d2p1⋅p2=s1s2+v1⋅v2=a1a2+b1b2+c1c2+d1d2

\mathbf{p_1} \cdot \mathbf{p_2} = s_1 s_2 + \mathbf{v_1} \cdot \mathbf{v_2} = a_1 a_2 + b_1 b_2 + c_1 c_2 + d_1 d_2
- 四元数的外积：Outer(p1,p2)Outer(p1,p2)Outer(\mathbf{p_1,p_2})
- 四元数的偶积：Even(p1,p2)Even(p1,p2)Even(\mathbf{p_1,p_2})
- 四元数的叉积：p1×p2p1×p2\mathbf{p_1} \times \mathbf{p_2}
p1×p2=p1p2−p2p22p1×p2=p1p2−p2p22

\mathbf{p_1} \times \mathbf{p_2} = \frac{\mathbf{p_1p_2} - \mathbf{p_2p_2}}{2}

p1×p2=v1×v2p1×p2=v1×v2

\mathbf{p_1} \times \mathbf{p_2} = \mathbf{v_1} \times \mathbf{v_2}

p1×p2=(c1d2−c2d1)i+(b1d2−b2d1)j+(b1c2−b2c1)kp1×p2=(c1d2−c2d1)i+(b1d2−b2d1)j+(b1c2−b2c1)k

\mathbf{p_1} \times \mathbf{p_2} = (c_1d_2-c_2d_1)i + (b_1d_2 - b_2d_1)j + (b_1c_2 - b_2c_1)k
- 四元数的共轭 q∗q∗\mathbf{q}^*
q∗=s−v=[s,−v]q∗=s−v=[s,−v]

\mathbf{q}^* = s - \mathbf{v} = [s, -\mathbf{v}]

qq∗=q20+q21+q22+q23qq∗=q02+q12+q22+q32

\mathbf{q} \mathbf{q}^* = q_0^2 + q_1^2 + q_2^2 + q_3^2
- 四元数的逆 q−1q−1\mathbf{q}^{-1}
qq−1=q−1q=1qq−1=q−1q=1

\mathbf{q}\mathbf{q}^{-1}=\mathbf{q}^{-1}\mathbf{q}=1

q−1=q∗||q||2q−1=q∗||q||2

\mathbf{q}^{-1} = \frac{\mathbf{q}^*}{||\mathbf{q}||^2}
- 单位四元数
  单位四元数的共轭和逆相等 q−1=q∗q−1=q∗\mathbf{q}^{-1}=\mathbf{q}^*
四元数乘法的性质：

1、满足结合律
2、不满足交换律
3、乘积的模等于模的乘积
4、乘积的逆等于各个四元数的逆以相反的顺序相乘

2.3.2 四元数求旋转矩阵

方法1：
- 已知四元数
  
  q=q0+q1i+q2j+q3k=[s,v]q=q0+q1i+q2j+q3k=[s,v]
  
  \mathbf{q} = q_0 + q_1 i + q_2 j + q_3 k = [s, \mathbf{v}]
- 利用Rodrigues公式可以由四元数求得旋转矩阵RRR:
  
  R=[1−2q22−2q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22]" role="presentation">R=⎡⎣⎢⎢1−2q22−2q232q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q21−2q232q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q21−2q22⎤⎦⎥⎥R=[1−2q22−2q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22]
  
  R = \left[ \begin{matrix} 1 - 2 q_2^2 - 2 q_3^2 & 2q_1 q_2 - 2q_0 q_3 & 2 q_1 q_3 + 2 q_0 q_2 \\ 2q_1 q_2 + 2q_0 q_3 & 1 - 2 q_1^2 - 2 q_3^2 & 2 q_2 q_3 - 2 q_0 q_1 \\ 2 q_1 q_3 - 2 q_0 q_2 & 2 q_2 q_3 + 2 q_0 q_1 & 1 - 2 q_1^2 - 2 q_2^2 \end{matrix} \right ]
方法2：

亦可以将四元数转化为轴角 θθ\theta 与向量 (x,y,z)(x,y,z)(x, y, z)，然后再计算旋转矩阵（这样需要多计算一个arccos，不划算）

q=(θxyz)Tq=(θxyz)T

\mathbf{q} = \begin{pmatrix} \theta & x & y & z \end{pmatrix}^T

利用Rodrigues公式可以由单位向量ω=(x,y,z)ω=(x,y,z)\mathbf{\omega} = (x,y,z)旋转θθ\theta角度后的四元数求得旋转矩阵RRR:

R=eωθ=I+ω×sinθ+ω×2(1−cosθ)=I+[0−zyz0−x−yx0]sinθ+[−y2−z2xyxzxy−x2−z2yzxzyz−x2−y2](1−cosθ)=[1−zsinθysinθzsinθ1−xsinθ−ysinθxsinθ1]+[−y2−z2xyxzxy−x2−z2yzxzyz−x2−y2](1−cosθ)=[1−(y2+z2)(1−cosθ)−zsinθ+xy(1−cosθ)ysinθ+xz(1−cosθ)zsinθ+xy(1−cosθ)1−(x2+z2)(1−cosθ)−xsinθ+yz(1−cosθ)−ysinθ+xz(1−cosθ)xsinθ+yz(1−cosθ)1−(x2+y2)(1−cosθ)]=[cosθ+x2(1−cosθ)−zsinθ+xy(1−cosθ)ysinθ+xz(1−cosθ)zsinθ+xy(1−cosθ)cosθ+y2(1−cosθ)−xsinθ+yz(1−cosθ)−ysinθ+xz(1−cosθ)xsinθ+yz(1−cosθ)cosθ+z2(1−cosθ)]" role="presentation">R=eωθ=I+ω×sinθ+ω2×(1−cosθ)=I+⎡⎣⎢0z−y−z0xy−x0⎤⎦⎥sinθ+⎡⎣⎢−y2−z2xyxzxy−x2−z2yzxzyz−x2−y2⎤⎦⎥(1−cosθ)=⎡⎣⎢1zsinθ−ysinθ−zsinθ1xsinθysinθ−xsinθ1⎤⎦⎥+⎡⎣⎢−y2−z2xyxzxy−x2−z2yzxzyz−x2−y2⎤⎦⎥(1−cosθ)=⎡⎣⎢1−(y2+z2)(1−cosθ)zsinθ+xy(1−cosθ)−ysinθ+xz(1−cosθ)−zsinθ+xy(1−cosθ)1−(x2+z2)(1−cosθ)xsinθ+yz(1−cosθ)ysinθ+xz(1−cosθ)−xsinθ+yz(1−cosθ)1−(x2+y2)(1−cosθ)⎤⎦⎥=⎡⎣⎢cosθ+x2(1−cosθ)zsinθ+xy(1−cosθ)−ysinθ+xz(1−cosθ)−zsinθ+xy(1−cosθ)cosθ+y2(1−cosθ)xsinθ+yz(1−cosθ)ysinθ+xz(1−cosθ)−xsinθ+yz(1−cosθ)cosθ+z2(1−cosθ)⎤⎦⎥R=eωθ=I+ω×sinθ+ω×2(1−cosθ)=I+[0−zyz0−x−yx0]sinθ+[−y2−z2xyxzxy−x2−z2yzxzyz−x2−y2](1−cosθ)=[1−zsinθysinθzsinθ1−xsinθ−ysinθxsinθ1]+[−y2−z2xyxzxy−x2−z2yzxzyz−x2−y2](1−cosθ)=[1−(y2+z2)(1−cosθ)−zsinθ+xy(1−cosθ)ysinθ+xz(1−cosθ)zsinθ+xy(1−cosθ)1−(x2+z2)(1−cosθ)−xsinθ+yz(1−cosθ)−ysinθ+xz(1−cosθ)xsinθ+yz(1−cosθ)1−(x2+y2)(1−cosθ)]=[cosθ+x2(1−cosθ)−zsinθ+xy(1−cosθ)ysinθ+xz(1−cosθ)zsinθ+xy(1−cosθ)cosθ+y2(1−cosθ)−xsinθ+yz(1−cosθ)−ysinθ+xz(1−cosθ)xsinθ+yz(1−cosθ)cosθ+z2(1−cosθ)]

R = e^{\mathbf{\omega} \theta} = I + \mathbf{\omega}_{\times} sin\theta + \mathbf{\omega}_{\times}^2 (1 - cos\theta) \\ = I + \left[ \begin{matrix} 0 & -z & y\\ z & 0 & -x \\ -y & x & 0 \end{matrix} \right ] sin\theta + \left[ \begin{matrix} -y^2-z^2 & xy & xz\\ xy & -x^2-z^2 & yz \\ xz & yz & -x^2-y^2 \end{matrix} \right ] (1-cos\theta)\\ = \left[ \begin{matrix} 1 & -zsin\theta & ysin\theta\\ zsin\theta & 1 & -xsin\theta \\ -ysin\theta & xsin\theta & 1 \end{matrix} \right ] + \left[ \begin{matrix} -y^2-z^2 & xy & xz\\ xy & -x^2-z^2 & yz \\ xz & yz & -x^2-y^2 \end{matrix} \right ] (1-cos\theta)\\ = \left[ \begin{matrix} 1-(y^2+z^2)(1-cos\theta) & -zsin\theta + xy(1-cos\theta) & ysin\theta + xz(1-cos\theta) \\ zsin\theta + xy(1-cos\theta) & 1 - (x^2+z^2)(1-cos\theta) & -xsin\theta + yz(1-cos\theta) \\ -ysin\theta + xz(1-cos\theta) & xsin\theta + yz(1-cos\theta) & 1-(x^2+y^2)(1-cos\theta) \end{matrix} \right ] \\ = \left[ \begin{matrix} cos\theta+x^2(1-cos\theta) & -zsin\theta + xy(1-cos\theta) & ysin\theta + xz(1-cos\theta) \\ zsin\theta + xy(1-cos\theta) & cos\theta+y^2(1-cos\theta) & -xsin\theta + yz(1-cos\theta) \\ -ysin\theta + xz(1-cos\theta) & xsin\theta + yz(1-cos\theta) & cos\theta+ z^2(1-cos\theta) \end{matrix} \right ]

R=⎡⎣⎢cosθ+x2(1−cosθ)zsinθ+xy(1−cosθ)−ysinθ+xz(1−cosθ)−zsinθ+xy(1−cosθ)cosθ+y2(1−cosθ)xsinθ+yz(1−cosθ)ysinθ+xz(1−cosθ)−xsinθ+yz(1−cosθ)cosθ+z2(1−cosθ)⎤⎦⎥R=[cosθ+x2(1−cosθ)−zsinθ+xy(1−cosθ)ysinθ+xz(1−cosθ)zsinθ+xy(1−cosθ)cosθ+y2(1−cosθ)−xsinθ+yz(1−cosθ)−ysinθ+xz(1−cosθ)xsinθ+yz(1−cosθ)cosθ+z2(1−cosθ)]

R = \left[ \begin{matrix} cos\theta+x^2(1-cos\theta) & -zsin\theta + xy(1-cos\theta) & ysin\theta + xz(1-cos\theta) \\ zsin\theta + xy(1-cos\theta) & cos\theta+y^2(1-cos\theta) & -xsin\theta + yz(1-cos\theta) \\ -ysin\theta + xz(1-cos\theta) & xsin\theta + yz(1-cos\theta) & cos\theta+ z^2(1-cos\theta) \end{matrix} \right ]

2.3.3 旋转矩阵求四元数

情况1：

q0=1+r11+r22+r33−−−−−−−−−−−−−−√2q0=1+r11+r22+r332

q_0 = \frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2}

q1=r32−r234q0q1=r32−r234q0

q_1 = \frac{r_{32}-r_{23}}{4q_0}

q2=r13−r314q0q2=r13−r314q0

q_2 = \frac{r_{13}-r_{31}}{4q_0}

q3=r21−r124q0q3=r21−r124q0

q_3 = \frac{r_{21}-r_{12}}{4q_0}

其中要满足 q0≠0q0≠0q_0 \neq 0，1+r11+r22+r33>01+r11+r22+r33>01+r_{11}+r_{22}+r_{33}>0，即 1+tr(R)>01+tr(R)>01+tr(R)>0
情况2：
如果q0q0q_0趋近于0，tr(R)tr(R)tr(R)趋近于-1,则求解四元数的过程为：
- 2.1 如果max{r11,r22,r33}=r11max{r11,r22,r33}=r11max\{r_{11},r_{22},r_{33}\}=r_{11}
  
  t=1+r11−r22−r33−−−−−−−−−−−−−−−√t=1+r11−r22−r33
  
  t = \sqrt{1+r_11 - r_{22} - r_{33}}
  
  q0=r32−r23tq0=r32−r23t
  
  q_0 = \frac{r_{32} - r_{23}}{t}
  
  q1=t/4q1=t/4
  
  q_1 = t/4
  
  q2=r13+r31tq2=r13+r31t
  
  q_2 = \frac{r_{13} + r_{31}}{t}
  
  q3=r12+r21tq3=r12+r21t
  
  q_3 = \frac{r_{12} + r_{21}}{t}
- 2.2 如果max{r11,r22,r33}=r22max{r11,r22,r33}=r22max\{r_{11},r_{22},r_{33}\}=r_{22}
  
  t=1−r11+r22−r33−−−−−−−−−−−−−−−√t=1−r11+r22−r33
  
  t = \sqrt{1 - r_11 + r_{22} - r_{33}}
  
  q0=r13−r31tq0=r13−r31t
  
  q_0 = \frac{r_{13} - r_{31}}{t}
  
  q1=r12+r21tq1=r12+r21t
  
  q_1 = \frac{r_{12} + r_{21}}{t}
  
  q2=t/4q2=t/4
  
  q_2 = t/4
  
  q3=r32+r23tq3=r32+r23t
  
  q_3 = \frac{r_{32} + r_{23}}{t}
- 2.3 如果max{r11,r22,r33}=r33max{r11,r22,r33}=r33max\{r_{11},r_{22},r_{33}\}=r_{33}
  
  t=1−r11−r22+r33−−−−−−−−−−−−−−−√t=1−r11−r22+r33
  
  t = \sqrt{1 - r_11 - r_{22} + r_{33}}
  
  q0=r21−r12tq0=r21−r12t
  
  q_0 = \frac{r_{21} - r_{12}}{t}
  
  q1=r13+r31tq1=r13+r31t
  
  q_1 = \frac{r_{13} + r_{31}}{t}
  
  q2=r23−r32tq2=r23−r32t
  
  q_2 = \frac{r_{23} - r_{32}}{t}
  
  q3=t/4q3=t/4
  
  q_3 = t/4

参考资料

相关文章
- 机器学习、深度学习、计算机视觉、自然语言处理及应用案例——干货分享（持续更新……）
- 相机标定（Camera calibration）原理、步骤
1、三维旋转：旋转矩阵，欧拉角，四元数
2、欧拉角、四元数和旋转矩阵
3、欧拉角和旋转矩阵的转换
4、三维空间中的旋转：旋转矩阵、欧拉角
5、旋转矩阵、欧拉角、四元数比较
6、刚体在三维空间的旋转(关于旋转矩阵、DCM、旋转向量、四元数、欧拉角)
7、3D计算机图形学-变换矩阵、欧拉角、四元数
8、欧拉角
9、四元数
10、【泡泡机器人SLAM原创专栏-四元数运动学】一：四元数和旋转
11、3D图形:矩阵、欧拉角、四元数与方位的故事

旋转矩阵、欧拉角、四元数理论及其转换关系相关推荐

彻底搞懂“旋转矩阵/欧拉角/四元数”，让你体会三维旋转之美
目录旋转矩阵坐标变换的作用实现坐标变换所需的数据位姿变换坐标变换中旋转的实质坐标变换中平移的实质如何计算坐标系B各坐标轴在坐标系A上的投影?(多坐标变换) 如何实现坐标变换? 欧拉角欧 ...
罗德里格斯公式推导（轴角与旋转矩阵的关系）以及四元数与旋转向量、旋转矩阵、欧拉角之间的转换关系
罗德里格斯公式推导(轴角与旋转矩阵的关系) 意义:罗德里格斯公式表示旋转向量到旋转矩阵之间爱你的转换关系旋转向量:一个向量,方向与旋转轴一致,长度等于旋转角度空间中任意旋转都可以用一个旋转轴和一个 ...
转换矩阵、平移矩阵、旋转矩阵关系以及python实现旋转矩阵、四元数、欧拉角之间转换
文章目录 1. 转换矩阵.平移矩阵.旋转矩阵之间的关系 2. 缩放变换.平移变换和旋转变换 2. python实现旋转矩阵.四元数.欧拉角互相转化由于在平时总是或多或少的遇到平移旋转的问题,每次都是 ...
欧拉角与旋转矩阵的转换关系
欧拉角因为其奇异性,虽然在优化和插值的不会使用,但是当我们对别人描述一个旋转的过程是怎么样的时候,欧拉角还是很有用的,比如,做无人机姿态控制的时候使用的就是欧拉角,但是搞明白欧拉角与旋转矩阵的转换确实 ...
Matlab ——旋转矩阵，四元数，欧拉角之间的转换
最近要用这方面的东西,整理,记录,分享一下基于Matlab现有函数下的内容 Matlab --旋转矩阵,四元数,欧拉角之间的转换旋转矩阵 dcm R 四元数 quat q = [q0 q1 q2 ...
【机器人】四元数与旋转矩阵的转换关系
导语:最近在搭建电机与机械臂的联合仿真,闲暇之余顺便看了一下姿态轨迹规划的文章,对姿态的插补有点感兴趣,发现用四元数表示姿态然后来进行姿态的插补非常方便,且不会出现奇异.这里对四元数与常规的旋转矩阵的 ...
欧拉角、旋转矩阵及四元数
欧拉角.旋转矩阵及四元数 1. 简介 2. 欧拉角 2.1 欧拉角定义 2.2 右手系和左手系 2.3 转换流程 3. 旋转矩阵 4. 四元数 4.1 四元数与欧拉角和旋转矩阵之间等效变换 4.2 测 ...
角速度与旋转矩阵的转换关系及思考
在机器人的控制和轨迹规划等领域,算法结果常常能够得到机器人执行器的速度,如移动机器人的角速度和线速度.在有些仿真情况下,或是其它需要实时更新机器人的运动状态时,我们常常需要根据这些计算出的速度参数(控 ...
PX4从放弃到精通（五）：PX4中的姿态表示方法及转换关系
文章目录前言坐标系无人机的姿态描述转换关系总结前言一个人可以走的更快,一群人才能走的更远,交流学习加qq:2096723956 更多保姆级PX4+ROS学习视频:https://b23. ...

旋转矩阵、欧拉角、四元数理论及其转换关系

旋转矩阵、欧拉角、四元数理论及其转换关系

1、概述

2、原理

2.1 旋转矩阵

2.2 欧拉角

2.2.1 由欧拉角求旋转矩阵

2.2.2 由旋转矩阵求欧拉角

2.3 四元数

2.3.1 四元数基本概念

2.3.2 四元数求旋转矩阵

2.3.3 旋转矩阵求四元数

参考资料

旋转矩阵、欧拉角、四元数理论及其转换关系相关推荐

最新文章

热门文章