SLAM中的空间坐标变换

在机器人开发时，经常遇到不同坐标系之间互相转换的问题。不同传感器要统一到同一个坐标系下，做slam时，地图坐标系，移动机器人base坐标系，机械臂坐标系之间也需要互相转换，所以，就按照自己的思路，同时参考网上资料，把坐标变换整理一下做个记录。内容原创，仅供参考，如有错误，希望大家可以留言相互学习。

 

对于不同的三维空间坐标系W，A，B，其中W为世界坐标系，A、B为任意机器人坐标系。

A在W中的位姿可以用平移加旋转表示，平移表示为三维向量twa，旋转表示为三维单位正交阵Rwa，旋转也可表示为四元数、欧拉角或轴角形式。平移向量twa和旋转矩阵组成变换矩阵Twa。

空间某一向量（点）p，在A系中坐标为pa，在W中的坐标可表示为pw = Twa * pa

同一点p在B系的坐标表示为 pw = Twb * pb。所以

pw = Twb * pb = Twa * pa

pb = Twb-1 * Twa * pa = Tbw * Twa * pa

其中Twb  = Twb-1

 

总结一下：

（1）A在W中的位姿可算出A投影到W的变换矩阵Twa，将A中坐标点pa投影到W的pw，pw = Twa * pa

（2）B在W中的位姿可算出B投影到W的变换矩阵Twb，将B中坐标点pb投影到W的pw，pw = Twb * pb

（3）A->B的变换矩阵Tba等于A->W->B，pb =  Tba * pa = Tbw * Twa * pa

---

这里不做详细证明，仅以二维空间为例，验证一下上面结论是否成立。

如果红色坐标系A（xa，ya）逆时针旋转α得到绿色坐标系B（xb，yb），则B在A中的位姿表示为（0，0，α），因为只有旋转，平移量为（0，0）。由欧拉角公式，B到A的旋转矩阵为：

空间中一点p，在B系中坐标为pb（rcosβ，rsinβ），其中r为向量模长，β为方向角，则p在A系的坐标为pa（rcos(α+β)，rsin(α+β)）。验证公式Rab * pb = pa如下：

  

 

等式成立。

如果平移量为0，B到A的变换矩阵为：

再考虑平移量不为0时，A系由蓝色世界坐标系W平移（tx，ty）得到。A到W的变换矩阵为：

空间中一点p，在A系中齐次坐标为pa（x，y，1），则p在W系的齐次坐标为pw（x+tx，y+ty，1)。验证公式Twa * pa = pw如下：

 

等式成立。

最后把平移和旋转写到一起，得到B到W的变换矩阵为：

确实与结论一致。