前言
现在我们要将 GPS 数据添加到车轮里程计和 IMU 数据中。这三个测量将通过使用 robots_localization 包进行融合。GPS 提供机器人相对于地球框架的位置。这是一个优点。然而，它也有一些缺点，包括：
- 误差较大。然而，特殊类型的 GPS（例如 DGPS 和 RTK）误差要小得多，但价格昂贵。
- 仅提供位置。GPS 本身不提供任何有关方向的信息。
当与车轮里程计和IMU数据融合时，可以克服GPS的大误差。
这篇博客将简单介绍下如何将 GPS 数据与车轮里程计和 IMU 数据融合。
数据下载
链接: https://pan.baidu.com/s/1zSXyNhMNQdaFlDFziDse1Q 密码: 6j6u
创建 robots_localization 包
cd ~/catkin_ws/src
catkin_create_pkg gps2odom
cd gps2odom
mkdir launch
mkdir yaml
这将创建具有以下结构的新包：
my_gps_localization:
|--launch
|--yaml
|--package.xml
|--CMakeLists.txt
创建gps2odom.launch
<launch>
<rosparam file="$(find gps2odom)/yaml/gps2odom.yaml" command="load" />
<node pkg="tf" type="static_transform_publisher" name="static_transform_publisher"
args="0 0 0 0 0 0 1 /base_link /imu_link 10" />
<!-- EKF GPS-->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_gps" respawn="true">
<remap from="odometry/filtered" to="odom" />
</node>
<!-- Navsat -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat" respawn="true">
<remap from="imu/data" to="imu_correct" />
<remap from="gps/fix" to="gps/fix" />
<remap from="odometry/filtered" to="odom" />
</node>
</launch>
创建配置文件gps2odom.yaml
# Navsat (convert GPS coordinates to Cartesian)
navsat:
frequency: 50
wait_for_datum: false
delay: 0.0
magnetic_declination_radians: 0
yaw_offset: 0
zero_altitude: true
broadcast_utm_transform: false
broadcast_utm_transform_as_parent_frame: false
publish_filtered_gps: false
# EKF for Navsat
ekf_gps:
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
frequency: 50
two_d_mode: false
sensor_timeout: 0.01
# -------------------------------------
# External IMU:
# -------------------------------------
imu0: imu_correct
# make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
imu0_config: [false, false, false,
true, true, true,
false, false, false,
false, false, true,
true, true, true]
imu0_differential: false
imu0_queue_size: 50
imu0_remove_gravitational_acceleration: true
# -------------------------------------
# Odometry (From Navsat):
# -------------------------------------
odom0: odometry/gps
odom0_config: [true, true, true,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom0_differential: false
odom0_queue_size: 10
# x y z r p y x_dot y_dot z_dot r_dot p_dot y_dot x_ddot y_ddot z_ddot
process_noise_covariance: [ 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 10.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
配置文件中参数说明
**frequency**：滤波器产生状态估计的实值频率（以 Hz 为单位）。
**two_d_mode**：如果您的机器人在平面环境中运行，并且您愿意忽略地面的细微变化（由 IMU 报告），则将其设置为 true。它将融合所有 3D 变量（Z、横滚、俯仰以及它们各自的速度和加速度）的 0 值。这可以防止这些值的协方差爆炸，同时确保机器人的状态估计保持固定在 XY 平面上。
**publish_tf**：如果为 true，状态估计节点会将 world_frame 参数指定的帧的变换发布到其子节点。如果world_frame与map_frame相同，它将发布从map_frame到odom_frame的变换，如果world_frame与odom_frame相同，它将发布从odom_frame到base_link_frame的变换。默认为 true。
**Coordinate frame specification**:
- “map_frame”、“odom_frame”和“base_link_frame”的默认值分别是“map”、“odom”和“base_link”。“world_frame”参数默认为“odom_frame”的值。
- 如果您的系统没有map_frame，只需删除或注释它，并确保“world_frame”设置为“odom_frame”的值。
- 如果您仅融合连续位置数据，例如车轮编码器里程计、视觉里程计或 IMU 数据，请将“world_frame”设置为“odom_frame”的值。这是 robots_localization 中状态估计节点的默认行为，也是它最常见的用途。这不是我们的情况，因为我们还使用不连续的 GPS 数据。
- 如果您正在融合受离散跳跃影响的全球绝对位置数据（例如，GPS 或来自地标观测的位置更新），则将“world_frame”设置为“map_frame”的值。还要确保其他东西正在生成 odom->base_link 变换。这甚至可以是 robots_localization 状态估计节点的另一个实例。但是，该实例不应融合全局数据。这是我们的案例，因为我们使用 GPS 数据。
[sensor]：对于每个传感器，您需要根据传感器发布的主题定义此参数。
[sensor]_config：由布尔 5×3 矩阵定义如下：
```bash
[ X, Y, Z,
Roll, Pitch, Yaw,
dX/dt, dY/dt, dZ/dt,
dRoll/dt, dPitch/dt, dYaw/dt,
d2X/dt2, d2Y/dt2, d2Z/dt2]
[sensor]_ Differential：使用此参数，您可以指定是否应对位姿变量进行差分积分。如果给定值设置为 true，则对于相关传感器在时间 t 的测量值，我们首先减去时间 t−1 的测量值，并将结果值转换为速度。如果您的机器人有两个绝对位姿信息源（例如，来自里程计和 IMU 的偏航测量），则此设置特别有用。在这种情况下，如果输入源的方差配置不正确，这些测量结果可能会彼此不同步并导致滤波器振荡，但通过对其中一个或两者进行差分积分，我们可以避免这种情况。
如果将“navsat_transform”节点的输出与 中的任何状态估计节点融合 robot_localization，则应确保 该输入odomN_differential 的设置（其中 N 是从 0 开始的数字）为 false 。
process_noise_covariance：通常表示为 Q，用于对滤波算法的预测阶段的不确定性进行建模。对角线上的值是状态向量的方差，其中包括姿态，然后是速度，然后是线性加速度。它可能很难调整，并且已作为参数公开以便于定制。该参数可以单独保留，但通过调整它您将获得更好的结果。一般来说，Q 值相对于输入消息中给定变量的方差越大，滤波器收敛到测量值的速度就越快。
initial_estimate_covariance：此参数允许设置状态估计协方差矩阵的初始值，这将影响滤波器收敛的速度。您应该遵循以下规则：如果您正在测量变量，请使initial_estimate_covariance 中的对角线值大于该测量的协方差。因此，例如，如果相关变量的测量协方差值为 1e-6，则将initial_estimate_covariance 对角线值设为 1e-3 或类似值。