矩阵乘法的本质

矩阵的每一列，都是基向量被搬去的地方。

在机器人学中，最重要的数学原理便是线性代数了，因为机器人学中的各类运算，如正运动学、坐标系映射等等，做的运算都是矩阵乘法，因此先搞懂矩阵乘法的本质很有必要。

矩阵和向量相乘

在3Blue1Brown线性代数系列视频中，最显著的一个观点就是“矩阵乘法就是线性变换”。

将矩阵与向量相乘，本质上是在同一个坐标系内，将该向量线性变换到另一个位置。矩阵的每一列，就是变换后的新基向量（即各个基向量经过变换后的像）在原来坐标系下的坐标；而相乘的结果，就是变换后的向量在原来坐标系下的坐标。

所以，矩阵 $M$ 的第 $j$ 列，就是基向量 $\hat e_j$ 经过变换后的像。

以逆时针旋转 90° 为例。取矩阵 $M=\begin{bmatrix}0 & -1\\ 1 & 0\end{bmatrix}$，向量 $v=(1,1)^T$，则

$$Mv=\begin{bmatrix}0 & -1\\ 1 & 0\end{bmatrix}(1,1)^T=(-1,\ 1)^T$$

先看矩阵的两列。第一列是 $(0,1)^T$，也就是原来坐标系下的 $\hat y$ 方向——它告诉我们，变换后新的 $x$ 轴指向了原来 $y$ 轴的正方向；第二列是 $(-1,0)^T$，也就是原来坐标系下的 $-\hat x$ 方向——新的 $y$ 轴指向了原来 $x$ 轴的负方向。这两列正是“基向量被搬去的地方”。

再看向量 $v=(1,1)^T$：它原本指向第一象限的对角方向，被逆时针转了 90° 之后落到 $(-1,1)^T$，也就是第二象限的对角方向，这正是上面的计算结果。

矩阵和矩阵相乘

在线性代数中，我们都学习过矩阵的乘法是满足结合律而不满足交换律的，这是为什么呢？矩阵和矩阵相乘的本质便解答了这一点。

先来说说矩阵乘法为什么满足结合律 $(AB)Cv=A(BC)v$ 。对于任意向量 $v$ ，$(AB)Cv$ 表示先对 $v$ 进行 $C$ 变换，再进行 $B$ 和 $A$ 的组合变换；而 $A(BC)v$ 表示先进行 $C$ 和 $B$ 的组合变换，再进行 $A$ 变换，无论哪种方式，最终向量经历的变换顺序都是 $C\rightarrow B\rightarrow A$，因此括号的位置不会影响结果。

再来解释一下矩阵的乘法为什么不满足交换律，即为什么 $ABv$ 与 $BAv$ 一般不相等。对于任意向量 $v$ ，$ABv$ 表示先对 $v$ 进行 $B$ 变换，再进行 $A$ 变换；而 $BAv$ 表示先对 $v$ 进行 $A$ 变换，再进行 $B$ 变换，先后顺序被搞错了，便得到了不同的结果。

下面我们便可以理解矩阵乘法的本质了，矩阵乘法 $AB$ ，本质上是将线性变换 $B$ 和线性变换 $A$ 进行复合，得到一个线性变换，这个线性变换作用在一个向量上，和先作用 $B$ 变换，再作用 $A$ 变换，得到的效果是一模一样的。

机器人学中的旋转矩阵

在机器人运动学中，我们认为各个关节连杆都是刚体，刚体有一个最首要的定义，就是其上任意两点之间的距离始终保持不变。而前面讲到的线性变换，是可以对空间（或物体）进行拉伸、剪切等操作的，因此机器人学中用到的旋转矩阵虽然也是线性变换，但要严格得多——它变换之后得到的坐标系，必须满足以下两点要求：

1. 三个新基向量必须两两垂直，且长度都为 1，即构成正交单位基——这正是 $R^TR=I$；
2. 变换不能把右手系翻成左手系，也就是不允许出现镜像——这正是 $\det R=+1$。

这样才能满足刚体的要求，不会对刚体进行拉伸或剪切。换句话说，只有正交矩阵的列才构成一个合法的坐标系：一般的线性变换会把基向量拉长或压斜，虽然仍能得到三个列向量，但它们既不等长也不垂直，那就已经不是一个坐标系了。

