旋转矩阵的求解

假如我们已知一个旋转矩阵为 $R$ ，则如何求出当初是通过三个轴怎么样的旋转得来的呢。即此时问题就是如何将旋转矩阵转换成旋转角的问题。

已知围绕Z轴旋转的公式为:

$$\left[\begin{matrix} x_2 \\ y_2 \\ z_2 \end{matrix}\right] = \left[\begin{matrix} cos(\alpha) & -sin(\alpha) & 0 \\ sin(\alpha) & cos(\alpha) & 0 \\ 0 & 0 & 1 \\ \end{matrix}\right] \cdot \left[\begin{matrix} x_1 \\ y_1 \\ z_1 \end{matrix}\right]$$

围绕Y轴旋转的公式为:

$$\left[\begin{matrix} x_2 \\ y_2 \\ z_2 \end{matrix}\right] = \left[\begin{matrix} cos(\beta) & 0 & sin(\beta) \\ 0 & 1 & 0 \\ -sin(\beta) & 0 & cos(\beta) \\ \end{matrix}\right] \cdot \left[\begin{matrix} x_1 \\ y_1 \\ z_1 \end{matrix}\right]$$

围绕X轴旋转的公式为:

$$\left[\begin{matrix} x_2 \\ y_2 \\ z_2 \end{matrix}\right] = \left[\begin{matrix} 1 & 0 & 0 \\ 0 & cos(\gamma) & -sin(\gamma) \\ 0 & sin(\gamma) & cos(\gamma) \\ \end{matrix}\right] \cdot \left[\begin{matrix} x_1 \\ y_1 \\ z_1 \end{matrix}\right]$$

统一的旋转记做:

$$R_{zyx} = R_z(\alpha) \cdot R_y(\beta) \cdot R_x(\gamma)$$

我们可以总结得到:

$$\left[\begin{matrix} x_2 \\ y_2 \\ z_2 \end{matrix}\right] = R_{zyx} \cdot \left[\begin{matrix} x_1 \\ y_1 \\ z_1 \end{matrix}\right]$$

这里我们将 ${^w_r}R_{zyx}$ 展开可得：

$$\begin{aligned} {^w_r}R_{zyx} &= \left[\begin{matrix} cos(\alpha) & -sin(\alpha) & 0 \\ sin(\alpha) & cos(\alpha) & 0 \\ 0 & 0 & 1 \\ \end{matrix}\right] \cdot \left[\begin{matrix} cos(\beta) & 0 & sin(\beta) \\ 0 & 1 & 0 \\ -sin(\beta) & 0 & cos(\beta) \\ \end{matrix}\right] \cdot \left[\begin{matrix} 1 & 0 & 0 \\ 0 & cos(\gamma) & -sin(\gamma) \\ 0 & sin(\gamma) & cos(\gamma) \\ \end{matrix}\right] \\ &= \left[\begin{matrix} cos(\alpha)cos(\beta) & -sin(\alpha) & cos(\alpha)sin(\beta) \\ sin(\alpha)cos(\beta) & cos(\alpha) & sin(\alpha)sin(\beta) \\ -sin(\beta) & 0 & cos(\beta) \\ \end{matrix}\right] \cdot \left[\begin{matrix} 1 & 0 & 0 \\ 0 & cos(\gamma) & -sin(\gamma) \\ 0 & sin(\gamma) & cos(\gamma) \\ \end{matrix}\right] \\ &= \left[\begin{matrix} cos(\alpha)cos(\beta) & -sin(\alpha)cos(\gamma) + cos(\alpha)sin(\beta)sin(\gamma) & sin(\alpha)sin(\gamma) + cos(\alpha)sin(\beta)cos(\gamma) \\ sin(\alpha)cos(\beta) & cos(\alpha)cos(\gamma) + sin(\alpha)sin(\beta)sin(\gamma) & -cos(\alpha)sin(\gamma) + sin(\alpha)sin(\beta)cos(\gamma) \\ -sin(\beta) & cos(\beta)sin(\gamma) & cos(\beta)cos(\gamma) \\ \end{matrix}\right] \end{aligned}$$

为了简化式子，我们将$sin$简写为$S$, 将$cos$简写为$C$, 那么我们可以简写为:

