ROS EKF 机器人位姿估计功能包:robot_pose_ekf 详解

ROS EKF 机器人位姿估计功能包:robot_pose_ekf 详解

  • 功能包使用
    • 文件结构
    • 配置参数
  • 订阅的话题
    • 具体代码
    • 轮速里程计
    • 惯导数据
    • 视觉里程计
  • 发布的话题
  • 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的信息,给出了红色的结束位置点。
ROS EKF 机器人位姿估计功能包:robot_pose_ekf 详解_第1张图片

你可能感兴趣的:(ros,机器人,EKF,ROS,robot_pose_ekf,位姿估计)