ROS学习系列---RViz中移动机器人来学习 URDF,TF,base_link, map,odom和odom 主题的关系



前面已经介绍了如何使用URDF建造机器人小车并显示在Rviz的仿真环境里面，但是小车是静止的。下面介绍如何让它在Rviz里面动起来，并理清URDF,TF 和 odom 的关系。

1. ROS中base_link, odom, fixed_frame, target_frame和虚拟大地图map的关系

一般在urdf文件中都要定义base_link，它代表了机器人的主干，其它所有的frame都是相对于base_link定义并粘在一起的。它们一起相对于大地图map移动，让机器人移动就是向tf发布 geometry_msgs::TransformStamped 消息通知ros  base_linke相对于map的tf转换关系。先看一下这几个概念在ros中的定义：

map

map是虚拟世界中的固定frame, 它的Z轴指向正上方，也就是天空。一个时间点上移动机器人的姿态相对于map不应该出现明显的漂移。如果一个map是不连续稳定的那就意味着机器人的位置在任何一个时间点上都会是变化的。

一般激光地位仪等设备会连续的计算map的位置因而造成它的不稳定性，这也使它不能成为一个好的参照体frame.

odom

odom是一个很好的固定世界参照frame.机器人的姿态相对odom而言是随时间是经常可变，所以在长远中它不是一个好的全局参照系。但在某一时间点而言它相对机器人的位置是不变的。

典型的 odom frame 是通过运动源来计算出来的, 例如轮子运动，视觉偏移等.

odom frame 的准确性使它在局部参照系中是很有用的。但是不适合作全局参照frame.

base_link

base_link参照系紧紧粘在移动机器人基座上的任何一个位置和角度。

各个Frames的关系

frame之间是按树状结构组织的。所以每个frame只有一个父节点和任意多个子节点。 上述几个frame的关系:

map --> odom --> base_link

Frame Authorities

odom到base_link的坐标转换是从运动源计算出来广播的。

map到base_link的坐标转换是被定位模块计算出来的. 但定位模块并不发布map到base_link的转换. 相反它先接受从odom到base_link的转换, 再计算并广播map到odom的位置转换关系。

fixed_frame

RViz中认定的大世界就是fixed_frame

target_frame

Rviz中视觉跟踪的frame是 target_frame

2. RViz如何动态确定各个frame之间的坐标转换

先看一下启动RViz的launch文件，里面定要了服务于RViz的参数和node

 <launch>
     <arg name="model" />
     <arg name="gui" default="False" />
     <param name="robot_description" textfile="$(arg model)" />
     <param name="use_gui" value="$(arg gui)"/>
     <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
     <node name="rviz" pkg="rviz" type="rviz" args="-d $(find sp1s)/urdf.rviz" required="true" />
 launch>

"robot_description" 参数定义了urdf文件的路径，它被 robot_state_publisher节点使用。该节点解析urdf文件后将各个frame的状态发布给tf. 因此在rviz里面就看到各个frame(link)之间的tf转换显示OK.否则会显示warning.

"joint_state_publisher"节点获取urdf里面定义的rotate link并发布坐标转换给tf.否则会显示warning. 注意:"joint_state_publisher" 是python写的，只支持ascii编码，不支持Unicode，如果发现RViz报告下面错误：

"No transform from [front_left] to [base_link]"

那就是因为urdf文件是Unicode编码的。

3. geometry_msgs/TransformStamped 消息的作用和机构

geometry_msgs/TransformStamped  就是通知ROS 两个frame之间的tf转换关系。当两个frame之间关系经常变化，如轮子移动，手臂关节移动等，则需要编写node来实时发布。查看该消息结构：

 <span style="font-size:18px;">rosmsg show -r geometry_msgs/TransformStamped
 # This expresses a transform from coordinate frame header.frame_id
 # to the coordinate frame child_frame_id
 #
 # This message is mostly used by the
 # <a href="http://www.ros.org/wiki/tf">tfa> package.
 # See its documentation for more information.

 Header header
 string child_frame_id # the frame id of the child frame
 Transform <strong><span style="color:#FF0000;">transformspan>strong>

 span>

Transform 其中transform就是我们关心的 tf 转换关系，child_frame_id是"base_link", header.frame_id 则是'odom'. 查看geometry_msgs/transform：

 rosmsg show -r geometry_msgs/Transform
 # This represents the transform between two coordinate frames in free space.

 Vector3 <strong><span style="color:#FF0000;">translationspan>strong>
 Quaternion rotation

 rosmsg show -r Vector3
 [geometry_msgs/Vector3]:
 # This represents a vector in free space.

 float64 x
 float64 y
 float64 z

这里的x, y, z 就定义了两个frame的tf转换关系。

4. RViz中如何定义 base_link 和 odom 之间的 tf 坐标转换