$${^w_r}R_{zyx} = \left[\begin{matrix} C_z(\alpha)C_y(\beta) & C_z(\alpha)S_y(\beta)S_x(\gamma) - S_z(\alpha)C_x(\gamma) & S_z(\alpha)S_x(\gamma) + C_z(\alpha)S_y(\beta)C_x(\gamma) \\ S_z(\alpha)C_y(\beta) & C_z(\alpha)C_x(\gamma) + S_z(\alpha)S_y(\beta)S_x(\gamma) & S_z(\alpha)S_y(\beta)C_x(\gamma) - C_z(\alpha)S_x(\gamma) \\ -S_y(\beta) & C_y(\beta)S_x(\gamma) & C_y(\beta)C_x(\gamma) \\ \end{matrix}\right]$$

为了方便计算，我们为这个 3x3 矩阵每个元素起一个名字，如下：

$${^w_r}R_{zyx} = \left[\begin{matrix} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \\ \end{matrix}\right]$$

三角函数中，有如下等式变换规则：

$$\frac{sin(\theta)}{cos(\theta)} = tan(\theta)$$

则如果要求解一个角度值，可以通过如下方式得到旋转的角度（单位为弧度）:

$$\theta = atan2(sin(\theta), cos(\theta))$$

α 求解

求 $R_z(\alpha)$ 绕Z轴旋转的角度 ，观察$R_{11}$和$R_{21}$, 他们分别是 $cos(\alpha) \cdot cos(\beta)$ 和 $sin(\alpha) \cdot cos(\beta)$.

我们发现:

$$\begin{aligned} \frac {R_{21}} {R_{11}} = \frac {sin(\alpha) \cdot cos(\beta)} {cos(\alpha) \cdot cos(\beta)} = \frac {sin(\alpha)} {cos(\alpha)} \end{aligned}$$

我们可以得到以下结论:

$$\alpha = atan2(R_{21}, R_{11})$$

β 求解

求 $R_y(\beta)$ 绕Y轴旋转的角度 ，由我们已知的公式:

$$\beta = atan2(sin(\beta), cos(\beta))$$

那么:

$$\beta = atan2(-R_{31}, cos(\beta))$$

我们继续观察 $R_{11}$和 $R_{21}$, 他们分别是 $cos(\alpha) \cdot cos(\beta)$ 和 $sin(\alpha) \cdot cos(\beta)$.

我们尝试：

$$\sqrt {R_{11}^2 + R_{21}^2} = \sqrt {cos(\alpha)^2 \cdot cos(\beta)^2 + sin(\alpha)^2 \cdot cos(\beta)^2} \\ = \sqrt {(cos(\alpha)^2 + sin(\alpha)^2) \cdot cos(\beta)^2}$$

化简完成后，发现一个结论:

$$cos(\beta) = \sqrt {R_{11}^2 + R_{21}^2}$$

总结前面的计算，我们可以得到以下结论:

$$\beta = atan2(-R_{31}, \sqrt {R_{11}^2 + R_{21}^2})$$

γ 求解

求 $R_x(\gamma)$ 绕X轴旋转的角度 ，观察$R_{32}$ 和 $R_{33}$, 他们分别是$cos(\beta) \cdot sin(\gamma)$ 和 $cos(\beta) \cdot cos(\gamma)$.

我们发现:

$$\begin{aligned} \frac {R_{32}} {R_{33}} = \frac {cos(\beta) \cdot sin(\gamma)} {cos(\beta) \cdot cos(\gamma)} = \frac {sin(\gamma)} {cos(\gamma)} \end{aligned}$$

我们可以得到以下结论:

$$\gamma = atan2(R_{32}, R_{33})$$

奇异值情况求解

这里根据以下值是否等于0（小于一个阈值）来判断此时的旋转矩阵是否是奇异矩阵，如果是奇异矩阵，则结果有所差异

$$cos(\beta) = \sqrt {R_{11}^2 + R_{21}^2}$$

如果 $cos(\beta) = 0$ 或接近零，说明其在 Pitch轴为90°，此时产生万向节死锁，求解方式有所差异，具体如下：

$${\gamma}_x = -atan2(R_{23}, R_{22}) \\ {\beta}_y = atan2(-R_{31}, \sqrt {R_{11}^2 + R_{21}^2}) \\ {\alpha}_z = 0$$

参见：机器人运动学3D | 坐标系旋转综合