前言

移动机器人的状态估计需要用到很多传感器，因为对单一的传感器来讲，都存在各自的优缺点，所以需要一种多传感器融合技术，将机器人的状态估计出来。对于移动机器人来讲，自身可能携带：

惯导
轮速里程计
激光里程计
视觉里程计
gps等

如何利用各传感器的优点，将所有数据结合起来，取长补短，就用到了本篇博客介绍的内容 robot_localization ，一个移动机器人状态估计功能包。

功能包简介

robot_localization是状态估计节点的集合，每个节点都是非线性状态估计器的一种实现，用于在3D空间中移动的机器人。它包括两个状态估计节点ekf_localization_node和ukf_localization_node。另外，robot_localization提供navsat_transform_node，它有助于集成GPS数据。

功能包特点
robot_localization中的所有状态估计节点都具有共同的特征，即：

融合任意数量的传感器。节点不限制传感器的数量。例如，如果您的机器人具有多个IMU或里程计信息，则robot_localization中的状态估计节点可以支持所有传感器。
支持多种ROS消息类型。robot_localization中的所有状态估计节点都可以接收nav_msgs/Odometry，sensor_msgs/Imu，geometry_msgs/PoseWithCovarianceStamped，或geometry_msgs/TwistWithCovarianceStamped消息。
自定义每个传感器的输入。如果给定的传感器消息包含您不希望包含在状态估计中的数据，则robot_localization中的状态估计节点允许您排除该数据。
连续估计。robot_localization中的每个状态估计节点在收到一次测量结果后便开始估算车辆的状态。如果传感器数据中有间歇（即很长一段时间，没有收到任何数据），则滤波器将继续通过内部运动模型来估算机器人的状态。

所有状态估计节点都跟踪车辆的15维状态：

相关参数

ekf_localization_node和ukf_localization_node共享它们的绝大多数参数，因为大多数参数控制在与核心滤波器融合之前如何处理数据。

大部分的参数配置在params 文件夹下的yaml文件中。

基本参数

frequency

滤波器产生状态估计值的真实频率（单位为Hz）。注意：滤波器只有从输入之一接收到至少一条消息后才会开始计算。
sensor_timeout

实值周期（以秒为单位），超过此值后我们认为任何传感器都已超时。在这种情况下，我们将对EKF进行预测周期，而无需对其进行校正。可以将此参数视为滤波器将生成新输出的最小频率的倒数。
two_d_mode

如果您的机器人在平面环境中运行，并且可以忽略地面的细微变化（如IMU所报告），则将其设置为true。它将所有3D变量（Z，侧倾，俯仰以及它们各自的速度和加速度）融合成值0。这样可以确保这些值的协方差不会越界，同时确保您的机器人的状态估算值仍固定在X-Y平面上。
frame

具体参数：map_frame、odom_frame、base_link_frame、base_link_output_frame、world_frame

这些参数定义了robot_localization的操作"模式"。REP-105指定三个主要坐标系：map，odom和base_link。base_link是固定在机器人上的坐标系。机器人在odom框架中的位置会随着时间而漂移，但在短期内是准确的，应该是连续的。map坐标系与odom坐标系一样，是固定的世界坐标系，虽然它包含您机器人的全局最准确的位置估计值，但由于GPS数据的融合，它会受到离散跳跃的影响。

坐标系设置方法：

1）将map_frame，odom_frame和base_link_frame参数设置为系统的适当坐标系名称。如果系统没有map_frame，则将其删除，并确保world_frame设置为odom_frame的值。

2）如果仅融合连续位置数据（例如车轮编码器里程计，视觉里程计或IMU数据），则将world_frame设置为odom_frame的值。这是robot_localization中状态估计节点的默认行为，也是最常见的用法。

3）如果您要融合受离散跳跃影响的全局绝对位置数据（例如GPS或来自地标观测的位置更新），则：将world_frame设置为map_frame的值；确保其他东西正在生成odom-> base_link的转换。这甚至可以是robot_localization状态估计节点的另一个实例。但是，该实例不应融合全局数据。
map_frame，odom_frame和base_link_frame的默认值分别是map，odom和base_link。base_link_output_frame参数默认为base_link_frame的值。world_frame参数默认为odom_frame的值。
transform_time_offset

