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）。启动文件包含许多可配置的参数：
参数功能如下：
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的信息，给出了红色的结束位置点。
精彩内容
您阅读本篇文章共花了：
发表评论