坐标变换(6)—齐次变换矩阵

前面的文章主要介绍了旋转矩阵，对于刚体的运动，除了旋转外还有平移。在机器人及自动驾驶中，经常用齐次变换矩阵将旋转和平移进行统一。前面的文章也介绍过齐次变换矩阵，本文算是一个总结。

1. SE(3)

将旋转矩阵和平移向量写在同一个矩阵中，形成的 $4\times4$ 矩阵，称为special Euclidean group,即 $SE(3)$ ，
$T=\left[\begin{array}{ll} R & p \\ 0 & 1 \end{array}\right]=\left[\begin{array}{llll} r_{11} & r_{12} & r_{13} & p_{1} \\ r_{21} & r_{22} & r_{23} & p_{2} \\ r_{31} & r_{32} & r_{33} & p_{3} \\ 0 & 0 & 0 & 1 \end{array}\right] \tag{1}$
很容易验证，齐次变换矩阵满足群所具有的性质，即封闭性，结合律，幺元，逆，所以称其为group是合理的。

$T^{-1}=\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]^{-1}=\left[\begin{array}{cc} R^{\mathrm{T}} & -R^{\mathrm{T}} p \\ 0 & 1 \end{array}\right] \tag{2}$
$(T_1T_2)T_3=T_1(T_2T_3) \tag{3}$
此外齐次变换矩阵还能保持变换前后的距离和角度不变，假定 $T\in SE(3)$ ，同时 $x,y,z \in \mathbb{R}^3$ ，则有，

$\|T x-T y\|=\|x-y\| \tag{4}$
$\langle T x-T z, T y-T z\rangle=\langle x-z, y-z\rangle \tag{5}$

2. 齐次变换矩阵的用法

2.1 描述坐标系

如上图所示， $v_b=(0,0,1.5)$ ，假设fixed frame为 $S$ ， $a$ 和 $S$ 重合，则 $a$ , $b$ , $c$ 可以描述为：
$\begin{aligned} T_{sa}&=(R_{sa},p_{sa}) \\ T_{sb}&=(R_{sb},p_{sb}) \\ T_{sc}&=(R_{sc},p_{sc}) \end{aligned} \tag{4}$
其中，
$R_{s a}=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right], \quad R_{s b}=\left[\begin{array}{rrr} 0 & 0 & 1 \\ 0 & -1 & 0 \\ 1 & 0 & 0 \end{array}\right], \quad R_{s c}=\left[\begin{array}{rrr} -1 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 1 & 0 \end{array}\right]$

$p_{s a}=\left[\begin{array}{c} 0 \\ 0 \\ 0 \end{array}\right], \quad p_{s b}=\left[\begin{array}{c} 0 \\ -2 \\ 0 \end{array}\right], \quad p_{s c}=\left[\begin{array}{c} -1 \\ 1 \\ 0 \end{array}\right]$

2.2 向量(坐标系)在不同坐标系下的描述

对于任意三个坐标系， $v$ 在 $b$ 下为 $v_b$ ,

$\begin{aligned} T_{a b} T_{b c} &=T_{a \not {b}} T_{\not {b} c}=T_{a c} \\ T_{a b} v_{b} &=T_{a b} v_{b}=v_{a} \end{aligned} \tag{5}$
$v$ 在 $a$ 下为 $v_a$ 。

2.3 对向量(坐标系)进行平移和旋转

可以将齐次变换矩阵写为公式(6)，这里将 $\operatorname{Rot}(\hat{\omega}, \theta)$ 改写为齐次形式，为了便于标记，
$\begin{aligned} T&=\operatorname{Trans}(p)\operatorname{Rot}(\hat{\omega}, \theta)\\ &=\left[\begin{array}{llll} 1 & 0 & 0 & p_{x} \\ 0 & 1 & 0 & p_{y} \\ 0 & 0 & 1 & p_{z} \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{ll} R & 0 \\ 0 & 1 \end{array}\right] \end{aligned} \tag{6}$

假设body frame $b$ 相对于fixed frame $S$ 的描述为 $T_{sb}$ ，则下面讨论左乘和右乘 $T$ 的区别，

$\begin{aligned} &T_{s b^{\prime}}=T T_{s b}=\operatorname{Trans}(p) \operatorname{Rot}(\hat{\omega}, \theta) T_{s b} \quad \text { (fixed frame) }\\ &=\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} R_{s b} & p_{s b} \\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} R R_{s b} & R p_{s b}+p \\ 0 & 1 \end{array}\right] \end{aligned} \tag{7}$
$\begin{aligned} &T_{s b^{\prime \prime}}=T_{s b} T=T_{s b} \operatorname{Trans}(p) \operatorname{Rot}(\hat{\omega}, \theta) \quad \text { (body frame) }\\ &=\left[\begin{array}{cc} R_{s b} & p_{s b} \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} R_{s b} R & R_{s b} p+p_{s b} \\ 0 & 1 \end{array}\right] \end{aligned} \tag{8}$

以下图为例，假设 $\hat{\omega}=(0,0,1)$ ， $\theta=90^{\operatorname {\omicron}}$ , $p=(0,2,0)$ ，分析左乘和右乘的区别，

首先是左乘，如上图左边所示，根据公式(7)：
首先分析 $\operatorname{Rot}(\hat{\omega},\theta)T_{s b}$ ， $T_{s b}$ 首先以 $S$ 为基准进行旋转， $R R_{s b}$ 为绕着 $\hat z_s$ 旋转 $R$ ， $Rp_{s b}$ 将 $p_{s b}$ 按照图中①的方式进行了旋转;
然后再左乘 $\operatorname{Trans}(p)$ ，将①的结果沿着 $\hat{y_s}$ 移动2个单位，即得到图中②最终的结果。

其次是右乘，如上图右边所示，根据公式(8)：
首先分析 $T_{s b} \operatorname{Trans}(p)$ ， $T_{s b}$ 首先以 $b$ 为基准进行平移，即沿着 $\hat{y_b}$ 平移2个单位， $R_{s b} p+p_{s b}$ ，即得到图中的①；
然后再右乘 $\operatorname{Rot}(\hat{\omega}, \theta)$ ，以 $b$ 为基准进行旋转，即沿着 $\hat{z_b}$ 旋转 $\theta=90^{\operatorname {\omicron}}$ ，得到图中②的结果。
综上所述，
左乘齐次矩阵，首先以 $S$ 为基准进行旋转，然后以 $S$ 为基准进行平移；
右乘齐次矩阵，首先以 $b$ 为基准进行平移，然后以 $b$ 为基准进行旋转；