robot_localization包使用tf2的lookupTransform方法请求转换。此参数指定如果转换尚不可用我们要等待多长时间。如果未设置，则默认为0。值0表示我们只是获取了最新的可用转换（请参阅tf2实现），因此我们不会阻塞滤波器。指定非零的transform_timeout会影响滤波器的时序，因为它会等待最大的transform_timeout时间以使转换可用。这直接意味着大多数指定的期望输出速率都无法满足，因为滤波器在更新时必须等待转换
sensor

对于每个传感器，用户需要根据消息类型定义此参数。例如，如果定义一个Imu消息源和两个Odometry消息源，则配置如下所示：

c 复制代码

<param name="imu0" value="robot/imu/data"/>
<param name="odom0" value="wheel_encoder/odometry"/>
<param name="odom1" value="visual_odometry/odometry"/>

每个参数名称的索引都是从0开始的（例如odom0，odom1等），并且必须按顺序定义（例如，如果尚未定义pose1，则不要使用pose0和pose2）。每个参数的值是该传感器的话题名称。

[sensor]_config
具体参数则是：odomN_config twistN_config imuN_config poseN_config
对于上面定义的每个传感器消息，用户必须指定应将这些消息的哪些变量融合到最终状态估计中。里程计配置示例可能如下所示：

c 复制代码

<rosparam param="odom0_config">[true,  true,  false,
                               false, false, true,
                               true,  false, false,
                               false, false, true,
                               false, false, false]</rosparam>

布尔值的顺序为：

示例是一个二维里程计，融合位置X,Y,航向角度yaw、机器人前方速度X',航向角速度yaw'

注意：该规范是在传感器的frame_id中完成的，而不是在world_frame或base_link_frame中完成的。

[sensor]_queue_size

具体参数：odomN_queue_size\ twistN_queue_size\ imuN_queue_size\ poseN_queue_size

用户可以使用这些参数来调整每个传感器的回调队列大小。如果您的频率参数值远低于传感器的频率，这很有用，因为它允许滤波器合并更新周期之间到达的所有测量值。
[sensor]_differential

具体参数：odomN_differential \ imuN_differential \ poseN_differential

对于上面定义的每个包含位姿信息的传感器消息，用户可以指定是否应差分集成位姿变量。如果给定值设置为true，则对于从相关传感器在时间t进行的测量，我们首先将减去在时间t-1处的测量值，然后将所得值转换为速度。如果您的机器人有两个绝对位姿信息源，例如来自里程计和IMU的偏航测量，则此设置特别有用。在这种情况下，如果未正确配置输入源上的方差，则这些测量值可能会彼此不同步，并在滤波器中引起振荡，但是通过对其中一个或两个进行差分集成，我们可以避免这种情况。
[sensor]_relative

具体参数：odomN_relative \ imuN_relative \ poseN_relative

如果将此参数设置为true，则来自该传感器的任何测量值都将相对于从该传感器接收到的第一个测量值进行融合。这在以下情况下很有用，例如，如果您希望状态估计值始终从(0,0,0)开始，并且横滚，俯仰和偏航角值为(0,0,0)。它与_differential参数相似，但是我们始终在时间0删除测量，而不是在时间t-1删除测量，并且不将测量转换为速度。
imuN_remove_gravitational_acceleration

如果从IMU融合加速度计数据，则此参数确定在融合之前是否从重力测量中去除了由于重力引起的加速度。

注意：这假定提供加速度数据的IMU也在产生绝对方向。需要方向数据才能正确消除重力加速度。
gravitational_acceleration

如果imuN_remove_gravitational_acceleration设置为true，则此参数确定由于重力引起的Z方向的加速度，该加速度将从IMU的线性加速度数据中删除。默认值为9.80665 (m/s^2)。
initial_state

指定滤波器的启动状态。以与传感器配置相同的顺序，将状态指定为双精度的15维向量。例如，要在位置为(5.0,4.0,3.0)，偏航角为1.57和线速度为(0.1,0.2,0.3)的状态下启动机器人，则可以使用：

c 复制代码

<rosparam param="initial_state">[5.0,  4.0,  3.0,
                                 0.0,  0.0,  1.57,
                                 0.1,  0.2,  0.3,
                                 0.0,  0.0,  0.0,
                                 0.0,  0.0,  0.0]</rosparam>

publish_tf

如果为true，则状态估计节点将发布从world_frame参数指定的坐标系到base_link_frame参数指定的坐标系的转换。默认为true。
publish_acceleration

如果为true，则状态估计节点将发布线性加速状态。默认为false。
print_diagnostics

如果为true，则状态估计节点将向/diagnostics主题发布诊断消息。这对于调试配置和传感器数据很有用。