旋转之三 - 旋转矩阵-旋转矩阵教程图解

 

绕坐标轴的旋转

绕着z轴旋转的3d矩阵写出来(推导过程可以见之前的文章)：

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} \\

类似可以写出绕x，y轴旋转的矩阵：

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}\\

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}\\

绕向量u旋转

假设物体绕单位向量 u=[uxuyuz]\mathbf{u} = \begin{bmatrix} u_x \\ u_y \\ u_z \end{bmatrix} 旋转 θ, 可以写出旋转矩阵：

R=[cos⁡θ+ux2(1−cos⁡θ)uxuy(1−cos⁡θ)−uzsin⁡θuxuz(1−cos⁡θ)+uysin⁡θuyux(1−cos⁡θ)+uzsin⁡θcos⁡θ+uy2(1−cos⁡θ)uyuz(1−cos⁡θ)−uxsin⁡θuzux(1−cos⁡θ)−uysin⁡θuzuy(1−cos⁡θ)+uxsin⁡θcos⁡θ+uz2(1−cos⁡θ)]R = \begin{bmatrix} \cos\theta + u_x^2(1- \cos \theta) & u_xu_y(1- \cos \theta) - u_z \sin \theta & u_xu_z(1- \cos \theta) + u_y \sin \theta \\ u_yu_x(1- \cos \theta) + u_z \sin \theta & \cos\theta + u_y^2(1- \cos \theta) & u_yu_z(1- \cos \theta) - u_x \sin \theta \\ u_zu_x(1- \cos \theta) - u_y \sin \theta & u_zu_y(1- \cos \theta) + u_x \sin \theta & \cos\theta + u_z^2(1- \cos \theta) \\ \end{bmatrix}\\

具体推导过程可以参见这篇文章9.2部分.

同时 R 也可以展开写成：

R=cos⁡θ[100010001]+(1−cos⁡θ)[ux2uxuyuxuzuyuxuy2uyuzuzuxuzuyuz2]+sin⁡θ[0−uzuyuz0−ux−uyux0]R = \cos \theta \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} + (1- \cos \theta) \begin{bmatrix} u_x^2 & u_xu_y & u_xu_z \\ u_yu_x & u_y^2 & u_yu_z \\ u_zu_x & u_zu_y & u_z^2 \end{bmatrix} + \sin \theta \begin{bmatrix} 0 & -u_z & u_y \\ u_z & 0 & -u_x \\ -u_y & u_x & 0 \end{bmatrix}\\

其中：

[ux2uxuyuxuzuyuxuy2uyuzuzuxuzuyuz2]=[uxuyuz][uxuyuz]=u⊗u=uuT\begin{bmatrix} u_x^2 & u_xu_y & u_xu_z \\ u_yu_x & u_y^2 & u_yu_z \\ u_zu_x & u_zu_y & u_z^2 \end{bmatrix} = \begin{bmatrix} u_x \\ u_y \\ u_z \end{bmatrix} \begin{bmatrix} u_x & u_y & u_z \end{bmatrix} = \mathbf{u} \otimes \mathbf{u} = \mathbf{u}\mathbf{u}^T\\

可以把这个矩阵看成外积或者转置形式。

而 [0−uzuyuz0−ux−uyux0]\begin{bmatrix} 0 & -u_z & u_y \\ u_z & 0 & -u_x \\ -u_y & u_x & 0 \end{bmatrix} 则是一个反对称矩阵（反对称矩阵为 AT=−AA^T = -A ，即 aij=−ajia_{ij} = - a_{ji} ），可以找到它对应的向量 u\mathbf{u} , 通常我们会这样表示，如果 u=[uxuyuz]\mathbf{u} =\begin{bmatrix} u_x \\ u_y \\ u_z \end{bmatrix} ，则我们用 [u]x[u]_x 表示它的反对称矩阵:

[u]x=[0−uzuyuz0−ux−uyux0][u]_x = \begin{bmatrix} 0 & -u_z & u_y \\ u_z & 0 & -u_x \\ -u_y & u_x & 0 \end{bmatrix}\\

所以R也可以写成：

