机器人学中的坐标变换问题探讨
在机器人学中，坐标变换是一个及其复杂但重要的问题。为了更清楚的表达，我们在本文中做出如下约定。
坐标、姿态的表达使用下标来表明坐标系。比如物体在A坐标系下的位置坐标为\(P_{A}\)
将坐标系B的坐标变换到A坐标系的坐标的变换矩阵，记为\(T_{AB}\)
本文中使用的向量\((x,y,z)\)是横向量，纵向量形式为\((x,y,z)^T\)
一般我们在描述不同物体之间的位姿变换关系时，会引入坐标系之间的变换概念。即描述如何将以该物体位姿坐标原点的坐标系变换到令一个坐标系的变换。本文接下来的部分，我们会以两个物体A与B之间变换关系来展开讨论。
位置变换
最简单的当然是只考虑位置变换的情况。
假设以物体A的位置作为原点的A坐标系中，B的坐标为\((x,y,z)\)。
则我们知道两点： 1. A到B的平移向量为\(t=(x,y,z)\)
- 将B坐标系的坐标变换到A坐标系的坐标的变换向量也是\(t_{AB} = (x,y,z)\)。（因为B在B坐标系中坐标是\((0,0,0)\),B在A坐标系中坐标是\((x,y,z)\)）
总结，只考虑位置变换，则物体B在A坐标系中的位置，就是坐标系B到A的变换向量。
姿态变换
现在来考虑只有姿态变换的情况。
由于只有姿态变换，因此A与B的位置是重合的，只是姿态不同，也就是A坐标系与B坐标系的原点重合，但是坐标轴不重合。
假设物体A的坐标系的三个坐标轴在A坐标系下分别是\[ax_{A}=(1,0,0)^T,ay_{A}(0,1,0)^T,az_{A}=(0,0,1)^T\]
经过旋转\(R\)后，变换成的B坐标系的坐标轴在A坐标系下的坐标为\[bx_{A}, by_{A}, bz_{A}\](假设是列向量形式)。
因为我们知道(R乘以\((1,0,0)^T\)得到的向量是R的第一列。。。)
\[ R * ax_{A} = bx_{A} \]
\[ R * ay_{A} = by_{A} \]
\[ R * az_{A} = bz_{A} \]
所以可以得到R，此处R也就是B在A坐标系中的姿态矩阵
\[ R = [bx_{A} , by_{A}, bz_{A}] \]
由于B在B坐标系中的三个坐标轴是\(bx_{B}=(1,0,0)^T,by_{B}(0,1,0)^T,bz_{B}=(0,0,1)^T\)，并且有
\[ R_{AB} * bx_{B} = bx_{A} \]
\[ R_{AB} * by_{B} = by_{A} \]
\[ R_{AB} * bz_{B} = bz_{A} \]
所以，B坐标系到A坐标系的变换矩阵\(R_{AB}\)为（矩阵S乘以\((1,0,0)^T\)得到的向量是矩阵S的第一列...）
\[ R_{AB} = [bx_{A} , by_{A}, bz_{A}] \]
总结，物体B在坐标系A中的姿态矩阵，也就是从B坐标系到A坐标系的姿态变换矩阵。
拓展：旋转矩阵是正交矩阵
由上面我们可以知道，假设旋转矩阵R可以用来将A坐标系的三个坐标轴旋转成B坐标系的三个坐标轴。即
\[ R * ax_{A} = bx_{A} \]
\[ R * ay_{A} = by_{A} \]
\[ R * az_{A} = bz_{A} \]
则此时我们可以得到R
\[ R = [bx_{A} , by_{A}, bz_{A}] \]
上面R矩阵的三个列向量是三个单位正交向量（因为是坐标轴）。由此，我们可以很轻松地得到
\[ R R^T = E \]
由上可证：旋转矩阵是正交矩阵。
位姿变换下的位置变换
下面来综合考虑位姿变换的情况。
一般对于这种情况，假设在A坐标系中，B的姿态是R，位置是t；(变换过程可以分解成，首先从原点位移t，然后进行姿态变换成R)
对于B点，在A坐标系坐标是\(bp_{A} = t\)，在B坐标系坐标是\(bp_{B} = (0,0,0)^T\)，在A坐标系中B坐标系原点的姿态是\(br_{A} = R\)，则应该是
\[ bp_{A} = R * bp_{B} + t = t \]
- 因此，对于一个物体c，假设其在A坐标系坐标是\(c_{A}\)，在B坐标系坐标是\(c_{B}\)
\[ c_{A} = R * c_{B} + t \]
总结，物体B在坐标系A中的姿态矩阵，也就是从B坐标系到A坐标系的姿态变换矩阵\(R_{AB}\)，物体B在A坐标系中的位置，就是坐标系B到A的变换向量。并且在进行位置变换时，首先进行姿态变换，然后进行位置变换。(如果顺序反过来，上面的推导就不对了)
总结
从B坐标系到A坐标系的旋转矩阵\(R_{AB}\)，其实就是B坐标系原点在A坐标系中的姿态。
从B坐标系到A坐标系的平移\(t_{AB}\)，其实就是B坐标系原点在A坐标系中的位置坐标。
首先对于B坐标系中的姿态\(r_{B}\)，变换到A坐标系中，可以直接使用旋转矩阵\(R_{AB}\)进行变换；
\[ r_{A} = R_{AB} * r_{B} \]
- 而对于一个位置p，从B坐标系变换到A坐标系，整体的变换执行过程如下：
\[ p_{A} = R_{AB} * p_{B} + t_{AB} \]
- 因此，对于位置的齐次坐标表示形式，从B坐标系到A坐标系的齐次变换矩阵\(T_{AB}\)，可以写成
\[ T_{AB} = \left[ \begin{array}{ccc} R_{AB} & t_{AB} \\ O_{1*3} & 1\end{array} \right] \]
我们会发现，坐标系之间的姿态变换比较简单，直接做累乘操作就可以了。而对于坐标系之间的位置变换，会相对麻烦些，因为需要考虑两个坐标系之间姿态的差异。
举一个例子：对于假设B坐标系原点在A坐标系中坐标是t，那A坐标系原点在B坐标系中的坐标是\(-t\)吗？
答案: 如下，只有在\(R_{AB}=E\)时，才会有\(p_{B}=-t\)。
\[ (0,0,0)^T = R_{AB} * p_{B} + t \]
上面这个式子的含义是将A坐标系原点在B坐标系中的坐标\(p_{B}\)变换到A坐标系中是\((0,0,0)^T\)