03-数据融合-机器人定位教程1-传感数据准备

robot_localization中的传感数据准备

开始state estimation nodes 之前需要确保传感数据已经准备完毕，传感数据类型繁多，建议用户在使用robot_localization之前阅读本教程

更多信息参见2015 ROSC presentation 。

1. 遵循ROS规范

两个重要的ROS规范:
REP-103 (Standard Units of Measure and Coordinate Conventions)
REP-105 (Coordinate Frame Conventions).

新手建议阅读两个规范，这会帮助准备传感数据。robot_localization尽可能使用这些标准，同样可以看一下下面的ROS消息格式。
nav_msgs/Odometry
geometry_msgs/PoseWithCovarianceStamped
geometry_msgs/TwistWithCovarianceStamped
sensor_msgs/Imu

2. 坐标系与传感数据变换

REP-103 指定三个基本坐标系: map, odom, and base_link. map 和 odom 坐标系是全局类的坐标系，与机器人起始位置相关， base_link 坐标系是机器本体固有坐标系。在 robot_localization所有估计节点中， 数据融合过程中将所有传感数据转换到一个坐标系中进行融合。数据类型如下可见：

• nav_msgs/Odometry -所有传感信息从传感器消息头的ID坐标系转换到全局内坐标系，通常是map 或者 odom。这个转换使用与包含姿态数据的所有消息数据，而所有twist数据(线速度和角速度)将会转换到 base_link坐标系。

• geometry_msgs/PoseWithCovarianceStamped - 与Odometry数据类似，处理pose姿态数据

• geometry_msgs/TwistWithCovarianceStamped - 与Odometry数据类似，处理twist速度数据

• sensor_msgs/Imu - IMU 消息 定义有一些含糊，大部分IMU只是广播世界坐标系的朝向角度，它的x朝地磁北极，y朝向地磁东，与NED (North, East, Down)朝向类似。然而, REP-103 定义是 ENU (East, North, Up)坐标系作为室外导航。本文中假定所有IMU传感器为ENU坐标系而不用NED. 这不是固定了，但是现在是按照此规则作坐标变换。
  IMU也可以按照机器人某个方向做基准而不是使用‘参考’位置。例如，用户可以将IMU固定在边上，或者旋转使其与前进方向一致。这个偏移通常静态的定义为base_link_frame到IMU传感frame_id的坐标变换。状态估计节点将自动校正传感器方向使其与机器人坐标系一致。

2.1 Handling tf_prefix

在ros-indigo数据迁移到 tf2 中, robot_localization允许使用 tf_prefix前缀参数，但与 tf2一致,所有坐标系以 '/' 开头

3.传感器msg类型介绍

3.1 Odometry里程计

许多机器人平台具有编码器采集车轮转速，再通过转速估计位姿，当生成里程计信息时，注意下面的事项：

• Velocities/Poses: robot_localization可以整合速度和位姿型号，实际上最好的方式是：当里程计型号提供位姿和线速度，融合线速度。当里程计提供朝向和角速度，融合朝向。警告：当具有两种类型的朝向数据时，需要小心融合，当两个都产生精确的协方差矩阵时融合起来较为可靠。然而，当其中一个信息不全，则融合其中较为精确的测量。对于其他传感器，使用角速度融合朝向角，这时需要开启 _differential模式。

• frame_id: 见之前文章中关于坐标系和坐标变换的章节。

• Covariance: 协方差值 robot_pose_ekf 将所有位姿融合在一个里程计信息里。 它在ROS的状态估计、融合工作包中广泛使用，许多机器人驱动按照它的需要提供支持。这意味着当一个传感器不产生某个变量 (e.g., 机器人不产生 Z 位姿), 唯一的生产 robot_pose_ekf的方式就是忽略它将协方差设置为非常大(10^3) 。故而变量被忽略。这种不是不是必要，同时可以影响 robot_localization的表现，这个可以通过另一个传感器采集数据来补偿。
  参见 Best Practices for Sensor Integration 和 Migration from robot_pose_ekf

• Signs: 按照 REP-103，需要确认 传感器的 signs 是正确的*。例如，有一个平面机器人操作逆时针转，这时yaw应该 增加，, yaw速度应该为 正. 当前进时，x应该增加，x的速度为正。

• Transforms: 广播 odom->base_link 变换.当世界坐标系设定为odom坐标系时, robot_localization的状态估计节点输出 nav_msgs/Odometry 的位姿估计和odom到base_link的坐标变换。然而，一些机器人驱动也随里程计变化广播坐标变换， 如果要robot_localization的坐标变换具有较高置信度，需要关闭驱动的广播。这个可以通过参数控制。

3.2 IMU

• Adherence to specifications: 需要按照 REP-103 和sensor_msgs/Imu 格式发布，同时确保frame_id正确。

• Covariance:与里程计相似，确保协方差的正确。不要使用较大的协方差数字来忽略一个变量，而应该将变量设置为false.

• Acceleration: 小心使用加速度信息，要考虑重力加速度的作用，z轴应该 +9.81
  当翻转了90度，想相应朝向应该加上加速度。

3.3 PoseWithCovarianceStamped

见里程计章节

3.4 TwistWithCovarianceStamped

见里程计章节

4. 常见错误

• 输入数据不依照REP-103。确保所有值方向为正向，这个在IMU中最为重要。

• 错误的frame IDs。例如，GPS给出的UTM数据用 nav_msgs/Odometrymsg，那它的坐标系不能是base_link or base_footprint. 同样速度信息不应该是base_link_frame，应该需要一个由速度坐标系到本体坐标系的转换。这是第二大重点问题.

• Inflated covariances协方差过大. 最好的忽略一个参数的方式是配odomN_config参数。

• Missing covariances. 当融合一个传感器到一个估计变量时，协方差(i.e., the covariance matrix value at position (i, i), where i is the index of that variable) 不应该是 0. 当协方差中出现0值，程序会加一个极小的数(1e-6) 最好的解决方式就是合理配置协方差。