【机器人学】从线性变换的角度理解机器人坐标变换

1. 矩阵论中的线性变换

1.1 基变换公式

在线性空间 ${\mathbf{V}}^n$ 中，同一个向量在不同的基下的坐标表示是不一样的。当由一个基 ${\mathbf{x}}_1,{\mathbf{x}}_2,...,{\mathbf{x}}_n$ 变换为另一个基 ${\mathbf{y}}_1,{\mathbf{y}}_2,...,{\mathbf{y}}_n$ 时，由基的定义可得：
$$\begin{gathered} {\mathbf{y}}_1=c_{11}{\mathbf{x}}_1+c_{21}{\mathbf{x}}_2+...+c_{n1}{\mathbf{x}}_n \\ {\mathbf{y}}_2=c_{12}{\mathbf{x}}_1+c_{22}{\mathbf{x}}_2+...+c_{n2}{\mathbf{x}}_n \\ ... \\ {\mathbf{y}}_n=c_{1n}{\mathbf{x}}_1+c_{2n}{\mathbf{x}}_2+...+c_{nn}{\mathbf{x}}_n \end{gathered}$$
或用矩阵形式写为：
$$\mathbf{Y}=\mathbf{XC}$$
这称为基变换公式，表示同一个线性空间中两个基之间的变换关系，其中矩阵 $\mathbf{C}$ 为：
$$\left[\begin{array}{cccc}{c}_{11}& c_{12}& ...& c_{1n}\\ c_{21}& c_{22}& ...& c_{2n}\\ ...& ...& ...& ...\\ c_{n1}& c_{n2}& ...& c_{nn}\end{array}\right]$$
称为由 旧基 $\mathbf{X}$ 到 新基 $\mathbf{Y}$ 的过渡矩阵。
$$\mathbf{Y}=\left[\begin{array}{cccc}{\mathbf{y}}_1& {\mathbf{y}}_2& ...& {\mathbf{y}}_n\end{array}\right],\mathbf{X}=\left[\begin{array}{cccc}{\mathbf{x}}_1& {\mathbf{x}}_2& ...& {\mathbf{x}}_n\end{array}\right]$$

注意：这里的 ${\mathbf{x}}_1,{\mathbf{x}}_2,...,{\mathbf{x}}_n$ 和 ${\mathbf{y}}_1,{\mathbf{y}}_2,...,{\mathbf{y}}_n$ 都是线性空间 ${\mathbf{V}}^n$ 中的列向量。（列向量的形式很关键，这是后续写机器人坐标系之间的变换矩阵时的依据。）

1.2 坐标变换公式

假设由一个基 ${\mathbf{x}}_1,{\mathbf{x}}_2,...,{\mathbf{x}}_n$ 变换为另一个基 ${\mathbf{y}}_1,{\mathbf{y}}_2,...,{\mathbf{y}}_n$ 的过渡矩阵为 $\mathbf{C}$，而向量 $\mathbf{x}$ 在基 ${\mathbf{x}}_1,{\mathbf{x}}_2,...,{\mathbf{x}}_n$ 和基 ${\mathbf{y}}_1,{\mathbf{y}}_2,...,{\mathbf{y}}_n$ 的坐标表示分别为
$$\mathbf{\xi }=[{\xi }_1,{\xi }_2,...,{\xi }_n{]}^T,\mathbf{\eta }=[{\eta }_1,{\eta }_2,...,{\eta }_n{]}^T$$

则有
$$\mathbf{x}=\mathbf{X}\cdot \mathbf{\xi }=\mathbf{Y}\cdot \mathbf{\eta }=(\mathbf{X}\cdot \mathbf{C})\cdot \mathbf{\eta }=\mathbf{X}\cdot (\mathbf{C}\cdot \mathbf{\eta })$$

从而有
$$\mathbf{\xi }=\mathbf{C}\cdot \mathbf{\eta }或者\mathbf{\eta }={\mathbf{C}}^{-1}\cdot \mathbf{\xi }$$
这称为坐标变换公式，表示同一个向量在不同基下的坐标表示之间的关系。

2. 机器人学中的坐标系变换

2.1 如何直接写出两个坐标系之间的变换矩阵？

假设现在为机器人定义了两个坐标系 $\mathbf{A}$ 和 $\mathbf{B}$，比如$\mathbf{A}$代表机器人搭载的相机坐标系，$\mathbf{B}$代表机器人本体坐标系，如果现在想要直接得到 B 坐标系相对于 A 坐标系的变换矩阵 ${}^A{\mathbf{T}}_B$ ，应该怎么做？

