机器人正运动学（5）—— 齐次变换矩阵

1. 引言

前面的文章中我们分别讨论了坐标系及其平移，旋转两种变换。但是到目前为止我们一直都在分开讨论平移和旋转，而在实际应用中两个坐标系之间的关系往往既有平移又有旋转，因此这篇文章我们将讨论一下如何以一种更为紧凑的方式来表达两个坐标系之间的位置及姿态关系。

2. 齐次坐标系变换

我们可以把这个问题分解开来看，详细说来就是当我们无法一下看出两个坐标系{A}和{B}的变换关系时，可以尝试在这两个坐标系之间插入一个中间坐标系{C}，只要我们找到了坐标系{A}和{C}的关系，然后又找到了坐标系{C}和{B}的关系，那么我们就可以间接确定{A}和{B}之间的关系。

2.1 坐标系之间的位姿关系

如下图所示，坐标系{A}经过平移变换可以得到坐标系{C}，坐标系{C}绕其自身z轴旋转可以得到坐标系{B}。根据我们在机器人正运动学（3）—— 坐标系及其变换中提到的方法，我们可以很容易的确定坐标系{A}与{C}之间的关系可以表达为： p M A = p M C + p C o r g A （ 1 ） p_{M}^{A}=p_{M}^{C}+p_{Corg}^{A} （1） pMA=pMC+pCorgA（1）

我们来解释一下这个等式的含义。等式的左侧是空间中任意一点M在坐标系{A}中的坐标；等式右侧 p M C p_{M}^{C} pMC 代表点M在坐标系{C}中的坐标； p C o r g A p_{Corg}^{A} pCorgA代表坐标系C的原点Corg（the origin of axis {C}）在坐标系{A}中的坐标。

考虑完了坐标系{A}和{C}之间的平移关系，接下来我们讨论坐标系{B}和{C}之间的旋转关系，前面我们已经叙述了坐标系{B}的由来，它是坐标系{C}沿着其z轴旋转一个角度 θ \theta θ 而来。因此，按照上一篇文章机器人正运动学（4）—— 理解变换矩阵中提到的方法，将坐标系{B}的各轴单位矢量在坐标系{C}下投影即可得到坐标系{B}与坐标系{C}的姿态变换关系，如下式所示： p M C = C R B ⋅ P M B （ 2 ） p_{M}^{C}= ^{C}\textrm{R}_{B} \cdot P_{M}^{B} （2） pMC=CRB⋅PMB（2）

C R B = [ c o s θ − s i n θ 0 s i n θ c o s θ 0 0 0 1 ] ^{C}\textrm{R}_{B}=\begin{bmatrix} cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix} CRB=⎣⎡cosθsinθ0−sinθcosθ0001⎦⎤

旋转矩阵 C R B ^{C}\textrm{R}_{B} CRB 的各列分别是坐标系{B}三个坐标轴单位矢量在坐标系{C}三个轴上的投影。我们再来解释一下上述坐标变换等式的含义，左侧代表空间点M在坐标系{C}中的坐标；等式右侧 C R B ^{C}\textrm{R}_{B} CRB 为坐标系{B}和{C}之间的变换关系； p M B p_{M}^{B} pMB 代表点M在坐标系{B}中的坐标。

接下来问题来了，有了坐标系{A}和{C}的关系，也有了坐标系{B}和{C}的关系，我们如何得到坐标系{A}和{B}之间的变换呢？其实很简单，可以从两种角度去思考，第一个是从变换的角度，假设空间一点M在坐标系{B}中坐标已知，为 p M B p_{M}^{B} pMB。那么通过坐标系{B}和{C}的变换关系（等式（2）），我们就可以得到点M在坐标系{C}下的坐标为 p M C p_{M}^{C} pMC，而我们又知道坐标系{A}和坐标系{C}之间的关系（等式（1）），因此我们可以最终得到点M在坐标系{A}中的坐标 p M A p_{M}^{A} pMA。第二个是从数学的角度，说白了就是将等式（2）代入到等式（1）就可以得到坐标系{A}和{B}之间的关系。这个关系可以用下式表示： p M A = A R B ⋅ P M B + p B o r g A （ 3 ） p_{M}^{A}=^{A}\textrm{R}_{B} \cdot P_{M}^{B}+p_{Borg}^{A} （3） pMA=ARB⋅PMB+pBorgA（3）

注意由于坐标系{B}和{C}原点是重合的，因此 p B o r g A = p C o r g A p_{Borg}^{A}=p_{Corg}^{A} pBorgA=pCorgA，又因为坐标系{A}和{C}各个轴分别平行，因此 A R B = C R B ^{A}\textrm{R}_{B}=^{C}\textrm{R}_{B} ARB=CRB。

2.2 齐次变换矩阵

在2.1节我们得出了任意两个坐标系之间的位姿关系描述，但是这种描述其实是不够紧凑的，特别是当我们面对多重连续变换时，等式会变得很臃肿。为了解决这个问题，我们在原有旋转矩阵的基础上扩展一维用于容纳平移关系。得到最终的齐次变换矩阵，这样任意两个坐标系{A}和{B}的位姿关系可以重新形式化为： [ p M A 1 ] = A T B ⋅ [ P M B 1 ] （ 4 ） \begin{bmatrix} p_{M}^{A}\\ 1 \end{bmatrix}=^{A}\textrm{T}_{B} \cdot \begin{bmatrix} P_{M}^{B}\\ 1 \end{bmatrix} （4） [pMA1]=ATB⋅[PMB1]（4）

其中，

A T B = [ A R B p B o r g A 0 1 ] ^{A}\textrm{T}_{B}=\begin{bmatrix} ^{A}\textrm{R}_{B} & p_{Borg}^{A}\\ 0 & 1 \end{bmatrix} ATB=[ARB0pBorgA1]

如果你大致理解什么是分块矩阵就会发现其实等式（4）和等式（3）是完全等价的。

2.3 齐次变换矩阵的逆

齐次变换矩阵的逆表达式为： B T A = [ B R A − B R A ⋅ p B o r g A 0 1 ] ^{B}\textrm{T}_{A}=\begin{bmatrix} ^{B}\textrm{R}_{A} & -^{B}\textrm{R}_{A}\cdot p_{Borg}^{A}\\ 0 & 1 \end{bmatrix} BTA=[BRA0−BRA⋅pBorgA1]

当你充分理解了齐次变换矩阵的本质后，它的逆是可以直接写出来的，无需特别记忆，但是在本篇文章我们暂不讨论它的由来，而是把它留到下一篇文章和齐次变换矩阵的用途一起讨论，因为他们之间的关系才更密切一些。

4. 总结

这篇文章主要介绍了齐次变换的由来，下一篇文章我们将讨论齐次变换矩阵的几个用途，这样你就不会为变换矩阵乘的顺序等一系列问题而感到烦恼了。由于个人能力有限，所述内容难免存在疏漏，欢迎指出，欢迎讨论。

下一篇：机器人正运动学（6）——齐次变换矩阵的三种解读
————————————————
版权声明：本文为CSDN博主「hitgavin」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/hitgavin/article/details/103572013