旋转矩阵的理解

矩阵 ${\mathbf{R}}_{12}$ 是旋转矩阵，其描述了坐标系1到坐标系2的旋转。矩阵 ${\mathbf{R}}_{12}$ 的下标有两个含义：

• 表示坐标系1到坐标系2的旋转
• 表示将坐标系2中点的坐标通过旋转变换，从而得到同一个点在坐标系1中的坐标

向量 ${\mathbf{t}}_{12}$ 是平移向量，其描述了坐标系1到坐标系2的平移。其下标同样也有两个含义：

• 表示坐标系1到坐标系2的平移（对应坐标系1的原点指向坐标系2的原点的向量）
• 表示将坐标系2中点的坐标通过平移变换，从而得到同一个点在坐标系1中的坐标

---

首先看一个简单的例子

如图1，$P_0,P_1$是坐标系 $W$ 下的两个点，其中 $P_1$ 由 $P_0$ 旋转 $\theta$ （逆时针为正）而来，则二者之间的坐标满足关系：
$$\begin{array}{rl}{x}_1& =r\ast \cos (\theta +{\theta }_0)\\ & =r\ast \cos (\theta )\cos ({\theta }_0)-r\ast \sin (\theta )\sin ({\theta }_0)\\ & =x_0\ast \cos (\theta )-y_0\ast \sin (\theta )\\ y_1& =r\ast \sin (\theta +{\theta }_0)\\ & =r\ast \sin (\theta )\cos ({\theta }_0)+r\ast \cos (\theta )\sin ({\theta }_0)\\ & =x_0\ast \sin (\theta )+y_0\ast \cos (\theta )\end{array}$$
即 $P_1=R{P}_0$, 其中
$$R=\left(\begin{array}{cc}\cos (\theta )& -\sin (\theta )\\ \sin (\theta )& \cos (\theta )\end{array}\right)$$
这个比较容易理解了，不多赘述。

如图2，$P_1$ 是 $P_0$ 在旋转之后多了一个平移而来，则 $P_0,P_1$ 之间的关系为：
$P_1=R{P}_0+t$, 其中 $t=(\Delta x,\Delta y{)}^T$

---

现在我们换一种角度，不再将点看作普通的点，而是一个新坐标系的原点，如图3。那么如何描述从一个坐标系到另一个坐标系呢? 例如在图3中，将 $W$ 坐标系转到 $P_0$ 坐标系，那么可以仿照上述两个例子写出这个变换：
$$R=\left(\begin{array}{cc}\cos ({\theta }_0)& -\sin ({\theta }_0)\\ \sin ({\theta }_0)& \cos ({\theta }_0)\end{array}\right),t=\left(\begin{array}{c}{x}_0\\ y_0\end{array}\right)$$

其中 $(x_0,y_0)$ 是 $P_0$ 坐标系原点在 $W$ 坐标系下的表示, ${\theta }_0$ 是坐标系旋转的角度.
具体地, 对于 $P_0$ 坐标系描述下的一点 $p(x_p,y_p)$, 其在 $W$ 坐标系下的表示为
$$\left(\begin{array}{c}{x}_w\\ y_w\end{array}\right)=R\left(\begin{array}{c}{x}_p\\ y_p\end{array}\right)+t$$
注意！此时我们称 $T_{P_0}^W=\left(\begin{array}{cc}R& t\\ 0& 1\end{array}\right)$ 为 $W$ 坐标系 到 $P_0$ 坐标系的变换，它可以将 点在 $P_0$ 坐标系的表示 转为在 $W$ 坐标系的表示。
通常，$W$ 坐标系 到 $P_0$ 坐标系的变换 称为 $P$ 的位姿。这种称呼比较好理解，因为 $T_{P_0}^W$ 的 $t$ 部分刚好就是 $P_0$ 在世界坐标系中的坐标，用欧拉角表示旋转的话，这个角度就是$W$ 转到 $P_0$ 的角度(逆时针为正)。另一方面，也可以将其视做在 $W$ 的基础上的旋转和平移变化。

---

最后一个例子，如图4，一辆车从 $P_0$ 移动到 $P_1$，它们也被两个时刻下的车坐标系，那么如何描述它们之间的变换呢?
显然根据上述例子，我们很容易地得到 $W$ 坐标系到两个车坐标系的变换：
$R$ 描述 从 $W$ 转到 目标系 的 角度，$t$ 是 从$W$原点指向 目标系原点 的向量 在 $W$ 系下的表示，即
$$\begin{array}{rl}{R}_{P_0}^W& =\left(\begin{array}{cc}\cos {\theta }_0& -\sin {\theta }_0\\ \sin {\theta }_0& \cos {\theta }_0\end{array}\right),t=\left(\begin{array}{c}{x}_0\\ y_0\end{array}\right)\\ R_{P_1}^W& =\left(\begin{array}{cc}\cos {\theta }_1& -\sin {\theta }_1\\ \sin {\theta }_1& \cos {\theta }_1\end{array}\right),t=\left(\begin{array}{c}{x}_1\\ y_1\end{array}\right)\end{array}$$

• 从坐标系变换的角度来看，$P_0$ 到 $P_1$ 的变换等同于 $P_0$ 到 $W$，再从 $W$ 到 $P_1$， 即
  $$T_{P_1}^{P_0}={\left(\begin{array}{c}{T}_{P_0}^W\end{array}\right)}^T{T}_{P_1}^W$$
  具体地，我们也可以直接根据规则直接写出二者之间的变换，令 $\theta ={\theta }_1-{\theta }_0$,
  $$\begin{array}{rl}{R}_{P_1}^{P_0}& =\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \sin \theta \end{array}\right)\\ t_{P_1}^{P_0}& ={\left(\begin{array}{c}{R}_{P_0}^W\end{array}\right)}^T(t_{P_1}^W-t_{P_0}^W)\end{array}$$
• 从坐标点转换来看，上述变换又可解释为 将 点在$P_1$坐标系下的表示 转到 点在$P_0$坐标系的表示，即
  $$p_0=T_{P_1}^{P_0}{p}_1={\left(\begin{array}{c}{T}_{P_0}^W\end{array}\right)}^T{T}_{P_1}^W{p}_1$$