当前位置：首页 > news >正文

SLAM从入门到精通（tf的使用）

在ros的机器人学习过程中，有一件事情是肯定少不了的。那就是坐标系的转换。其实这也很容易理解。假设有一个机器人，它有一个3d camera、有一个机械手臂。这个时候有一个需求，需要通过3d camera告知物体的位置，然后通知另外一个机械手臂取走。

看上去这个任务很简单，但是这中间就涉及到了坐标系的转换。比如说，摄像头识别到物体，这个时候物体是摄像头坐标系下的一个pose，需要通过robot1坐标系、全局坐标系的转换关系，进一步变成世界坐标系下的位置。这样，camera看到的物体pose，就有一个全局pose信息，虽然还是同一个物品。

那么这个物体的位姿信息怎么通知到机械手臂呢？那么又需要进行反向的坐标系转换。即将物体的全局pose，转成robot2坐标系下的pose，然后进一步转换成机械手臂下的局部pose。这样机械手臂，就可以根据这个局部的pose信息，实现物体的抓取了。

1、tf的作用

tf的主要作用，其实就是实现坐标系的转换。它的作用，有可能是局部坐标系到世界坐标系，也有可能是世界坐标系到局部坐标系。

坐标系的主要沟通有两部分，一部分是xyz，一部分是围绕xyz坐标轴的旋转。其中xyz就是直接用浮点数表示，旋转一般用xyzw的四元数来表示。如果要转成rpy的角度转换，需要用tf提供的公式转换一下。

目前计算的方法很多，不过主要还是利用矩阵计算，如果是计算一个点在另外一个坐标系下的坐标，过程中仅仅涉及到旋转的话，一般是

$p1 = r * p0$

中间如果涉及到位移的话，一般还会添加一个offset，

$p1=r*p0+offset$

这样的公式虽然比较简单，但是转成矩阵不太方便，我们可以通过补齐1来处理，

$\begin{bmatrix} p1\\ 1 \end{bmatrix} = \begin{bmatrix} r & offset\\ 0& 1 \end{bmatrix} * \begin{bmatrix} p0\\ 1 \end{bmatrix}$

这样看上去公式完美一些了，可以进一步简化一下，

$P1=R*P0$

公式上面似乎回到了原点，但是每一个变量的含义其实都发生了改变。当然，这里的R仅仅表示P0到P1的转变。很多人也许会问了，如果是P1到P0的转变，这个时候应该怎么处理呢。这个时候矩阵的优势就发挥出来了，

$R^{-1} * P 1=P0$

所以，如果是需要P1坐标系下面的一个点，此时需要秀姐P0坐标系下的坐标，它所需要的就是求解旋转矩阵R的逆即可。有了单一的坐标转换，那么连续的坐标转换就变得容易了，

$P\_final = Rn * ... * R1 * P\_start$

反之也是一样，

$R1^{-1} *R2^{-1} * ... * Rn^{-1} * P\_final = P\_start$

坐标系转换中，有的是静态转换，有的是动态转换。所谓的静态转换，就是那种确定了之后，就一直不变化的。比如传感器和robot之间的固定位置。还有一种就是动态变换，它所指向的就是那种一直在变化的坐标映射关系，比如robot和map之间的位置转换关系。

关于amcl算法，大家可以参考这个链接http://wiki.ros.org/amcl。在这中间就包含了大家想学习的tf信息。

根据输入，它所以依赖的消息有scan雷达、tf坐标系转换、初始位置、map地图四个数据。第1、3、4都比较好理解。而第2个数据就和今天学习的知识相关，包含了lidar到robot、robot到odom的转换等。一开始的时候，我们还在寻找算法里面怎么没有里程计odom的数据，其实答案就在tf里面。

经过算法求解，这个时候会输出三种数据，分别是amcl位姿、粒子云和tf。第1、2都比较好理解，还有一个tf数据。根据英文解释，它发布的就是map坐标系下odom的里程数据。

了解一些tf的编程接口，也对我们理解和认识tf很有帮助。如果是tf的发布，一般会涉及到这样的接口，

tf::TransformBroadcaster
tf::Transform
tf::Quaternion

相反，如果是一些接收的接口，也会有一些tf的数据结构，

tf::TransformListener
geometry_msgs::PointStamped