旋转矩阵|万向锁|四元数

文章目录

旋转公式2D3D万向锁四元数3D旋转公式Reference

旋转公式

2D

矩阵形式

v′=[cos(θ)−sin(θ)sin(θ)cos(θ)]vv' = \begin{bmatrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta) \\ \end{bmatrix}v v′=[cos(θ)sin(θ)​−sin(θ)cos(θ)​]v

复数形式

v′=zv=(cos(θ)+isin(θ))vv' = zv = (cos(\theta) + i sin(\theta))v v′=zv=(cos(θ)+isin(θ))v

指数形式

v′=eiθvv' = e^{i\theta}v v′=eiθv

复合形式

复数的乘法遵守交换律，对于两个复数有：

z=z1z2=z2z1z = z_1z_2 = z_2z_1 z=z1​z2​=z2​z1​

所以有复合形式：

z=(cos(θ)+isin(θ))(cos(ϕ)+isin(ϕ))=cos(θ+ϕ)+isin(θ+ϕ)z = (cos(\theta) + isin(\theta))(cos(\phi) + isin(\phi)) = cos(\theta+\phi) + isin(\theta+\phi) z=(cos(θ)+isin(θ))(cos(ϕ)+isin(ϕ))=cos(θ+ϕ)+isin(θ+ϕ)

实际上就是指数形式的乘法：

z=ei(θ+ϕ)z = e^{i(\theta + \phi)} z=ei(θ+ϕ)

3D

欧拉角形式

绕着三个坐标轴的旋转（以物体为原点）

绕着x轴：

Rx(θ)=[1000cos(θ)−sin(θ)0sin(θ)cos(θ)]R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta) \\ \end{bmatrix} Rx​(θ)=⎣⎡​100​0cos(θ)sin(θ)​0−sin(θ)cos(θ)​⎦⎤​

绕着y轴：

Ry(θ)=[cos(θ)0sin(θ)010−sin(θ)0cos(θ)]R_y(\theta) = \begin{bmatrix} cos(\theta) & 0 & sin(\theta) \\ 0 & 1 & 0 \\ -sin(\theta) & 0 & cos(\theta) \\ \end{bmatrix} Ry​(θ)=⎣⎡​cos(θ)0−sin(θ)​010​sin(θ)0cos(θ)​⎦⎤​

绕着z轴：

Rz(θ)=[cos(θ)−sin(θ)0sin(θ)cos(θ)0001]R_z(\theta) = \begin{bmatrix} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0\\ 0 & 0 & 1 \\ \end{bmatrix} Rz​(θ)=⎣⎡​cos(θ)sin(θ)0​−sin(θ)cos(θ)0​001​⎦⎤​

实际上用四维的矩阵比较多，方便做别的线性变换，如下图^1：

Rodrigues’ Rotation Formula

轴角式，绕着任意单位向量 u⃗\vec{u}u（以 u⃗\vec{u}u 作为旋转的轴，以物体作为原点）旋转 θ\thetaθ 角度

