ROS Navigation-----robot_pose_ekf简介

  这个包用于评估机器人的3D位姿，使用了来自不同源的位姿测量信息，它使用带有6D（3D position and 3D orientation）模型信息的扩展卡尔曼滤波器来整合来自轮子里程计，IMU传感器和视觉里程计的数据信息。 基本思路就是用松耦合方式融合不同传感器信息实现位姿估计。

1 如何使用EKF

1.1 配置

    EKF node的缺省启动文件位于robot_pose_ekf包中，文件中有许多配置参数：

• freq: 滤波器更新和发布频率，注意：频率高仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不表示可以提高每次位姿评估的精度
• sensor_timeout: 当某传感器停止给滤波器发送信息时，滤波器应该等多长时间方才可以在没有该传感器信息状况下继续工作
• odom_used, imu_used, vo_used: enable/disable输入源

启动文件中配置参数设置可以被修改，看起来大致如下所示：

1.2 编译并运行该包

• Build the package

   $ rosdep install robot_pose_ekf
   $ roscd robot_pose_ekf
   $ rosmake

• Run the robot pose ekf

   $ roslaunch robot_pose_ekf.launch

2 Nodes

2.1 robot_pose_ekf

  为确定机器人位姿，robot_pose_ekf包实现了扩展卡尔曼滤波器

2.1.1 Subscribed Topics

odom ( nav_msgs/Odometry)

• 2D pose (used by wheel odometry): 该2D pose包含了机器人在地面的位置（position）和方位（orientation）信息以及该位姿的协方差（covariance）。
• 用来发送该2D位姿的消息实际上表示了一个3D位姿，只不过把z，pitch和roll分量简单忽略了。

imu_data ( sensor_msgs/Imu)

• 3D orientation (used by the IMU): 3D方位提供机器人底座相对于世界坐标系的Roll, Pitch and Yaw偏角。 Roll and Pitch角是绝对角度（因为IMU使用了重力参考），而YAW角是相对角度。 协方差矩阵用来指定方位测量的不确定度。当仅仅收到这个主题消息时，机器人位姿ekf还不会启动，因为它还需要来自主题'vo'或者'odom'的消息。

vo ( nav_msgs/Odometry)

• 3D pose (used by Visual Odometry): 3D位姿可以完整表示机器人的位置和方位并给出位姿协方差。当用传感器只测量部分3D位姿(e.g. the wheel odometry only measures a 2D pose)时候, 可以给还未真正开始测量的部分3D位姿先简单指定一个大的协方差。

  robot_pose_ekf node并不需要所有3个传感器源一直同时可用。 每个源都能提供位姿和协方差，且这些源以不同速率和延时工作。 随着时间推移某个源可能出现和消失，该node可以自动探测当前可用的源。 如果要把自己想使用的传感器加入到输入源中，请参考指南 the Adding a GPS sensor tutorial。

2.1.2 Published Topics

robot_pose_ekf/odom_combined ( geometry_msgs/PoseWithCovarianceStamped)

• 滤波器输出 (估计的3D机器人位姿)

2.1.3 Provided tf Transforms

odom_combined → base_footprint

3 EKF如何工作

3.1 Pose interpretation

   给滤波器node提供信息的所有传感器源都有自己的参考坐标系，并且随着时间推移都可能出现漂移现象。因此，每个传感器发出来的绝对位姿不能直接对比。 因此该node使用每个传感器的相对位姿差异来更新扩展卡尔曼滤波器。

3.2 Covariance interpretation

    当机器人在四周移动时候，随着时间推移位姿不确定性会变得越来越大，协方差会无边界的增长。这样一来发布位姿自身绝对协方差没有意义，因此传感器源会发布一段时间协方差的变化（例如，速速协方差）。

3.3 Timing

   假定机器人上次更新位姿滤波器在t_0时刻， 该node只有在收到每个传感器测量值（时间戳>t_0）之后才会进行下一次的滤波器更新。 例如，在odom topic收到一条消息时间戳（t_1 > t_0）, 且在imu_data topic上也收到一条消息（ 其时间戳t_2 > t_1 > t_0）, 滤波器将被更新到所有传感器信息可用的最新时刻，这个时刻是t_1。 在t_1时刻odom位姿直接给出了，但是imu位姿需要通过在t_0和t_2两时刻之间进行线性插值求得。 在t_0到 t_1时间段，机器人位姿滤波器使用odom和IMU相对位姿进行更新。

   上图是PR2机器人机器人的实验结果显示，从绿色初始位置开始移动最后回到出发位置。 完美的odometry x-y曲线图应该是一个精确的闭环曲线图。 上图蓝色线是轮式里程计的输入，蓝色点是其估计的结束位置。红色线是robot_pose_ekf的输出, robot_pose_ekf整合了轮式里程计和IMU的信息，给出了红色的结束位置点。

4 Package Status

4.1 Stability

   该包的基础部分代码已经被充分测试且稳定较长时间。 但是ROS API一直在随着消息类型的变化在被升级。

4.2 Roadmap

• 目前该滤波器设计被用于在PR2上使用的三种传感器 (wheel odometry, imu and vo)。 下一步计划是让该包更加通用可以监听更多传感器源，所有发布均使用消息 (nav_msgs/Odometry) 。

• 增加速度到扩展卡尔曼滤波器状态中

5 Tutorials

1. robot_pose_ekf/Tutorials/AddingGpsSensor