State Estimation Nodes

ekf_localization_node

ekf_localization_node是扩展卡尔曼滤波器的实现。它使用全向运动模型及时预测状态，并使用感知的传感器数据校正该投影估计值。

ukf_localization_node

ukf_localization_node是无迹卡尔曼滤波器的实现。它使用一组精心挑选的西格玛点通过 EKF 中使用的相同运动模型投影状态，然后使用这些投影西格玛点来恢复状态估计值和协方差。这消除了雅可比矩阵的使用，并使滤波器更加稳定。但是，它也比 更费力。ekf_localization_node

参数

ekf_localization_node并共享其绝大多数参数，因为大多数参数控制数据在与核心过滤器融合之前如何处理。ukf_localization_node

状态估计节点可用的参数数量相对较多，这使得启动和配置文件成为启动其任何节点的首选方法。该包包含模板启动和配置文件，可帮助用户入门。

ekf_localization_node和ukf_localization_node通用参数

标准参数

~频率

滤波器生成状态估计值的实值频率（以 Hz 为单位）。

注意：筛选器不会开始计算，直到它从其中一个输入接收至少一条消息。

~sensor_timeout

实数周期，以秒为单位，表示我们何时将任何传感器视为已超时。在此情况下，我们会在不进行校正的情况下对扩展卡尔曼滤波器（EKF）执行预测周期。可以将这个参数视为滤波器生成新输出的最低频率的倒数。

~two_d_mode

如果您的机器人在平面环境中运行，并且您愿意忽略地面的细微变化（如IMU报告的那样），则将其设置为true。它将为所有 0D 变量（Z、横滚、俯仰及其各自的速度和加速度）融合 3 值。这样可以防止这些值的协方差爆炸，同时确保机器人的状态估计值仍然固定在 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 的值。

注意:如果您正在运行多个 EKF 实例，并且希望“覆盖”输出转换和消息以使其child_frame_id此帧，则可以进行此设置。base_link_output_frame是可选的，将默认为base_link_frame。这有助于在运行多个 EKF 实例时启用断开连接的 TF 树。当计算最终状态时，我们“覆盖”输出转换和消息，使其具有此帧作为其child_frame_id。

1. 如果仅融合连续位置数据（如车轮编码器测程法、视觉里程计或 IMU 数据），请设置world_frame 为您的odom_frame值。这是robot_localization中状态估计节点的默认行为，也是最常见的用法。
2. 如果要融合受离散跳跃影响的全局绝对位置数据（例如，GPS 或地标观测值的位置更新），则：

1. 将您的world_frame 值设置为您的map_frame值
2. 确保其他内容正在生成 odom->base_link 转换。这甚至可以是robot_localization状态估计节点的另一个实例。但是，该实例不应融合全局数据。

map_frame odom_frame base_link_frame 的默认值分别为map_frame、odom 和 base_link。base_link_output_frame 的默认值为base_link_frame 。world_frame 默认值为odom_frame。

~transform_time_offset

某些软件包要求您的变换需要在时间上稍微超前一小段时间。该参数的值将被添加到由机器人定位中的状态估计节点生成的 map->odom 或 odom->base_link 变换的时间戳中。

~transform_timeout

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

~[sensor]

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

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

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

~[sensor]_config

具体参数：

• ~odomN_config
• ~twistN_config
• ~imuN_config
• ~poseN_config

对于上面定义的每个传感器消息，用户必须指定这些消息的哪些变量应融合到最终状态估计中。测程配置示例可能如下所示：

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

布尔值的顺序$(X, Y, Z, roll, pitch, yaw, \dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}, \ddot{X}, \ddot{Y}, \ddot{Z})$

注意:规范是在传感器的frame_id中完成的，而不是在world_frame或base_link_frame中完成的。有关详细信息，请参阅配置教程。

~[sensor]_queue_size

具体参数：

• ~odomN_queue_size
• ~twistN_queue_size
• ~imuN_queue_size
• ~poseN_queue_size

用户可以使用这些参数来调整每个传感器的回调队列大小。如果frequency参数值远低于传感器的频率，这将非常有用，因为它允许筛选器合并在更新周期之间到达的所有测量值。

~[sensor]_differential

具体参数：

• ~odomN_differential
• ~imuN_differential
• ~poseN_differential

对于上述定义了包含姿态信息的每个传感器消息，用户可以指定是否应对姿态变量进行差分积分。如果某个值设置为 true，那么对于来自所讨论传感器的时间 t 的测量，首先会减去时间 t−1 的测量值，然后将得到的值转换为速度。这个设置在以下情况下特别有用：如果您的机器人具有两个绝对姿态信息的来源，例如，里程计和惯性测量单元（IMU）提供的偏航角测量。在这种情况下，如果输入源的方差没有正确配置，这些测量可能会与彼此不同步，并导致滤波器中的振荡，但通过进行差分积分，我们可以避免出现这种情况。

用户在方向数据使用此参数时应小心，因为转换为速度意味着方向状态变量的协方差将无限制地增长（除非正在融合另一个绝对方向数据源）。如果您只是希望所有姿势变量都从0，然后请使用_relative参数。

注意:如果要通过navsat_transform_node或utm_transform_node融合 GPS 信息，则应确保_differential设置为 false。

~[sensor]_relative

具体参数：

• ~odomN_relative
• ~imuN_relative
• ~poseN_relative

