机器人之POSE_EKF

0. 摘要

1. 用于评估机器人的3D位置姿态，使用来自不同来源的位置姿态信息。
2. 使用扩展卡尔曼滤波器（EKF）来整合轮式里程计、IMU传感器、视觉里程计、GPS的数据信息，得到机器人的6D模型信息（3D position and 3D orientation）。
3. 基本思想是不同传感器的松散耦合，其中传感器信号作为 ROS 消息接收。

参考连接：http://wiki.ros.org/robot_pose_ekf

1. 信息流图

1.0订阅的话题

1. odom （nav_msgs/Odometry)
3D位姿，但同样适用于2D。在由轮式机器人的编码器创建的轮式里程计中，在设置position时可以仅提供x、y坐标，z坐标置0，在设置角度时，可以仅设置yaw，其余为0，即完成了2D的适配。需要注意的是，该2Dpose包含了机器人在地面位姿的协方差。
2. imu_data (sensor_msgs/Imu)
3D 方位（由 IMU 提供）。提供包含机器人相对于世界参考系的欧拉角信息，其中Roll、Pitch是绝对的，但Yaw是可以是相对的。需要注意的是，当滤波器仅接收此传感器信息时，算法并不会启动，因为它还需要odom或者vo数据。同时需要注意的时，若为平面机器人，发布imu数据时的roll、pitch可置0，但对应方差需要相对设置非常大。
3. vo (nav_msgs/Odometry)
3D位姿（视觉里程计提供）。其可以完整表示机器人的位置、方位、协方差。当传感器仅测量3D姿势的一部分时（例如，车轮测距仅测量2D姿势），可以先指定大的协方差。

robot_pose_ekf节点不要求所有三个传感器源始终可用。每个源都能给出一个姿态估计值和一个协方差，且这些源可以在不同的速率和不同的延迟下运行。随着时间的推移，源可能会出现和消失，节点将自动检测并使用可用的传感器。

1.1 发布的主题

1. robot_pose_ekf/odom_combined （geometry_msgs/PoseWithCovarianceStamped)
过滤器的输出（估计的 3D 机器人姿势）。

1.2 提供的TF

odom_combined → base_footprint

1.3 运行的时序

该节点只有在接收到每个传感器测量值之后才会进行融合。例如，假设机器人姿势滤波器上次更新时间t_0，在时间t_1 > t_0时收到odom topic消息，且在时间戳t_2 > t_1 > t_0时收到imu_data topic消息，滤波器现在将更新到有关所有传感器的信息可用的最新时刻，这个时刻为t_1。需要注意的是，在t_1时刻处给出了odom，t_1处的imu姿态是通过t_0和t_2之间的imu姿态的线性插值获得的。在t_0和t_1之间，机器人位姿滤波器使用odom和imu的相对姿势进行更新。

1.4 整体框图

2. 配置使用

2.0 launch文件的配置

 <launch>
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
  
  	<!-- output_frame：输出融合定位信息的坐标系 -->
    <param name="output_frame" value="odom"/>
    <!-- base_footprint_frame：机器人基坐标系 -->
    <param name="base_footprint_frame" value="base_link"/>
    
    <!-- freq: 滤波器融合和发布频率，不会提高准确性，默认30Hz -->
    <param name="freq" value="30.0"/>
    <!-- sensor_timeout:滤波器无传感器信息时的最大更新时间 -->
    <param name="sensor_timeout" value="1.0"/>
    
    <!-- 滤波器融合时所使用的传感器配置 -->
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>
    
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
    
	<!-- 一般需要重映射所订阅的传感器话题 -->
    
  </node>
 </launch>

3. 注意事项

1. 订阅的话题一般需要重映射
2. 发布的话题也需要重映射，并且需要重新处理后才为一般的odom数据