ROS与navigation教程-robot_pose_ekf

ROS与navigation教程-robot_pose_ekf

说明：

介绍了 robot_pose_ekf 的概念及相关知识

代码库

参考链接：https://github.com/ros-planning/navigation

概要

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

How to use the Robot Pose EKF

( 1 ) Configuration

EKF节点的默认启动文件位于robot_pose_ekf包目录中。启动文件包含多个可配置参数：
- freq: 滤波器更新和发布频率。注意：频率高仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不表示可以提高每次位姿评估的精度。
- sensor_timeout: 当传感器停止向滤波器发送信息时，滤波器在没有传感器的情况下等待多长时间才重新开始工作。
- odom_used, imu_used, vo_used: 确认是否输入。
在启动文件中修改配置，例如：

     <launch>
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom"/>
    <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"/>
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
  </node>
 </launch>

( 2 ) Running

编译

 $ rosdep install robot_pose_ekf

 $ roscd robot_pose_ekf

 $ rosmake

运行

 $ roslaunch robot_pose_ekf.launch

Nodes

( 1 ) robot_pose_ekf

robot_pose_ekf包实现扩展卡尔曼滤波器，用于确定机器人位姿。

( 1.1 ) Subscribed Topics

odom ([nav_msgs/Odometry][2])

2D pose (用于轮式里程计)：其包含机器人在地面中的位置（position）和方位（orientation）以及该位姿的协方差。发送此2D位姿的消息实际上表示3D位姿，但z，pitch和roll分量被简单忽略了。

imu_data ([sensor_msgs/Imu][3])

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

vo ([nav_msgs/Odometry][4])

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

robot_pose_ekf节点不要求所有三个传感器源一直可用。每个源给出位态估计和协方差。源以不同的速率和延迟进行操作。源可以随着时间的推移出现并消失，节点将自动检测并使用可用的传感器。假如要添加您自己的传感器到输入源中，请查看 the Adding a GPS sensor tutorial。

( 1.2 ) Published Topics

robot_pose_ekf/odom_combined ([geometry_msgs/PoseWithCovarianceStamped][6])

滤波器输出 (评估的3D机器人位姿)。

( 1.3 ) Provided tf Transforms

odom_combined → base_footprint

How Robot Pose EKF works

( 1 ) Pose interpretation

向滤波器节点发送信息的所有传感器源都有自己的参考坐标系，并且随着时间推移都可能出现漂移现象。
因此，不同传感器发送的绝对位姿不能相互比较。
节点使用每个传感器的相对位姿差来更新扩展卡尔曼滤波器。

( 2 ) Covariance interpretation

随着机器人的移动，其在参考坐标系中的位姿的不确定性越来越大。同时随着时间的推移，协方差将会无限增长。因此，在位姿本身上公布协方差是没有意义的，而传感器源则会随时间的变化来公布协方差，即速度的协方差。
Note that using observations of the world (e.g. measuring the distance to a known wall) will reduce the uncertainty on the robot pose; this however is localization, not odometry.

( 3 ) Timing

想象一下，机器人姿势过滤器最后在时间t_0更新。在每个传感器的至少一次测量到达时间晚于t_0的时间戳之后，节点将不会更新机器人姿态滤波器。
当例如收到有关时间戳t_1> t_0的odom主题的消息，并且在时间戳t_2> t_1> t_0的imu_data主题上，过滤器现在将更新为关于所有传感器的信息可用的最新时间，在这种情况下为时间T_1。直接给出t_1处的odom姿态，并且通过在t_0和t_2之间的imu姿态的线性插值来获得t_1上的imu姿态。在t_0和t_1之间，使用odom和imu的相对姿势来更新机器人姿势过滤器。

上图显示了PR2机器人从给定的初始位置（绿点）开始移动并返回到初始位置时的实验结果。完美的odometry x-y曲线图显示精确的闭环曲线图。蓝线显示来自轮式里程计的输入，蓝点是估计的最终位置。红线显示的是robot_pose_ekf的输出，其整合了轮式里程计和imu的信息与红点的估计结束位置。

Package Status

( 1 ) Stability

该包的基础部分代码已经被充分测试且稳定较长时间。
但是ROS API一直在随着消息类型的变化而被升级。
在未来的版本中，ROS API可能会再次更改为简化的单主题界面（参见下面的路线图）。

( 2 ) Roadmap

目前该滤波器被设计用于在PR2机器人上使用的三个传感器信号(wheel odometry, imu and vo) 。下一步计划是让该包更加通用可以监听更多传感器源，所有发布均使用消息 (nav_msgs/Odometry) 。每个源将在“里程计”消息中设置3D位姿的协方差，来指定其实际测量的3D位姿的哪一部分。
增加速度到扩展卡尔曼滤波器状态中。

Tutorials

robot_pose_ekf/Tutorials/AddingGpsSensor

参考资料

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号