R(u,θ)=cos⁡θI+(1−cos⁡θ)(u⊗u)+sin⁡θ[u]xR(\mathbf{u},\theta) = \cos \theta \mathbf{I} + (1- \cos \theta)(\mathbf{u} \otimes \mathbf{u}) + \sin \theta [\mathbf{u}]_x \\

非常类似 罗德里格旋转公式 ，其实我们也可以用推导罗德里格旋转公式的方法来推导出旋转矩阵。

正交矩阵

旋转矩阵的一个重要特点是它是一个正交矩阵，它的转置等于其逆，也就是满足: RT=R−1,RRT=IR^T = R^{-1}, RR^T = I 。

旋转矩阵的逆就是绕着轴转 -θ，所以它的逆可以写成:

R−1=[cos⁡(−θ)+ux2(1−cos⁡(−θ))uxuy(1−cos⁡(−θ))−uzsin⁡(−θ)uxuz(1−cos⁡(−θ))+uysin⁡(−θ)uyux(1−cos⁡(−θ))+uzsin⁡(−θ)cos⁡(−θ)+uy2(1−cos⁡(−θ))uyuz(1−cos⁡(−θ))−uxsin⁡(−θ)uzux(1−cos⁡(−θ))−uysin⁡(−θ)uzuy(1−cos⁡(−θ))+uxsin⁡(−θ)cos⁡(−θ)+uz2(1−cos⁡(−θ))]R^{-1} = \begin{bmatrix} \cos(-\theta) + u_x^2(1- \cos (-\theta)) & u_xu_y(1- \cos (-\theta)) - u_z \sin (-\theta) & u_xu_z(1- \cos (-\theta)) + u_y \sin (-\theta) \\ u_yu_x(1- \cos (-\theta)) + u_z \sin (-\theta) & \cos(-\theta) + u_y^2(1- \cos (-\theta)) & u_yu_z(1- \cos (-\theta)) - u_x \sin (-\theta) \\ u_zu_x(1- \cos (-\theta)) - u_y \sin (-\theta) & u_zu_y(1- \cos (-\theta)) + u_x \sin (-\theta) & \cos(-\theta) + u_z^2(1- \cos (-\theta)) \\ \end{bmatrix} \\

又 cos⁡θ=cos⁡(−θ)\cos \theta = \cos (- \theta) , sinθ=−sin⁡θsin \theta = -\sin \theta :

R−1=[cos⁡θ+ux2(1−cos⁡θ)uxuy(1−cos⁡θ)+uzsin⁡θuxuz(1−cos⁡θ)−uysin⁡θuyux(1−cos⁡θ)−uzsin⁡θcos⁡θ+uy2(1−cos⁡θ)uyuz(1−cos⁡θ)+uxsin⁡θuzux(1−cos⁡θ)+uysin⁡θuzuy(1−cos⁡θ)−uxsin⁡θcos⁡θ+uz2(1−cos⁡θ)]R^{-1} = \begin{bmatrix} \cos \theta + u_x^2(1- \cos \theta) & u_xu_y(1- \cos \theta) + u_z \sin \theta & u_xu_z(1- \cos \theta) - u_y \sin \theta \\ u_yu_x(1- \cos \theta) - u_z \sin \theta & \cos \theta + u_y^2(1- \cos \theta) & u_yu_z(1- \cos \theta) + u_x \sin \theta \\ u_zu_x(1- \cos \theta) + u_y \sin \theta & u_zu_y(1- \cos \theta) - u_x \sin \theta & \cos \theta + u_z^2(1- \cos \theta) \\ \end{bmatrix} \\

对比旋转矩阵：

