3D坐标系旋转平移叠加
分析推导总结¶
平移矩阵:
\Delta P
=
\left[\begin{matrix}
1 & 0 & 0 & \delta x \\
0 & 1 & 0 & \delta y \\
0 & 0 & 1 & \delta z \\
0 & 0 & 0 & 1
\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) & \delta x \\
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) & \delta y \\
-S_y(\beta) & C_y(\beta)S_x(\gamma) & C_y(\beta)C_x(\gamma) & \delta z \\
0 & 0 & 0 & 1
\end{matrix}\right]
变换矩阵公式总结¶
^1_2T
=
\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) & \delta x \\
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) & \delta y \\
-S_y(\beta) & C_y(\beta)S_x(\gamma) & C_y(\beta)C_x(\gamma) & \delta z \\
0 & 0 & 0 & 1
\end{matrix}\right]
我们将公式简化一下:
^1_2T
=
\left[\begin{matrix}
R_{11} & R_{12} & R_{13} & \delta x \\
R_{21} & R_{22} & R_{23} & \delta y \\
R_{31} & R_{32} & R_{33} & \delta z \\
0 & 0 & 0 & 1
\end{matrix}\right]
姿态角度计算¶
-
围绕ｚ轴旋转的角度\alpha: $$ \alpha = atan2(R_{21}, R_{11}) $$
-
围绕ｙ轴旋转的角度\beta $$ \beta = atan2(-R_{31}, \sqrt {R_{11}^2 + R_{21}^2}) $$
-
围绕ｙ轴旋转的角度\gamma $$ \gamma = atan2(R_{32}, R_{33}) $$