3D坐标系旋转综合

三轴旋转推导总结

已知围绕 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_{xyz} \cdot \left[\begin{matrix} x_1 \\ y_1 \\ z_1 \end{matrix}\right]$$

我们可以定义$P_2$ 表示转换后的点，$P_1$为转换前的点：

$$P_1 = \left[\begin{matrix} x_1 \\ y_1 \\ z_1 \end{matrix}\right] \\ P_2 = \left[\begin{matrix} x_2 \\ y_2 \\ z_2 \end{matrix}\right]$$

我们我们可以得到一个简易的表达:

$$P_2 = R_{xyz} \cdot P_1$$

我们可以将$R_{xyz}$称之为转换矩阵，为了更明确的表达，我们可以这样表示:

$$^1_2R_{xyz}$$

意思是将点1转换为点2的旋转矩阵：

$$P_2 = \ ^1_2R_{xyz} \cdot P_1$$

推导过程

$$\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]$$

$$\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]$$

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

$$\left[\begin{matrix} C(\alpha)C(\beta) & -S(\alpha)C(\gamma) + C(\alpha)S(\beta)S(\gamma) & S(\alpha)S(\gamma) + C(\alpha)S(\beta)C(\gamma) \\ S(\alpha)C(\beta) & C(\alpha)C(\gamma) + S(\alpha)S(\beta)S(\gamma) & -C(\alpha)S(\gamma) + S(\alpha)S(\beta)C(\gamma) \\ -S(\beta) & C(\beta)S(\gamma) & C(\beta)C(\gamma) \\ \end{matrix}\right]$$

$$^1_2R_{xyz} = \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]$$

观察

$$\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]$$

矩阵看成：

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

三角函数中，如果要求解一个角度值，可以通过:

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

$\alpha$求解

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

我们发现: $$ \frac {R_{21}} {R_{11}} = \frac {sin(\alpha) \cdot cos(\beta)} {cos(\alpha) \cdot cos(\beta)} = \frac {sin(\alpha)} {cos(\alpha)} $$

我们可以得到以下结论:

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

$\beta$求解

观察$R_{31}$，这里的值是$-sin(\beta)$。

由我们已知的公式:

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

$\gamma$求解

观察$R_{32}$ 和 $R_{33}$, 他们分别是$cos(\beta) \cdot sin(\gamma)$ 和 $cos(\beta) \cdot cos(\gamma)$.

我们发现:

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

我们可以得到以下结论:

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