齐次变换矩阵的逆矩阵的求解

问题描述

已知坐标系 $A$ 相对于坐标系 $B$ 的齐次变换矩阵为 $T_A^B$，有时在求解问题时需要依据 $T_A^B$ 求取坐标系 $B$ 相对于坐标系 $A$ 的齐次变换矩阵 $T_B^A$ ，这个问题可以简单的描述为齐次变换矩阵求逆问题．

问题求解

假设齐次变换矩阵 ${\mathbf{T}}_A^B$ 的旋转部分为 ${\mathbf{R}}_A^B=\left[\begin{array}{ccc}{u}_x& v_x& w_x\\ u_y& vy& w_y\\ u_z& v_z& w_z\end{array}\right]$ ，平移部分为 ${\mathbf{t}}_A^B={\left[\begin{array}{ccc}{t}_x& t_y& t_z\end{array}\right]}^T$，则:
$${\mathbf{T}}_A^B=\left[\begin{array}{cc}{\mathbf{R}}_A^B& {\mathbf{t}}_A^B\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}{u}_x& v_x& w_x& t_x\\ u_y& vy& w_y& t_y\\ u_z& v_z& w_z& t_z\\ 0& 0& 0& 1\end{array}\right]$$
由于旋转矩阵是对称正定的(性质)，所以有:
$$({\mathbf{R}}_A^B{)}^{-1}=({\mathbf{R}}_A^B{)}^T$$
于是可得:
$$\begin{array}{rl}{\mathbf{T}}_B^A& =({\mathbf{T}}_A^B{)}^{-1}=(\left[\begin{array}{cc}{\mathbf{R}}_A^B& {\mathbf{t}}_A^B\\ 0& 1\end{array}\right]{)}^{-1}\\ & =(\left[\begin{array}{cccc}{u}_x& v_x& w_x& t_x\\ u_y& vy& w_y& t_y\\ u_z& v_z& w_z& t_z\\ 0& 0& 0& 1\end{array}\right]{)}^{-1}\\ & =(\left[\begin{array}{cccc}1& 0& 0& t_x\\ 0& 1& 0& t_y\\ 0& 0& 1& t_z\\ 0& 0& 0& 1\end{array}\right]\ast \left[\begin{array}{cccc}{u}_x& v_x& w_x& 0\\ u_y& v_y& w_y& 0\\ u_z& v_z& w_z& 0\\ 0& 0& 0& 1\end{array}\right]{)}^{-1}\\ & =(\left[\begin{array}{cccc}{u}_x& v_x& w_x& 0\\ u_y& v_y& w_y& 0\\ u_z& v_z& w_z& 0\\ 0& 0& 0& 1\end{array}\right]{)}^{-1}\ast (\left[\begin{array}{cccc}1& 0& 0& t_x\\ 0& 1& 0& t_y\\ 0& 0& 1& t_z\\ 0& 0& 0& 1\end{array}\right]{)}^{-1}\\ & =(\left[\begin{array}{cccc}{u}_x& u_y& u_z& 0\\ v_x& v_y& v_z& 0\\ w_x& w_y& w_z& 0\\ 0& 0& 0& 1\end{array}\right])\ast (\left[\begin{array}{cccc}1& 0& 0& -t_x\\ 0& 1& 0& -t_y\\ 0& 0& 1& -t_z\\ 0& 0& 0& 1\end{array}\right])\\ & =\left[\begin{array}{cc}({\mathbf{R}}_A^B{)}^T& 0\\ 0& 1\end{array}\right]\ast \left[\begin{array}{cc}\mathbf{I}& -{\mathbf{t}}_A^B\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}({\mathbf{R}}_A^B{)}^T& -({\mathbf{R}}_A^B{)}^T{\mathbf{t}}_A^B\\ 0& 1\end{array}\right]\end{array}$$

结论

$${\mathbf{T}}_B^A=\left[\begin{array}{cc}({\mathbf{R}}_A^B{)}^T& -({\mathbf{R}}_A^B{)}^T{\mathbf{t}}_A^B\\ 0& 1\end{array}\right]$$