如果将此参数设置为true，那么来自该传感器的任何测量将与首次接收到的该传感器的测量相对融合。这在某些情况下非常有用，例如，如果您希望状态估计始终从真实值 (0,0,0) 开始，并具有横滚、俯仰和偏航的值 (0,0,0)。这类似于参数_differential，但不同之处在于，不是在时间t- 1 处删除测量，而是始终在时间 0 处删除测量，并且测量不会被转换为速度。

~imuN_remove_gravitational_acceleration

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

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

~gravitational_acceleration

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

~initial_state

使用指定的状态启动滤波器。状态以15维的浮点数向量的形式给出，与传感器配置的顺序相同。例如，要将您的机器人的起始位置设置为 (5.0, 4.0, 3.0)，偏航角为 1.57，并且线性速度为 (0.1, 0.2, 0.3)，您可以使用以下方式：

<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参数指定的框架到其子框架的变换。如果world_frame与map_frame相同，它将发布从map_frame到odom_frame的变换；如果world_frame与odom_frame相同，它将发布从odom_frame到base_link_frame的变换。默认值为 true。

~publish_acceleration

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

~print_diagnostics

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

高级参数

~use_control

如果为 true，则状态估计节点将侦听 geometry_msgs/Twist 消息的cmd_vel主题，并使用该主题生成加速项。然后，该术语用于机器人的状态预测。这在给定状态变量的收敛中出现少量滞后会导致应用中出现问题（例如，旋转期间的 LIDAR 偏移）的情况下特别有用。默认为 false。

注意:来自 IMU 的线性加速度数据的存在和包含当前将“覆盖”预测的线性加速度值。

~stamped_control

如果为 true 并且use_control也为 true，则查找 geometry_msgs/TwistStamped 消息，而不是 geometry_msgs/Twist 消息。

~control_timeout

如果use_control设置为 true，并且在此时间内未收到控制命令（以秒为单位），则不再应用基于控制的加速项。

~control_config

控制消息中的哪些变量用于状态预测。这些值的顺序是 cmd_vel、 X'、Y'、Z'、roll'、pitch'、yaw'。仅当 use_control]设置为 true 时才会使用。

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

~acceleration_limits

您的机器人在每个维度上的加速速度。匹配control_config 中的参数顺序。仅在use_control设置为 true 时使用。

<rosparam param="acceleration_limits">[1.3, 0.0, 0.0,
                                       0.0, 0.0, 3.2]</rosparam>

~deceleration_limits

您的机器人在每个维度上减速的速度。匹配 control_config 中的参数顺序。仅在use_control设置为 true 时使用。

~acceleration_gains

如果您的机器人无法立即达到其加速度极限，则可以通过这些增益来控制允许的变化。仅在use_control设置为 true 时使用。

<rosparam param="acceleration_limits">[0.8, 0.0, 0.0,
                                       0.0, 0.0, 0.9]</rosparam>

~deceleration_gains

如果您的机器人无法立即达到其减速极限，则可以通过这些增益来控制允许的变化。仅在use_control设置为 true 时使用。

~smooth_lagged_data

如果您的任何传感器生成的数据具有早于最近的滤波器更新的时间戳（更明显地说，如果您有一个来源于滞后传感器数据的数据源），将此参数设置为true将使滤波器在接收到滞后数据时，返回到滞后测量之前的最后状态，然后处理直到当前时间的所有测量。这对于来自需要大量CPU使用量来生成姿态估计的节点的测量特别有用（例如，激光扫描匹配器），因为它们经常滞后于当前时间。

~history_length

如果smooth_lagged_data设置为 true，则此参数指定筛选器将保留其状态和测量历史记录的秒数。此值应至少与滞后测量与当前时间之间的时间增量一样大。

~[sensor]_nodelay

具体参数：

• ~odomN_nodelay
• ~twistN_nodelay
• ~imuN_nodelay
• ~poseN_nodelay

如果为 true，则设置 tcpNoDelay 传输提示。有证据表明，Nagle算法干扰了大型消息类型的及时接收，例如nav_msgs/里程计消息。对于输入，如果将其设置为 true，则会禁用该订阅者的 Nagle 算法。默认为 false。

~[传感器]_threshold

具体参数：

• ~odomN_pose_rejection_threshold
• odomN_twist_rejection_threshold
• poseN_rejection_threshold
• twistN_rejection_threshold
• imuN_pose_rejection_threshold
• imuN_angular_velocity_rejection_threshold
• imuN_linear_acceleration_rejection_threshold

如果您的数据受到异常值的影响，请使用这些阈值设置（以马氏距离表示）来控制允许传感器测量值与当前车辆状态的距离。如果numeric_limits<double>::max()未指定，则每个默认为。

~debug

指定是否在调试模式下运行的布尔标志。警告：将其设置为 true 将生成大量数据。数据将写入debug_out_file参数的值。默认为 false。

~debug_out_file

如果debug为 true，则调试输出写入到的文件。

~process_noise_covariance

过程噪声协方差，通常表示为Q，用于模拟滤波算法预测阶段的不确定性。它可能很难进行调整，因此将其暴露为一个参数，以便更容易进行自定义设置。可以不必去调整此参数，但通过调整它，您可以获得更优越的结果。一般来说，对于输入消息中给定变量的方差而言，Q的值越大，滤波器越快地收敛到测量值。

