使用 GPS 定位导航

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

概述

本教程介绍如何以 GPS 传感器作为全局定位源搭建定位系统，如何用 robot_localization（RL）进行传感器融合，以及如何用 Nav2 跟随 GPS 航点。本教程由 Pedro Gonzalez 在 Kiwibot 编写。

环境要求

假定 ROS 2 和 Nav2 依赖包已安装或在本地构建。此外，你还需要安装 robot_localization 和 mapviz：

source /opt/ros/<ros2-distro>/setup.bash
sudo apt install ros-$ROS_DISTRO-nav2-minimal-tb3*
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
sudo apt install ros-$ROS_DISTRO-teleop-twist-keyboard

如果某些软件包无法通过 apt 安装获取，请前往对应的项目仓库从源码构建。

本教程的代码托管在 nav2_gps_waypoint_follower_demo。虽然我们会介绍搭建过程的关键步骤，但仍强烈建议你在搭建开发环境时克隆并构建该软件包。本教程适用于 ROS 2 Iron 及更新版本。

如果你还没有做过之前的教程或 Nav2 演示，可能还需要安装 Gazebo 和 TurtleBot 3 仿真。更多信息请参阅 Nav2 快速入门页面。

GPS 定位概述

GPS（全球定位系统），或更广义的 GNSS（全球导航卫星系统），是一种依靠卫星为接收器提供地球表面位置估计的技术。这些卫星在约 20000 公里的高度运行，持续以无线电频率广播时间信号。当卫星处于接收器的视线范围内时，接收器接收这些信号，通过三边测量（trilateration）来估计自身的纬度、经度和海拔。

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

WGS84 reference frame

然而，用这个参考系来描述地球表面或附近物体的运动、表示环境并不实用。假设你的机器人在足球场上，需要从一端移动到另一端，导航任务看起来会是这样：

“从 X=4789.413km, Y=177.511km, z=4194.292km 到 X=4789.475km, Y=177.553km, z=4194.22km”

此外，如果机器人配有 2D 激光雷达等传感器，还需要将其数据变换到这个参考系中。更好的做法是创建一个局部参考系，在其中可以直接指示机器人「向前走 100 米」，同时传感器数据也能自然地填充环境表示。

为此，大地测量学提出了几种相对于地球表面定位的平面投影系统。其中一种是 UTM 坐标系：它将地球视为椭球体，划分为 60 个带，每个带跨 6 个经度。每个带是椭球体表面在平行于其中央子午线的割圆柱上的投影；各带再划分为 20 个纬度带，每个纬度带跨 8 个纬度，由此形成局部网格区，区内的位置用相对于该区原点的平面坐标表示。下图展示了跨越南美洲的网格区。

UTM grid zones in South America

robot_localization 利用这种投影系统，将 WGS84 参考系中的 GPS 测量值转换为笛卡尔坐标系，坐标原点设在 GPS 所在的网格区原点。这一转换由 navsat_transform 节点完成。该节点遵循 REP 103 的 ENU 约定，即 utm 坐标系的 +x 轴朝东、+y 轴朝北、+z 轴朝上。

在实际应用中，GPS 传感器可能存在噪声：独立式 GPS 在理想条件下精度约为 1–2 米，最差可达 10 米。随着接收到的卫星数量变化，位置会频繁跳变，显著影响导航质量。有多种定位增强技术可减小 GPS 测量误差，其中最常用的是 RTK（实时动态定位），可将接收器精度提高到 1 厘米级别。如果对精度有较高要求，强烈推荐使用 RTK。虽然需要部署第二个固定 GPS 作为基站（base），但美国和欧洲大部分地区已有可免费使用的公共基站。更多关于 RTK 及入门信息，请参阅此文。本教程假设机器人的 GPS 能产生准确、平滑的位置估计。

此外，要完整描述机器人的定位，还需要知道它的航向（heading）。然而独立式 GPS 传感器只提供位置测量，不提供方向测量。本教程中，「绝对航向」（absolute heading）指相对于基本方向（如东）给出的偏航角测量；「相对航向」（relative heading）则相对于机器人转过的角度或其他不能直接映射到基本方向的参考给出。