要移动机器人，就需要向tf发布geometry_msgs/TransformStamped 消息通知ros  base_linke相对于map的tf转换关系，也就是说告诉ROS运动的base_link相对于不动的odom位置偏移。这里的odom frame并不能定义在urdf文件里面，它是一个虚拟的。我们只需要RViz知道fixed_frame就是odom就可以了。所以在urdf.rviz文件中这样定义虚拟odom：

 <span style="font-size:18px;">Global Options:
     Background Color: 48; 48; 48
     Fixed Frame: <strong><span style="color:#FF0000;">/odomspan>strong>
     Target Frame: <Fixed Frame>
     Frame Rate: 30span>

如果现在启动 ＲＶiz来观察机器人：

 <span style="font-size:18px;">roslaunch sp1s display.launch model:=urdf/sp1s.urdf span>

肯定会得到错误警告，而且RViz中无法显示完整的机器人：

"No transform from [odom] to [base_link]"

这个错误很容易理解，没有任何地方定义odom和base_link之间的tf关系，他们之间是连续变化的，我们当然不能在任何地方写固定偏移量定义tf transform.

但是我们可以写一个node来不断的发送geometry_msgs/TransformStamped消息。被发送对象就是tf。 ROS官方有个实例完整代码如何发布odom到base_link的变换，代码实例直接拷贝并在本地编译。这个例子不断向odom主题发送消息更改odom与base_link之间的位移，位移的轨迹就是一个圆周。这个node名字叫 odom_publisher. 它其实做了两件事情：

1. 向 tf 发送geometry_msgs/TransformStamped 消息, 就是让机器人运动起来。

2. 向odom主题发送nav_msgs/Odometry导航消息，报告角速度，线速度和巡航角度。这部分代码相对本文来讲不是必须的。

同"joint_state_publisher" node一样，“ odom_publisher”需要在RViz启动之前启动，添加它的启动项。修改后的dispaly.launch文件：

 <launch>
 <arg name="model" />
 <arg name="gui" default="False" />
 <param name="robot_description" textfile="$(arg model)" />
 <param name="use_gui" value="$(arg gui)"/>
        <strong><span style="color:#FF0000;"><node name="odom_publisher" pkg="sp1s" type="odom_publisher" />span>strong>
        <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
 <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
 <node name="rviz" pkg="rviz" type="rviz"  args="-d  $(find sp1s)/urdf.rviz" required="true"/>
 lt;/launch>span>

再次打开RViz:

 roslaunch sp1s display.launch model:=urdf/sp1s.urdf

这次就看到机器人在地图空间中做规则的圆周运动了! 因为RViz收到了odom_publisher向tf发送的坐标转换内容。

4. RViz中观察移动轨迹

“ odom_publisher”中还向 odom主题发布了nav_msgs/Odometry导航消息，这样就可以在RViz中显示运动轨迹了。在RViz中点击 'add'，选中Odometry,配置该dispaly的topic为 "odom" 就可以看到不断变化的运动轨迹了。这是因为nav_msgs/Odometry中包含了线速度，角速度和巡航角度，所以RViz可以显示出来。

5.odom frame和 odom  topic

这两个完全是不同的东西，很容易混淆。一个是地图上的一个参照系，一个是topic用来接收导航输入的。名字一样，纯属巧合！

Simultaneous Localization and Mapping (SLAM)讲义1

简介：Slam问题的核心是将一个移动机器人放置在一个未知环境中，机器人通过自身传感器逐步建立环境的地图同时使用这张地图计算自身的位置。Slam问题由R. Smith, M. Self, and P.Cheeseman在1990年的一篇论文中提出。这个问题被认为是机器人领域的“圣杯”。因为它将使得机器人可以在毫无先验知识的环境中进行动作。
Slam问题由明显的结构，它有两个问题组成，一个是定位问题（localization），另一个是构建地图问题（Mapping）。
Localization problem：已知一张地图中有若干路标landmark，对这些路标进行测量，获得了他们的测量位置zk，然后通过zk再决定机器人的位置xk。在获取路标位置的过程中如有噪声，需要滤波。

Mapping problem：机器人的位置Xk已经知道，对路标点进行测量，获得测量位置zk，给予zk创建map m。

Slam 问题的基础：
过程模型（process model）：一个已知动力学模型的机器人通过拥有若干路标landmark的环境。

观测模型（observation model）：机器人装备了若干传感器，他们可以测量任意路标间的相对位置，以及机器人自身的位置。

其中过程模型：
方便初步理解，我们采用线性模型。把机器人的位置状态命名为：xv(k)，模型为：

– Fv(k) is the state transition matrix
– uv(k) is a vector of control inputs
– wv(k) is a vector of uncorrelated process noise errors with zero mean and covariance Qv(k)。

对于第i个路标的状态传递方程是：

Slam问题认为路标是不动的。
那么，机器人位置了路标位置的联合状态变量可以表示为：

系统的状态方程是：

– Ipi is the dim(pi) x dim(pi) identity matrix
– 0pi is the dim(pi) null vector

观测模型（observation Model）：

对于第i个路标的观测模型是：

– vi(k) is a vector of uncorrelated observation errors with zero mean and
variance Ri(k)
– Hi is the observation matrix that relates the sensor output zi(k) to the state vector x(k) when observing the ith landmark and is written as

离散的状态xk需要从对环境的测量zk中估计得出。这正是kalman filter的用武之地。