~dynamic_process_noise_covariance

如果设置为true，将根据机器人的速度动态调整process_noise_covariance。这在机器人停止移动时希望机器人的估计误差协方差不再增加时非常有用。默认值为false。

~initial_estimate_covariance

估计协方差（通常表示为 P）定义了当前状态估计中的误差。该参数允许用户设置矩阵的初始值，这将影响滤波器收敛的速度。例如，如果用户将值设置为[0,0]到一个非常小的值，例如 1e-12，然后尝试将 X 位置的测量值与高方差值融合X，那么滤波器“信任”这些测量值的速度将非常慢，并且收敛所需的时间将增加。同样，用户应注意此参数。当仅融合速度数据（例如，没有绝对姿势信息）时，用户可能不希望将绝对姿势变量的初始协方差值设置为大数字。这是因为这些误差将无限制地增长（由于缺乏绝对姿势测量来减少误差），并且从大值开始它们将不利于状态估计。

~reset_on_time_jump

如果设置为 true且ros::Time::isSimTime()为 true，则在主题上检测到时间跳回时，筛选器将重置为其未初始化状态。这在处理包数据时很有用，因为包可以在不重新启动节点的情况下重新启动。

~predict_to_current_time

如果设置为 true，筛选器将预测并校正到最新测量的时间（默认情况下），但现在也会预测到当前时间步长。

~disabled_at_startup

如果设置为 true，则不会在启动时运行筛选器。

Node-specific Parameters

标准参数和高级参数对于robot_localization中的所有状态估计节点都是通用的。本节详细介绍了其各自状态估计节点所特有的参数。

ukf_localization_node

ukf_localization_node的参数遵循原始论文和维基文章的命名法。

• ~alpha - 控制西格玛点的扩散。除非您熟悉无味卡尔曼滤波器，否则此设置最好保持其默认值 （0.001）。
• ~kappa - 也控制西格玛点的传播。除非您熟悉无味卡尔曼滤波器，否则此设置最好保持其默认值 （0）。
• ~beta - 与状态向量的分布有关。默认值 2 表示分布为高斯分布。与其他参数一样，除非用户熟悉无味卡尔曼滤波器，否则这应该保持不变。

已发布的主题

