集成 VIO（视觉惯性里程计）

概述
设置 ZED X 相机
设置 ZED ROS
将 VIO 融合到局部状态估计
将 VSLAM 融合到全局状态估计

概述

本教程介绍如何在基于 Nav2 和 ROS 2 的机器人系统中配置视觉惯性里程计（Visual-Inertial Odometry, VIO），以提升机器人的里程计质量。

许多现代机器人平台的机械结构难以提供高质量的轮式里程计。例如，差速滑移转向（skid-steer）、履带式（tracked）、使用麦克纳姆轮的全向机器人以及腿式机器人，普遍存在里程计精度较差的问题。这类平台在机器人领域越来越常见，需要额外增强或替换里程计来源。此外，某些应用场景还涉及改造现有设备，而这些设备可能根本没有里程计。

计算机视觉与机器人领域的一个研究方向致力于利用视觉（如相机）和惯性（如 IMU）数据，计算与机器人机械结构无关的高频相对运动。对于缺乏精确内部里程计的无人机，或内部里程计质量较差的移动机器人，这项技术尤其有用。

本教程将带你完成 VIO 集成的完整流程，替换或增强轮式里程计，使机器人能够凭借高质量的状态估计实现自主导航——这是构建能够准确、可预测地完成任务的工程系统的必要条件。

本教程使用 Stereolabs SDK 的 Position Tracking 功能作为 VIO 方案，搭配 ZED X 相机。该方案易于使用，搭配 ZED 相机模块即可免费获得生产级性能。

<h1 align="center">
  <div style="position: relative; padding-bottom: 0%; height: 0; overflow: hidden; max-width: 100%; height: 100%;">
      <iframe src="https://cdn2.stereolabs.com/docs/positional-tracking/images/zed-positional-tracking.mp4" frameborder="0" allowfullscreen width="550" height="400"></iframe>
  </div>
</h1>

注意：虽然我们使用 Stereolabs SDK 和 ZED X 相机，但本教程的方法同样适用于其他方案。我们推荐这一方案，因为它与相机硬件和 Jetson 计算架构紧密耦合，经过深度优化，性能出色且开箱即用。根据 Open Navigation 的经验，要使开源 VIO 方案达到实用级别的质量，可能需要花费数月时间进行测试，并排查立体相机 ROS 驱动的时间同步问题。而这一方案则是一个方便的一站式集成解决方案。

设置 ZED X 相机

本教程选用 ZED X，原因如下：

尺寸小巧，与其他 AMR 深度传感器相当
在移动机器人操作距离范围内提供高质量的深度信息
硬件同步的 IMU
宽视场，大多数情况下只需一台相机即可覆盖
通过 GMSL2 连接器经 USB 连接
全局快门相机，画质更好且消除了运动模糊

任何其他带 IMU 的 ZED 相机同样适用（ZED2、ZED2i、ZED mini 等）。如果你使用的是 ZED X，可以参考这个 YouTube 播放列表，其中分步展示了如何为 ROS 2 设置 NVIDIA Jetson 和 ZED X 或 ZED X 入门页面，安装后续所需的 SDK、ZED X 驱动和 ROS 2 驱动。

完成安装后，运行以下命令之一即可启动驱动并在 RViz 中可视化传感器数据。这些是 ROS 2 组件节点（component nodes），也可以加载到系统的组件管理器中，以最大程度减少序列化带来的延迟。

$ ros2 launch zed_wrapper zedx.launch.py
$ ros2 launch zed_wrapper zedxm.launch.py

VIO 示意图

截至 2023 年 9 月，该驱动会自动生成完整的 map->odom->base_link->camera TF 树。这是因为 Pose SDK 不仅产生 VIO，还产生带回环检测的 VSLAM，表示完整的状态估计 TF 树。

我们需要将外部 IMU、轮式里程计、GPS 等其他传感器信息融合到局部或全局状态估计中，因此需要禁用 map->odom 和 odom->base_link 的 TF 发布，改由融合输出来提供。特别是 ZED X 相机并不了解 base_link 坐标系的属性。当然，如果你不需要额外的传感器融合，直接使用 ZED 的状态估计也未尝不可。

设置 ZED ROS

要单独运行 VIO，需要完成以下配置：

停止计算 VSLAM 的 map->odom TF 变换。这部分 TF 树由全局定位方案（如 AMCL、GPS、融合的全局状态估计）提供。
禁用 VIO 对 odom->camera 的 TF 发布，改为发布 VIO 位姿解算结果的 nav_msgs/Odometry 消息用于融合。默认情况下，zed_wrapper 会在 odom 话题下发布该消息，但建议将其重映射到一个非保留的话题名称（例如 camera_odom）。
重新配置 ZED Wrapper 的参数，以获得尽可能最佳的 VIO 效果。

two_d_mode：强制位姿跟踪在 2D 维度上进行（例如 X、Y、Yaw），适用于室内或 2D 应用。
pos_tracking_enabled：启用或禁用位姿跟踪，这里需要启用。
path_max_count：设置位姿随时间变化可视化的最大长度，默认无限制，建议设置为有限值。
qos_depth：设置整个驱动的 QoS 深度。建议设为 3–5。设为 1 可能在极短的计算卡顿中丢消息；3–5 允许缓冲少量测量值以应对短暂卡顿，但在 CPU 过载时也会全部清空。

因此，对 zed_wrapper 的默认参数做如下修改：

