三维空间刚体变换：变换矩阵

变换矩阵

坐标系描述

用${}^A{p}_{Bo}$表示在参考坐标系$\{A\}$下坐标系$\{B\}$的原点坐标，并用旋转矩阵${}_B^{A}R$描述坐标系$\{B\}$在参考坐标系$\{A\}$下的姿态，则坐标系$\{B\}$的姿态可表示为：
$$\{B\}=\{{}^A{p}_{Bo}{}_B^{A}R\}$$
坐标系的描述概括了刚体的位置和姿态的描述，当表示位置时，旋转矩阵${}_B^{A}R=I$；当表示姿态时，位移矢量${}^A{p}_{Bo}=0$

平移映射

当两坐标系具有相同的方位，当坐标原点不重合时，可用位移矢量${}^A{p}_{Bo}$描述坐标系$\{B\}$相对于坐标系$\{A\}$的位置：

称${}^A{p}_{Bo}$为坐标系$\{B\}$相对于$\{A\}$的平移矢量，若点$p$在坐标系$\{B\}$下的位置为${}^{B}p$，则它相对于坐标系$\{A\}$的位置${}^{A}p$可表示为：
$${}^{A}p={}^{B}p+{}^A{p}_{Bo}$$

旋转映射

当两坐标系原点重合，但方位不同时，可用旋转矩阵${}_B^{A}R$描述坐标系$\{B\}$相对于坐标系$\{A\}$的姿态：

称${}_B^{A}R$为坐标系$\{B\}$相对于$\{A\}$的方位，若点$p$在坐标系$\{B\}$下的位置为${}^{B}p$，则它相对于坐标系$\{A\}$的位置${}^{A}p$可表示为：
$${}^{A}p={}_B^{A}R{}^{B}p$$
同样，用${}_A^{B}R$表示坐标系$\{A\}$相对于$\{B\}$的方位，则可由旋转矩阵的正交约束得：
$${}_A^{B}R={}_B^A{R}^{-1}={}_B^A{R}^T$$

一般刚体变换

在三维空间中，最常见的情况如下：坐标系$\{B\}$与$\{A\}$不但原点不重合，同时姿态也不相同。此时采用位移矢量${}^A{p}_{Bo}$描述坐标系$\{B\}$相对于坐标系$\{A\}$的位置，用旋转矩阵${}_B^{A}R$描述坐标系$\{B\}$相对于坐标系$\{A\}$的姿态：

则若点$p$在坐标系$\{B\}$下的位置为${}^{B}p$，则它相对于坐标系$\{A\}$的位置${}^{A}p$可表示为：
$${}^{A}p={}_B^{A}R{}^{B}p+{}^A{p}_{Bo}$$
也即，为旋转映射与平移映射的复合。先将坐标系$\{B\}$根据旋转矩阵进行旋转，在沿着位移矢量进行平移。

将上式进行齐次变换，得到变换矩阵${}_B^{A}T$：
$$\left[\begin{array}{c}{}^{A}p\\ 1\end{array}\right]=\left[\begin{array}{cc}{}_B^{A}R& {}^A{p}_{Bo}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{}^{B}p\\ 1\end{array}\right]={}_B^{A}T{}^{B}p$$
其中齐次变换矩阵${}_B^{A}T$为$4\times 4$的矩阵，它表示了由坐标系$\{B\}$到坐标系$\{A\}$的变换关系：
$${}_B^{A}T=\left[\begin{array}{cc}{}_B^{A}R& {}^A{p}_{Bo}\\ 0& 1\end{array}\right]$$

齐次坐标

若一点的$p$的坐标为${\left[\begin{array}{ccc}x& y& z\end{array}\right]}^T$，则它的齐次坐标表示如下，齐次坐标并不唯一，可将隔行同乘一个常数$\omega$表示的仍为同一坐标：
$$p=\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{c}\omega x\\ \omega y\\ \omega z\\ \omega \end{array}\right]$$
同时，规定坐标${\left[\begin{array}{cccc}0& 0& 0& 0\end{array}\right]}^T$无意义，各个轴的无穷远处如下表示：
$$X轴:\left[\begin{array}{c}1\\ 0\\ 0\\ 0\end{array}\right]Y轴:\left[\begin{array}{c}0\\ 1\\ 0\\ 0\end{array}\right]Z轴:\left[\begin{array}{c}0\\ 0\\ 1\\ 0\end{array}\right]任意无穷远处:\left[\begin{array}{c}a\\ b\\ c\\ 0\end{array}\right]$$

变换矩阵运算与刚体群

运动算子

变换矩阵${}_B^{A}T$可分解为两个矩阵相乘的形式，其中用$Trans({}^A{p}_{Bo})$表示平移变换矩阵，用$Rot(k,\theta )$旋转变换矩阵：
$$\begin{array}{rl}{}_B^{A}T=& \left[\begin{array}{cc}{}_B^{A}R& {}^A{p}_{Bo}\\ 0& 1\end{array}\right]\\ =& \left[\begin{array}{cc}{I}_{3\times 3}& {}^A{p}_{Bo}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{}_B^{A}R(k,\theta )& 0\\ 0& 1\end{array}\right]\\ =& Trans({}^A{p}_{Bo})Rot(k,\theta )\end{array}$$
$Trans({}^A{p}_{Bo})$表示沿着位移矢量${}^A{p}_{Bo}$进行平移，$Rot(k,\theta )$则表示绕着过原点的轴$k$旋转$\theta$角。

平移算子