旋转矩阵的三个基本形式

前面说了，矩阵的列就是基向量的像。反过来想，只要知道三根基向量分别被搬到了哪里，这个矩阵就唯一确定了。对于绕某根坐标轴旋转 $\theta$这种最基本的变换，我们完全可以用这个思路把矩阵直接写出来，不需要死记。

以绕 $\hat z$ 轴旋转 $\theta$ 为例。 假设坐标系 $\{B\}$ 一开始与参考坐标系 $\{A\}$ 完全重合，然后让它绕 $\hat z_A$ 轴逆时针转 $\theta$。转完之后：

• $\{B\}$ 的 $x$ 轴是 $\hat x_A$ 绕 $\hat z_A$ 转了 $\theta$，在 $\{A\}$ 中就是 $(\cos\theta,\ \sin\theta,\ 0)^T$；
• $\{B\}$ 的 $y$ 轴是 $\hat y_A$ 绕 $\hat z_A$ 转了 $\theta$，也就是 $(-\sin\theta,\ \cos\theta,\ 0)^T$；
• $\{B\}$ 的 $z$ 轴就是转轴本身，纹丝不动，还是 $(0,0,1)^T$。

把这三个矢量按列排起来，就得到

$$R_z(\theta)=\begin{bmatrix}\cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1\end{bmatrix}$$

同理，绕 $\hat x$ 轴和绕 $\hat y$ 轴旋转的矩阵分别是

$$R_x(\theta)=\begin{bmatrix}1 & 0 & 0\\ 0 & \cos\theta & -\sin\theta\\ 0 & \sin\theta & \cos\theta\end{bmatrix}$$ $$R_y(\theta)=\begin{bmatrix}\cos\theta & 0 & \sin\theta\\ 0 & 1 & 0\\ -\sin\theta & 0 & \cos\theta\end{bmatrix}$$

齐次变换矩阵的第四列

到这里，$3\times3$ 的旋转矩阵算是讲完了。但它只处理了两个坐标系原点重合的情况；一旦原点不重合，就还得把平移也装进同一个矩阵里，这就是齐次变换矩阵：

$${\pstrut}^{A}_{B}T=\begin{bmatrix}{\pstrut}^{A}_{B}R & {}^{A}P_{B\,\mathrm{ORG}}\\ \begin{matrix} 0 & 0 & 0 \end{matrix} & 1\end{bmatrix}$$

它是一个 $4\times4$ 的矩阵：左上角是我们熟悉的 $3\times3$ 旋转矩阵，右上角是 $\{B\}$ 的原点在 $\{A\}$ 中的位置，最后一行固定写成 $(0,0,0,1)$。

那么回到最开始那条规则：矩阵的第 $j$ 列，就是第 $j$ 个基向量的像。$3\times3$ 矩阵有三列，对应三个基向量；现在变成 $4\times4$ 有四个基向量，那第四列对应的是谁？答案是：原点。

在齐次坐标里，一个点写成 $(x,y,z,1)^T$，最后一位固定是 1。于是第四个基向量

$$e_4=(0,0,0,1)^T$$

正好就是原点本身。所以矩阵的第四列，就是原点的像——它记录的是“这个坐标系的原点被搬到了哪里”。

为什么 3×3 里没有这一列

因为线性变换必然把原点送到原点：把 $x=0$ 代进 $Mx$，得到的还是零向量。所以用 $3\times3$ 矩阵做变换时，原点永远待在原地，不需要专门记它，三列就够了。

而一旦允许平移，原点就会被搬走，那就必须多出一列来记录它去了哪儿。这就是齐次变换矩阵比旋转矩阵多一列的根本原因，并不是为了凑数。

最后一行在保证什么

最后一行 $(0,0,0,1)$ 看上去像是凑数的，其实它在保证一件很要紧的事：最后一位 $w$ 在变换前后保持不变。

• 点是 $w=1$ 的。变换后 $0\times x+0\times y+0\times z+1\times 1=1$，仍然是 1，点还是点；
• 只关心大小和方向的量（比如位移差矢量、线速度这类自由矢量）是 $w=0$ 的。变换后仍然是 0，方向还是方向。

如果最后一行被写坏了，$w$ 就不再守恒，得到的结果既不是点也不是方向——那就不再是刚体变换了。