robot_pose_ekf 功能包

功能包简介

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

安装

下载源码

git clone https://github.com/ros-planning/robot_pose_ekf

安装完成后提示：

正克隆到 ‘robot_pose_ekf’…
remote: Enumerating objects: 569, done.
remote: Counting objects: 100% (28/28), done.
remote: Compressing objects: 100% (15/15), done.
remote: Total 569 (delta 12), reused 22 (delta 12), pack-reused 541
接收对象中: 100% (569/569), 100.28 KiB | 251.00 KiB/s, 完成.
处理 delta 中: 100% (357/357), 完成.

将 robot_pose_ekf 文件夹拷贝到 ros的工作空间下，进行编译

catkin_make

— +++ processing catkin package: ‘robot_pose_ekf’
— ==> add_subdirectory(robot_pose_ekf)
— Checking for module ‘orocos-bfl’
— No package ‘orocos-bfl’ found
CMake Error at /usr/share/cmake-3.16/Modules/FindPkgConfig.cmake:463 (message):
A required package was not found
Call Stack (most recent call first):
/usr/share/cmake-3.16/Modules/FindPkgConfig.cmake:643 (_pkg_check_modules_internal)
robot_pose_ekf/CMakeLists.txt:6 (pkg_check_modules)

有个这个报错

原因是在 robot_pose_ekf 功能包的 CMakeLists.txt 的 第6行

# bfl (Bayesian Filtering Library) is a third party package that uses pkg-config
find_package(PkgConfig)
pkg_check_modules(BFL REQUIRED orocos-bfl)

orocos-bfl 没有找到，所以报错

解决办法：
ubuntu16.04 or 18.04 :

sudo apt-get install ros-kinetic-bfl
or
sudo apt-get install ros-melodic-bfl

ubuntu20.04：sudo apt-get install ros-noetic-bfl是不行的，需要

sudo apt-get install liborocos-bfl-dev

安装完成后提示：

准备解压 …/liborocos-bfl0.8_0.8.0-5build1_amd64.deb …
正在解压 liborocos-bfl0.8:amd64 (0.8.0-5build1) …
正在选中未选择的软件包 liborocos-bfl-dev:amd64。
准备解压 …/liborocos-bfl-dev_0.8.0-5build1_amd64.deb …
正在解压 liborocos-bfl-dev:amd64 (0.8.0-5build1) …
正在设置 liborocos-bfl0.8:amd64 (0.8.0-5build1) …
正在设置 liborocos-bfl-dev:amd64 (0.8.0-5build1) …
正在处理用于 libc-bin (2.31-0ubuntu9.9) 的触发器 …

再次对ros工作空间进行编译：

catkin_make

成功后会提示：

[100%] Linking CXX executable /home/jk-jone/jone_ws/devel/lib/robot_pose_ekf/test_robot_pose_ekf_zero_covariance
[100%] Linking CXX executable /home/jk-jone/jone_ws/devel/lib/robot_pose_ekf/test_robot_pose_ekf
[100%] Built target test_robot_pose_ekf_zero_covariance
[100%] Built target test_robot_pose_ekf
[100%] Linking CXX executable /home/jk-jone/jone_ws/devel/lib/robot_pose_ekf/robot_pose_ekf
[100%] Built target robot_pose_ekf

功能包使用

文件结构

• 没有launch文件夹，有两个launch文件都在外面
• 没有config文件夹，参数设置在launch文件中进行
• src文件夹—-存放cpp文件
• include文件夹—-存放头文件
• srv—-存放服务器参数文件
• CMakeLists.txt —- 编译文件
• package.xml —- 功能包信息文件

  配置参数

  robot_pose_ekf的功能包参数配置都在launch文件中进行，没有yaml文件

可以在robot_pose_ekf软件包目录中找到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>

参数功能如下：

• output_frame ：输出的坐标系名称
• base_footprint_frame ：机器人坐标系
• freq ：滤波器更新和发布频率 注意：较高的频率仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不可以提高每次位姿估计的精度
• sensor_timeout ：当传感器停止向滤波器发送信息时，滤波器在没有传感器的情况下等待多长时间才重新开始工作
• odom_used ：里程计数据是否输入
• imu_used ：IMU数据是否输入
• vo_used ：视觉里程计数据是否输入

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

订阅的话题

具体代码

话题的订阅代码设置在odom_estimation_node.cpp中

    // subscribe to odom messages
    if (odom_used_){
      ROS_DEBUG("Odom sensor can be used");
      odom_sub_ = nh.subscribe("odom", 10, &OdomEstimationNode::odomCallback, this);
    }
    else ROS_DEBUG("Odom sensor will NOT be used");

    // subscribe to imu messages
    if (imu_used_){
      ROS_DEBUG("Imu sensor can be used");
      imu_sub_ = nh.subscribe("imu_data", 10,  &OdomEstimationNode::imuCallback, this);
    }
    else ROS_DEBUG("Imu sensor will NOT be used");

    // subscribe to vo messages
    if (vo_used_){
      ROS_DEBUG("VO sensor can be used");
      vo_sub_ = nh.subscribe("vo", 10, &OdomEstimationNode::voCallback, this);
    }
    else ROS_DEBUG("VO sensor will NOT be used");

    if (gps_used_){
      ROS_DEBUG("GPS sensor can be used");
      gps_sub_ = nh.subscribe("gps", 10, &OdomEstimationNode::gpsCallback, this);
    }
    else ROS_DEBUG("GPS sensor will NOT be used");

如果需要修改话题名，适配自己的机器人，可以在launch文件中以remap的形式，也可以在源码里直接修改

轮速里程计

源码话题名称：odom
消息类型：nav_msgs/Odometry

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

原始消息定义如下所示

# 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

惯导数据

源码话题名称：imu_data
消息类型：sensor_msgs/Imu

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

原始消息定义如下所示

# 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

视觉里程计

源码话题名称：vo
消息类型：nav_msgs/Odometry

3D pose (used by Visual Odometry)：3D位姿可以完整表示机器人的位置和方位，以及该位姿的协方差。当传感器只测量3D位姿的一部分(e.g. the wheel odometry only measures a 2D pose)时, 可以给3D位姿没有实际测量的部分指定一个较大的协方差。

原始消息定义如下所示

# 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

发布的话题

话题名称：robot_pose_ekf/odom_combined
话题类型：geometry_msgs/PoseWithCovarianceStamped

滤波器的输出（估计的机器人3D位姿）

原始消息定义如下所示

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

Header header
PoseWithCovariance pose

注意这里面没有速度

robot_pose_ekf 的工作原理

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

当机器人在四周移动时候，随着时间推移位姿不确定性会变得越来越大，协方差将无限增长。这样一来发布位姿自身协方差没有意义，因此传感器源公布协方差如何随时间变化（例如，速度的协方差）。请注意，使用世界观测（例如测量到已知墙的距离）将减少机器人姿势的不确定性; 然而，这是定位，而不是里程计

假定机器人上次更新位姿滤波器在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的信息，给出了红色的结束位置点。