通俗易懂机器人运动学左乘右乘理解
机器人运动学变换矩阵引入
本文主要涉及机器人运动方程的表示问题，即有关机器人正向动力学中变换矩阵相乘先后顺序推导。机器人正向动力学指对一给定机器人，已知连杆几何参数和关节变量，求解机器人末端执行器相对于参考坐标系的位姿（位置和姿态）。
在机器人正向动力学的求解过程中，常常涉及变换矩阵左乘与右乘的问题（实际上只是思考问题的角度不同）。当时上机器人学基础课程的时候，只是死记住绕固定坐标系变换是左乘，绕变换坐标系是右乘这一结论，并未从问题本质上对矩阵乘法的先后顺序加以了解。现在正好趁做项目这一机会对变换矩阵的具体顺序做一个全新的梳理，从问题本质上理解运动学求解过程中矩阵相乘的次序。
固定坐标系变换矩阵说明
始终绕固定坐标系进行坐标变换是最为常见，也是最容易被我们接受的一种坐标变换方式。该变换的大致思路是：
确定一个原始的坐标系（可以选择世界坐标系作为原始坐标系），
始终以原始坐标系为基准，进行多次平移或旋转变换。
该思路下求解的任务是：
- 求 原始坐标系 下某一向量经多次坐标变换后在 原始坐标系 下最终的坐标值。
变化坐标系变换矩阵说明
绕变化坐标系变换相对而言是不太容易理解却又经常遇到的一种变换，该变换的大致思路是：
确定一个原始的坐标系，以原始坐标系为基准进行一次坐标变换（平移或旋转）
之后以变换后的坐标系为基准，再次进行坐标变换。
在该思路下的求解目标为：
- 求多次坐标变换后 最终坐标系 下某一向量在 原始坐标系 下的坐标值。
区别于之前始终以固定坐标系为基准进行坐标变换的情况，以变化坐标系为基准的进行坐标变换时参考坐标系始终是变化的，最后求解的却是相对于原始坐标系的坐标值。
这一情况对应于实际机器人学，就是知道末端执行器对应坐标系下操作目标点的坐标值，且知道各关节相对于前一关节的变换矩阵，求解操作目标点在原始世界坐标系下的坐标值。
以下图为例，我们知道末端执行器坐标系 Axis6 下目标点的坐标，也知道 Axis6 相对于 Axis5 的坐标变换矩阵 6 5 T ^5_6T 65T、 Axis5 相对于 Axis4 的坐标变换矩阵 5 4 T ^4_5T 54T 等等。最终要求解的是末端执行器坐标系 Axis6 下目标点的坐标在原始坐标系 Axis1 下的坐标。
按变换次序左乘的固定坐标系变换矩阵
首先我们讨论一下绕固定坐标系进行一次坐标变换后向量的表示。如下图所示，黑色向量表示的是原始坐标系下的向量。向量 p ⃗ \vec{p} p 是我们需要表示的向量，向量 i ^ \hat{i} i^ 和 j ^ \hat{j} j^ 是原始坐标系下的基向量。向量 p ⃗ \vec{p} p 在逆时针旋转90°变换前可用 i ^ \hat{i} i^ 和 j ^ \hat{j} j^ 表示为： p ⃗ = − 1 i ^ + 2 j ^ \vec{p}=-1\hat{i}+2\hat{j} p=−1i^+2j^，用矩阵可表示如下：
p ⃗ = − 1 [ 1 0 ] + 2 [ 0 1 ] \vec{p}=-1 \left[ \begin{matrix} 1\\ 0 \end{matrix} \right]+2 \left[ \begin{matrix} 0\\ 1 \end{matrix} \right] p=−1[10]+2[01]
因为变换是线性的，所以变换后的 p ⃗ \vec{p} p 依然可以用新的基向量表示，且依旧为 p ⃗ = − 1 i ∗ ^ + 2 j ∗ ^ \vec{p}=-1\hat{i^*}+2\hat{j^*} p=−1i∗^+2j∗^，所以工作的重点就是找出新的基向量 i ∗ ^ \hat{i^*} i∗^ 和 j ∗ ^ \hat{j^*} j∗^。
下图中的例子是将向量逆时针旋转90°，所以新的基向量为 [ 0 1 ] \left[ \begin{matrix} 0\\ 1 \end{matrix} \right] [01] 和 [ − 1 0 ] \left[ \begin{matrix} -1\\ 0 \end{matrix} \right] [−10] ，即：
p ⃗ = − 1 [ 0 1 ] + 2 [ − 1 0 ] \vec{p}=-1 \left[ \begin{matrix} 0\\ 1 \end{matrix} \right]+2 \left[ \begin{matrix} -1\\ 0 \end{matrix} \right] p=−1[01]+2[−10]
不失一般性的，逆时针旋转 θ \theta θ角，得到的新基向量 i ∗ ^ = [ c o s ( θ ) s i n ( θ ) ] \hat{i^*}=\left[ \begin{matrix} cos(\theta)\\ sin(\theta) \end{matrix} \right] i∗^=[cos(θ)sin(θ)]、 j ∗ ^ = [ − s i n ( θ ) c o s ( θ ) ] \hat{j^*}=\left[ \begin{matrix} -sin(\theta)\\ cos(\theta) \end{matrix} \right] j∗^=[−sin(θ)cos(θ)]，因而通用的坐标描述为：
p ⃗ = − 1 [ c o s ( θ ) s i n ( θ ) ] + 2 [ − s i n ( θ ) c o s ( θ ) ] = [ c o s ( θ ) − s i n ( θ ) s i n ( θ ) c o s ( θ ) ] ∗ [ − 1 2 ] \vec{p}=-1 \left[ \begin{matrix} cos(\theta)\\ sin(\theta) \end{matrix} \right]+2 \left[ \begin{matrix} -sin(\theta)\\ cos(\theta) \end{matrix} \right]=\left[ \begin{matrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta)\end{matrix} \right] * \left[ \begin{matrix} -1\\ 2 \end{matrix} \right] p=−1[cos(θ)sin(θ)]+2[−sin(θ)cos(θ)]=[cos(θ)sin(θ)−sin(θ)cos(θ)]∗[−12]
上式中的矩阵是旋转矩阵的一般情况，在实际的坐标变换矩阵中，包括平移变换和旋转变换两种，我们统一记为坐标变换矩阵 T T T。
类似于我们在代数中所学的函数，多次坐标变换矩阵是以左乘格式叠加作用于向量的。这一过程类似于函数作用，先进行内部函数的作用，在把内部函数作用的结果作为变量用于外部函数。如我们有两次坐标变换的过程，第一次对应的坐标变换矩阵为 T 1 T_1 T1，第二次对应的坐标变换矩阵为 T 2 T_2 T2。两次坐标变换综合作用于原先的向量 p ⃗ \vec{p} p，可得变换后的向量 p ∗ ⃗ \vec{p^*} p∗ 如下：
p ∗ ⃗ = T 2 T 1 p ⃗ \vec{p^*}=T_2T_1\vec{p} p∗=T2T1p
这样的推导过程从几何直观以及代数推导上都是很好理解的。
按变换次序右乘的变化坐标系变换矩阵
区别于前文中始终以固定坐标系为基准进行坐标变换，另一种是不断的建立新的变换后的坐标系，且知道目标点在变换后坐标系中的坐标，最终的求解目标是求出目标点在原始坐标系中的坐标值。
对应于机器人学中就是知道操作点在末端执行器坐标系中的坐标，求该坐标在世界坐标系中的坐标。这是十分有意义的一件事，因为大多是情况下我们知道后一关节相对于前一关节的变化，相应的可以求出对应的坐标变换矩阵。且我们可以通过末端执行器上搭载的传感器获取到距离操作目标点的距离，由此解算出目标点在末端执行器坐标系下的坐标，这时候转换到世界坐标系下可以与世界坐标系中其他物体进行距离判断等操作。
仍旧以二维空间中坐标变换为例，我们记初始坐标系为 O 0 {O_0} O0，经过坐标变换后的坐标系为 O 1 {O_1} O1。本例中为方便起见，坐标系 O 1 {O_1} O1 是在坐标系 O 0 {O_0} O0 逆时针旋转 α \alpha α角度 的基础上得到的，具体如下图所示：
比如本例中向量 p ⃗ \vec{p} p 在坐标系 O 1 {O_1} O1 中投影到 x x x轴 和 y y y轴 上的点为 J J J和 K K K，对应的坐标为 [ x y ] \left[ \begin{matrix} x\\ y \end{matrix} \right] [xy]，我们需要求的是向量 p ⃗ \vec{p} p 在坐标系 O 0 {O_0} O0 中的坐标。
可以引入一个新的向量 p ′ ⃗ \vec{p'} p′，新向量是将向量 p ⃗ \vec{p} p 顺时针旋转 α \alpha α角度 的一个向量，很容易得到新向量 p ′ ⃗ \vec{p'} p′ 在原始坐标系 O 0 {O_0} O0 中的坐标值与向量 p ⃗ \vec{p} p 在变换坐标系 O 1 {O_1} O1 中的值相同，因而求原向量 p ⃗ \vec{p} p 在原始坐标系 O 0 {O_0} O0 中的坐标与新向量 p ′ ⃗ \vec{p'} p′ 逆时针旋转 α \alpha α角度 的结果是相同的。
上面这段论述将求解 变换后坐标系 中向量坐标在 原坐标系 中坐标的问题转化为：已知 原坐标系下 引入的新向量的坐标，求将该坐标逆时针旋转 α \alpha α角度 的向量在 原坐标系下 的坐标值。
可能语言表述上有些绕，不易理解，用下述公式看更为直观，坐标系 O 1 {O_1} O1 下的向量 p ⃗ \vec{p} p 在坐标系 O 0 {O_0} O0 下的坐标 p 1 ⃗ \vec{p_1} p1 为：
p 1 ⃗ = T 1 p ⃗ \vec{p_1}=T_1\vec{p} p1=T1p
当然，在只变换一次的情况下不易清楚的看出该公式与上述公式的区别。如果我们已知的向量 p 2 ⃗ \vec{p_2} p2 是在变换两次后的坐标系 O 2 {O_2} O2 下的坐标，且我们已知 坐标系 O 1 {O_1} O1 到 坐标系 O 2 {O_2} O2 的变换矩阵 T 2 T_2 T2。
- 那么坐标系 O 2 {O_2} O2 下的向量 p ⃗ \vec{p} p 在坐标系 O 1 {O_1} O1 下的坐标 p 1 ⃗ = T 2 p 2 ⃗ \vec{p_1}=T_2\vec{p_2} p1=T2p2
- 坐标系 O 1 {O_1} O1 下的坐标 p 1 ⃗ \vec{p_1} p1 在坐标系 O 0 {O_0} O0 下的坐标 p ∗ ⃗ = T 1 p 1 ⃗ \vec{p^*}=T_1\vec{p_1} p∗=T1p1
- 结合以上两条，已知向量 p 2 ⃗ \vec{p_2} p2 在坐标系 O 0 {O_0} O0 下的坐标 p ∗ ⃗ = T 1 T 2 p 2 ⃗ \vec{p^*}=T_1T_2\vec{p_2} p∗=T1T2p2
注意，这与始终围绕固定坐标系进行旋转变换，最后求原始向量在最终变换后的坐标系中向量 左乘 不同，该变换是右乘的。简要来说就是第一次的变换矩阵 T 1 T_1 T1 在右式中处于最左侧。
其实该变换仍旧可以看作内部函数作用完，外部函数接着作用的结果，内层的变换矩阵将向量映射到前一层的坐标系中，最终多次映射后回到 初始的坐标系 下。