使用 VIO 增强机器人里程计

概述

本教程重点介绍如何将视觉惯性里程计 (VIO) 设置到基于 Nav2 和 ROS 2 的机器人系统中以增强机器人里程计。

许多现代机器人平台的配置并不理想，无法实现高质量的轮式测距。例如，滑移转向机器人、履带式机器人、使用麦克纳姆轮的全向机器人和腿式机器人因测距不理想而臭名昭著。这些类型的平台在机器人领域越来越常见，需要增强或更换测距源。此外，机器人技术的一些应用涉及改造可能完全没有测距功能的现有设备。

计算机视觉和机器人技术的一个子领域专注于如何使用视觉（例如相机）和惯性（例如 IMU）数据来计算独立于机器人力学的高速相对运动。对于没有精确内在里程计传感能力的无人机，或者内部里程计测量较差的移动机器人来说，这是一项特别有用的技术。

因此，本教程将介绍如何将 VIO 集成到机器人系统中，以替代或增强车轮里程计，从而使您的机器人能够自主导航，并进行精心设计的系统所需的质量状态估计，从而能够准确、可预测地完成其任务。

在本教程中，我们将使用 Stereolabs SDK 的 Position Tracking 功能作为我们选择的 VIO 解决方案，并与新的 ZED X 相机配对。此 VIO 解决方案易于使用，并且在使用 ZED 相机模块时可*免费*提供生产质量性能。

Note

虽然我们使用 Stereolabs SDK 和 ZED X 相机，但本教程可能广泛用于其他解决方案。但是，我们建议使用此选项作为优化解决方案，它与相机硬件和 Jetson 计算架构紧密耦合，以实现高性能和快速的开箱即用结果。根据 Open Navigation 的经验，可能需要数月时间测试开源 VIO 解决方案并尝试修复立体相机 ROS 驱动程序的时间同步问题才能获得实用质量的结果。这是一个非常方便的一站式集成解决方案。

设置 ZED X 相机

我们在本教程中使用 ZED X，因为它：

  • 与其他 AMR 深度传感器类似，尺寸较小

  • 为移动机器人和操控提供相关范围内的高质量深度信息

  • 硬件同步 IMU

  • 在大多数情况下，仅需一个摄像头即可实现宽视野

  • 使用 USB 上的 GMSL2 连接器（太棒了！）

  • 全局快门摄像头可提高质量并消除运动模糊

不过，任何其他带有 IMU 的 ZED 相机也可以正常工作（ZED2、ZED2i、ZED mini 等）。如果您特别使用 ZED X，请查看`YouTube 上的这个播放列表，其中逐步展示了如何为 ROS 2 设置 Nvidia Jetson 和 ZED X <https://www.youtube.com/watch?v=sEH07WwB8X0&list=PLekRVIRfsmj-P74wmB5qXLYujbelQsW5O>`_ 或 ZED X 入门页面 以安装继续操作所需的 SDK、ZED X 驱动程序和 ROS 2 驱动程序。

此时，您应该能够运行以下命令之一来启动驱动程序并在 Rviz 中可视化传感器数据。请注意，这些是 ROS 2 组件节点，也可以加载到系统的组件管理器中，以最大限度地减少由于序列化造成的延迟。

$ ros2 launch zed_wrapper zedx.launch.py
$ ros2 launch zed_wrapper zedxm.launch.py
../../_images/vio.png

截至 2023 年 9 月，开箱即用的驱动程序会自行生成完整的 map->odom->base_link->camera 树。这是因为 Pose SDK 不仅可以生成 VIO，还可以生成代表完整状态估计 TF 树的闭环 VSLAM。

我们希望能够将其他信息（例如外部 IMU、车轮里程计、GPS 或其他传感器）融合到我们的本地或全局状态估计中，因此我们需要禁用由我们的融合输出提供的 map->odom 和 odom->base_link 的 TF 发布。特别是考虑到 ZED X 相机对 base_link 帧的性质一无所知。但是，如果您想将 ZED 的状态估计用于整个系统而无需进一步的传感器融合，您当然可以！

设置 ZED ROS