将 robot_localization 与 GPS 配合使用时，测量绝对方向是必需的。获取绝对方向数据有多种策略，例如带磁力计的 IMU、双 GPS 系统、或在已知地图上进行匹配。本教程假设机器人配备了能按 ENU 约定准确测量绝对方向的 IMU，即面朝东时输出零偏航角，面朝北时输出 +90 度。

尽管有上述假设，在真实环境中，安装在真实机器人上的商用级 IMU 通常无法产生准确的绝对航向测量，原因在于：

它们可能没有磁力计。
难以校准：户外机器人通常又大又重——想象一下用自主拖拉机在空中画一个八字形。
机器人本身可能是磁力计的巨大电磁噪声源：电动机中布满永磁体，可能消耗数安培电流，对传感器造成显著干扰。

因此，在决定如何估计绝对航向时，应根据具体应用的需求来权衡定位精度和行为表现。使用没有相对于基本方向航向的 IMU 时，机器人可能需要先移动一段时间，进行「初始化舞蹈」，滤波器才能收敛到正确的航向。使用双 GPS 或 3D 地图匹配系统时，初始航向的准确度要好得多。

本教程模拟了一个理想系统，使用已具备绝对方向测量能力的 IMU。实际系统中，可用上述技术之一（或其他方案）来增强或替代。

教程步骤

0- 搭建 Gazebo 世界

要使用 GPS 导航，首先需要创建一个带 GPS 传感器的户外 Gazebo 世界，为导航搭建机器人环境。本教程使用 Sonoma Raceway，因为它与真实地理位置对齐。我们提供了一个示例 world，使用 Gazebo 的 navsat 和球面坐标插件，在设定的地理原点处创建局部切平面，并为世界中的每个点提供纬度、经度和海拔坐标：

<!-- Navsat plugin >
<plugin
  filename="gz-sim-navsat-system"
  name="gz::sim::systems::NavSat">
</plugin>

...

<spherical_coordinates>
  <surface_model>EARTH_WGS84</surface_model>
  <world_frame_orientation>ENU</world_frame_orientation>
  <latitude_deg>38.161479</latitude_deg>
  <longitude_deg>-122.454630</longitude_deg>
  <elevation>0</elevation>
  <heading_deg>0</heading_deg>
</spherical_coordinates>

要从 Gazebo 获取 GPS 读数，还需要修改 TurtleBot 3 机器人的 Xacro 和 URDF。最终文件可在 nav2_minimal_turtlebot_simulation 仓库中找到，它在 /gps/fix 话题上输出 NavSatFix 消息。

在 Xacro 文件 gz_waffle_gps.sdf.xacro 中，所做的修改是添加了带 navsat 传感器的 gps_link，并为该 link 创建了一个关节，发布相对于 base_link 的静态变换。

<link name="gps_link">
  <sensor name="navsat" type="navsat">
    <always_on>true</always_on>
    <update_rate>1</update_rate>
    <topic>$(arg namespace)/gps/fix</topic>
    <gz_frame_id>gps_link</gz_frame_id>
    <navsat>
      <position_sensing>
        <horizontal>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>0.0</stddev>
          </noise>
        </horizontal>
        <vertical>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>0.0</stddev>
          </noise>
        </vertical>
      </position_sensing>
    </navsat>
  </sensor>
</link>

此外，还需要在 URDF 文件 turtlebot3_waffle_gps.urdf 中再次定义该关节：

<joint name="gps_joint" type="fixed">
 <parent link="base_link"/>
 <child link="gps_link"/>
 <origin xyz="-0.05 0 0.05" rpy="0 0 0"/>
</joint>
<link name="gps_link"/>

此外，还需要在 Gazebo 到 ROS 的桥接文件中包含 GPS 话题，相关示例可在 turtlebot3_waffle_gps_bridge.yaml 中找到（位于 nav2_minimal_turtlebot_simulation 仓库）。

# replace navsat_bridge - check gz topic name
- topic_name: "gps/fix"
  ros_type_name: "sensor_msgs/msg/NavSatFix"
  gz_type_name: "gz.msgs.NavSat"
  direction: GZ_TO_ROS

构建 nav2_gps_waypoint_follower_demo 软件包，source 工作空间，然后启动以下命令测试 Gazebo 世界是否设置正确：

ros2 launch nav2_gps_waypoint_follower_demo gazebo_gps_world.launch.py

