[学习SLAM]基于EKF的Odometry+IMU+VO+GPS融合定位（robot_pose_ekf） 篇章 (1)

了解ROS的robot_pose_ekf软件包中扩展卡尔曼滤波器的用法：

robot_pose_ekf软件包用于基于来自不同来源的（部分）位姿测量值来估计机器人的3D姿势。它使用带有6D模型（3D位置和3D方向）的扩展卡尔曼滤波器，将来自车轮里程计，IMU传感器和视觉里程计的测量结果结合起来。基本思想是提供与不同传感器的松散耦合集成，其中传感器信号以ROS消息的形式接收。

主页：http://ros.org/wiki/robot_pose_ekf
Github：https://github.com/ros-planning/robot_pose_ekf

robot_pose_ekf软件包可用于融合里程计，惯性测量单元和视觉里程计的传感器输出，从而减少测量中的总体误差。

1 如何使用

1.1 配置

可以在robot_pose_ekf软件包目录中找到EKF节点的默认启动文件（launch）。启动文件包含许多可配置的参数：

• 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>

1.2 运行

（1）编译

 $ rosdep install robot_pose_ekf
 $ roscd robot_pose_ekf
 $ rosmake

（2）运行

 $ roslaunch robot_pose_ekf.launch

2 节点

2.1 订阅的话题

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

2. imu_data (sensor_msgs/Imu)
   3D orientation (used by the IMU)：3D方位提供机器人底座相对于世界坐标系的Roll，Pitch和Yaw信息。 Roll和Pitch角是绝对角度（因为IMU具有重力参考），而Yaw角是相对角度。 协方差矩阵用来指定方位测量的不确定度。当仅仅收到这个话题消息时， robot_pose_ekf不会启动，因为它还需要来自话题vo或者odom的消息。

3. 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节点不需要所有三个传感器源始终都可用。每个源给出一个位姿估计和一个协方差。这些源以不同的速率和不同的延迟运行。源会随时间出现或消失，节点将自动检测并使用可用的传感器。要添加自己的传感器输入，请查看教程：添加GPS传感器。

总结：

1. Odometry message contains the 2D pose, ie x, y and yaw angle
2. IMU message contains the 3D orientation, ie. roll, pitch and yaw angles.
3. VO message contains the full 3D pose, ie. x,y,z and roll, pitch, yaw angles.

2.2 发布的话题

robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)
滤波器的输出（估计的机器人3D位姿）。

注意：/odom和/robot_pose_ekf/odom_combined消息类型不同,前者是nav_msgs/Odometry，后者是geometry_msgs/PoseWithCovarianceStamped 。两者的区别：后者的内容是前者的一部分。

将geometry_msgs/PoseWithCovarianceStamped转换为nav_msgs/Odometry：

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
from nav_msgs.msg import Odometry

class OdomEKF():
   def __init__(self):
       # Give the node a name
       rospy.init_node('odom_ekf', anonymous=False)

       # Publisher of type nav_msgs/Odometry
       self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=10)
       
       # Wait for the /odom_combined topic to become available
       rospy.wait_for_message('input', PoseWithCovarianceStamped)
       
       # Subscribe to the /odom_combined topic
       rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
       
       rospy.loginfo("Publishing combined odometry on /odom_ekf")
       
   def pub_ekf_odom(self, msg):
       odom = Odometry()
       odom.header = msg.header
       odom.header.frame_id = '/odom'
       odom.child_frame_id = 'base_footprint'
       odom.pose = msg.pose
       
       self.ekf_pub.publish(odom)
       
if __name__ == '__main__':
   try:
       OdomEKF()
       rospy.spin()
   except:
       pass

2.3 提供的tf转换

odom_combined → base_footprint

3 如何工作

3.1 基本流程

1. 从每个传感器获取数据
2. 检查它们是否有效
3. 如果它们有效，则将它们相对于参考基准坐标系进行转换
4. 当获得传感器信息时，它将被存储，直到所有传感器的信息可用为止。收到的每个数据都有自己的时间戳
5. 一旦所有数据都可用，则在所有传感器数据均可用时，针对每个可用传感器数据更新扩展卡尔曼滤波器（在Orocos-BFL库中定义）。即：如果来自里程计的数据在时间t_0（> 0）可用，则来自imu的数据在时间t_1（> t_0）处获得，而来自视觉里程计的数据在时间t_2（> t_1）处获得，则在时间t_1对所有三组数据进行滤波。
6. 该融合的传感器数据被转换为里程计消息，并在话题/odom_combined上发布

3.2 位姿解释

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

3.3 协方差解释

随着机器人的移动，其位姿的不确定性越来越大。随着时间的流逝，协方差将无限增长。因此，在位姿本身上发布协方差是没有用的，而是传感器源发布协方差如何随时间变化，即速度的协方差。请注意，使用对世界的观测（例如，测量到已知墙壁的距离）将减少机器人位姿的不确定性；但是，这是定位而不是里程计。

3.4 Timing

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

上图显示了PR2机器人从给定的初始位置（绿点）启动，四处行驶并返回初始位置时的实验结果。完美的里程计x-y图应显示出精确的回路闭合。

蓝线显示来自车轮里程计的输入，蓝点表示估计的终点位置。

红线显示robot_pose_ekf的输出，该输出结合了车轮里程计和imu的信息，红点表示估计的最终位置。

4 源码解读

• https://blog.csdn.net/zhxue_11/article/details/83828877
• https://blog.csdn.net/weixin_42048023/article/details/84550630
• https://blog.csdn.net/shoufei403/article/details/102655696

5 总结

首先表明，通过测试，这个包可以用，而且效果还不错。但是，有些注意事项和踩坑环节，过不去就容易放弃，甚至说它无用。而且，这个高度依赖ROS和TF，虽然方便，但是如果要自己整合或者剥离ROS，也需要下一番功夫。