R=[cos⁡θ+ux2(1−cos⁡θ)uxuy(1−cos⁡θ)−uzsin⁡θuxuz(1−cos⁡θ)+uysin⁡θuyux(1−cos⁡θ)+uzsin⁡θcos⁡θ+uy2(1−cos⁡θ)uyuz(1−cos⁡θ)−uxsin⁡θuzux(1−cos⁡θ)−uysin⁡θuzuy(1−cos⁡θ)+uxsin⁡θcos⁡θ+uz2(1−cos⁡θ)]R = \begin{bmatrix} \cos\theta + u_x^2(1- \cos \theta) & u_xu_y(1- \cos \theta) - u_z \sin \theta & u_xu_z(1- \cos \theta) + u_y \sin \theta \\ u_yu_x(1- \cos \theta) + u_z \sin \theta & \cos\theta + u_y^2(1- \cos \theta) & u_yu_z(1- \cos \theta) - u_x \sin \theta \\ u_zu_x(1- \cos \theta) - u_y \sin \theta & u_zu_y(1- \cos \theta) + u_x \sin \theta & \cos\theta + u_z^2(1- \cos \theta) \\ \end{bmatrix}\\

可以直接看出来： RT=R−1R^T = R^{-1}

旋转轴与旋转角度

有了旋转矩阵，如果我们想从中得到旋转角度很容易：

R(u,θ)=cos⁡θI+(1−cos⁡θ)(u⊗u)+sin⁡θ[u]xR(\mathbf{u},\theta) = \cos \theta \mathbf{I} + (1- \cos \theta)(\mathbf{u} \otimes \mathbf{u}) + \sin \theta [\mathbf{u}]_x\\

对式子两边取矩阵的迹(矩阵的迹: n x n 矩阵的迹是指对角线元素的和, tr⁡(A)=A1,1+A2,2+⋯+An,n\operatorname{tr}(\mathbf{A}) = \mathbf{A}{1, 1} + \mathbf{A}{2, 2} + \cdots + \mathbf{A}_{n, n} )：

tr(R)=cos⁡θtr(I)+(1−cos⁡θ)tr(u⊗u)+sin⁡θtr([u]x)tr(R) = \cos \theta tr(I) + (1- \cos \theta)tr(u \otimes u) + \sin \theta tr([u]_x)\\

tr(I) = 3 ，因为I是3x3的单位矩阵tr( [u]x[u]_x ) = 0，对角线和为0tr( u⊗u=uuT \mathbf{u} \otimes \mathbf{u} = \mathbf{u} \mathbf{u}^T ) = 1, 因为向量是单位向量，对角线算出来为u的模，即为1

所以：

tr(R)=1+2cos⁡θθ=arccos⁡(12[tr(R)−1])tr(R) = 1 + 2 \cos \theta\\ \theta = \arccos(\frac{1}{2}[tr(R) - 1]) \\

旋转轴的求法可以这样来看，平行于旋转轴的向量经过变换之后依旧平行于旋转轴，并不发生变化：

Ru=uR \mathbf{u} = \mathbf{u} \\

所以 u\mathbf{u} 是 R 特征值为1的一个特征向量(特征向量：对于一个给定的方阵A，它的特征向量v 经过这个线性变换之后，得到的新向量仍然与原来的保持在同一条直线上，但其长度或方向也许会改变)。

当然也可以有取巧的办法，观察一下旋转矩阵：

R=[cos⁡θ+ux2(1−cos⁡θ)uxuy(1−cos⁡θ)−uzsin⁡θuxuz(1−cos⁡θ)+uysin⁡θuyux(1−cos⁡θ)+uzsin⁡θcos⁡θ+uy2(1−cos⁡θ)uyuz(1−cos⁡θ)−uxsin⁡θuzux(1−cos⁡θ)−uysin⁡θuzuy(1−cos⁡θ)+uxsin⁡θcos⁡θ+uz2(1−cos⁡θ)]R = \begin{bmatrix} \cos\theta + u_x^2(1- \cos \theta) & u_xu_y(1- \cos \theta) - u_z \sin \theta & u_xu_z(1- \cos \theta) + u_y \sin \theta \\ u_yu_x(1- \cos \theta) + u_z \sin \theta & \cos\theta + u_y^2(1- \cos \theta) & u_yu_z(1- \cos \theta) - u_x \sin \theta \\ u_zu_x(1- \cos \theta) - u_y \sin \theta & u_zu_y(1- \cos \theta) + u_x \sin \theta & \cos\theta + u_z^2(1- \cos \theta) \\ \end{bmatrix}\\

把它写成：

