使用 GPS 定位导航

概述
要求
GPS 定位概述
教程步骤
结论

概述

本教程展示了如何使用 GPS 传感器作为全球定位源、robot_localization (RL) 进行传感器融合来设置定位系统，以及如何使用 Nav2 跟踪 GPS 航点。它由 Kiwibot 的 Pedro Gonzalez 编写。

要求

假设 ROS2 和 Nav2 依赖包已在本地安装或构建。此外，您还必须安装 robot_localization 和 mapviz：

source /opt/ros/<ros2-distro>/setup.bash
sudo apt install ros-$ROS_DISTRO-robot-localization
sudo apt install ros-$ROS_DISTRO-mapviz
sudo apt install ros-$ROS_DISTRO-mapviz-plugins
sudo apt install ros-$ROS_DISTRO-tile-map

本教程的代码托管在 nav2_gps_waypoint_follower_demo 上。虽然我们将介绍设置中最重要的步骤，但强烈建议您在设置开发环境时克隆并构建包。

如果您尚未执行以前的教程或 Nav2 演示，您可能还需要安装 gazebo 和 turtlebot3 模拟。有关更多信息，请参阅 Nav2 的入门页面。

GPS 定位概述

GPS（全球定位系统）或更宽泛的 GNSS（全球导航卫星系统）是一种依靠卫星为接收器提供其在地球上位置估计值的技术。这些卫星在轨道上的高度约为 20,000 公里，并使用无线电频率连续广播时间信号，当卫星沿着接收器的视线时，接收器会接收到这些信号，它们使用三边测量法来估计其纬度、经度和高度。

通常，GPS 设备使用 WGS84 标准来计算其位置，该标准定义了一个笛卡尔系统，其原点位于地球的质心上，z 轴指向北方，x 轴指向第一子午线，如下图所示。

但是，这种参考系统对于描述运动和表示地球表面或附近物体周围的环境是不切实际的：假设您的机器人位于足球场上，您希望它从一端移动到另一端，您的导航任务将如下所示：

“go from X=4789.413km, Y=177.511km z=4194.292km to X=4789.475km, Y=177.553km z=4194.22km”

此外，如果您的机器人有 2D 激光雷达，您还必须将其数据转换为此参考系统。创建一个本地参考系统会更有意义，您可以告诉您的机器人“向前走 100 米”，您的传感器数据可以相应地填充您的环境表示，对吗？

为了解决这个问题，大地测量学提出了几种平面投影系统，用于相对于地球表面进行定位。其中之一是 UTM 坐标系统，该系统假设地球是一个椭圆体，并将其划分为 60 个区域，每个区域横跨 6 个经度。区域表示椭圆体表面在与其中央子午线平行的割圆柱上的投影；然后，每个区域被划分为 20 个纬度带，横跨 8 个纬度，从而创建局部网格区域，其中的位置使用来自区域原点的平面坐标来表示。下图显示了横跨南美洲的网格区域。

2,861 / 5,000 robot_localization 使用此投影系统将 WGS84 参考系统中的 GPS 测量值转换为笛卡尔系统，该系统以 GPS 所在网格区域的原点为中心。这是通过 navsat_transform 节点实现的。此节点符合 REP 103 中的 ENU 约定，这意味着 utm 坐标系的 +x 轴朝东，+y 朝北，+z 轴朝上。

在现实世界中，GPS 传感器可能会产生噪音：使用独立 GPS，在良好条件下，精度应为 1-2 米，最高可达 10 米，并且由于 GPS 传感器接收到的卫星数量或多或少，位置会频繁跳变，这会严重降低导航质量。存在几种定位增强技术来减少 GPS 测量的误差，其中最常见的一种称为 RTK <https://en.wikipedia.org/wiki/Real-time_kinematic_positioning>`_（实时动态定位），它可以将接收器的精度降低到 1 厘米。如果精度在您的应用中很重要，则强烈建议使用这项技术；虽然这需要部署第二个固定 GPS（称为基站），但美国和欧洲大部分地区已经覆盖了您可以连接的公共免费基站。您可以在 `此处阅读有关 RTK 的更多信息以及如何开始使用。在本教程中，我们假设机器人的 GPS 可以准确、平稳地估计机器人的位置。

此外，为了全面描述机器人的定位，我们还需要知道它的航向，但独立的 GPS 传感器不提供方向测量，只提供位置测量。在本教程中，我们将“绝对航向”称为偏航测量，它是相对于基本方向（例如东方）给出的，与相对航向相反，相对航向是相对于机器人开启的角度或任何其他不能直接映射到基本方向的参考给出的。

