Rodrigues旋转公式-绕任意轴旋转

Rodrigues旋转公式

给定旋转轴单位向量$\mathbf{k}=(k_x,k_y,k_z)$和旋转角度$\theta$，旋转矩阵$R$可以表示为：
$$R=I+\sin \theta K+(1-\cos \theta )K^2$$

其中：

• $I$是单位矩阵
• $K$是向量$\mathbf{k}$的叉积矩阵：

$$K=\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]$$

推导过程

将向量$\mathbf{v}$分解为平行和垂直于$\mathbf{k}$的分量：
$$\mathbf{v}={\mathbf{v}}_{\parallel }+{\mathbf{v}}_{\perp }$$

其中：
$$\begin{gathered} {\mathbf{v}}_{\parallel }=(\mathbf{v}\cdot \mathbf{k})\mathbf{k} \\ {\mathbf{v}}_{\perp }=\mathbf{v}-{\mathbf{v}}_{\parallel } \end{gathered}$$

平行分量${\mathbf{v}}_{\parallel }$旋转后不变
垂直分量${\mathbf{v}}_{\perp }$旋转后在垂直于$\mathbf{k}$的平面内旋转$\theta$角度：
$${\mathbf{v}}_{\perp }^{ROT}=\cos \theta {\mathbf{v}}_{\perp }+\sin \theta (\mathbf{k}\times \mathbf{v})$$

旋转后的向量为：
$${\mathbf{v}}^{ROT}={\mathbf{v}}_{\parallel }+{\mathbf{v}}_{\perp }^{ROT}$$

$$=(\mathbf{v}\cdot \mathbf{k})\mathbf{k}+\cos \theta (\mathbf{v}-(\mathbf{v}\cdot \mathbf{k})\mathbf{k})+\sin \theta (\mathbf{k}\times \mathbf{v})$$

利用叉积矩阵$K$，其中$\mathbf{k}\times \mathbf{v}=K\mathbf{v}$，可以写成：
$${\mathbf{v}}^{ROT}=\cos \theta \mathbf{v}+\sin \theta K\mathbf{v}+(1-\cos \theta )(\mathbf{v}\cdot \mathbf{k})\mathbf{k}$$

又因为$(\mathbf{v}\cdot \mathbf{k})\mathbf{k}=M\mathbf{v}$，$M=\mathbf{k}\cdot {\mathbf{k}}^T$，可以写成：
$${\mathbf{v}}^{ROT}=\cos \theta \mathbf{v}+\sin \theta K\mathbf{v}+(1-\cos \theta )M\mathbf{v}$$

$$=\left[\cos \theta I+\sin \theta K+(1-\cos \theta )M\right]\mathbf{v}$$
因此旋转矩阵为：

$$R=I+\sin \theta K+(1-\cos \theta )M$$

展开后得到：
$$R=\left[\begin{array}{ccc}\cos \theta +k_x^2(1-\cos \theta )& k_x{k}_y(1-\cos \theta )-k_z\sin \theta & k_x{k}_z(1-\cos \theta )+k_y\sin \theta \\ k_y{k}_x(1-\cos \theta )+k_z\sin \theta & \cos \theta +k_y^2(1-\cos \theta )& k_y{k}_z(1-\cos \theta )-k_x\sin \theta \\ k_z{k}_x(1-\cos \theta )-k_y\sin \theta & k_z{k}_y(1-\cos \theta )+k_x\sin \theta & \cos \theta +k_z^2(1-\cos \theta )\end{array}\right]$$