刚体运动学姿态推导——弄懂欧拉角、四元数、旋转矩阵（一）姿态表示
最近做项目用到了刚体姿态方面的知识，乱七八糟东西一堆，尤其用到速度表示的微分运动学，特别容易弄混，因此写个帖子方便复习和查找 。主要是记录机器人方向用到的知识。
旋转矩阵
对于空间中的两组坐标系[ x 1 x_1 x1， y 1 y_1 y1, z 1 z_1 z1],[ x 0 x_0 x0， y 0 y_0 y0, z 0 z_0 z0]我们可以用一个旋转矩阵来描述二者之间的姿态变化：
R 1 0 = ( x 1 ⋅ x 0 y 1 ⋅ x 0 z 1 ⋅ x 0 x 1 ⋅ y 0 y 1 ⋅ y 0 z 1 ⋅ y 0 x 1 ⋅ z 0 y 1 ⋅ z 0 z 1 ⋅ z 0 ) R_{1}^{0}=\left(\begin{array}{lll} x_{1} \cdot x_{0} & y_{1} \cdot x_{0} & z_{1} \cdot x_{0} \\ x_{1} \cdot y_{0} & y_{1} \cdot y_{0} & z_{1} \cdot y_{0} \\ x_{1} \cdot z_{0} & y_{1} \cdot z_{0} & z_{1} \cdot z_{0} \end{array}\right) R10=⎝⎛x1⋅x0x1⋅y0x1⋅z0y1⋅x0y1⋅y0y1⋅z0z1⋅x0z1⋅y0z1⋅z0⎠⎞
他的意思是，坐标系0经过 R 1 0 R_{1}^{0} R10的旋转，姿态会和坐标系1相同。也就是说 R 1 0 R_{1}^{0} R10表示了从0到1的旋转。上标是出发系，下标是目标系。 R 1 0 R_{1}^{0} R10的具体数值由出发系的转置点乘目标系得到，即
R 1 0 = [ x 0 y 0 z 0 ] T ⋅ [ x 1 y 1 z 1 ] R_{1}^{0}=[x_{0} y_{0}z_{0}]^T \cdot [x_{1} y_{1}z_{1}] R10=[x0y0z0]T⋅[x1y1z1]
当然，旋转矩阵除了表示坐标系之间的变换，还有其他的作用，首先，可以表示同一个点在不同系下坐标的关系。设点 p p p在0系下的坐标是 p 0 p^0 p0，在1系下的坐标是 p 1 p^1 p1，则：
p 0 = R 1 0 p 1 p^{0}=R_{1}^{0}p^{1} p0=R10p1
此外，它还有第三层含义，即表示向量在同一个坐标系中的旋转。设有一个向量 v 0 v_{0} v0，经过一个旋转 R 1 0 R_{1}^{0} R10后变为向量 v 1 v_{1} v1，则：
v 1 = R 0 1 v 0 v^{1}=R_{0}^{1}v^{0} v1=R01v0
因此，R其实有三种含义：
- 表示点在不同坐标系下的转换
- 表示坐标系之间的旋转
- 表示向量之间的旋转 。
使用时，要分清楚究竟是哪一层意思，从哪一系到哪一系，就能避免混淆。
欧拉角
旋转矩阵用9个元素表示一个三维的运动（旋转），显然是包含约束的。这样使用非常不方便。因此提出了一种只用三个元素的表示方法，欧拉角。欧拉角表示刚体按顺序绕三个轴依次旋转，这三个轴有很多选择，既可以选则固定轴，又可以选择旋转后的轴。因此，广义上的欧拉角有非常多种，Z-X-Z,X-Y-Z等等。在机器人学中，常用的是两种：
- RPY角：刚体绕固定系的X轴转γ角，再绕固定系的Y轴旋转β角，最后绕固定系的Z轴旋转α角，三次都是绕固定系。
B A R X Y Z ( γ , β , α ) = R Z ( α ) R Y ( β ) R X ( γ ) = [ c α c β c α s β s γ − s α c γ c α s β c γ + s α s ν s α s β s α s β s γ + c α c γ s α s β c γ − c α s γ − s β c β s γ c β c γ ] { }_{B}^{A} R_{X Y Z}(\gamma, \beta, \alpha)=R_{Z}(\alpha) R_{Y}(\beta) R_{X}(\gamma)=\left[\begin{array}{ccc} c \alpha c \beta & c \alpha s \beta s \gamma-s \alpha c \gamma & c \alpha s \beta c \gamma+s \alpha s \nu \\ s \alpha s \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{array}\right] BARXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=⎣⎡cαcβsαsβ−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γ⎦⎤
因为是绕固定系，所以每次是左乘。这种方法测出的三个角为roll（绕x轴），pitch（绕y轴），yaw（绕z轴）。 - ZYX角，刚体绕绕自身的Z轴旋转α，然后绕自身的Y轴旋转β ，最后绕自身的X轴旋转γ，后两次旋转是绕旋转后的系。
A B R Z / Y ′ X ′ ( γ , β , α ) = R Z ( α ) R Y ( β ) R X ( γ ) = [ c α c β c α s β s γ − s α c γ c α s β c γ + s α s γ s α s β s α s β s γ + c α c γ s α s β c γ − c α s γ − s β c β s γ c β c γ ] A_{B} R_{Z / Y \prime X \prime}(\gamma, \beta, \alpha)=R_{Z}(\alpha) R_{Y}(\beta) R_{X}(\gamma)=\left[\begin{array}{ccc} 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 s \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{array}\right] ABRZ/Y′X′(γ,β,α)=RZ(α)RY(β)RX(γ)=⎣⎡cαcβsαsβ−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γ⎦⎤
我们发现，这两种方法是等价的。也就是说，只要绕对应轴转动的角度相同，虽然顺序不同，但转动的结果也是相同的。因此，之后对这两种方法不再区分 。
欧拉角有一个很重要的问题，那就是万向节死锁。当绕y轴旋转90度时，我们发现，之前绕x轴转和之后绕z轴转，转动方向是一致的。也就是说，这两个运动其实是在一个自由度上。那么势必有些姿态就无法达到，也就无法用欧拉角表示了。因此，我们提出了四元数的方法。
四元数
四元素用四个数来表示旋转。即 Q = { η , ε } \mathrm{Q}=\{\eta, \varepsilon\} Q={η,ε}
η = cos θ 2 ε x = sin θ 2 r x ε y = sin θ 2 r y ε z = sin θ 2 r z \begin{aligned} \eta &=\cos \frac{\theta}{2} \\ \varepsilon_{x} &=\sin \frac{\theta}{2} r_{x}\\ \varepsilon_{y} &=\sin \frac{\theta}{2} r_{y}\\ \varepsilon_{z} &=\sin \frac{\theta}{2} r_{z} \end{aligned} ηεxεyεz=cos2θ=sin2θrx=sin2θry=sin2θrz
四元数不存在奇异点，但比较抽象，不容易理解。
旋转矩阵与欧拉角的转换
欧拉角转旋转矩阵
欧拉角其实就是三次独立的旋转，每次旋转的矩阵为：
R x = [ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ] R y = [ cos ϕ 0 sin ϕ 0 1 0 − sin ϕ 0 cos ϕ ] R z = [ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ] R_{x}=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{array}\right]\\ R_{y}=\left[\begin{array}{ccc} \cos \phi & 0 & \sin \phi \\ 0 & 1 & 0 \\ -\sin \phi & 0 & \cos \phi \end{array}\right]\\ R_{z}=\left[\begin{array}{ccc} \cos \psi & -\sin \psi & 0 \\ \sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{array}\right] Rx=⎣⎡1000cosθsinθ0−sinθcosθ⎦⎤Ry=⎣⎡cosϕ0−sinϕ010sinϕ0cosϕ⎦⎤Rz=⎣⎡cosψsinψ0−sinψcosψ0001⎦⎤
得到：
R = R z ( ϕ ) R y ( θ ) R x ( ψ ) = [ cos θ cos ϕ sin ψ sin θ cos ϕ − cos ψ sin θ cos ψ sin θ cos ϕ + sin ψ sin ϕ cos θ sin ϕ sin ψ sin θ sin ϕ + cos ψ cos ϕ cos ψ sin θ sin ϕ − sin ψ cos θ − sin θ sin ψ cos θ cos ψ cos θ ] R=R_{z}(\phi) R_{y}(\theta) R_{x}(\psi)=\left[\begin{array}{ccc} \cos \theta \cos \phi & \sin \psi \sin \theta \cos \phi-\cos \psi \sin \theta & \cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi \\ \cos \theta \sin \phi & \sin \psi \sin \theta \sin \phi+\cos \psi \cos \phi & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \theta \\ -\sin \theta & \sin \psi \cos \theta & \cos \psi \cos \theta \end{array}\right] R=Rz(ϕ)Ry(θ)Rx(ψ)=⎣⎡cosθcosϕcosθsinϕ−sinθsinψsinθcosϕ−cosψsinθsinψsinθsinϕ+cosψcosϕsinψcosθcosψsinθcosϕ+sinψsinϕcosψsinθsinϕ−sinψcosθcosψcosθ⎦⎤
旋转矩阵转欧拉角
做一些简单的代数运算即可得到：
θ x = atan 2 ( r 32 , r 33 ) θ y = atan 2 ( − r 31 , r 32 2 + r 33 2 ) θ z = atan 2 ( r 21 , r 11 ) \begin{array}{c} \theta_{x}=\operatorname{atan} 2\left(r_{32}, r_{33}\right) \\ \theta_{y}=\operatorname{atan} 2\left(-r_{31}, \sqrt{r_{32}^{2}+r_{33}^{2}}\right) \\ \theta_{z}=\operatorname{atan} 2\left(r_{21}, r_{11}\right) \end{array} θx=atan2(r32,r33)θy=atan2(−r31,r322+r332 )θz=atan2(r21,r11)
旋转矩阵与四元数的转换
四元数转旋转矩阵
推导过程比较复杂，首先，利用罗德里格旋转公式获得从角轴到旋转矩阵的转换，对于单位向量 ( x , y , z ) (x,y,z) (x,y,z)旋转 θ \theta θ角后得到的旋转矩阵：
R = I + ω ^ sin ( θ ) + ω ^ 2 ( 1 − cos ( θ ) ) R=I+\widehat{\omega} \sin (\theta)+\widehat{\omega}^{2}(1-\cos (\theta)) R=I+ω sin(θ)+ω 2(1−cos(θ))
R = [ cos θ + x 2 ( 1 − cos θ ) x y ( 1 − cos θ ) y sin θ x y ( 1 − cos θ ) cos θ + y 2 ( 1 − cos θ ) − x sin θ − y sin θ x sin θ cos θ ] R=\left[\begin{array}{ccc} \cos \theta+x^{2}(1-\cos \theta) & x y(1-\cos \theta) & y \sin \theta \\ x y(1-\cos \theta) & \cos \theta+y^{2}(1-\cos \theta) & -x \sin \theta \\ -y \sin \theta & x \sin \theta & \cos \theta \end{array}\right] R=⎣⎡cosθ+x2(1−cosθ)xy(1−cosθ)−ysinθxy(1−cosθ)cosθ+y2(1−cosθ)xsinθysinθ−xsinθcosθ⎦⎤
再从角轴转化为四元数：
[ 1 − 2 y 2 − 2 z 2 2 x y + 2 w z 2 x z − 2 w y 2 x y − 2 w z 1 − 2 x 2 − 2 z 2 2 y z + 2 w x 2 x z + 2 w y 2 y z − 2 w x 1 − 2 x 2 − 2 y 2 ] \left[\begin{array}{ccc} 1-2 y^{2}-2 z^{2} & 2 x y+2 w z & 2 x z-2 w y \\ 2 x y-2 w z & 1-2 x^{2}-2 z^{2} & 2 y z+2 w x \\ 2 x z+2 w y & 2 y z-2 w x & 1-2 x^{2}-2 y^{2} \end{array}\right] ⎣⎡1−2y2−2z22xy−2wz2xz+2wy2xy+2wz1−2x2−2z22yz−2wx2xz−2wy2yz+2wx1−2x2−2y2⎦⎤
旋转矩阵转四元数
旋转矩阵转四元数需要用到上面的结论：
m 12 + m 21 = ( 2 x y + 2 w z ) + ( 2 x y − 2 w z ) = 4 x y m 12 − m 21 = ( 2 x y + 2 w z ) − ( 2 x y − 2 w z ) = 4 w z m 31 + m 13 = ( 2 x z + 2 w y ) + ( 2 x z − 2 w y ) = 4 x z m 31 − m 13 = ( 2 x z + 2 w y ) − ( 2 x z − 2 w y ) = 4 w y m 23 + m 32 = ( 2 y z + 2 w x ) + ( 2 y z − 2 w x ) = 4 y z m 23 − m 32 = ( 2 y z + 2 w x ) − ( 2 y z − 2 w x ) = 4 w x \begin{array}{l} m_{12}+m_{21}=(2 x y+2 w z)+(2 x y-2 w z)=4 x y \\ m_{12}-m_{21}=(2 x y+2 w z)-(2 x y-2 w z)=4 w z \\ m_{31}+m_{13}=(2 x z+2 w y)+(2 x z-2 w y)=4 x z \\ m_{31}-m_{13}=(2 x z+2 w y)-(2 x z-2 w y)=4 w y \\ m_{23}+m_{32}=(2 y z+2 w x)+(2 y z-2 w x)=4 y z \\ m_{23}-m_{32}=(2 y z+2 w x)-(2 y z-2 w x)=4 w x \end{array} m12+m21=(2xy+2wz)+(2xy−2wz)=4xym12−m21=(2xy+2wz)−(2xy−2wz)=4wzm31+m13=(2xz+2wy)+(2xz−2wy)=4xzm31−m13=(2xz+2wy)−(2xz−2wy)=4wym23+m32=(2yz+2wx)+(2yz−2wx)=4yzm23−m32=(2yz+2wx)−(2yz−2wx)=4wx
于是：
w = m 11 + m 22 + m 33 + 1 2 w=\frac{\sqrt{m_{11}+m_{22}+m_{33}+1}}{2} w=2m11+m22+m33+1
x = m 11 − m 22 − m 33 + 1 2 x=\frac{\sqrt{m_{11}-m_{22}-m_{33}+1}}{2} x=2m11−m22−m33+1
y = − m 11 + m 22 − m 33 + 1 2 y=\frac{\sqrt{-m_{11}+m_{22}-m_{33}+1}}{2} y=2−m11+m22−m33+1
z = − m 11 − m 22 + m 33 + 1 2 z=\frac{\sqrt{-m_{11}-m_{22}+m_{33}+1}}{2} z=2−m11−m22+m33+1
欧拉角与四元数之间的转换
欧拉角转四元数
欧拉角相当于三次绕定轴的旋转，每次选择都可以方便地用四元数进行转换，因此得到以下结果：
q = [ cos γ 2 0 0 sin γ 2 ] [ cos β 2 0 sin β 2 0 ] [ cos α 2 sin α 2 0 0 ] = [ cos α 2 cos β 2 cos γ 2 + sin α 2 sin β 2 sin γ 2 sin α 2 cos β 2 cos γ 2 − cos α 2 sin β 2 sin γ 2 cos α 2 sin β 2 cos γ 2 + sin α 2 cos β 2 sin γ 2 cos α 2 cos β 2 sin γ 2 − sin α 2 sin β 2 cos γ 2 ] q=\left[\begin{array}{c} \cos \frac{\gamma}{2} \\ 0 \\ 0 \\ \sin \frac{\gamma}{2} \end{array}\right]\left[\begin{array}{c} \cos \frac{\beta}{2} \\ 0 \\ \sin \frac{\beta}{2} \\ 0 \end{array}\right]\left[\begin{array}{c} \cos \frac{\alpha}{2} \\ \sin \frac{\alpha}{2} \\ 0 \\ 0 \end{array}\right]=\left[\begin{array}{c} \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{array}\right] 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γ⎦⎥⎥⎤
四元数转欧拉角
求得上述方程的逆解，即可求得欧拉角：
[ α β γ ] = [ atan 2 ( 2 ( q 0 q 1 + q 2 q 3 ) , 1 − 2 ( q 1 2 + q 2 2 ) ) arcsin ( 2 ( q 0 q 2 − q 1 q 3 ) ) atan 2 ( 2 ( q 0 q 3 + q 1 q 2 ) , 1 − 2 ( q 2 2 + q 3 2 ) ) ] \left[\begin{array}{c} \alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c} \operatorname{atan} 2\left(2\left(q_{0} q_{1}+q_{2} q_{3}\right), 1-2\left(q_{1}^{2}+q_{2}^{2}\right)\right) \\ \arcsin \left(2\left(q_{0} q_{2}-q_{1} q_{3}\right)\right) \\ \operatorname{atan} 2\left(2\left(q_{0} q_{3}+q_{1} q_{2}\right), 1-2\left(q_{2}^{2}+q_{3}^{2}\right)\right) \end{array}\right] ⎣⎡αβγ⎦⎤=⎣⎡atan2(2(q0q1+q2q3),1−2(q12+q22))arcsin(2(q0q2−q1q3))atan2(2(q0q3+q1q2),1−2(q22+q32))⎦⎤