TurtleBot Waffle 应出现在 Sonoma Raceway 世界中。也可以 echo /gps/fix 话题来验证机器人确实在产生 GPS 测量：

Turtlebot in the sonoma raceway

1- 搭建 GPS 定位系统

仿真（或真实机器人）启动并运行后，就可以搭建定位系统了。注意，Nav2 使用 map -> odom -> base_link -> [传感器坐标系] 的 tf 链：全局定位（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 还提供可动态重配置的状态估计节点，使用卡尔曼滤波器融合多个数据源，这也是选用它的另一个原因。

我们将设置一个用于局部里程计的扩展卡尔曼滤波器（EKF），融合轮式里程计和 IMU 数据；第二个用于全局定位，融合经局部笛卡尔转换的 GPS 坐标、轮式里程计和 IMU 数据；再加一个 navsat_transform 节点，从 GPS 数据输出笛卡尔里程计消息。这是使用 GPS 数据时 robot_localization 的常见配置，更多细节可参阅 RL 文档。

为此，我们提供了配置文件和启动文件。继续之前，建议先花时间理解这两个文件及其配置内容。下面逐一讲解每个节点最相关的设置。

局部里程计

局部里程计由 ekf_filter_node_odom 提供，发布 odom 到 base_footprint 的变换，其中 base_footprint 是 Gazebo 中 TurtleBot 差速驱动插件的基坐标系。机器人状态发布器提供 base_footprint 到 base_link 的静态变换，但请确保根据你的配置在 RL 中正确设置基坐标系。注意，EKF 设置为 2D 模式，因为 Nav2 代价地图的环境表示是二维的，且多个层依赖 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/data
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 的里程计输出，即 navsat_transform 节点在 /odometry/gps 上发布的里程计消息：

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_transform 产生里程计输出，其中包含 GPS 在 map 坐标系中的位置，如上所述由全局 EKF 获取。它提供了 datum 参数，用于设置 map 原点的 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_cartesian_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

在另一个终端中，使用仓库中预构建的 config 文件启动 mapviz。有多种方式可在 mapviz 上可视化卫星地图：

Stadiamaps：需要 API key，参见获取 Stadiamaps API key。按如下方式修改配置文件：

- type: mapviz_plugins/tile_map
  name: new display
  config:
    visible: true
    collapsed: false
    custom_sources:
      - base_url: https://tiles.stadiamaps.com/tiles/alidade_satellite/{level}/{x}/{y}.png?api_key=api_key
        max_zoom: 15
        name: Stadia
        type: wmts
    bing_api_key: ""
    source: Stadia (alidade_satellite)

如需本地托管地图，请参阅本地 tile_map 图像

ros2 launch nav2_gps_waypoint_follower_demo mapviz.launch.py

正确设置 API key 后，应看到以下窗口：

Turtlebot in the sonoma raceway

最后运行 teleop_twist_keyboard 节点来遥控仿真中的 TurtleBot：

ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --param stamped:=True

一切启动并运行后，开始遥控 TurtleBot 并检查以下事项：

当机器人面朝东（默认初始航向）并向前移动时，base_link 坐标系（绿色箭头）与原始 GPS 测量（蓝点）一致地向东移动。
面朝其他方向运动时也整体一致，即 GPS 测量与机器人航向和运动方向一致，且与世界中的机器人位置一致（例如，当机器人向终点线移动时，mapviz 中的 GPS 测量也随之移动）。

以下 gif 展示了预期效果：

localization_check

真实机器人上的传感器可能不如 Gazebo 中的准确，尤其是 GPS 和来自 IMU 的绝对航向测量。为缓解这一问题，可利用 robot_localization 的 EKF 来弥补传感器能力的不足：

如果 IMU 不能准确提供绝对偏航测量，考虑将输入到 RL 的 differential 参数设为 true。这样滤波器只融合方向变化量，从内部运动模型推导绝对值——通过位置变化的差分来估计机器人朝向（例如，机器人根据轮式里程计以 1 m/s 前进，同时根据 GPS 向北移动了 1 米，则说明它面朝北）。注意，在此情况下，在机器人移动一段时间、滤波器从运动中估计出绝对航向之前，绝对航向值不准确。如果应用场景不允许这样做，考虑添加另一个能准确测量绝对航向的传感器，例如双 GPS 系统。
如果 GPS 存在噪声但你还有其他可信的里程计源（如轮式里程计、视觉里程计），考虑调整传感器和过程噪声协方差，使滤波器更多或更少地「信任」某个数据源或自身的内部状态估计。调整得当的滤波器应能在一定程度上剔除错误的 GPS 测量。

