罗德里格斯参数与旋转矩阵转换

罗德里格斯参数与旋转矩阵是描述三维空间中旋转姿态的两种方法，它们之间可以相互转换。

将罗德里格斯参数 $\boldsymbol{r} = (r_1, r_2, r_3)$ 转换为旋转矩阵 $\boldsymbol{R}$ 的公式为：

$$
\boldsymbol{R} = \boldsymbol{I} + \frac{2\boldsymbol{r}\times}{1+\boldsymbol{r}\cdot\boldsymbol{r}} + \frac{2(\boldsymbol{r}\times)^2}{1+\boldsymbol{r}\cdot\boldsymbol{r}}
$$

其中，$\boldsymbol{I}$ 是单位矩阵，$\boldsymbol{r}\times$ 表示向量 $\boldsymbol{r}$ 的叉积矩阵，即：

$$
\boldsymbol{r}\times = \begin{bmatrix} 0 & -r_3 & r_2 \\ r_3 & 0 & -r_1 \\ -r_2 & r_1 & 0 \end{bmatrix}
$$

将旋转矩阵 $\boldsymbol{R}$ 转换为罗德里格斯参数 $\boldsymbol{r}$ 的公式为：

$$
\boldsymbol{r} = \frac{1}{2\sin\theta}\begin{bmatrix} R_{3,2}-R_{2,3} \\ R_{1,3}-R_{3,1} \\ R_{2,1}-R_{1,2} \end{bmatrix}
$$

其中，$\theta$ 是旋转角度，可以通过旋转矩阵 $\boldsymbol{R}$ 的迹 $\mathrm{tr}(\boldsymbol{R})$ 计算得到：

$$
\theta = \arccos\left(\frac{\mathrm{tr}(\boldsymbol{R})-1}{2}\right)
$$

需要注意的是，在将旋转矩阵转换为罗德里格斯参数时，需要判断 $\sin\theta\neq 0$ 的情况，否则会出现除以 $0$ 的错误。