v′⃗=cos(θ)v⃗+(1−cos(θ))(u⃗⋅v⃗)u⃗+sin(θ)(u⃗×v⃗)\vec{v'} = cos(\theta)\vec{v} + (1-cos(\theta))(\vec{u} \cdot \vec{v})\vec{u} + sin(\theta)(\vec{u} \times \vec{v}) v′=cos(θ)v+(1−cos(θ))(u⋅v)u+sin(θ)(u×v)

推导如下

实际上，将v⃗\vec{v}v分解为平行于和垂直于单位向量u⃗\vec{u}u的两个向量v∥⃗\vec{v_{\parallel}}v∥​​和v⊥⃗\vec{v_{\perp}}v⊥​​，

其中，

v∥⃗=Proju⃗(v⃗)=u⃗⋅v⃗∣∣u⃗∣∣2u⃗\vec{v_{\parallel}} = Proj_{\vec{u}}(\vec{v}) = \frac{\vec{u} \cdot {\vec{v}}} {||\vec{u}||^2}\vec{u} v∥​​=Proju​(v)=∣∣u∣∣2u⋅v​u

由于 u⃗\vec{u}u 是单位向量，则平行分量为：

v∥⃗=(u⃗⋅v⃗)u⃗\vec{v_{\parallel}} = (\vec{u} \cdot \vec{v}) \vec{u} v∥​​=(u⋅v)u

再根据向量的分解，就可以得到垂直分量：

v⊥⃗=v⃗−v∥⃗=v⃗−(u⃗⋅v⃗)u⃗\vec{v_{\perp}} = \vec{v} - \vec{v_{\parallel}} = \vec{v} - (\vec{u} \cdot \vec{v}) \vec{u} v⊥​​=v−v∥​​=v−(u⋅v)u

而且在旋转 θ\thetaθ 角度后，平行分量保持不变：

v∥′⃗=v⊥⃗\vec{v'_{\parallel}} = \vec{v_{\perp}} v∥′​​=v⊥​​

垂直分量变为：

v⊥′⃗=v⊥⃗cos(θ)+w⃗sin(θ)\vec{v'_{\perp}} = \vec{v_{\perp}} cos(\theta) + \vec{w} sin(\theta) v⊥′​​=v⊥​​cos(θ)+wsin(θ)

其中，

w⃗=u⃗×v⊥⃗\vec{w} = \vec{u} \times \vec{v_{\perp}} w=u×v⊥​​

则旋转后，向量变为：

v′⃗=v∥′⃗+v⊥′⃗\vec{v'} = \vec{v'_{\parallel}} + \vec{v'_{\perp}} v′=v∥′​​+v⊥′​​

万向锁

根据欧拉角公式可知，在3D空间内的任何一个旋转都可以分解为绕着三个坐标轴（以物体为原点）的旋转^1：

E(pitch,roll,yaw)=Rz(yaw)Ry(roll)Rx(pitch)E(pitch,roll,yaw) = R_z(yaw)R_y(roll)R_x(pitch) E(pitch,roll,yaw)=Rz​(yaw)Ry​(roll)Rx​(pitch)

但是该公式的旋转顺序是写死的，因为矩阵乘法是没有交换律的，不可逆；

可以看到，在特殊的顺序和角度下，丢失了一个自由度。对于任意的角度，我们对z轴的旋转效果已经失效了。这种现象被称为万向锁（Gimbal Lock）^23。

四元数

怎么解决这种万向锁的情况？

引入四元数（Quaternion）^{4，其由一个实部和三个虚部组成，三个虚部可以使用三维的向量表示，但是由于我们只关心它与3D旋转之间的关系，所以省去推导，快进⏩，直接得到3D旋转公式}

3D旋转公式

四元数形式

令：

v=[0,v⃗]q=[cos(12θ),sin(12θ)u⃗]v = [0, \vec{v}] \\ q = [cos(\frac{1}{2}\theta), sin(\frac{1}{2}\theta)\vec{u}] v=[0,v]q=[cos(21​θ),sin(21​θ)u]

则有：

v′=qvq∗=qvq−1v' = qvq^* = qvq^{-1} v′=qvq∗=qvq−1

也就是说如果有这种情况下的 qqq 能够实现Rodrigues’ Rotation Formula的功能，即

绕着任意单位向量u⃗\vec{u}u（以u⃗\vec{u}u作为旋转的轴，以物体作为原点）旋转θ\thetaθ角度

矩阵形式

由于有时候我们需要将旋转与平移等线性变换相结合，所以矩阵形式还是十分必要的；

在经过巴拉巴拉的推导之后，令：

a=cos(12θ)b=sin(12θ)uxc=sin(12θ)uyd=sin(12θ)uza = cos(\frac{1}{2}\theta) \\ b = sin(\frac{1}{2}\theta)u_x \\ c = sin(\frac{1}{2}\theta)u_y \\ d = sin(\frac{1}{2}\theta)u_z a=cos(21​θ)b=sin(21​θ)ux​c=sin(21​θ)uy​d=sin(21​θ)uz​

则有：

v′=[1−2c2−2d22bc−2ad2ac+2bd2bc+2ad1−2b2−2d22cd−2ab2bd−2ac2ab+2cd1−2b2−2c2]vv' = \begin{bmatrix} 1−2c^2−2d^2 & 2bc−2ad & 2ac+2bd \\ 2bc+2ad & 1-2b^2-2d^2 & 2cd-2ab \\ 2bd-2ac & 2ab+2cd & 1-2b^2-2c^2 \\ \end{bmatrix}v v′=⎣⎡​1−2c2−2d22bc+2ad2bd−2ac​2bc−2ad1−2b2−2d22ab+2cd​2ac+2bd2cd−2ab1−2b2−2c2​⎦⎤​v

在有预计算的情况下，也还是比较快的。

Reference

[GAMES101] ↩︎ ↩︎

gimbal lock ↩︎

万向锁与欧拉角 ↩︎

quaternion ↩︎