当使用带有 GPS 的 robot_localization 时，测量绝对方向是强制性的。有几种获取绝对方向数据的策略，例如带有磁力计的 IMU、双 GPS 系统或已知地图上的匹配技术；在本教程中，我们假设机器人配备了可以按照 ENU 约定准确测量绝对方向的 IMU，这意味着它在面向东方时将输出零偏航，在面向北方时将输出 +90 度。

尽管有上述假设，但在现实世界中，安装在实际机器人上的商用级 IMU 通常不会产生准确的绝对航向测量，因为：

它们可能没有磁力计。
它们很难校准：户外机器人通常又大又重：想象一下用自动拖拉机在空中做八位数。
机器人可能是磁力计的巨大电磁噪声源：电动机充满了永磁体，可以吸收几安培的电流，对传感器产生重大干扰。

因此，对于特定应用，在决定如何估计绝对航向时，您应该考虑所需的行为和定位质量。当使用没有相对航向的 IMU 时，机器人可能需要在“初始化舞蹈”中移动一点，以使用过滤器收敛到正确的航向。使用双 GPS 或 3D 地图系统叠加，初始航向非常好。

出于本教程的目的，我们使用已经具有绝对方向的 IMU 建模一个构建良好的系统，但可以使用上述技术之一（或其他技术）在实际系统上进行增强或替换。

教程步骤

0- 设置 Gazebo World

要使用 GPS 导航，我们首先需要创建一个户外 Gazebo 世界，其中的机器人配有 GPS 传感器，用于导航。在本教程中，我们将使用 Sonoma Raceway 因为它与真实位置一致。已在 here 设置了一个示例世界，该插件使用 gazebo 的球面坐标插件创建以设置的地理原点为中心的局部切平面，并为世界上的每个点提供纬度、经度和高度坐标：

<spherical_coordinates>
  <!-- currently gazebo has a bug: instead of outputting lat, long, altitude in ENU
  (x = East, y = North and z = Up) as the default configurations, it's outputting (-E)(-N)U,
  therefore we rotate the default frame 180 so that it would go back to ENU
  see: https://github.com/osrf/gazebo/issues/2022 -->
  <surface_model>EARTH_WGS84</surface_model>
  <latitude_deg>38.161479</latitude_deg>
  <longitude_deg>-122.454630</longitude_deg>
  <elevation>488.0</elevation>
  <heading_deg>180</heading_deg>
</spherical_coordinates>

要从 Gazebo 获取 GPS 读数，我们需要创建一个带有 GPS 传感器的机器人模型。 tutorial repo 中提供了带有此类传感器的更新的 Turtlebot 模型，它在主题 /gps/fix 上输出 NavSatFix 消息：

<sensor name="tb3_gps" type="gps">
  <always_on>true</always_on>
  <update_rate>1</update_rate>
  <pose>0 0 0 0 0 0</pose>
  <gps>
    <position_sensing>
      <horizontal>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </horizontal>
      <vertical>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </vertical>
    </position_sensing>
  </gps>
  <plugin name="my_gps_plugin" filename="libgazebo_ros_gps_sensor.so">
    <ros>
      <remapping>~/out:=/gps/fix</remapping>
    </ros>
  </plugin>
</sensor>

此外，由于我们在 gps_link 中添加了新的 GPS 传感器，因此我们需要为该链接添加一个关节，以发布相对于 ``base_link``的静态变换。

<joint name="base_joint" type="fixed">
  <parent link="base_link"/>
  <child link="base_footprint" />
  <origin xyz="0 0 -0.010" rpy="0 0 0"/>
</joint>

构建 nav2_gps_waypoint_follower_demo 包，获取你的工作区并通过启动来测试你的 Gazebo 世界是否设置正确：

ros2 launch nav2_gps_waypoint_follower_demo gazebo_gps_world.launch.py

Turtlebot 华夫饼应该出现在 Sonoma Raceway 世界中。您也可以回显主题 /gps/fix 以验证机器人确实在进行 GPS 测量

1- 设置 GPS 定位系统

一旦您的模拟（或真实机器人）启动并运行，就该设置您的定位系统了。请记住，Nav2 使用 tf 链，其结构为 map -> odom -> base_link -> [sensor frames]；全局定位 (map -> odom) 通常由 amcl 提供，而 odom -> base_link 通常由用户的里程计系统（车轮里程计、视觉里程计等）提供。

