跳到主要内容

08 - 矩阵,欧拉角和四元数的转换

在这篇文章中，我们简单了解一下表示3D旋转的矩阵、欧拉角以及四元数之间是如何互相转换的。

部分内容可能有计算错误，如果有错希望大家可以指出~

转换为矩阵​

欧拉角​

欧拉角即绕轴旋转，但轴是有不同定义的。例如对象空间(Object Space)的轴是轴，直立空间(Upright Space, 原点位于物体中心，三个轴平行于世界坐标轴)的轴也是轴。因此，还得分情况讨论。

在模型(Model)变换阶段，物体由对象空间变换到世界空间，这里就需要从对象空间到直立空间的矩阵了：

M对象→直立=PHB\nonumber \mathrm{M_{\text{对象}\to\text{直立}}}=\mathbf{PHB}

其中，它们分别对应的是俯仰、航向和滚转这三个动作。这里的旋转顺序可以自由决定。这三个旋转矩阵的内容如下(右手系，变成左手系转置一下就行)：

P=Rx(p)=[1000cos⁡p−sin⁡p0sin⁡pcos⁡p]H=Ry(h)=[cos⁡h0sin⁡h010−sin⁡h0cos⁡h]B=Rz(b)=[cos⁡b−sin⁡b0sin⁡bcos⁡b0001]\nonumber \begin{aligned} \mathbf{P} &= \mathbf{R}_x(p) &= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos p & -\sin p \\ 0 & \sin p & \cos p \end{bmatrix} \\ \nonumber \mathbf{H} &= \mathbf{R}_y(h) &= \begin{bmatrix} \cos h & 0 & \sin h \\ 0 & 1 & 0 \\ -\sin h & 0 & \cos h \end{bmatrix} \\ \nonumber \mathbf{B} &= \mathbf{R}_z(b) &= \begin{bmatrix} \cos b & -\sin b & 0 \\ \sin b & \cos b & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{aligned}

在视图(View)变换阶段，物体由世界空间变换到相机的对象空间，这里需要将直立空间变换到对象空间的矩阵：

M直立→对象=M对象→直立−1=B−1H−1P−1\nonumber \mathrm{M_{\text{直立}\to\text{对象}}}= \mathrm{M_{\text{对象}\to\text{直立}}}^{-1}=\mathbf{B}^{-1}\mathbf{H}^{-1}\mathbf{P}^{-1}

接下来要求旋转矩阵的逆矩阵，由于旋转矩阵是正交矩阵，因此只需求旋转矩阵的转置即可。

四元数​

可以通过扩展四元数乘法qvq−1qvq^{-1}得出，这里直接给出结论了：

定义四元数

