robot_pose_ekf学习

源码： https://github.com/ros-planning/robot_pose_ekf

http://wiki.ros.org/robot_pose_ekf?distro=kinetic

1 如何使用EKF

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

1.1 配置

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

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

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

robot_pose_ekf.launch

<launch>

<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
  <param name="output_frame" value="odom_combined"/>
  <param name="base_footprint_frame" value="base_footprint"/>
  <param name="freq" value="30.0"/>		
  <param name="sensor_timeout" value="1.0"/>  	
  <param name="odom_used" value="true"/>
  <param name="imu_used" value="true"/>
  <param name="vo_used" value="true"/>

  <remap from="odom" to="pr2_base_odometry/odom" />
</node>

</launch>

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分量简单忽略了。

File: nav_msgs/Odometry.msg

Raw Message Definition

# This represents an estimate of a position and velocity in free space.  
# The pose in this message should be specified in the coordinate frame given by header.frame_id.
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

Compact Message Definition

std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

imu_data (sensor_msgs/Imu)

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

File: sensor_msgs/Imu.msg

Raw Message Definition

# This is a message to hold data from an IMU (Inertial Measurement Unit)
#
# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the 
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
# data a covariance will have to be assumed or gotten from some other source
#
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 
# estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each 
# covariance matrix, and disregard the associated estimate.

Header header

geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z 

Compact Message Definition

std_msgs/Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance

vo (nav_msgs/Odometry)

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

File: nav_msgs/Odometry.msg

Raw Message Definition

# This represents an estimate of a position and velocity in free space.  
# The pose in this message should be specified in the coordinate frame given by header.frame_id.
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

Compact Message Definition

std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

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机器人位姿)

File: geometry_msgs/PoseWithCovarianceStamped.msg

Raw Message Definition

# This expresses an estimated pose with a reference coordinate frame and timestamp

Header header
PoseWithCovariance pose

Compact Message Definition

std_msgs/Header header
geometry_msgs/PoseWithCovariance pose

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一直在随着消息类型的变化在被升级。在未来的版本中，ROS API可能会再次更改为简化的单主题界面（请参阅下面的Roadmap）。

4.2 Roadmap

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

5 Tutorials

1. robot_pose_ekfTutorialsAddingGpsSensor
2. 简单介绍协方差:转载http://blog.sina.com.cn/s/blog_4aa4593d01012am3.html

https://blog.csdn.net/datase/article/details/83095458