在本教程中，机器人上的 GPS 传感器将取代 amcl 来提供全局定位。尽管您可以构建一个自定义模块，接收 GPS 和 imu 的 NavSatFix 和 Imu 消息，并使用平面投影在 map 和 odom 框架之间输出 tf ，但 Nav2 的 GPS 航点跟随器当前使用 robot_localization 将 GPS 目标转换为笛卡尔目标，因此在 navsat_transform_node 处应该处于活动状态。此外， robot_localization 具有可重构状态估计节点，这些节点使用卡尔曼滤波器融合多个数据源，这也是使用它的另一个原因。

我们将为局部里程计设置一个扩展卡尔曼滤波器，融合车轮里程计和 IMU 数据；第二个节点用于全局定位，融合本地笛卡尔转换的 GPS 坐标、车轮里程计和 IMU 数据；以及 navsat_transform 节点，用于从 GPS 数据输出笛卡尔里程计消息。这是 robot_localization 在使用 GPS 数据时的常见设置，有关其配置的更多详细信息，请参阅 RL’s docs 。

为此，我们提供了一个配置文件和一个启动文件。您可能需要花一些时间才能继续了解这两个文件及其配置。让我们来看看每个节点最相关的设置。

局部里程计

局部里程计由 ekf_filter_node_odom 提供，它发布 odom 和 base_footprint 之间的转换，后者是 gazebo 中 turtlebot 的 diff 驱动插件的基准框架。机器人状态发布者提供 base_footprint 和 base_link 之间的静态转换，但请确保根据您的配置在 RL 中正确设置基准框架。请注意，EKF 设置为在 2D 模式下工作，这是因为 nav2 的 costmap 环境表示是二维的，并且多个层依赖于 base_link 框架与其全局框架位于同一平面上，以使高度相关参数有意义。这在以下参数中编码：

ekf_filter_node_odom:
  ros__parameters:
    two_d_mode: true
    publish_tf: true

    base_link_frame: base_footprint
    world_frame: odom

因为根据 REP 105 ，机器人在 odom 框架中的位置必须随时间连续，在这个过滤器中，我们只想融合通过 /odom 发布的机器人车轮测量的速度，以及在 /imu 上发布的 imu 航向：

odom0: odom
odom0_config: [false, false, false,
              false, false, false,
              true,  true,  true,
              false, false, true,
              false, false, false]

imu0: imu
imu0_config: [false, false, false,
              false,  false,  true,
              false, false, false,
              false,  false,  false,
              false,  false,  false]

全局里程计

全局里程计由 ekf_filter_node_map 提供，它发布 map 和 base_footprint 之间的变换。此 EKF 也设置为在 2D 模式下工作。除了 IMU 和车轮里程计数据外，此过滤器还接收 GPS 的里程计输出，由 /odometry/gps 上的 navsat_transform 节点作为里程计消息发布：

ekf_filter_node_map:
  ros__parameters:
    two_d_mode: true
    publish_tf: true

    base_link_frame: base_footprint
    world_frame: map

    odom1: odometry/gps
    odom1_config: [true,  true,  false,
                  false, false, false,
                  false, false, false,
                  false, false, false,
                  false, false, false]

Navsat 变换

Navsat 变换生成一个里程计输出，其中包含 GPS 在“地图”框架中的位置，如上所述，该输出由全局 EKF 摄取。它公开了 datum 参数来设置 GPS 坐标和“地图”原点的航向；如果未声明，它将自动设置为它收到的第一个有效“NavSatFix”消息的坐标，并且它可能会在运行时更改，也可以通过调用 /datum 服务进行更改。

在本教程中，我们将使用自动 datum 初始化，因为笛卡尔坐标中没有存储有关环境的信息（静态地图、语义导航航点、3D 点云地图等），但是如果您的应用程序中存在这种情况，您可以修复 datum ，以便 GPS 生成的给定坐标对始终对应于参考系统中的相同笛卡尔坐标。

该节点还公开了 yaw_offset 参数，以补偿 IMU 绝对偏航测量可能相对于东方存在的已知误差。由于 Gazebo 的 IMU 遵循 ENU 约定，因此在本教程中将其设置为 0 ，但如果您事先知道数据中有一个固定偏移量，您可能需要更改它。

这是 navsat_transform 节点的完整配置：

navsat_transform:
  ros__parameters:
    frequency: 30.0
    delay: 3.0
    magnetic_declination_radians: 0.0
    yaw_offset: 0.0
    zero_altitude: true
    broadcast_utm_transform: true
    publish_filtered_gps: true
    use_odometry_yaw: true
    wait_for_datum: false
    # datum: [38.161491, -122.4546443, 0.0] # pre-set datum if needed, [lat, lon, yaw]