q=[w, (x,y,z)]=[cos⁡(θ2), (nxsin⁡(θ2), nysin⁡(θ2), nzsin⁡(θ2))\begin{align} \nonumber q &= [w,\,(x,y,z)] \\ \nonumber &= [\cos(\frac{\theta}{2}),\, (n_x\sin(\frac{\theta}{2}),\, n_y\sin(\frac{\theta}{2}),\, n_z\sin(\frac{\theta}{2}) ) \end{align}

其中，θ\theta是要旋转的角度，n^=(nx,ny,nz)\mathbf{\hat{n}}=(n_x,n_y,n_z)是单位旋转轴。

那么它转换的矩阵如下（右手系，变成左手系转置一下就行）

[1−2y2−2z22xy−2wz2xz+2wy2xy+2wz1−2x2−2z22yz−2wx2xz−2wy2yz+2wx1−2x2−2y2]\nonumber \begin{bmatrix} 1-2y^2-2z^2 & 2xy-2wz & 2xz+2wy \\ 2xy+2wz & 1-2x^2-2z^2 & 2yz-2wx \\ 2xz - 2wy & 2yz+2wx & 1-2x^2-2y^2 \end{bmatrix}

转换为欧拉角​

矩阵​

可以直接从旋转矩阵来求解三个欧拉角，需要具体情况具体分析。例如下面的矩阵是左手坐标系中混合好的旋转矩阵：

[cos⁡hcos⁡b+sin⁡hsin⁡psin⁡bsin⁡bcos⁡p−sin⁡hcos⁡b+cos⁡hsin⁡psin⁡b−cos⁡hsin⁡b+sin⁡hsin⁡pcos⁡bcos⁡bcos⁡psin⁡hsin⁡b+cos⁡hsin⁡pcos⁡bsin⁡hcos⁡p−sin⁡pcos⁡hcos⁡p]\nonumber \begin{bmatrix} \cos h\cos b+\sin h\sin p\sin b & \sin b\cos p & -\sin h\cos b + \cos h\sin p \sin b \\ -\cos h\sin b+\sin h\sin p\cos b & \cos b\cos p & \sin h\sin b + \cos h\sin p \cos b \\ \sin h\cos p & -\sin p & \cos h\cos p \end{bmatrix}

首先立马就能求出俯仰角pp：

p=arcsin⁡(−m32),p∈[−90°,90°]\nonumber p = \arcsin(-m_{32}),\quad p\in[-90°,90°]

知道pp后，就能确定cos⁡p\cos p的值了，然后就能求出hh和bb的值了。这得分两种情况来考虑：

  • cos⁡p≠0\cos p \neq 0：先求hh，发现m31=sin⁡hcos⁡pm_{31}=\sin h\cos p，m33=cos⁡hcos⁡pm_{33}=\cos h\cos p，那么有

    sin⁡h=m31cos⁡pcos⁡h=m33cos⁡p\begin{align} \nonumber \sin h &= \frac{m_{31}}{\cos p} \\ \nonumber \cos h &= \frac{m_{33}}{\cos p} \end{align}

    然后就能用C/C++提供的atan2(y, x)来求出hh了，这个函数返回以弧度表示的 y/x 的反正切

    atan2(sin⁡h,cos⁡h)=arctan⁡(sin⁡hcos⁡h)=arctan⁡(tan⁡h)=h\nonumber \mathrm{atan2}(\sin h, \cos h) = \arctan(\frac{\sin h}{\cos h}) = \arctan(\tan h) = h

    因此有

    h=atan2(m31,m33)\nonumber h=\mathrm{atan2}(m_{31},m_{33})

    这里利用 y/x 的特性把cos⁡p\cos p约分了，cos⁡p\cos p是正的，所以能约。

    类似有

    b=atan2(m12,m22)\nonumber b=\mathrm{atan2}(m_{12},m_{22})

  • cos⁡p=0\cos p = 0： 此时p=±90°p=\pm 90°，说明 发生了万向锁。这里令b=0°b=0°，然后化简上面的大矩阵：

    [cos⁡h0−sin⁡hsin⁡hsin⁡p0cos⁡hsin⁡p0−sin⁡p0]\nonumber \begin{bmatrix} \cos h & 0 & -\sin h \\ \sin h\sin p & 0 & \cos h\sin p \\ 0 & -\sin p & 0 \end{bmatrix}

    然后就能求hh了：

    h=atan2(−m13,m11)\nonumber h=\mathrm{atan2}(-m_{13},m_{11})

将矩阵转换为欧拉角总结如下：

p=arcsin⁡(−m32)h={atan2(m31,m33),cos⁡p≠0atan2(−m13,m11),cos⁡p=0b={atan2(m12,m22),cos⁡p≠00,cos⁡p=0\begin{align} \nonumber p &= \arcsin(-m_{32}) \\ \nonumber h &= \left\{\begin{matrix} \mathrm{atan2}(m_{31},m_{33}), \quad \cos p \neq 0 \\ \mathrm{atan2}(-m_{13},m_{11}),\quad \cos p = 0 \end{matrix}\right. \\ \nonumber b &= \left\{\begin{matrix} \mathrm{atan2}(m_{12},m_{22}), \quad \cos p \neq 0 \\ 0,\quad \cos p = 0 \end{matrix}\right. \end{align}

四元数​

可以对 “欧拉角->四元数”中得出的的结论进行逆向工程；也可以先把四元数转换为矩阵，然后再把得到的矩阵转换为欧拉角。这里康康第二种思路。

把矩阵的值代入上边的结论中，然后进行化简便能得到：

p=arcsin⁡(−2(yz−wx))h={atan2(xz+wy,1/2−x2−y2),cos⁡p≠0atan2(−xz+wy,1/2−y2−z2),cos⁡p=0b={atan2(xy+wz,1/2−x2−z2),cos⁡p≠00,cos⁡p=0\begin{align} \nonumber p &= \arcsin(-2(yz-wx)) \\ \nonumber h &= \left\{\begin{matrix} \mathrm{atan2}(xz+wy,1/2-x^2-y^2), \quad \cos p \neq 0 \\ \mathrm{atan2}(-xz+wy,1/2-y^2-z^2),\quad \cos p = 0 \end{matrix}\right. \\ \nonumber b &= \left\{\begin{matrix} \mathrm{atan2}(xy+wz,1/2-x^2-z^2), \quad \cos p \neq 0 \\ 0,\quad \cos p = 0 \end{matrix}\right. \end{align}

这个是从对象空间到直立空间的欧拉角，如果想要从直立空间到对象空间的欧拉角，只需求四元数的共轭，将x，y，z取负即可。

转换为四元数​

矩阵​

可以对由四元数转换的矩阵进行逆向工程。检查对角线元素的总和，即矩阵的迹(Trace)，有：

tr(M)=m11+m22+m33=(1−2y2−2z2)+(1−2x2−2z2)+(1−2x2−2y2)=3−4(x2+y2+z2)=3−4(1−w2)(单位四元数模长是1)=4w2−1\begin{align} \nonumber \mathrm{tr}(\mathbf{M}) &= m_{11}+m_{22}+m_{33} \\ \nonumber &= (1-2y^2-2z^2) + (1-2x^2-2z^2) + (1-2x^2-2y^2) \\ \nonumber &= 3-4(x^2+y^2+z^2) \\ \nonumber &= 3-4(1-w^2)\qquad\qquad\qquad\qquad\quad \text{(单位四元数模长是1)} \\ \nonumber &=4w^2-1 \end{align}

因此有

w=m11+m22+m33+12\nonumber w=\frac{\sqrt{m_{11}+m_{22}+m_{33} + 1}}{2}

类似地，有

x=m11−m22−m33+12y=−m11+m22−m33+12z=−m11−m22+m33+12\begin{align} \nonumber x &= \frac{\sqrt{m_{11}-m_{22}-m_{33} + 1}}{2} \\ \nonumber y &= \frac{\sqrt{-m_{11}+m_{22}-m_{33} + 1}}{2} \\ \nonumber z &= \frac{\sqrt{-m_{11}-m_{22}+m_{33} + 1}}{2} \\ \end{align}

由于平方根是非负的，我们只能在这4个分量里选1个分量，让它作为非负根，而不是让所有4个分量全非负。

因此还得寻找其他方法计算剩余的3个分量——检查对角矩阵元素的总和与差值：

m21+m12=4xym21−m12=4wzm13+m31=4xzm13−m31=4wym23+m32=4yzm32−m23=4wx\begin{align} \nonumber m_{21}+m_{12} &= 4xy \\ \nonumber m_{21}-m_{12} &= 4wz \\ \nonumber m_{13}+m_{31} &= 4xz \\ \nonumber m_{13}-m_{31} &= 4wy \\ \nonumber m_{23}+m_{32} &= 4yz \\ \nonumber m_{32}-m_{23} &= 4wx \end{align}

使用这些式子和上面其中一个分量就能将四元数从旋转矩阵中提取出来：

w=m11+m22+m33+12⇒x=m32−m234wy=m13−m314wz=m21−m124wx=m11−m22−m33+12⇒w=m32−m234xy=m21+m124xz=m13+m314xy=−m11+m22−m33+12⇒w=m13−m314yx=m21+m124yz=m32+m234yz=−m11−m22+m33+12⇒w=m21−m124zx=m13+m314zy=m32+m234z\begin{align} \nonumber w &= \frac{\sqrt{m_{11}+m_{22}+m_{33} + 1}}{2} \quad\Rightarrow\quad x=\frac{m_{32}-m_{23}}{4w} \quad y=\frac{m_{13}-m_{31}}{4w} \quad z=\frac{m_{21}-m_{12}}{4w} \\ \nonumber x &= \frac{\sqrt{m_{11}-m_{22}-m_{33} + 1}}{2} \quad\Rightarrow\quad w=\frac{m_{32}-m_{23}}{4x} \quad y=\frac{m_{21}+m_{12}}{4x} \quad z=\frac{m_{13}+m_{31}}{4x} \\ \nonumber y &= \frac{\sqrt{-m_{11}+m_{22}-m_{33} + 1}}{2} \quad\Rightarrow\quad w=\frac{m_{13}-m_{31}}{4y} \quad x=\frac{m_{21}+m_{12}}{4y} \quad z=\frac{m_{32}+m_{23}}{4y} \\ \nonumber z &= \frac{\sqrt{-m_{11}-m_{22}+m_{33} + 1}}{2} \quad\Rightarrow\quad w=\frac{m_{21}-m_{12}}{4z} \quad x=\frac{m_{13}+m_{31}}{4z} \quad y=\frac{m_{32}+m_{23}}{4z} \end{align}

那我们该使用哪一行呢？w,x,y,z哪个有最大绝对值就用哪个，因为如果太小了的话会让数值不稳定，也就是突然抖动。为了避免开平方，只需比较这四个分量对应的m11±m22±m33m_{11}\pm m_{22}\pm m_{33}​哪个最大就行，然后先计算出它，再用后面的式子去计算其他分量。

欧拉角​

和欧拉角转换矩阵一样，也要分两种情况讨论：

  • 从对象空间到直立空间的四元数：

    由于直立空间和世界空间的坐标轴平行，有

p=[cos⁡(p/2)(sin⁡(p/2)00)]h=[cos⁡(h/2)(0sin⁡(h/2)0)]b=[cos⁡(b/2)(00sin⁡(b/2))] \begin{align} \nonumber \mathbf{p} &= \begin{bmatrix} \cos(p/2) & (\sin(p/2) & 0 & 0) \end{bmatrix} \\ \nonumber \mathbf{h} &= \begin{bmatrix} \cos(h/2) & (0 & \sin(h/2) & 0) \end{bmatrix} \\ \nonumber \mathbf{b} &= \begin{bmatrix} \cos(b/2) & (0 & 0 & \sin(b/2)) \end{bmatrix} \\ \end{align}

它们分别按x，y，z轴旋转。接下来按ZYX顺序进行旋转的组合，得到四元数：

q对象→直立(b,h,p)=phb=[cos⁡(p/2)(sin⁡(p/2)00)][cos⁡(h/2)(0sin⁡(h/2)0)][cos⁡(b/2)(00sin⁡(b/2))]=[cos⁡(p/2)cos⁡(h/2)(sin⁡(p/2)cos⁡(h/2)cos⁡(p/2)sin⁡(h/2)sin⁡(p/2)sin⁡(h/2))][cos⁡(b/2)(00sin⁡(b/2))]=[cos⁡(p/2)cos⁡(h/2)cos⁡(b/2)−sin⁡(p/2)sin⁡(h/2)sin⁡(b/2)(sin⁡(p/2)cos⁡(h/2)cos⁡(b/2)+sin⁡(p/2)cos⁡(h/2)sin⁡(b/2)cos⁡(p/2)sin⁡(h/2)cos⁡(b/2)−sin⁡(p/2)cos⁡(h/2)sin⁡(b/2)cos⁡(p/2)cos⁡(h/2)sin⁡(b/2)+sin⁡(p/2)sin⁡(h/2)cos⁡(b/2))] \begin{align} \nonumber q_{\text{对象}\to\text{直立}}(b,h,p) &= \mathbf{phb} \\ \nonumber &= \begin{bmatrix} \cos(p/2) \\ \begin{pmatrix} \sin(p/2) \\ 0 \\ 0 \end{pmatrix} \end{bmatrix} \begin{bmatrix} \cos(h/2) \\ \begin{pmatrix} 0 \\ \sin(h/2) \\ 0 \end{pmatrix} \end{bmatrix} \begin{bmatrix} \cos(b/2) \\ \begin{pmatrix} 0 \\ 0 \\ \sin(b/2) \end{pmatrix} \end{bmatrix} \\ \nonumber &= \begin{bmatrix} \cos(p/2)\cos(h/2) \\ \begin{pmatrix} \sin(p/2)\cos(h/2) \\ \cos(p/2)\sin(h/2) \\ \sin(p/2)\sin(h/2) \end{pmatrix} \end{bmatrix} \begin{bmatrix} \cos(b/2) \\ \begin{pmatrix} 0 \\ 0 \\ \sin(b/2) \end{pmatrix} \end{bmatrix} \\ \nonumber &= \begin{bmatrix} \cos(p/2)\cos(h/2)\cos(b/2) - \sin(p/2)\sin(h/2)\sin(b/2) \\ \begin{pmatrix} \sin(p/2)\cos(h/2)\cos(b/2)+\sin(p/2)\cos(h/2)\sin(b/2) \\ \cos(p/2)\sin(h/2)\cos(b/2)-\sin(p/2)\cos(h/2)\sin(b/2) \\ \cos(p/2)\cos(h/2)\sin(b/2)+\sin(p/2)\sin(h/2)\cos(b/2) \end{pmatrix}\end{bmatrix} \\ \end{align}

  • 从直立空间到对象空间的四元数：

    只需求 q对象→直立∗q_{\text{对象}\to\text{直立}}* 即可，因为单位四元数的逆和共轭相同：

q直立→对象(b,h,p)=q对象→直立(b,h,p)∗=[cos⁡(p/2)cos⁡(h/2)cos⁡(b/2)−sin⁡(p/2)sin⁡(h/2)sin⁡(b/2)(−sin⁡(p/2)cos⁡(h/2)cos⁡(b/2)−sin⁡(p/2)cos⁡(h/2)sin⁡(b/2)−cos⁡(p/2)sin⁡(h/2)cos⁡(b/2)+sin⁡(p/2)cos⁡(h/2)sin⁡(b/2)−cos⁡(p/2)cos⁡(h/2)sin⁡(b/2)−sin⁡(p/2)sin⁡(h/2)cos⁡(b/2))] \begin{align} \nonumber q_{\text{直立}\to\text{对象}}(b,h,p) &= q_{\text{对象}\to\text{直立}}(b,h,p)^* \\ \nonumber &= \begin{bmatrix} \cos(p/2)\cos(h/2)\cos(b/2) - \sin(p/2)\sin(h/2)\sin(b/2) \\ \begin{pmatrix} -\sin(p/2)\cos(h/2)\cos(b/2)-\sin(p/2)\cos(h/2)\sin(b/2) \\ -\cos(p/2)\sin(h/2)\cos(b/2)+\sin(p/2)\cos(h/2)\sin(b/2) \\ -\cos(p/2)\cos(h/2)\sin(b/2)-\sin(p/2)\sin(h/2)\cos(b/2) \end{pmatrix}\end{bmatrix} \\ \end{align}

参考资料​