三维空间与位姿变换

在使用相机或者机械臂的过程中，经常会用到三维空间变换，大部分时候我们可以使用TF去获取空间中的位姿关系，但是也存在一些时候无法使用现有TF表示的情况，例如：使用相机识别工件时，已知通过图像确定了相机到工件的空间关系，此时需要将相机移动到工件正上方二次识别。但是在给机械臂发目标指令时，使用的时机械臂末端坐标系。所以此时就需要根据相机需要到达的目标位姿去计算一个机械臂末端应该到达的位姿。

关于上述问题，文章《空间位姿描述与坐标变换》讲述的还是比较清楚的。过多的内容不做详细叙述，简单记录一下这里用到的三维空间变换内容。

假设工件坐标系为世界坐标系A，相机坐标系为工具坐标系B，则问题可以描述为在已知世界坐标系与工具坐标系之间关系以及已知目标点在工具坐标系下位姿的情况下求目标点在世界坐标系下的位姿。

先看平移变换：

假设从世界坐标系A到工具坐标系B只有平移没有旋转，则目标点在两个坐标系下的角度是一致的，它们之间的变换关系只有平移变换，则假设目标点在工具坐标系下的位姿为:

$$P=\{P_B,R_B\}$$
其中，P_B为点P在坐标系B中的平移矩阵，R_B为点P在坐标系B中的旋转矩阵。则点P在坐标系A中的位姿应该表示为：
$$P=\{P_B+P_{AB},R_B\}$$
其中，P_AB为坐标系B在坐标系A中的平移矩阵。

然后再看旋转变换：

假设坐标系A与坐标系B之间经过了一个纯旋转变换，则P点在坐标系A下的坐标又可以表示为：
$$P=\{R_{AB}{P}_B,R_{AB}{R}_B\}$$
对于旋转而言，它不仅会影响到原来点的位置，也会影响到原来点的朝向。所以这里的R与P都会进行左乘一个旋转矩阵的操作。

最后对于既存在平移也存在旋转的情况下：

对于这个时候，也是我们最常见的情况，此时可以通过结合上述两个问题的解得到最后的结果，将坐标先进行旋转，再进行平移，使AB坐标系相同，此时得到问题的解为：
$$P=\{R_{AB}{P}_B+P_{AB},R_{AB}{R}_B\}$$

这里思考一个问题，对于第三种情况，如果我先进行平移变换得到中间变量：
$$P=\{P_B+P_{AB},R_B\}$$
然后再进行旋转得到最后位姿，为什么不是
$$P=\{R_{AB}(P_B+P_{AB}),R_{AB}{R}_B\}$$
这样子看起来好像也很有道理？

参考：

1、三维空间中“点的坐标变换”和“坐标系(位姿)变换

2、空间位姿描述与坐标变换