五自由度机械臂的建模 (1) —— 齐次坐标及其变换矩阵
齐次坐标系
在欧氏几何中，我们常使用笛卡尔坐标系 (Cartesian Coordinate System) 来作为二维平面或三维空间中坐标系统。与此类似，在投影几何中，则使用齐次坐标系 (Homogeneous Coordinates)，也称投影坐标系 (Projective Coordinates) 作为坐标系统。
齐次坐标可以让包括无穷远点在内的点以有限的坐标表示，使用齐次坐标的公式通常会比使用笛卡尔坐标的公式更简单、且更对称。为了表示无穷远点，齐次坐标用来表示坐标的数值数目比投影空间的维度多 1。
实投影平面可以看作是包含无穷远点在内的欧氏平面，这些无穷远点位于各自的无穷远线上，无穷远点的坐标可由无穷远线的斜率给出。对于欧氏平面上的平行线，在投影平面可以看作相交于该方向上的无穷远点。
给定欧氏平面上一点 \((x, y)\)，对任意非零实数 \(Z\)，三元组 \((xZ, yZ, Z)\) 即为该点的齐次坐标。由齐次坐标的定义，我们可以得到如下几个性质：
- 齐次坐标所表示的点的笛卡尔坐标可由前两个数值除以第三个数值得到：齐次坐标 \((X, Y, Z)\)，当 \(Z\) 不为 0 时，表示欧氏平面上的点 \((\frac{X}{Z}, \frac{Y}{Z})\)；当 \(Z\) 为 0 时，则表示无穷远点
- 一个点的齐次坐标乘以一个非零实数所得的齐次坐标仍然表示该点，故一个点可以有无限多个齐次坐标表示
- 齐次坐标的原点为 \((0, 0, 1)\)，而 \((0, 0, 0)\) 不表示任何点
对于向量 \((x, y, 1)\)，可以在几何意义上理解为空间中第三个维度为常数的平面上的一个二维向量。同样地，在三维空间的点 \((x, y, z)\)，其齐次坐标为 \((a x, a y, a z, a)\)，其中 \(a\) 为一个非零实数。齐次坐标的实质就是用 \(n + 1\) 维向量来表示 \(n\) 维向量。
使用齐次坐标可以将平移和旋转变换统一成 \(4 \times 4\) 的矩阵运算，便于计算和处理。
旋转矩阵
旋转矩阵 (Rotation Matrix) 描述两个坐标系之间的旋转变换。假设空间中有坐标系 \(\{A\}\) 和坐标系 \(\{B\}\)，则 \(\{A\}\) 到 \(\{B\}\) 的旋转变换可以描述为
\[ \begin{aligned} _{B}^{A}R & = \begin{bmatrix} ^{A}\hat{X}_{B} & ^{A}\hat{Y}_{B} & ^{A}\hat{Z}_{B} \end{bmatrix} \\ & = \begin{bmatrix} \hat{X}_{B} \cdot \hat{X}_{A} & \hat{Y}_{B} \cdot \hat{X}_{A} & \hat{Z}_{B} \cdot \hat{X}_{A} \\ \hat{X}_{B} \cdot \hat{Y}_{A} & \hat{Y}_{B} \cdot \hat{Y}_{A} & \hat{Z}_{B} \cdot \hat{Y}_{A} \\ \hat{X}_{B} \cdot \hat{Z}_{A} & \hat{Y}_{B} \cdot \hat{Z}_{A} & \hat{Z}_{B} \cdot \hat{Z}_{A} \end{bmatrix} \end{aligned} \]
其中，\(^{A}\hat{X}_{B}\)、\(^{A}\hat{Y}_{B}\)、\(^{A}\hat{Z}_{B}\) 分别为坐标系 \(\{B\}\) 的 \(X\) 轴、\(Y\) 轴、\(Z\) 轴的方向向量在坐标系 \(\{A\}\) 坐标轴上的投影，即坐标系 \(\{B\}\) 主轴方向的单位矢量。
根据以上定义，旋转矩阵各列的模均为 1，并且这些单位矢量均相互正交，且有
\[ _{B}^{A}R = {_{A}^{B}R^T} = {_{A}^{B}R^{-1}} \]
所以 \(_{B}^{A}R\) 的列是 \(\{B\}\) 的单位矢量在 \(\{A\}\) 中的描述；$ _{B}^{A}R$ 的行是 \(\{A\}\) 的单位矢量在 \(\{B\}\) 中的描述。
齐次变换矩阵
三维空间中的齐次变换矩阵表示为 \(4 \times 4\) 阶矩阵形式
\[ _{B}^{A}T = \begin{bmatrix} & _{B}^{A}R & & ^{A}P_{B\ ORG} \\ 0 & 0 & 0 & 1 \end{bmatrix} \]
其中，\(_{B}^{A}R\) 为 \(3 \times 3\) 阶旋转矩阵，\(^{A}P_{BORG}\) 为 \(3 \times 1\) 阶平移矢量。
常见的几何变换都是线性变换，包括平移、旋转、缩放等，而机器人中最常见的是平移和旋转变换，这里就以这两种变换为例分析其在齐次坐标系下的变换形式。变换都在笛卡尔坐标系下进行，三维的笛卡尔坐标系满足右手定则。
平移变换
给定空间中点 \(P\)，笛卡尔坐标系下的坐标为 \((x, y, z)\)，将点 \(P\) 分别沿 \(X\) 轴、\(Y\) 轴、\(Z\) 轴平移 \(t_{x}\)、\(t_{y}\) 和 \(t_{z}\) 个单位，得到点 \(P'\)，其坐标为 \((x', y', z')\)，则用齐次坐标变换矩阵可以表示为
\[ \begin{bmatrix} x' \\ y' \\ z' \\ 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & t_{x} \\ 0 & 1 & 0 & t_{y} \\ 0 & 0 & 1 & t_{z} \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} \]
旋转变换
三维空间内的旋转变换可以分解为绕 \(X\) 轴、绕 \(Y\) 轴和绕 \(Z\) 轴的旋转，旋转的方向同样满足右手定则：大拇指沿旋转轴线的正方向，四指的指向即为旋转的正方向。
绕 \(X\) 轴的旋转
点 \(P (x, y, z)\) 绕 \(X\) 轴旋转 \(\alpha\) 得到点 \(P' (x', y', z')\)。由于绕 \(X\) 轴旋转，因此 \(x' = x\)，即点 \(P\) 在 \(YOZ\) 平面上进行二维旋转，则有
\[ \begin{aligned} x' & = x \\ y' & = y \cos{\alpha} - z \sin{\alpha} \\ z' & = y \sin{\alpha} + z \cos{\alpha} \end{aligned} \]
用齐次坐标变换可表示为
\[ \begin{bmatrix} x' \\ y' \\ z' \\ 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos{\alpha} & -\sin{\alpha} & 0 \\ 0 & \sin{\alpha} & \cos{\alpha} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} \]
绕 \(Y\) 轴的旋转
点 \(P (x, y, z)\) 绕 \(Y\) 轴旋转 \(\theta\) 得到点 \(P' (x', y', z')\)。由于绕 \(Y\) 轴旋转，因此 \(y' = y\)，即点 \(P\) 在 \(ZOX\) 平面（注意正方向）上进行二维旋转，则有
\[ \begin{aligned} x' & = z \sin{\theta} + x \cos{\theta} \\ y' & = y' \\ z' & = z \cos{\theta} - x \sin{\theta} \end{aligned} \]
用齐次坐标变换可表示为
\[ \begin{bmatrix} x' \\ y' \\ z' \\ 1 \end{bmatrix} = \begin{bmatrix} \cos{\theta} & 0 & \sin{\theta} & 0 \\ 0 & 1 & 0 & 0 \\ -\sin{\theta} & 0 & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} \]
绕 \(Z\) 轴的旋转
点 \(P (x, y, z)\) 绕 \(Z\) 轴旋转 \(\gamma\) 得到点 \(P' (x', y', z')\)。同理，由于绕 \(Z\) 轴旋转，因此 \(z' = z\)，即点 \(P\) 在 \(XOY\) 平面上进行二维旋转，则有
\[ \begin{aligned} x' & =\ x \cos{\gamma} - y \sin{\gamma} \\ y' & =\ x \sin{\gamma} + y \sin{\gamma} \\ z' & =\ z \end{aligned} \]
用齐次坐标变换可表示为
\[ \begin{bmatrix} x' \\ y' \\ z' \\ 1 \end{bmatrix} = \begin{bmatrix} \cos{\gamma} & -\sin{\gamma} & 0 & 0 \\ \sin{\gamma} & \cos{\gamma} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} \]