pos_tracking:
    publish_tf: false # Disables odom -> base_link TF transformation
    publish_map_tf: true # Disables map -> odom TF transformation
    area_memory: false # Disables loop closure computation, but Pose topic for global VSLAM still published (but now pure VIO)

    # Optional optimizations
    two_d_mode: false # Or true, up to you!
    pos_tracking_enabled: true # of course!
    path_max_count: 30
    qos_depth: 5

可选地，将 ZED 的 odom 话题重映射到一个未被其他系统保留或常用的话题。在你的 ZED launch 文件中，向节点/launch 文件添加：

remappings=[('odom', 'camera_odom')]

注意：ZED 驱动会发布两个位姿跟踪话题：pose 和 odom。pose 是完整的 V-SLAM 位姿，带回环检测（如果 area_memory: false 则不带）。odom 话题包含我们实际想要使用的 VIO 数据，以帧采集频率发布。pose 话题可能根据回环检测以不规则的频率发布。因此，局部融合中应使用 odom。

将 VIO 融合到局部状态估计

现在，ZED ROS 2 驱动已配置为将 VIO 发布到一个话题，并将 TF 树留给融合算法和 Robot State Publisher（例如 URDF），接下来就可以使用 robot_localization 包将 VIO 融合到整体状态估计中。

该包是一个通用的 EKF 和 UKF 状态估计方案，可以融合来自不同话题、以不同速率发布的多种传感器数据。如果你不熟悉 robot_localization，可以查看环境配置指南中的 Odometry 页面了解基本信息，以及该包的详细文档。

此时，大多数用户的机器人系统中已经有一个 robot_localization 配置文件，用于融合现有传感器，如轮式里程计（即使质量较差）和机器人 IMU。我们将向配置中添加一个新的里程计字段 odom1，将 VIO 的位置和姿态融合到滤波器中。如果这是你的第一个里程计字段，请使用 odom0，并参考 ekf.yaml 编写配置文件。

odom1: camera_odom # Adjust if namespacing ZED camera (e.g. /zed/odom)
odom1_config: [true,  true,  true,  # X, Y, Z
               true,  true,  true,  # Roll, Pitch, Yaw
               false, false, false, # Vx, Vy, Vz
               false, false, false,  # Vroll, Vpitch, Vyaw
               false, false, false] # Ax, Ay, Az
odom1_differential: false
odom1_relative: true
odom1_queue_size: 2

注意：这里融合了 Roll、Pitch 和 Yaw。如果你的 EKF 或 ZED 以 2D 模式运行，应将 Roll 和 Pitch 字段设置为 false。如果担心 VIO 出现跳变，可考虑设置 odom1_pose_rejection_threshold，该参数会设置一个阈值，当更新相对于最近的更新偏离过大时拒绝该更新。在这种情况下，将 differential 设置为 true 也有帮助，这样单个错误的更新不会导致整个坐标系偏移。

请务必评估 EKF 的 frequency、two_d_mode、publish_tf 和关键帧设置是否适合你的应用场景。在平坦的室内环境中导航时，通常希望发布 TF 并开启 2D 模式。

将 VSLAM 融合到全局状态估计

虽然这超出了本教程的范围，但继续生成带回环检测的 VSLAM 结果用于全局定位是可行的（无论是通用方案还是使用 Stereolabs Position Tracking SDK）。集成步骤与前面几节类似，区别在于：

继续禁用 map->odom 的 TF 变换，但全局位姿话题将继续在 pose 下发布用于融合
将该话题与其他信息源（如外部 IMU、AMCL、GPS 等）一起融合到 world_frame: map 的全局定位 EKF 中
多种全局定位技术的融合应谨慎进行。最可信的来源应设置 _differential: false 以使用实际的位姿信息。所有其他后续系统应使用 _differential: true，以免在坐标系发散时解来回跳动。也就是说，将一个融合为绝对位姿，其余的融合为迭代之间的位姿变化。

测试一下

在下面的示例中，我们将 Stereolabs SDK 的 Pose Tracking VIO 方案与机器人的外部 IMU 和里程计（例如 robot_localization 有 odom0、odom1 和 imu0）融合，以在户外环境中提升腿式机器人平台的导航性能。机器人基于腿部运动的内部里程计精度很差，导致自主导航性能整体较差。

在这些数据集上，视觉惯性里程计在 70 米路径上的误差为 4.1%。作为参考，经过充分调优的「良好」轮式编码器 + IMU 里程计通常能达到 2–3%（「优秀」的里程计则低于 1%），因此这是一个很好的数据来源。与腿式机器人里程计融合后，整体性能提升到了可接受的水平。

注意：Steve 正拿着手柄，在加州旧金山的金门公园遛他的机器狗来收集这些数据。Steve 是个糟糕的机器人驾驶员（他不玩电子游戏），你看到的蛇形走位是他手柄操控不佳的体现，加上四足机器人还背负了大量额外的不对称重量。这并不能代表 Nav2 的水平，该被嘲笑的是驾驶技术。它的本意是在更严苛的条件下测试 VIO 方案的精度……嗯……我们就这么说吧。

<h1 align="center">
  <div style="position: relative; padding-bottom: 0%; overflow: hidden; max-width: 100%; height: auto;">
    <iframe width="708" height="400" src="https://www.youtube.com/embed/VWfzeZJdtpc?autoplay=1&mute=1&t=143" frameborder="1" allowfullscreen></iframe>
  </div>
</h1>

<h1 align="center">
  <div style="position: relative; padding-bottom: 0%; overflow: hidden; max-width: 100%; height: auto;">
    <iframe width="708" height="400" src="https://www.youtube.com/embed/Flf6fyeyzIw?autoplay=1&mute=1" frameborder="1" allowfullscreen></iframe>
  </div>
</h1>