为了单独运行 VIO，我们需要执行以下操作：

  1. 停止计算 VSLAM 的 map->odom、TF 变换。TF 树的这一部分由我们的全局定位解决方案（例如 AMCL、GPS、融合全局状态估计）提供。

  2. 禁用 VIO 发布 odom->camera，而是发布 VIO 的姿势解决方案的 nav_msgs/Odometry 进行融合。默认情况下，zed_wrapper 在 odom 主题下发布此内容，但建议将其重新映射到非保留主题名称（例如，camera_odom）。

  3. 重新配置 ZED Wrapper 的参数以获得尽可能最佳的 VIO。

  • two_d_mode 将强制姿势跟踪在 2D 维度（例如 X、Y、偏航）中，用于室内或 2D 应用。

  • pos_tracking_enabled 将禁用或启用姿势跟踪（如果您需要的话）（我们在这里就是这么做的！）。

  • path_max_count 将设置随时间变化的姿势可视化的最大大小。默认情况下它是无限的。我们应该让它有限。

  • qos_depth 将设置整个驱动程序的 QoS 深度。将所有选项设置为 3-5。1 可能会导致在计算中丢弃非常临时的 blib 中的消息。3-5 允许我们缓冲少量测量值以处理非常短期的 blib，但在 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 启动文件中，向节点/启动文件添加以下内容：

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

Note

ZED 驱动程序将发布两个姿势跟踪主题，pose 和 odom。Pose 是带有循环闭合的完整 V-SLAM 姿势（如果为 area_memory: false，则不包含循环闭合）。odom 主题包含我们想要使用的实际 VIO，它以帧捕获速率发布。Pose 主题可能会根据循环闭合以不规则的频率发布。因此，我们想使用 odom 进行这种局部融合。

将 VIO 融合到本地状态估计中

现在我们已经设置了 ZED ROS 2 驱动程序来将我们的 VIO 发布到主题，并将我们的 TF 树留给融合算法和机器人状态发布器（例如 URDF），我们终于可以使用 robot_localization 包将 VIO 融合到我们更广泛的状态估计中。

此包是一个通用的 EKF 和 UKF 解决方案，用于对可能许多不同主题进行状态估计，以不同的速率发布不同类型的内容。如果您不熟悉 robot_localization，请查看我们的 首次机器人设置指南 的 Odometry 页面以获取基本信息和 package’s extensive documentation。

此时，大多数用户已经在他们的机器人系统中拥有一个 robot_localization 配置文件，用于将现有传感器融合在一起，例如车轮里程表（甚至很差）和机器人 IMU。我们将在配置中添加一个新的 odom 字段 odom1，以将 VIO 的位置​​和方向融合到我们的过滤器中。如果这是您的第一个里程计字段，请使用“odom0”，您可以根据 ekf.yaml <https://github.com/cra-ros-pkg/robot_localization/blob/ros2/params/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

Note

我们融合了滚动、俯仰和偏航。如果在 2D 模式下操作 EKF 或 ZED，则将滚动和俯仰字段设置为 false。如果您认为 VIO 中可能会出现跳跃，请考虑使用 odom1_pose_rejection_threshold ，它会设置一个阈值，如果与最近的更新相比，更新足够离谱，则拒绝更新。在这种情况下，将 diff 设置为 true 也可能有帮助，这样单个虚假更新就不会移动整个坐标系。

确保评估 EKF 的 频率 、 two_d_mode 、 publish_tf 和 关键帧，以适合您的应用程序。我们通常只希望在平坦的室内环境中导航时发布 TF 并打开 2D 模式。

将 VSLAM 融合到全局状态估计中

虽然超出了本教程的范围，但可以继续使用循环闭合生成用于全局定位的 VSLAM 结果（一般情况下和使用 Stereolabs 位置跟踪 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% 的“优秀”里程计），所以这是一个很好的来源！与有腿机器人里程计融合后，它将整体性能提高到可接受的水平！

Note

史蒂夫正在带着他的机器狗穿过加利福尼亚州旧金山的金门公园，用操纵杆收集这些数据。史蒂夫是个糟糕的机器人驾驶员（他不玩电子游戏），你看到的之字形行驶是由于他缺乏良好的操纵杆控制 + 四足动物身上有很多额外的不对称重量。它不代表 Nav2，应该被嘲笑。它旨在测试 VIO 解决方案在更恶劣条件下的准确性……是的……让我们开始吧。