首先明白，这里的坐标系 $\mathbf{A}$ 和 $\mathbf{B}$ 其实就是三维欧氏空间（一个特殊的线性空间）中的两个基，分别由各自三个方向的坐标轴向量 ${\mathbf{x}}_A,{\mathbf{y}}_A,{\mathbf{z}}_A$ 或者 ${\mathbf{x}}_B,{\mathbf{y}}_B,{\mathbf{z}}_B$ 组成，而现在待求的变换矩阵 ${}^A{\mathbf{T}}_B$ 其实就是坐标系 $\mathbf{A}$ 到坐标系 $\mathbf{B}$ 的过渡矩阵，其关系可以表示为
$$[{\mathbf{x}}_B,{\mathbf{y}}_B,{\mathbf{z}}_B]=[{\mathbf{x}}_A,{\mathbf{y}}_A,{\mathbf{z}}_A]\cdot {}^A\mathbf{T}{}_B$$

明白这点之后，接下来我们对照前面的基变换公式：$\mathbf{Y}=\mathbf{XC}$，那么这里机器人的相机坐标系 $\mathbf{A}$ 就是公式中的旧基 $\mathbf{X}$，本体坐标系 $\mathbf{B}$ 是公式中的新基 $\mathbf{Y}$。

根据上面基变换公式推导的过程可以得到 $\mathbf{C}$ 的求解过程：将新基 $\mathbf{Y}$ 的每个向量都分解到旧基 $\mathbf{X}$ 上（比如将 ${\mathbf{y}}_1$ 分解为 $c_{11}{\mathbf{x}}_1+c_{21}{\mathbf{x}}_2+...+c_{n1}{\mathbf{x}}_n$），然后将分解得到的权重按列向量来写，组成过渡矩阵 $\mathbf{C}$ 的第1列。同理将 ${\mathbf{y}}_2$, ${\mathbf{y}}_3$, … , ${\mathbf{y}}_n$ 都进行分解，得到 $\mathbf{C}$ 的第2列、第3列 … 第n列。

同样的，机器人坐标系之间的变换矩阵也可以这样求解：将新坐标系 $\mathbf{B}$ 的 X 轴单位向量分解到旧坐标系 $\mathbf{A}$ 的三个坐标轴方向上，然后将得到的三维向量按列来写，构成变换矩阵的第1列。以此类推，将 $\mathbf{B}$ 的 Y 轴和 Z 轴单位向量分解到 $\mathbf{A}$ 坐标系，然后构成第2列、第3列，就得到了 B 坐标系相对于 A 坐标系的变换矩阵 ${}^A{\mathbf{T}}_B$ 。

2.2 空间点在新旧坐标系下的坐标关系

假设一个空间点 P 在旧坐标系 A 的表示为 ${\mathbf{p}}_A$，在新坐标系 B 的表示为 ${\mathbf{p}}_B$，而新坐标系B相对于旧坐标系A的变换矩阵为${}^A{\mathbf{T}}_B$，那么根据坐标变换公式，可以得到两个坐标之间的关系为
$${\mathbf{p}}_B={(}^A{\mathbf{T}}_B{)}^{-1}\cdot {\mathbf{p}}_A或者{\mathbf{p}}_A={}^A{\mathbf{T}}_B\cdot {\mathbf{p}}_B$$

2.3 二维旋转的例子

为了便于理解，下面给出一个最简单的二维旋转例子。

如下图所示，坐标系 $\mathbf{A}$ （坐标轴分别为 M 和 N）逆时针旋转了 $\theta$ 度后，得到了坐标系 $\mathbf{B}$（坐标轴分别为 U 和 V），现在想求坐标系 $\mathbf{B}$ 相对于坐标系 $\mathbf{A}$ 之间的旋转变换 ${}^A{R}_B$。

根据上面结论，先将新坐标系 $\mathbf{B}$ 的坐标轴U、V 上的单位向量分解到旧坐标系$\mathbf{A}$：
$${\mathbf{x}}_U=\cos \theta \cdot {\mathbf{x}}_M+\sin \theta \cdot {\mathbf{y}}_N,{\mathbf{y}}_V=-\sin \theta \cdot {\mathbf{x}}_M+\cos \theta \cdot {\mathbf{y}}_N$$
然后将分解的权重按照列来写，则可以得到：
$${}^A\mathbf{R}{}_B=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]$$

同一空间点 $\mathbf{P}$ 在旧坐标系$\mathbf{A}$下的表示为 ${\mathbf{p}}_A(m,n)$，新坐标系 $\mathbf{B}$ 下的表示为 ${\mathbf{p}}_B(u,v)$，那么根据上面的坐标变换结论，两者之间的关系为：
$${\mathbf{p}}_A={}^A\mathbf{R}{}_B\cdot {\mathbf{p}}_B$$
展开为：
$$\left[\begin{array}{c}m\\ n\end{array}\right]=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]\cdot \left[\begin{array}{c}u\\ v\end{array}\right]$$

为了验证结果的正确性，可以将同一个空间点 $\mathbf{P}$ 分解到两个坐标系下，如下所示：

根据几何关系可以得到两组坐标(m, n)和(u, v)之间的关系：
$$\begin{gathered} m=ucos\theta -vsin\theta \\ n=usin\theta +vcos\theta \end{gathered}$$
表示为矩阵形式：
$$\left[\begin{array}{c}m\\ n\end{array}\right]=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{c}u\\ v\end{array}\right]$$
这里表明，两种方式的结果是一样的。