ROS坐标系统与cartographer坐标系统和其含义

常见的坐标系

在使用ROS进行定位导航等操作时，我们经常会遇到各种坐标系。每种坐标系都有明确的含义。理论上坐标系的名称可以是随意的，但是为了方便不同的软件间共享坐标信息，ROS定义了几个常见的坐标系。

1.base_link

base_link坐标系和机器人的底盘直接连接。其具体位置和方向都是任意的。对于不同的机器人平台，底盘上会有不同的参考点。不过ROS也给了推荐的坐标系取法。

x 轴指向机器人前方
y 轴指向机器人左方
z 轴指向机器人上方

2.odom

odom是一个固定在环境中的坐标系也就是world-fixed。它的原点和方向不会随着机器人运动而改变。但是odom的位置可以随着机器人的运动漂移。漂移导致odom不是一个很有用的长期的全局坐标。然而机器人的odom坐标必须保证是连续变化的。也就是在odom坐标系下机器人的位置必须是连续变化的，不能有突变和跳跃。
在一般使用中odom坐标系是通过里程计信息计算出来的。比如轮子的编码器或者视觉里程计算法或者陀螺仪和加速度计。odom是一个短期的局域的精确坐标系。但是却是一个比较差的长期大范围坐标。

3.map

map和odom一样是一个固定在环境中的世界坐标系。map的z轴是向上的。机器人在map坐标系下的坐标不应该随着时间漂移。但是map坐标系下的坐标并不需要保证连续性。也就是说在map坐标系下机器人的坐标可以在任何时间发生跳跃变化。
一般来说map坐标系的坐标是通过传感器的信息不断的计算更新而来。比如激光雷达，视觉定位等等。因此能够有效的减少累积误差，但是也导致每次坐标更新可能会产生跳跃。
map坐标系是一个很有用的长期全局坐标系。但是由于坐标会跳跃改变，这是一个比较差的局部坐标系（不适合用于避障和局部操作）。

坐标系的约定

在非结构化的环境中（比如开放环境），如果我们要定义一个全球坐标系

1. 默认的方向要采用 x轴向东，y轴向北，z轴向上
2. 也就是z代表水平面高度)
   如果在开发中这个约定不能完全保证，也要求尽量满足。比如对于没有GPS，指南针等传感器的机器人，仍然可以保证坐标系z轴向上的约定。如果有指南针传感器，这样就能保证x和y轴的初始化方向。

4.earth

这个坐标系是为了多个机器人相互交互而设计的。当有多个机器人的时候，每个机器人都有自己的map坐标系，他们之间的map坐标系并不相同。如果想要在不同的机器人间共享数据，则需要这个坐标系来进行转化。
如果map坐标系是一个全局坐标系，那么map到earth坐标系的变化可以是一个静态变换。如果不是的话，就要每次计算map坐标系的原点和方向。
在刚启动的时候map坐标系的全局位置可能是不知道的。这时候可以先不发布到earth的变换，直到有了比较精确的全局位置。

坐标系之间的关系
坐标系之间的关系可以用树图的方式表示。每一个坐标系只能有一个父坐标系和任意多个子坐标系。

earth -> map -> odom -> base_link
按照之前的说明，odom和map都应该连接到base_link坐标系。但是这样是不允许的，因为每一个坐标系只能有一个父坐标系。

额外的中间坐标系
这个图只表示了最少的坐标系。在保证基本的结构不变的情况下可以在其中加入中间的坐标系以提供额外的功能。

多机器人坐标系的例子

earth --> map_1 --> odom_1 --> base_link1
earth --> map_2 --> odom_2 --> base_link2

坐标系变换的计算

odom到base_link的变换由里程计数据源中的一个发布

map到base_link通过定位组件计算得出。但是定位组件并不发布从map到base_link的变换。它首先获取odom到base_link的变换然后利用定位信息计算出map到odom的变换。

earth到map的变换是根据map坐标系选取所发布的一个静态变换。如果没有设置，那么就会使用机器人的初始位置作为坐标原点。

Map之间的切换

如果机器人的运动范围很大，那么极有可能是要切换地图的。在室内环境下，在不同的建筑物中，和不同的楼层地图都会不同。
在不同的地图间切换的时候，定位组件要恰当的把odom的parent替换成新的地图。主要是map到base_link之间的变换要选取恰当的地图，然后在转换成map到odom之间的变换。

odom坐标系的连续性
在切换地图的时候，odom坐标系不应该受到影响。odom坐标系要保证连续性。可能影响连续性的情况包括进出电梯，机器人自身没有运动，但是周围环境发生很大的变化。还有可能由于运动距离太远，造成数据溢出。这些都要特殊进行处理。

参考链接：ROS REP 105.

cartographer坐标参考定义见：backpack_3d.lua

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 2,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66

return options

Local map frame是一次slam过程中的原点。但是现在cartographer支持Incremental mapping。global map是整个地图的原点，local map是每一次建图的原点。
map_frame = “map”：cartographer中使用的全局坐标系，最好保持默认，否则ROS的Rviz不认识其它的定义，导致无法可视化建图过程。
tracking_frame=”base_link”：机器人中心坐标系，其它传感器数据都是以这个为基础进行插入的，它是整个SLAM系统的核心坐标系；cartographer_ros里面有个tf_bridge的类就是专门用来查询其它坐标系到此坐标系的转换关系。
published_frame = “base_link”
odom_frame = “odom” ：published_frame与odom_frame配合使用，如果参数provide_odom_frame = true 那么最后可视化时，发布的转换消息是从 published_frame->odom_frame->map_frame, 也即cartographer内部计算出了未经回环检测的局部图坐标到优化后的全局图坐标之间的转换关系并发布了出来。在跑官网的二维背包例子时，在map坐标周围一直跳动的odom就是这个玩意。