本地化测试

为了检查一切是否正常工作，在 Gazebo 仍在运行时启动 RL 的启动文件：

ros2 launch nav2_gps_waypoint_follower_demo dual_ekf_navsat.launch.py

在另一个终端上，使用 repo 中预先构建的 config file 启动 mapviz。获取 bing 地图 API 密钥并使用它来显示卫星图片。

ros2 launch nav2_gps_waypoint_follower_demo mapviz.launch.py

正确设置 API 密钥后，您应该会看到下面的窗口：

最后运行 teleop twist 键盘节点来遥控模拟的 Turtlebot：

ros2 run teleop_twist_keyboard teleop_twist_keyboard

当一切准备就绪并运行后，开始遥控 Turtlebot 并检查：

当机器人面朝东（默认初始航向）并向前移动时， base_link 框架（绿色箭头）与原始 GPS 测量值（蓝点）一致向东移动。
移动不仅在面朝东时总体上是一致的，这意味着 GPS 测量值与机器人航向和移动方向一致，并且与机器人在世界上的位置一致（例如，当机器人朝终点线移动时，mapviz 中的 GPS 测量值也是如此）。

下面的 gif 显示了您应该看到的内容：

真实机器人中的传感器可能不如 Gazebo 的传感器准确，尤其是 GPS 和 IMU 的绝对航向测量。为了缓解这种情况，您可以利用 robot_localization 的 EKF 来补充传感器的功能：

如果您的 IMU 无法准确提供绝对偏航测量值，请考虑将其输入到 RL 的 差分 参数设置为 真 。这样，过滤器将仅融合方向的变化并从其内部运动模型中得出绝对值，区分位置的变化以估计机器人的前进方向（例如，如果根据车轮里程表，机器人向前的速度为 1 米/秒，根据 GPS，机器人向北移动 1 米，则意味着它应该朝北）。请注意，如果是这种情况，您将无法获得准确的绝对航向，直到您的机器人移动一点并且过滤器可以从该运动中估算出它；如果您的应用程序中无法做到这一点，请考虑添加另一个可以准确测量绝对航向的传感器，例如双 GPS 系统。
如果您的 GPS 噪声很大，但您有另一个值得信赖的里程计源（例如：车轮里程计、视觉里程计），请考虑调整传感器并处理噪声协方差，以使过滤器或多或少 信任 一个数据源或其自己的内部状态估计。适当调整的过滤器应该能够在一定程度上拒绝错误的 GPS 测量值。

2- 设置导航系统

一旦您的定位系统启动并运行，就该设置 Nav2 了。由于 RL 已经提供了 tf 树，我们不需要启动 amcl ，因此我们可以从 params 文件中删除其参数，而不启动 Nav2 的定位启动文件。

全局成本地图有三种主要可能的设置：

**滚动**（本教程中使用）：户外环境可能变得相当大，以至于在单个成本地图上表示它们可能不切实际。因此，在本教程中，我们使用滚动的全局成本地图，该地图足够大，可以容纳连续的航点对。在这种情况下，您可以选择或不选择使用静态层，但是如果您这样做，请确保修复 navsat_transform 的“基准”，以便 GPS 坐标在您的地图上始终具有相同的笛卡尔表示。

global_costmap:
  global_costmap:
    ros__parameters:
      ...
      rolling_window: True
      width: 50
      height: 50

静态地图的大小和位置：您也可以选择保留 Nav2 默认设置，并通过添加静态层和使用“map_server”根据预建地图调整全局成本地图的大小和位置。在这种情况下，您还需要确保您的“基准”和地图原点的一致性。

global_costmap:
  global_costmap:
    ros__parameters:
      ...
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]

静态位置和大小：最后，根据您的应用，如果您事先知道受限的操作环境，您仍然可以选择使用固定的全局成本地图，只需记住使其适合机器人可能访问的所有潜在位置。在这种情况下，您需要在参数中设置大小和原点位置：

global_costmap:
  global_costmap:
    ros__parameters:
      ...
      width: 50
      height: 50
      origin_x: 25.0
      origin_y: 25.0

我们提供了一个 Nav2 params 文件，其中包含滚动成本地图设置和一个 launch 文件，以将所有内容组合在一起。请记住，robot_localization 的 GPS 设置只是设置全球定位系统的一种手段，但 Nav2 仍然是笛卡尔导航堆栈，您仍然可以使用其所有笛卡尔工具。为了确认一切正常，启动提供的文件（这也会启动 gazebo 和 RL，因此如果您按照前面的步骤运行了它们，请关闭它们）并使用 rviz 向机器人发送目标：

