前言

在这里插入图片描述

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

惯导
轮速里程计
激光里程计
视觉里程计
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文件中。
在这里插入图片描述

高级参数

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，则不会在启动时运行滤波器。