前言

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

• 惯导
• 轮速里程计
• 激光里程计
• 视觉里程计
• 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维状态：

安装

安装环境：ubuntu 20.04
ros 版本：noetic

使用ubuntu软件源安装

sudo apt-get install ros-noetic-robot-localization

使用源码安装

github网址：https://github.com/cra-ros-pkg/robot_localization
注意现在默认是ros2
要选择分支 noetic-devel

git clone https://github.com/cra-ros-pkg/robot_localization --branch noetic-devel

成功后显示：

正克隆到 ‘robot_localization’… remote: Enumerating objects: 7078, done.
remote: Counting objects: 100% (72/72), done. remote: Compressing
objects: 100% (46/46), done. remote: Total 7078 (delta 33), reused 47
(delta 24), pack-reused 7006 接收对象中: 100% (7078/7078), 15.87 MiB |
370.00 KiB/s, 完成. 处理 delta 中: 100% (4739/4739), 完成.

将功能包移动到ros工作空间下面
进行编译

catkin_make

编译成功后提示：

[ 98%] Linking CXX executable /home/jk-jone/jone_ws/devel/lib/robot_localization/ukf_localization_node
[100%] Linking CXX executable /home/jk-jone/jone_ws/devel/lib/robot_localization/ekf_localization_node
[100%] Built target ukf_localization_node
[100%] Built target ekf_localization_node
[100%] Linking CXX shared library /home/jk-jone/jone_ws/devel/lib/libekf_localization_nodelet.so
[100%] Linking CXX shared library /home/jk-jone/jone_ws/devel/lib/libukf_localization_nodelet.so
[100%] Built target ekf_localization_nodelet
[100%] Built target ukf_localization_nodelet

相关参数

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消息源，则配置如下所示：

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

  <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)的状态下启动机器人，则可以使用：

<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主题发布诊断消息。这对于调试配置和传感器数据很有用。

高级参数

• use_control
  如果为true，则状态估计节点将在话题cmd_vel中监听geometry_msgs/Twist消息，并使用该消息生成加速度。然后，该加速度将用于机器人的状态预测。在给定状态变量的收敛滞后即使很小的情况下，也会在您的应用程序中引起问题（例如，旋转期间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_gains">[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/Odometry消息）有关。将输入设置为true会禁用该订阅者的Nagle算法。默认为false。

• [sensor]_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::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，则在检测到某个话题的时间跳回时，过滤器将重置为未初始化状态。这在处理bag数据时很有用，因为可以在不重新启动节点的情况下重新启动bag。

• predict_to_current_time
  如果设置为true，则滤波器可以预测和校正直到最近一次测量的时间（默认情况下），但现在还将预测到当前时间步长。

• disabled_at_startup
  如果设置为true，则不会在启动时运行滤波器。

发布的话题

• odometry/filtered (nav_msgs/Odometry)
  

• accel/filtered (geometry_msgs/AccelWithCovarianceStamped) (if enabled)
  

发布的变换

如果用户的world_frame参数设置为odom_frame的值，则将转换从odom_frame参数给出的坐标系发布到base_link_frame参数给出的坐标系。如果用户的world_frame参数设置为map_frame的值，则将转换从map_frame参数给出的坐标系发布到odom_frame参数给出的坐标系。

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