2- 搭建导航系统

定位系统启动并运行后，就可以搭建 Nav2 了。由于 RL 已经提供了 tf 树，无需启动 amcl，因此可以从参数文件中移除其参数，且不启动 Nav2 的定位启动文件。

全局代价地图主要有三种配置方式：

滚动窗口（Rolling）（本教程使用）：户外环境可能非常大，用单一代价地图表示不太实际。因此本教程使用足够大的滚动全局代价地图来容纳连续的航点对。此时可选择是否使用静态层，但若使用，请务必固定 navsat_transform 的 datum，使 GPS 坐标在地图上始终有相同的笛卡尔表示。

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

由静态地图确定大小和位置：也可以保持 Nav2 默认设置，通过添加静态层并使用 map_server，让全局代价地图根据预构建的地图确定大小和位置。此时还需确保 datum 与地图原点一致。

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 自主导航的效果：

navigation_check

3- 交互式 GPS 航点跟随器

至此完整的系统已经搭建完毕。接下来利用 Nav2 GPS 航点跟随器的能力，导航到直接用 GPS 坐标表示的目标。本演示将构建一个类似 RViz 的交互式界面，允许点击地图让机器人导航到点击位置。具体做法是使用 mapviz 在 wgs84 参考系上的点点击发布器（point click publisher），它会发布一个 PointStamped 消息，包含在卫星图像上点击的点的 GPS 坐标。这是开始构建自定义 GPS 导航方案的好方法！

为此，我们提供了 interactive_waypoint_follower Python 节点，它订阅 mapviz 的话题，并使用 nav2_simple_commander 中的 BasicNavigator，以点击的点为目标调用 /follow_gps_waypoints 动作服务器。运行方式：source 工作空间，在系统其余部分运行时输入：

ros2 run nav2_gps_waypoint_follower_demo interactive_waypoint_follower

现在可以在 mapviz 地图上点击想要机器人前往的位置。下面的 gif 显示了机器人导航到终点线并穿过一些障碍物的过程：

interactive_wpf

4- 记录式 GPS 航点跟随器与航点记录

最后，让机器人依次通过一组预定义的 GPS 航点。我们提供了一个航点记录工具，它订阅机器人的 GPS 和 IMU，提供一个简单的 GUI，可按需将机器人坐标和航向保存到 yaml 文件中，格式如下：

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

下面记录一些航点供机器人跟随。source 工作空间，在系统其余部分运行时输入：

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

如果未指定保存路径，航点默认以 gps_waypoints.yaml 的名称保存在用户主目录中。节点启动后，应出现一个带「记录航点」按钮的小 GUI。移动机器人并点击该按钮即可记录其位置，如下面的 gif 所示：

waypoint_logging

之后，指定位置会生成一个上述格式的 yaml 文件。接下来让机器人跟随记录的航点。为此，我们提供了 logged_waypoint_follower 节点，它以航点文件路径为参数，使用 nav2_simple_commander 中的 BasicNavigator 将记录的目标发送到 /follow_gps_waypoints 动作服务器。如果未提供文件路径，该节点使用 nav2_gps_waypoint_follower_demo 软件包中的默认航点。

运行该节点：source 工作空间，在系统其余部分运行时输入：

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

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

logged_waypoint_follower

结论

本教程介绍了使用 GPS 传感器结合 RL 和 navsat_transform 节点进行全局定位的方法，并涵盖了搭建带 GPS 机器人的 Gazebo 仿真。同时介绍了 Nav2 中使用 GPS 定位导航的配置更改，重点说明了全局代价地图的几种配置方式。最后展示了 Nav2 GPS 航点跟随器在户外环境中的实际应用能力。

本教程应能作为使用 Nav2 在户外机器人上搭建自主导航的良好起点。需要注意的是，GPS 只是为该栈提供全局定位的一种手段，Nav2 中的所有笛卡尔工具仍然可用——完全可以超越 GPS 航点跟随器，根据具体场景构建自定义的自主应用。

祝户外导航愉快！