刚体运动位姿变换推导

下面公式中上标表示为基于某系，下标表示当前系。设ｋ为第ｋ时刻，Ｒ为旋转３＊３，Ｐ为平移３＊１，Ｔ表示旋转和平移。设k时刻刚体在世界坐标系下的位姿（包括旋转和平移）表示为$T_k^w$,刚体运动后k+1时刻位姿态表示为$T^w_{k+1} $
如图

位姿变换推导

$$T_{k+1}^w=T_k^w\ast T_{k+1}^k$$
分解如下
$$\begin{gathered} T_k^w=\left[\begin{array}{cc}{R}_k^w& P_k^w\\ 0& 1\end{array}\right] \\ {T}_{k+1}^k=\left[\begin{array}{cc}{R}_{k+1}^k& P_{k+1}^k\\ 0& 1\end{array}\right] \\ {R}_{k+1}^w=R_k^w\ast R_{k+1}^k \\ {P}_{k+1}^w=P_k^w+R_k^w\ast P_{k+1}^k \end{gathered}$$

求$T_k^w$的逆推导
$$\begin{gathered} T_k^w\ast (T_k^w{)}^{-1}=E \\ \left[\begin{array}{cc}{R}_k^w& P_k^w\\ 0& 1\end{array}\right]\ast \left[\begin{array}{cc}(R_k^w{)}^{-1}& -(R_k^w{)}^{-1}\ast P_k^w\\ 0& 1\end{array}\right]=E \end{gathered}$$

中左边矩阵第一行乘以右边矩阵的第二列为０
$$(R_k^w)\ast (-(R_k^w{)}^{-1}\ast P_k^w)+P_k^w=0$$
所以
$$\begin{gathered} (T_k^w{)}^{-1}=T_w^k=\left[\begin{array}{cc}(R_k^w{)}^{-1}& -(R_k^w{)}^{-1}\ast P_k^w\\ 0& 1\end{array}\right] \\ {P}_w^k=-(R_k^w{)}^{-1}\ast P_k^w=－R_w^k\ast P_k^w \end{gathered}$$