R=[abcdefghi]R = \begin{bmatrix} a & b & c \\ d & e & f \\ g & h & i \\ \end{bmatrix}\\

[h−fc−gd−b]=[2uxsin⁡θ2uysin⁡θ2uzsin⁡θ]\begin{bmatrix} h - f \\ c - g \\ d - b \end{bmatrix} = \begin{bmatrix} 2u_x \sin \theta \\ 2u_y \sin \theta \\ 2u_z \sin \theta \end{bmatrix} \\

normalize 即可得 u\mathbf{u} .

当然我们也可以先来求它的反对称矩阵 [u]x[u]_x ，然后再来 u\mathbf{u} ：

R−RT=[cos⁡θ+ux2(1−cos⁡θ)uxuy(1−cos⁡θ)−uzsin⁡θuxuz(1−cos⁡θ)+uysin⁡θuyux(1−cos⁡θ)+uzsin⁡θcos⁡θ+uy2(1−cos⁡θ)uyuz(1−cos⁡θ)−uxsin⁡θuzux(1−cos⁡θ)−uysin⁡θuzuy(1−cos⁡θ)+uxsin⁡θcos⁡θ+uz2(1−cos⁡θ)]−[cos⁡θ+ux2(1−cos⁡θ)uxuy(1−cos⁡θ)+uzsin⁡θuxuz(1−cos⁡θ)−uysin⁡θuyux(1−cos⁡θ)−uzsin⁡θcos⁡θ+uy2(1−cos⁡θ)uyuz(1−cos⁡θ)+uxsin⁡θuzux(1−cos⁡θ)+uysin⁡θuzuy(1−cos⁡θ)−uxsin⁡θcos⁡θ+uz2(1−cos⁡θ)]=[0−2uzsin⁡θ2uysin⁡θ2uzsin⁡θ0−2uxsin⁡θ−2uysin⁡θ2uxsin⁡θ0]\begin{aligned} R - R^T = {}& \begin{bmatrix} \cos\theta + u_x^2(1- \cos \theta) & u_xu_y(1- \cos \theta) - u_z \sin \theta & u_xu_z(1- \cos \theta) + u_y \sin \theta \\ u_yu_x(1- \cos \theta) + u_z \sin \theta & \cos\theta + u_y^2(1- \cos \theta) & u_yu_z(1- \cos \theta) - u_x \sin \theta \\ u_zu_x(1- \cos \theta) - u_y \sin \theta & u_zu_y(1- \cos \theta) + u_x \sin \theta & \cos\theta + u_z^2(1- \cos \theta) \\ \end{bmatrix} - \begin{bmatrix} \cos \theta + u_x^2(1- \cos \theta) & u_xu_y(1- \cos \theta) + u_z \sin \theta & u_xu_z(1- \cos \theta) - u_y \sin \theta \\ u_yu_x(1- \cos \theta) - u_z \sin \theta & \cos \theta + u_y^2(1- \cos \theta) & u_yu_z(1- \cos \theta) + u_x \sin \theta \\ u_zu_x(1- \cos \theta) + u_y \sin \theta & u_zu_y(1- \cos \theta) - u_x \sin \theta & \cos \theta + u_z^2(1- \cos \theta) \\ \end{bmatrix} \\ =& \begin{bmatrix} 0 & -2u_z \sin \theta & 2u_y \sin \theta \\ 2u_z \sin \theta & 0 & -2u_x \sin \theta \\ -2u_y \sin \theta & 2 u_x \sin \theta & 0 \\ \end{bmatrix} \end{aligned}\\

所以：

12sin⁡θ(R−RT)=[0−uzuyuz0−ux−uyux0]\frac{1}{2 \sin \theta}(R - R^T ) = \begin{bmatrix} 0 & -u_z & u_y \\ u_z & 0 & -u_x \\ -u_y & u_x & 0 \\ \end{bmatrix}\\

得到了 [u]x[u]_x ， 当然 u\mathbf{u} 不成问题。

当然这个取巧的办法是有条件的，如果 R 是一个对称的矩阵，那么我们只能 ‘对角线化R并找到对应于特征值1的特征向量’。

参考：

旋转矩阵