相对坐标系$\{A\}$，位移矢量${}^A{p}_1$沿着${}^{A}p$移动至位移矢量${}^A{p}_2$可用矢量相加表示：
$${}^A{p}_2={}^A{p}_1+{}^{A}p$$
将其写为算子形式如下：
$${}^A{p}_2=Trans({}^{A}p){}^A{p}_1$$
平移算子为$Trans({}^{A}p)$表示沿着位移矢量${}^{A}p$的大小和方向进行平移。

旋转算子

相对坐标系$\{A\}$，某点由${}^A{p}_1$旋转至${}^A{p}_2$可表示为：
$${}^A{p}_2=R{}^A{p}_1$$
将其写为算子形式如下：
$${}^A{p}_2=Rot(k,\theta ){}^A{p}_1$$
旋转算子$Rot(k,\theta )$表示沿着过原点的轴$k$旋转角度$\theta$。

运动算子一般形式

齐次变换矩阵可用作为运动算子，描述某点在坐标系内的运动（包括平移和旋转）。当变换矩阵作为运动算子使用时，不带上、下标。

例如，质点$p$在坐标系$\{A\}$中的运动轨迹为时间$t$的函数${}^{A}p(t)$，其初始位置为${}^{A}p(0)$，则用运动算子表示该质点的运动轨迹如下：
$${}^{A}p(t)=T(t){}^{A}p(0)$$

变换矩阵的运算

变换矩阵乘法

给定变换矩阵${}_B^{A}T、{}_C^{B}T$，则可以得到变换矩阵${}_C^{A}T$：
$${}_C^{A}T={}_B^{A}T{}_C^{B}T=\left[\begin{array}{cc}{}_B^{A}R{}_C^{B}R& {}_B^{A}R{}^B{p}_{Co}+{}^A{p}_{Bo}\\ 0& 1\end{array}\right]$$
也即，坐标系$\{C\}$在坐标系$\{A\}$中的位姿，可以拆为两步进行：先将坐标系$\{C\}$转换至坐标系$\{B\}$中的位姿；再将此位姿根据坐标系$\{B\}$至$\{A\}$的变换进行变换，从而得到最终变换矩阵。

变换矩阵乘法满足如下条件：

• 运动相对固定坐标系而言：满足左乘规则，即变换顺序从右至左
• 运动相对运动坐标系而言：满足右乘规则，即变换顺序从左至右
• 变换的顺序不能调换进行

例如，对于一个变换矩阵${}_B^{A}T=Trans(\begin{array}{ccc}1& -3& 4\end{array})Rot(y,90°)Rot(z,90°)$

若相对固定坐标系（坐标系$\{A\}$）而言，采用左乘规则进行：首先绕$z_A$轴旋转$90°$，再绕$y_A$轴旋转$90°$，最后相对$\{A\}$平移${\left[\begin{array}{ccc}1& -3& 4\end{array}\right]}^T$即可。

若相对于运动坐标系（坐标系$\{B\}$）而言，采用右乘规则进行：首先相对坐标系$\{A\}$平移${\left[\begin{array}{ccc}1& -3& 4\end{array}\right]}^T$，再绕$y_B$轴旋转$90°$，最后绕$z_B$轴旋转$90°$即可。

变换矩阵求逆

若已知变换矩阵${}_B^{A}T$，此时需要求解坐标系$\{A\}$相对于坐标系$\{B\}$的变换${}_A^{B}T$，则需要对变换矩阵进行求逆。

此处利用齐次变换矩阵特点，简化计算：已知${}_B^{A}T$求解${}_A^{B}T$，只需要由${}_B^{A}R$和${}^A{p}_{Bo}$求解${}_A^{B}R$和${}^B{p}_{Ao}$即可。

首先，通过旋转矩阵正交约束知：
$${}_A^{B}R={}_B^A{R}^{-1}={}_B^A{R}^T$$
随后，根据平移映射关系，可知在坐标系$\{A\}$下坐标系$\{B\}$的原点${}^A{p}_{Bo}$，在坐标系$\{B\}$的坐标${}^B({}^A{p}_{Bo})$：
$${}^B({}^A{p}_{Bo})={}_A^{B}R{}^A{p}_{Bo}+{}^B{p}_{Ao}$$
已知，在坐标系$\{B\}$中，点$Bo$为原点，即上式右侧为零：
$${}^B{p}_{Ao}=-{}_A^{B}R{}^A{p}_{Bo}=-{}_B^A{R}^T{}^A{p}_{Bo}$$
由此得到变换矩阵的逆矩阵：
$${}_A^{B}T=\left[\begin{array}{cc}{}_B^A{R}^T& -{}_B^A{R}^T{}^A{p}_{Bo}\\ 0& 1\end{array}\right]$$
易得：${}_A^{B}T={}_B^A{T}^{-1}$

刚体变换群

变换矩阵$T$由旋转矩阵$R$和位移矢量$t$决定，也即任意刚体的位姿由$(t,R):t\in {\Re }^3,R\in SO(3)$决定。

定义刚体变换群$SE(3)$为乘积空间${\Re }^3\times SO(3)$：
$$SE(3)=\{T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in {\Re }^{4\times 4}|R\in SO(3),t\in {\Re }^3\}={\Re }^3\times SO(3)$$
$SE(3)$又称为三维空间的特殊欧式群（Special Euclidean Group ），推广到n维空间中可得：
$$SE(n)=\{T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\in {\Re }^{n+1\times n+1}|R\in SO(n),t\in {\Re }^n\}={\Re }^n\times SO(n)$$
$SE(n)$为李群，当n=2时，$SE(2)$表示刚体的平面运动，其单位元为$I_3$；当n=3时，$SE(3)$表示刚体