前言

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

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

基本使用

数据输入与数据输出

想要基本使用任何的功能包或者算法，首先要清楚的是功能包或算法的数据输入与输出具体是什么。

也就是说功能包或者算法可以接收什么数据，通过其处理可以生成什么数据。

对于robot_localization功能包中的ekf节点，在简介中有提到，输入数据可以是任意数量的传感器。

输入数据：
输入传感器的数据类型有：

• nav_msgs/Odometry （ROS消息类型的规范）
• sensor_msgs/Imu（ROS消息类型的规范）
• geometry_msgs/PoseWithCovarianceStamped（ROS消息类型的规范）
• geometry_msgs/TwistWithCovarianceStamped(ROS消息类型的规范)

nav_msgs/Odometry 可以来自轮速里程计数据、视觉里程计数据、激光里程计数据等
sensor_msgs/Imu 基本均来自惯导
geometry_msgs/PoseWithCovarianceStamped与geometry_msgs/PoseWithCovarianceStamped通常可以自己根据传感器的类型构建。

输出数据：

输出数据有：

• odometry/filtered (nav_msgs/Odometry)
• accel/filtered (geometry_msgs/AccelWithCovarianceStamped) (if enabled)
• tf

odometry/filtered (nav_msgs/Odometry)

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

发布的变换(tf)
如果用户的world_frame参数设置为odom_frame的值，则将转换从odom_frame参数给出的坐标系发布到base_link_frame参数给出的坐标系。如果用户的world_frame参数设置为map_frame的值，则将转换从map_frame参数给出的坐标系发布到odom_frame参数给出的坐标系。
注意：此模式假定另一个节点正在广播从odom_frame参数给定的坐标系到base_link_frame参数给定的坐标系的转换。这可以是robot_localization状态估计节点的另一个实例。

robot_localization中ekf的输入输出总结如下图：

坐标系设置

robot_localization的坐标系遵守REP-105(ROS 坐标系约定)

REP-105指定了四个主要坐标系：base_link，odom，map和Earth。base_link坐标系固定在机器人上。map和odom是固定的世界坐标系，其原点通常与机器人的起始位置对齐。Earth坐标系用于为多个map坐标系（例如，分布在较大区域的机器人）提供公共参考坐标系。

robot_localization的状态估计节点会生成状态估计，其位姿状态在map或odom坐标系中给出，其速度状态在base_link坐标系中给出。在与状态融合之前，所有传入的数据都将转换为这些坐标系之一。每种消息类型中的数据如下转换：

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

将 map_frame、odom_frame 和 base_link 框架设置为适合您系统的框架名称。如果系统没有 map_frame，只需将其删除，并确保“world_frame”设置为odom_frame。
如果要融合连续位置数据，例如车轮编码器里程计、视觉里程计或 IMU 数据，设置"world_frame" 到 odom_frame 值。

base_link_frame 设置为imu数据中的frame_id，则默认为惯导的方向和base_link_frame重合，如果实际的惯导有偏移和旋转，则需要发布imu数据中的frame_id到base_link_frame的实际tf。
odom_frame设置为odom数据中的frame_id

由于需要将里程计中的速度数据转换为base_link_frame下的，所以需要有里程计坐标系到base_link_frame的坐标变换，即使里程计中仅有位置数据，但是通过差分也可以得到速度信息，所以需要上述的转换。

如果没有上述的坐标转换的话如下图：

则终端会输入报警信息：

Could not obtain transform from odom_mapping to firefly/imu_link. Error was “odom_mapping” passed to lookupTransform argument source_frame does not exist

性能参数调试

在坐标系配置正确，输入数据配置正确后，ekf即可正常输出数据，但是默认参数的输出结果肯定不理想
优化性能参数使输出结果更接近真值，则是下面要做的内容。

主要调整的数据包括：
process_noise_covariance，该值相当于ekf中的Q矩阵。
测量数据的协方差，该值相当于ekf中的R矩阵。
initial_estimate_covariance，该值相当于ekf中的P0矩阵。

参数不理想情况


其中红色曲线为真值，深蓝色曲线为一种估计算法输出值，浅蓝色曲线为ekf输出值。

参数优化后ekf曲线为：

可以看到浅蓝色曲线更加贴近真值了。

至此robot_localization功能包使用方法介绍完毕！