• odometry/filtered (nav_msgs/里程计)
• accel/filtered (geometry_msgs/加速与协方差标记）（如果启用）

已发布的转换

• 如果用户的world_frame参数设置为odom_frame的值，将会发布一个从odom_frame参数指定的框架到base_link_frame参数指定的框架的变换。

  如果用户的world_frame参数设置为map_frame的值，将会发布一个从map_frame参数指定的框架到odom_frame参数指定的框架的变换。

注意:此模式假定另一个节点正在将转换从 odom_frame 参数给出的帧广播到 base_link_frame 参数给出的帧。这可以是robot_localization状态估计节点的另一个实例。

服务业

• 通过向set_pose主题发布geometry_msgs/PoseWithCovarianceStamped消息，用户可以手动设置滤波器的状态。这在测试期间重置滤波器并允许与rviz进行交互时非常有用。另外，状态估计节点提供了一个SetPose服务，其类型为robot_localization/SetPose，也可以用于设置滤波器的状态。

navsat_transform_node

navsat_transform_node将nav_msgs/里程计消息（通常是ekf_localization_node ukf_localization_node 的输出）、包含机器人航向准确估计的 sensor_msgs/Imu 和包含 GPS 数据的 sensor_msgs/NavSatFix 消息作为输入。它以与机器人世界坐标系一致的坐标生成里程计信息。此值可以直接融合到状态估计值中。

注意:如果将此节点的输出与 robot_localization 中的任何状态估计节点融合在一起，则应确保该输入的odomN_differential设置为 false。

参数

~频率

navsat_transform_node检查新的 sensor_msgs/NavSatFix消息并在publish_filtered_gps参数设置为true时发布经过滤波的 sensor_msgs/NavSatFix消息的实际频率，以赫兹（Hz）为单位。

~延迟

在计算从 GPS 坐标到机器人世界框架的转换之前等待的时间（以秒为单位）。

~magnetic_declination_radians

输入您所在位置的磁偏角。如果您不知道，请参阅 http://www.ngdc.noaa.gov/geomag-web（确保将值转换为弧度）。如果您的 IMU 提供相对于磁北的方向，则需要此参数。

~yaw_offset

面向东方时，您的 IMU 应读取 0 表示偏航。如果没有，请在此处输入偏移量 （desired_value = 偏移量 + sensor_raw_value）。例如，如果您的 IMU 在朝北时报告 0，就像大多数 IMU 一样，则此参数将为pi/2 （~1.5707963）。此参数在版本2.2.1 中已更改。以前，navsat_transform_node假设 IMU 在朝北时读数为 0，因此yaw_offset被一致使用。

~zero_altitude

如果为 true，则此节点生成的 nav_msgs/里程计消息的姿势 Z 值设置为 0。

~publish_filtered_gps

如果设置为true，navsat_transform_node还将把机器人的世界坐标系（例如，map坐标系）的位置转换回GPS坐标，并在/gps/filtered主题上发布一个 sensor_msgs/NavSatFix 消息。

~broadcast_utm_transform

如果这是真的，navsat_transform_node将广播UTM网格和输入里程计数据帧之间的转换。有关详细信息，请参阅下面的已发布转换。

~use_odometry_yaw

如果设置为true，navsat_transform_node将不会从IMU数据获取方向，而是从输入的里程消息获取方向。用户应该注意只有在您的里程消息中指定了以地球为参考的坐标系的方向数据时，才将其设置为true，例如，由磁力计生成的方向数据。此外，如果里程源是robot_localization中的一个状态估计节点之一，用户应该至少有一个输入到节点的绝对方向数据源，且_differential和_relative参数设置为false。

~wait_for_datum

如果为 true，navsat_transform_node则等待从以下任一位置获取数据：

• 参数datum

• 服务set_datum

~broadcast_utm_transform_as_parent_frame

如果为 true，navsat_transform_node则将发布 utm->world_frame 转换，而不是 world_frame->utm 转换。 请注意，对于要发布的转换，还必须broadcast_utm_transform设置为 true。

~transform_timeout

此参数指定在转换尚不可用时，我们希望等待多长时间。如果未设置，则默认为 0。值 0 表示我们只是获得最新的可用（参见tf2实现）转换。

订阅主题

• imu/data带有方向数据的 sensor_msgs/Imu 消息
• odometry/filtered机器人当前位置的nav_msgs/里程计消息。如果您的机器人达到一些非零姿势后，您的第一个 GPS 读数是必需的。
• gps/fix包含机器人 GPS 坐标的 sensor_msgs/NavSatFix 消息

已发布主题

• odometry/gps: 一个包含机器人的GPS坐标的nav_msgs/里程计消息，已经转换到机器人的世界坐标系。这个消息可以直接融合到robot_localization的状态估计节点中。

• gps/filtered (可选): 一个sensor_msgs/NavSatFix 消息，包含了机器人的世界坐标系位置，已经转换为GPS坐标。

已发布的转换

• world_frame->utm（可选） - 如果broadcast_utm_transform参数设置为true，navsat_transform_node会计算从utm坐标系到输入里程数据的frame_id之间的变换。默认情况下，使用逆变换方式，utm坐标系被发布为里程坐标系的子坐标系。使用broadcast_utm_transform_as_parent_frame参数，utm坐标系将被发布为里程坐标系的父坐标系。这在一个TF树内有多个机器人时非常有用。

Preparing Your Data for Use with robot_localization

在开始使用robot_localization状态估计节点之前，用户必须确保其传感器数据格式正确。每类传感器数据都有各种注意事项，建议用户在尝试使用 robot_localization之前完整阅读本教程。

如需更多信息，建议用户观看 ROSCon 2015 的此演示文稿。

遵守ROS标准

要考虑的两个最重要的 ROS REP 是：

• REP-103（标准计量单位和坐标约定）
• REP-105（坐标系约定）。

我们鼓励不熟悉 ROS 或状态估计的用户仔细阅读这两个 REP，因为它几乎肯定会帮助您准备传感器数据。 尝试尽可能遵守这些标准。robot_localization

此外，查看每种受支持的 ROS 消息类型的规范可能会使用户受益：

• nav_msgs/里程计
• geometry_msgs/PoseWithCovarianceStamped
• geometry_msgs/TwistWithCovarianceStamped
• sensor_msgs/IMU

坐标系和转换传感器数据

REP-105规定了四个主要的坐标框架：base_link、odom、map和earth。base_link坐标框架与机器人固定连接。map和odom坐标框架是世界固定坐标系，它们的原点通常与机器人的起始位置对齐。earth坐标框架用于为多个map坐标框架提供一个共同的参考坐标系（例如，分布在大区域内的机器人）。在本教程中，earth坐标框架与无关。

robot_localization的状态估计节点产生一个状态估计，其姿态在map或odom坐标框架中给出，速度在base_link坐标框架中给出。在融合到状态之前，所有传入的数据都会被转换成这些坐标框架之一。不同类型的消息数据的转换方式如下：

• nav_msgs/里程计：所有姿态数据（位置和方向）从消息头的frame_id转换为world_frame参数指定的坐标框架（通常是map或odom）。在消息本身中，这特别指的是pose属性内包含的所有内容。所有扭矩数据（线性和角速度）从消息的child_frame_id转换为base_link_frame参数指定的坐标框架（通常是base_link）。

• geometry_msgs/PoseWithCovarianceStamped ：处理方式与Odometry消息中的姿态数据相同。

• geometry_msgs/PoseWithCovarianceStamped ：处理方式与Odometry消息中的扭矩数据相同。

• sensor_msgs/Imu：目前IMU消息存在一些歧义，但ROS社区正在解决这个问题。大多数IMUs在本地坐标系中本来就报告方向数据，其X和Z轴分别由指向磁北极和地球中心的矢量定义，Y轴朝东（与磁北极矢量有90度的偏移）。这个坐标系通常被称为NED（North, East, Down）。然而，REP-103规定了一个用于户外导航的ENU（East, North, Up）坐标系。截止到本文撰写时，robot_localization假定所有IMU数据采用ENU坐标系，不支持NED坐标系的数据。这个情况可能会在未来发生变化，但目前用户应确保数据在与robot_localization中的任何节点一起使用之前被转换为ENU坐标系。

IMU可能还会以不同于其“中性”位置的方向放置在机器人上。例如，用户可以侧向安装IMU，或将其旋转，使其面向机器人前方以外的方向。这个偏移通常由从base_link_frame参数到IMU消息的frame_id的静态变换来指定。robot_localization中的状态估计节点将自动校正传感器的方向，使其数据与base_link_frame参数指定的坐标系对齐。

处理tf_prefix

在ROS Indigo版本迁移到tf2后，robot_localization仍然允许使用tf_prefix参数，但与tf2一致，所有frame_id值都会去除任何前导的'/'字符。

每种传感器消息类型的注意事项

里程计

对于具有提供瞬时平移和旋转速度的轮式编码器的许多机器人平台，它们通常也内部集成这些速度以生成位置估计。如果您负责处理这些数据或可以编辑它们，请记住以下事项：

• 速度/姿态：robot_localization可以集成速度或绝对姿态信息。一般来说，最佳实践是：

  • 如果里程提供了位置和线性速度，请融合线性速度。

  • 如果里程提供了方向和角速度，请融合方向。

    注意：如果您有两个来源的方向数据，那么需要小心。如果两者都生成带有准确协方差矩阵的方向数据，那么可以安全地融合方向数据。然而，如果一个或两个方向数据的协方差矩阵不准确，那么应该只融合来自更准确传感器的方向数据。对于另一个传感器，可以使用角速度（如果提供），或者继续融合绝对方向数据，但需要为该传感器打开_differential模式。

• frame_id：参见上面有关坐标框架和变换的部分。

• 协方差：协方差值对于robot_localization非常重要。robot_pose_ekf试图将里程消息中的所有姿态变量进行融合。一些机器人的驱动程序已经按照其要求编写。这意味着如果某个传感器不生成某个特定变量（例如，不报告Z位置的机器人），那么唯一的方法让robot_pose_ekf忽略它就是将其方差膨胀到一个非常大的值（大致为1e3次方），以便有效地忽略相关变量。这种做法既是不必要的，甚至对robot_localization的性能有害。唯一的例外情况是，如果您有第二个测量相关变量的输入源，那么膨胀的协方差将有效。

  注意：有关详细信息，请参阅配置robot_localization和从 robot_pose_ekf 迁移。

• 符号：遵守 REP-103意味着您需要确保数据的符号是正确的。例如，如果您有一台地面机器人并将其逆时针旋转，那么它的偏航角应增加，其偏航角速度应为正。如果您将其向前驱动，其X坐标位置应增加，其X坐标速度应为正。

• 变换：odom->base_link变换的广播。当world_frame参数设置为配置文件中的odom_frame参数的值时，robot_localization的状态估计节点会输出一个包含位置估计的nav_msgs/里程计消息，并从由其odom_frame参数指定的框架到其base_link_frame参数指定的框架的变换。然而，一些机器人驱动程序也会在里程消息一起广播此变换。如果用户希望robot_localization负责这个变换，那么他们需要通过机器人驱动程序禁用此变换的广播。这通常是作为一个参数公开的。

IMU

除以下内容外，请务必阅读上述有关 IMU 数据坐标系和变换的部分。

1. **遵守规范：**与里程计一样，请确保您的数据符合 REP-103 和 sensor_msgs/Imu 规范。仔细检查数据的迹象，并确保值正确。frame_id
2. **协方差：**呼应里程计的建议，确保您的协方差有意义。不要使用较大的值来使筛选器忽略给定变量。将要忽略的变量的配置设置为 false。
3. **加速度：**小心加速度数据。中的状态估计节点假定放置在平面上中性右侧朝上位置的 IMU 将：robot_localization

• 测量值为Z轴方向上的加速度，为+9.81米每秒平方。
• 如果传感器被翻转+90度（左侧朝上），则Y轴的加速度应为+9.81米每秒平方。
• 如果传感器被俯仰+90度（前端朝下），X轴的加速度应为-9.81米每秒平方。

PoseWithCovarianceStamped

请参阅里程计部分。

TwistWithCovarianceStamped

请参阅里程计部分。

常见错误

• 输入数据不符合 REP-103。确保所有值（尤其是方向角）在正确的方向上增加和减少。

• 值不正确。不正确的数值。速度数据应该报告在参数给定的框架内，或者速度数据和base_link_frame之间应存在一个变换。

• 膨胀的协方差。忽略测量中的变量的首选方法是通过参数 odomN_config。

• 缺少协方差。如果您已经配置了某个传感器将某个变量融合到状态估计节点中，那么该值的方差（即方差矩阵中位置(i, i)处的值，其中i是该变量的索引）不应为0。如果遇到正在融合的变量的方差值为0，状态估计节点将向该值添加一个小的epsilon值（1e-6）。更好的解决方案是用户适当设置协方差值。

配置robot_localization

当将传感器数据合并到任何状态估计节点的位置估计中时，提取尽可能多的信息是很重要的。本教程详细介绍了传感器集成的最佳实践。robot_localization

如需更多信息，建议用户观看 ROSCon 2015 的此演示文稿。

传感器配置

配置向量的格式对于所有传感器都是相同的，即使涉及的消息类型不包含配置向量中的某些变量（例如，一个geometry_msgs/TwistWithCovarianceStamped消息可能不包含任何姿态数据，但配置向量仍然具有姿态变量的值）。未使用的变量将被简单地忽略。

请注意，配置向量是根据输入消息的frame_id给定的。例如，考虑一个产生geometry_msgs/TwistWithCovarianceStamped消息的速度传感器，其frame_id为velocity_sensor_frame。在这个示例中，我们假设存在一个从velocity_sensor_frame到您的机器人的base_link_frame（例如，base_link）的变换，并且该变换会将velocity_sensor_frame中的X轴速度转换为base_link_frame中的Z轴速度。要将传感器中的X轴速度数据包含到滤波器中，配置向量应将X轴速度值设置为true，而不是$\dot{Z}$速度值：

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

注意:布尔值的顺序为$(X, Y, Z, roll, pitch, yaw, \dot{X}, \dot{Y}, \dot{Z}, \dot{roll}, \dot{pitch}, \dot{yaw}, \ddot{X}, \ddot{Y}, \ddot{Z})$

以 2D 方式操作

配置传感器时要做出的第一个决定是机器人是否在平面环境中运行，并且您可以忽略 IMU 可能报告的接地平面变化的细微影响。如果是这样，请将参数two_d_mode设置为 true。这有效地将每次测量中的 3D 位姿变量归零，并强制将它们融合到状态估计中。

融合未测量的变量

让我们从一个例子开始。假设您有一个在平面环境中工作的轮式非完整机器人。您的机器人有一些车轮编码器，用于估计瞬时 X 速度以及绝对姿态信息。此信息在nav_msgs/里程计消息中报告。此外，您的机器人还有一个 IMU，可以测量旋转速度、车辆姿态和线性加速度。其数据在 sensor_msgs/Imu 消息中报告。由于我们在平面环境中操作，因此我们将参数two_d_mode设置为 true。这将自动将所有 3D 变量归零，例如Z,roll,pitch，它们各自的速度，以及Z加速度。我们从以下配置开始：

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

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

首先，这是有道理的，因为平面机器人只需要关注自己$X,Y,X,\dot{X},\dot{Y},\ddot{X},\ddot{Y},yaw$和$\dot{yaw}$.但是，这里有几点需要注意。

1. 对于odom0 ，我们包括X和Y（在世界坐标系中报告），$yaw,\dot{X}$（在body frame中报告），以及$\dot{yaw}$。然而，除非您的机器人内部使用IMU，否则它很可能只是使用轮式编码器数据来生成其测量中的值。因此，其速度、航向和位置数据都是从相同的源生成的。在这种情况下，我们不希望使用所有的值，因为这会将重复的信息输入到滤波器中。相反，最好只使用速度数据：

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

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

2. 接下来，我们注意到我们没有融合$\dot{Y}$。乍一看，这是正确的选择，因为我们的机器人不能瞬间侧向移动。但是，如果 nav_msgs/里程计消息报告$\dot{Y}$的值为0（且$\dot{Y}$协方差不会膨胀到一个大值），最好将该值提供给过滤器。在这种情况下，0测量表示机器人根本不会沿着那个方向移动，因此它作为一个完全有效的测量值：

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

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

您可能想知道为什么我们没有出于同样的原因融合$\dot{Z}$速度。答案是，当我们设置two_d_mode为 false 时，我们实际上融合了$\dot{Z}$速度。如果我们没有这样做，事实上，我们可以将$\dot{Z}$速度的0测量值融入滤波器中。

3. 最后，我们来到IMU。您可能会注意到，我们已经将$\ddot{Y}$更改为 false。这是因为许多系统，包括我们在这里讨论的假设系统，都不会发生瞬时Y加速度。但是，IMU 可能会报告 Y 加速度的非零噪声值，这可能会导致您的估计值快速漂移。

微分参数和相对参数

“robot_localization”中的状态估计节点允许用户融合任意数量的传感器。这允许用户使用多个源来测量某些状态向量变量，特别是姿态变量。例如，您的机器人可能从多个 IMU 获取绝对方向信息，或者它可能有多个数据源提供对其绝对位置的估计。在这种情况下，用户有两个选择：

1. 按原样融合所有绝对位置/方向数据，例如，

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

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

在这种情况下，用户应非常小心，确保每个测量的方向变量的协方差设置正确。例如，如果每个IMU都宣传了偏航方差为0.1，但是IMUs的偏航测量之间的差异大于0.1，那么滤波器的输出将在每个传感器提供的值之间来回振荡。用户应确保每个测量周围的噪声分布重叠。

1. 另外，用户可以使用_differential参数。通过为给定的传感器将其设置为true，所有姿态（位置和方向）数据将通过计算两个连续时间步骤之间的测量值变化来转换为速度。然后，数据将作为速度进行融合。然而，用户应该谨慎使用：当测量值被绝对融合时（尤其是IMUs），如果测量在给定变量上具有静态或非递增的方差，那么估计的协方差矩阵中的方差将受到限制。如果该信息被转换为速度，那么在每个时间步骤，估计值将获得一些小误差，并且相关变量的方差将无限增长。对于位置（X，Y，Z）信息，这不是问题，但对于方向数据，这是一个问题。例如，对于机器人来说，经过一段时间后，其在X方向上累积了1.5米的误差是可以接受的。但是，如果同一台机器人在偏航方向上累积了1.5弧度的误差，那么当机器人下次前进时，其位置误差将急剧增加。

关于_differential参数的一般经验法则是，如果一个给定的机器人只有一个方向数据来源，那么_differential参数应设置为false。如果有N个方向数据来源，用户可以为其中N-1个设置_differential参数为true，或者简单地确保协方差值足够大，以消除振荡。

从 robot_pose_ekf 迁移

迁移robot_pose_ekf过程相当简单。本页旨在突出显示软件包之间的相关差异，以促进快速转换。

源消息中的协方差

对于robot_pose_ekf，让滤波器忽略测量的常见方法是将其协方差大幅膨胀，通常在10^3数量级。然而，robot_localization中的状态估计节点允许用户指定应与当前状态融合的测量中的哪些变量。如果您的传感器针对特定变量报告为零，而您不希望将该值与滤波器融合，或者如果已知该传感器对该字段产生较差的数据，那么只需将其xxxx_config参数值设置为false以针对相关变量（请参考主页面以获取有关此参数的描述）。

然而，用户应该小心：有时平台约束会隐式导致变量的0测量。例如，一个不能瞬间向Y方向移动的差动驱动机器人可以安全地将Y˙的0测量与小协方差值一起融合。

''differential'' 参数

默认情况下，robot_pose_ekf将在时间t接收到一个姿态测量，确定它与时间t−1接收到的测量之间的差异，将该差异转换为当前帧，然后对该测量进行积分。这在两个传感器测量相同的姿态变量的情况下非常有用：随着时间的推移，每个传感器报告的值将开始发散。如果至少一个这些测量的协方差没有适当增长，滤波器最终将在测量值之间开始振荡。通过进行微分积分，可以避免这种情况，并且测量始终与当前状态一致。

这种情况可以通过使用robot_localization状态估计节点的三种不同方法来避免：

1. 如果要融合两个不同源的相同姿态数据（例如，两个不同的传感器测量Z位置），确保这些源准确报告它们的协方差。如果这两个源开始发散，那么它们的协方差应反映至少一个源中必定发生的增长误差。

2. 如果有可用的话，融合速度数据而不是姿态数据。如果有两个单独的数据源测量相同的变量，将最准确的一个作为姿态数据融合，将另一个作为速度融合。

3. 作为第二种方法的替代，如果某个姿态测量中无法获得速度数据，可以为其中一个传感器启用_differential参数。这将导致其被微分并作为速度融合。

集成 GPS 数据

GPS数据的集成是用户的常见需求。robot_localization包含一个节点，navsat_transform_node，用于将GPS数据转换为与机器人在其世界坐标系中的起始姿态（位置和方向）一致的坐标系。这极大地简化了GPS数据的融合。本教程将解释如何使用navsat_transform_node，并深入探讨其背后的一些数学原理。

如需更多信息，建议用户观看 ROSCon 2015 的此演示文稿。

融合GPS数据的注意事项

在开始本教程之前，用户应该确保熟悉REP-105。用户应该认识到，使用包含GPS数据的位置估计可能不适合供导航模块使用，因为GPS数据容易出现离散性的不连续性（"跳跃"）。如果您想将GPS数据融入您的位置估计中，一种潜在的解决方案是执行以下操作：

1. 运行一个robot_localization状态估计节点的实例，仅融合连续数据，如里程计和IMU数据。将此实例的world_frame参数设置为与odom_frame参数相同的值。在此坐标系中执行本地路径规划和运动。

2. 运行另一个robot_localization状态估计节点的实例，融合所有数据源，包括GPS。将此实例的world_frame参数设置为与map_frame参数相同的值。

这仅仅是一个建议，用户可以选择将GPS数据融入单个robot_localization状态估计节点中。

使用 navsat_transform_node

下图说明了一个示例设置：

必需的输入

navsat_transform_node需要三种信息来源：机器人在其世界坐标系中的当前姿态估计、地球参考的朝向以及以纬度/经度对（可选高度）表示的地理坐标。

这些数据可以通过三种不同的方式获得：

1. （默认行为）数据可以完全来自机器人的传感器和姿态估计软件。要启用此模式，请确保wait_for_datum参数设置为false（其默认值）。所需的消息包括：

• 包含原始 GPS 坐标的 sensor_msgs/NavSatFix 消息。
• 具有绝对（地球参考）标题的 sensor_msgs/Imu 消息。
• 一个包含机器人当前位置估计的nav_msgs/Odometry消息，该估计是在其起始位置指定的坐标系中的（通常是robot_localization状态估计节点的输出）。

2. 基准（全局坐标系原点）可以通过参数datum指定。

注意：若要使用此模式，必须将 wait_for_datum 参数设置为 true。

datum参数采用以下形式：

<rosparam param="datum">[55.944904, -3.186693, 0.0, map, base_link]</rosparam>

在这种模式下，参数的顺序是纬度（以十进制度数表示）、经度（以十进制度数表示）、朝向（以弧度表示）、机器人的世界坐标系（即robot_localization状态估计节点中world_frame参数的值）和机器人的机体坐标系（即robot_localization状态估计节点中base_link_frame参数的值）。当使用这种模式时，机器人假定您的机器人的世界坐标系原点位于指定的纬度和经度，并且其朝向为0（朝东）。

3. 通过使用set_datum服务和 robot_localization/SetDatum 服务消息，可以手动设置基准点。

GPS数据

请注意，所有navsat_transform_node开发都是使用 Garmin 18x GPS 装置完成的，因此可能需要处理其他装置生成的数据错综复杂。

出色的nmea_navsat_driver包提供所需的 sensor_msgs/NavSatFix 消息。下面是我们将在本教程中使用的nmea_navsat_driver启动文件：

<node pkg="nmea_navsat_driver" type="nmea_serial_driver" name="navsat" respawn="true">
<param name="port" value="/dev/ttyUSB0"/>
<param name="baud" value="19200"/>
</node>

这些信息仅在用户未通过datum参数或set_datum服务手动指定原点时才相关。

IMU数据

注意:从版本2.2.1开始，navsat_transform_node已经采用了一个标准，其中假定所有的朝向数据都以零点朝东开始。如果您的IMU不符合这个标准，而是在朝北时报告零点，您仍然可以使用yaw_offset参数来进行校正。在这种情况下，yaw_offset的值将为π/2（约1.5707963）。

用户应确保他们的 IMU 符合 REP-105。特别是，检查方向角的迹象是否在正确的方向上增加。此外，用户应查找机器人工作区域的磁偏角，将其转换为弧度，然后将该值用于magnetic_declination_radians参数。

这些信息仅在用户未通过datum参数或set_datum服务手动指定原点时才相关。

里程计数据

这应该只是您用于融合GPS数据的robot_localization状态估计节点实例的输出。

配置navsat_transform_node

下面是我们将用于本教程的navsat_transform_node启动文件：

<launch>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">

    <param name="magnetic_declination_radians" value="0"/>

    <param name="yaw_offset" value="0"/>

    <remap from="/imu/data" to="/your/imu/topic" />
    <remap from="/gps/fix" to="/your/gps/fix/topic" />
    <remap from="/odometry/filtered" to="/your/robot_localization/output/topic" />

  </node>

</launch>

这些参数在主页上讨论。

配置robot_localization

在这一点上，与robot_localization的集成非常简单。只需将以下块添加到您的状态估计节点启动文件中：

<param name="odomN" value="/your_state_estimation_node_topic">

<rosparam param="odomN_config">[true,  true,  false,
                                false, false, false,
                                false, false, false,
                                false, false, false,
                                false, false, false]</rosparam>
<param name="odomN_differential" value="false"/>

确保将odomN更改为您的里程输入值（例如，odom1、odom2等）。另外，如果您希望包括高度数据，请将odomN_config的第三个值设置为true。

注意，如果您在二维环境中操作，并且没有任何传感器测量Z或Z速度，您可以选择以下之一：

1. 将navsat_transform_node的zero_altitude参数设置为true，然后将odomN_config的第三个值设置为true。
2. 在robot_localization状态估计节点中将two_d_mode设置为true。

您不应该需要修改状态估计节点内的_differential设置。GPS是一个绝对位置传感器，启用差分积分会破坏使用它的目的。

Details

我们将从一张图片开始。考虑一个机器人，它从某个纬度和经度开始，并带有一些航向。在本教程中，我们假设航向来自一个 IMU，该 IMU 在朝东时读数为 0，并根据 ROS 规范（即逆时针方向）增加。本教程的其余部分将参考图 2：

REP-105提出了四个坐标系：base_link、odom、map和earth。base_link坐标系是与车辆刚性连接的坐标系。odom和map坐标系是世界固定坐标系，通常将它们的原点设置在车辆的起始位置和方向。earth坐标系用作多个map坐标系的共同参考坐标系，目前navsat_transform_node不支持earth坐标系。请注意，在图2中，机器人刚开始（t = 0），因此它的base_link、odom和map坐标系是对齐的。我们还可以为UTM网格定义一个坐标系，我们将其称为utm。在本教程中，我们将UTM网格坐标系称为utm。因此，我们要做的是创建一个utm->map的坐标变换。

参考图2，这些概念（希望）是清晰的。UTM原点是与机器人的GPS位置相关联的UTM区域的$（0_{UTM},0_{UTM}）$点。机器人在UTM区域内的某个位置开始，位置为$（x_{UTM},y_{UTM}）$。机器人的初始方向是相对于UTM网格的X轴的某个角度$θ$。因此，我们的变换将需要知道$x_{UTM}$、$y_{UTM}$和$θ$。现在，我们需要将纬度和经度转换为UTM坐标。UTM网格假定X轴面向东，Y轴面向（真正的）北，Z轴朝上伸出地面。这符合REP-105规定的右手坐标系。REP还规定，偏航角为0表示我们正朝下X轴，偏航增加是逆时针方向。navsat_transform_node假定您的航向数据符合这个标准。但需要考虑两个因素：：

1. IMU 驱动程序可能不允许用户应用磁偏角校正因子

2. IMU驱动程序可能在面朝北时不正确地报告0，而不是在面朝东时（尽管其航向正确增减）。幸运的是，navsat_transform_node公开了两个参数来解决IMU数据可能的这些问题：magnetic_declination_radians和yaw_offset。参考图2，对于当前测量yaw值为imu_yaw的IMU，

$ yaw_{imu} =-\omega-offset_{yaw}+θ $

$θ = yaw_{imu}+\omega+offset_{yaw} $

现在我们有一个平移$（x_{UTM},y_{UTM}）$和旋转$θ$，可以用来创建所需的utm -> map变换。我们使用这个变换将所有未来的GPS位置转换为机器人的本地坐标系。如果设置broadcast_utm_transform参数为true，navsat_transform_node还会广播这个变换。

如果您对本教程有任何疑问，请随时在 answers.ros.org 上提问。