SLAM中的空间坐标变换

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

 

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

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

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

同一点p