ros2 launch nav2_gps_waypoint_follower_demo gps_waypoint_follower.launch.py use_rviz:=True

下面的 gif 展示了您应该看到的 Nav2 自主导航机器人的过程！

3- 交互式 GPS 航点跟随器

1,117 / 5,000 现在我们已经完成了完整的系统设置，让我们利用 Nav2 GPS 航点跟踪器功能导航到直接以 GPS 坐标表示的目标。对于此演示，我们希望构建一个类似于 rviz 的交互式界面，允许我们点击地图，让机器人导航到点击的位置。为此，我们将在 wgs84 参考框架上使用 mapviz 的点点击发布器，它将发布一个 PointStamped 消息，其中包含在卫星图像上点击的点的 GPS 坐标。这是开始自定义 GPS 导航设置的绝佳方式！

为此，我们提供了 interactive_waypoint_follower python 节点，该节点订阅 mapviz 的主题并使用 nav2_simple_commander` 中的 BasicNavigator 调用 /follow_gps_waypoints 动作服务器，以点击点为目标。要运行它，请输入您的工作区源和系统其余部分运行的类型：

ros2 run nav2_gps_waypoint_follower_demo interactive_waypoint_follower

现在，您可以单击 mapviz 地图上的您希望机器人采取的姿势。下面的 gif 显示了机器人穿越一些障碍物导航到终点线：

4- 记录 GPS 航点跟随器和航点记录

最后，让我们让机器人通过一组预定义的 GPS 航点。我们提供一个航点记录工具<https://github.com/ros-planning/navigation2_tutorials/tree/master/nav2_gps_waypoint_follower_demo/nav2_gps_waypoint_follower_demo/gps_waypoint_logger.py> ` 它订阅机器人的 GPS 和 IMU，并提供一个简单的 GUI 来根据需要将机器人坐标和航向保存到 ``yaml` 文件中，格式如下：

waypoints:
- latitude: 38.161491054181276
  longitude: -122.45464431092836
  yaw: 0.0
- latitude: 38.161587576524845
  longitude: -122.4547994038464
  yaw: 1.57

让我们记录一些机器人要遵循的路径点。获取工作区并在系统其余部分运行的情况下输入：

ros2 run nav2_gps_waypoint_follower_demo gps_waypoint_logger </path/to/yaml/file.yaml>

如果您不提供保存航点的路径，它们将默认保存在您的 home 文件夹中，名称为 gps_waypoints.yaml。节点启动后，您应该会看到一个小型 GUI，其中有一个用于记录航点的按钮，您现在可以移动机器人并单击该按钮来记录其位置，如下面的 gif 所示：

之后，您应该在指定的位置获得一个 yaml 文件，其格式如上所示；现在让我们让机器人跟随记录的航点。为此，我们提供了 logged_waypoint_follower 节点，该节点将航点文件的路径作为参数，并使用 nav2_simple_commander 中的 BasicNavigator 将记录的目标发送到 /follow_gps_waypoints 操作服务器。如果未提供，节点将使用“nav2_gps_waypoint_follower_demo”包中的“默认航路点<https://github.com/ros-planning/navigation2_tutorials/tree/master/nav2_gps_waypoint_follower_demo/config/demo_waypoints.yaml>”。

要运行此节点，请输入您的工作区源和系统其余部分的运行类型：

ros2 run nav2_gps_waypoint_follower_demo logged_waypoint_follower </path/to/yaml/file.yaml>

您现在应该看到机器人正在跟随您之前记录的航点：

../../_images/logged_waypoint_follower.gif

结论

本教程讨论了使用 RL 和 navsat_transform 节点的 GPS 传感器进行全球定位的使用，还介绍了使用配备 GPS 的机器人进行 Gazebo 模拟的设置。它还介绍了 Nav2 中用于使用 GPS 定位导航的配置更改，强调了设置全球成本地图的一些不同可能性。最后，它展示了 Nav2 的 GPS 航点跟随器的功能，作为如何在户外环境中使用堆栈的演示。

本教程应该是使用 Nav2 在户外机器人上设置自主导航的良好起点，但用户应记住，GPS 只是为堆栈提供全球定位的一种手段，并且 Nav2 中的所有笛卡尔工具仍然可用于超越 GPS 航点跟随器并根据每个用例构建自定义自主应用程序。

户外导航愉快！