Chuiyu Wiki

Appearance

2026-03-10
1567
7.4m

融入RTK-GPS插件进行仿真 ​

GPS 融合定位

navsat_transform + EKF · 仿真 GPS 接入完整流程

本章节衔接前一篇《Nav2无图导航仿真搭建》，在原有基础上加入 GPS 定位链路

0. 为什么要加 GPS ​

前一篇笔记中，EKF 只融合了里程计和 IMU，map 帧是 EKF 自己维护的局部坐标系，和真实地理坐标没有关系。加入 GPS 之后：

•       长距离行驶时，里程计积分误差会随时间累积，GPS 可以周期性修正

•       map 帧与真实 UTM 坐标系对齐，可以用真实 RTK 坐标作为导航目标点

•       为后续接入真实 UM982 RTK 模块打通链路

仿真阶段说明：仿真里里程计没有噪声，GPS 融合效果几乎感知不到。GPS 的真正价值体现在真实硬件阶段：长距离行驶、GPS 信号稳定时修正里程计漂移。

1. GPS 融合架构 ​

完整的定位链路如下：

bash
Gazebo GPS插件（仿真）/ UM982驱动（真实硬件）

    ↓ /gps/fix (sensor_msgs/NavSatFix)

navsat_transform_node  ←  /odometry/filtered (EKF输出)

    ↓ /odometry/gps (GPS转局部坐标)

ekf_node  ←  /odom + /imu + /odometry/gps

    ↓ /odometry/filtered + map→odom TF

注意启动顺序：EKF 必须先于 navsat_transform 启动，因为 navsat_transform 需要订阅 /odometry/filtered 才能初始化。

2. 给 Gazebo 加 GPS 插件 ​

Turtlebot3 默认不带 GPS，需要修改两个系统文件：URDF 和 model.sdf。

2.1 修改 URDF 加入 gps_link ​

复制 URDF 到工程目录再修改：

bash
mkdir -p ~/carplaning/nav_ws/src/campus_nav/urdf
cp /opt/ros/humble/share/turtlebot3_gazebo/urdf/turtlebot3_waffle.urdf \
   ~/carplaning/nav_ws/src/campus_nav/urdf/

用 Python 脚本在 </robot> 前插入 GPS 内容（不能用 cat >> 追加，会破坏 XML 结构）：

bash
cd ~/carplaning/nav_ws/src/campus_nav/urdf/

python3 << 'EOF'
with open('turtlebot3_waffle.urdf', 'r') as f:
    content = f.read()
gps_xml = '''
  <link name="gps_link">
    <visual><geometry><box size="0.01 0.01 0.01"/></geometry></visual>
  </link>
  <joint name="gps_joint" type="fixed">
    <parent link="base_link"/>
    <child link="gps_link"/>
    <origin xyz="0 0 0.2" rpy="0 0 0"/>
  </joint>
  <gazebo reference="gps_link">
    <sensor name="gps_sensor" type="gps">
      <always_on>true</always_on>
      <update_rate>10.0</update_rate>
      <plugin name="gps_plugin" filename="libgazebo_ros_gps_sensor.so">
        <ros><remapping>~/out:=/gps/fix</remapping></ros>
        <frame_name>gps_link</frame_name>
      </plugin>
    </sensor>
  </gazebo>
'''
content = content[:content.index('&lt;/robot&gt;')]
content = content.rstrip() + '\n' + gps_xml + '\n&lt;/robot&gt;\n'
with open('turtlebot3_waffle.urdf', 'w') as f:
    f.write(content)
print('Done')
EOF

覆盖系统文件：

bash
sudo cp ~/carplaning/nav_ws/src/campus_nav/urdf/turtlebot3_waffle.urdf \
  /opt/ros/humble/share/turtlebot3_gazebo/urdf/turtlebot3_waffle.urdf

2.2 修改 model.sdf 加入 GPS 传感器 ​

robot_state_publisher 读 URDF，但 Gazebo 实际加载的是 model.sdf，GPS 传感器必须加到 SDF 里才会生效：

bash
sudo python3 << 'EOF'
with open('/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf', 'r') as f:
    content = f.read()
gps_xml = '''
    <link name="gps_link">
      <pose>0 0 0.2 0 0 0</pose>
      <sensor name="gps_sensor" type="gps">
        <always_on>true</always_on>
        <update_rate>10.0</update_rate>
        <plugin name="gps_plugin" filename="libgazebo_ros_gps_sensor.so">
          <ros><remapping>~/out:=/gps/fix</remapping></ros>
          <frame_name>gps_link</frame_name>
        </plugin>
      </sensor>
    </link>
    <joint name="gps_joint" type="fixed">
      <parent>base_link</parent>
      <child>gps_link</child>
    </joint>
'''
content = content.replace('  </model>', gps_xml + '  </model>')
with open('/opt/ros/humble/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf', 'w') as f:
    f.write(content)
print('Done')
EOF

关键坑：只改 URDF 不改 SDF，robot_state_publisher 能看到 gps_link，但 Gazebo 不会实际发布 GPS 数据。两个文件都要改。

2.3 设置地理坐标参考点 ​

Gazebo 默认坐标是 (0°, 0°)（非洲几内亚湾附近），UTM 转换会报范围错误。需要在 world 文件里设置真实地理参考坐标：

bash
sudo python3 << 'EOF'
with open('/opt/ros/humble/share/turtlebot3_gazebo/worlds/turtlebot3_world.world', 'r') as f:
    content = f.read()
coords = '''
    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <latitude_deg>39.1</latitude_deg>
      <longitude_deg>117.2</longitude_deg>
      <elevation>5.0</elevation>
      <heading_deg>0</heading_deg>
    </spherical_coordinates>
'''
content = content.replace('  </world>', coords + '  </world>')
with open('/opt/ros/humble/share/turtlebot3_gazebo/worlds/turtlebot3_world.world', 'w') as f:
    f.write(content)
print('Done')
EOF

坐标用天津附近 (39.1°N, 117.2°E)，实际部署时换成真实校园坐标。不设置此项会导致 navsat_transform 报错：Northing not in UTM range。

2.4 验证 GPS 话题 ​

重启仿真后验证：

bash
ros2 topic list | grep gps

应看到：/gps/fix  /gps_plugin/vel

bash
ros2 topic hz /gps/fix

应看到：average rate: 10.0

3. 配置 navsat_transform ​

3.1 创建 navsat.yaml ​

bash
cat > ~/carplaning/nav_ws/src/campus_nav/config/navsat.yaml << 'EOF'
navsat_transform:
  ros__parameters:
    use_sim_time: true
    frequency: 10.0
    delay: 0.0
    magnetic_declination_radians: 0.0
    yaw_offset: 1.5707963
    zero_altitude: true
    broadcast_utm_transform: true
    broadcast_utm_transform_as_parent_frame: false
    publish_filtered_gps: false
    use_odometry_yaw: false
    wait_for_datum: false
EOF

3.2 更新 ekf.yaml 加入 GPS 融合 ​

在原有 odom0 和 imu0 基础上，新增 odom1 对应 /odometry/gps：

在 ekf.yaml 末尾追加（imu0 配置后面）

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

说明：odom1_config 只融合 x y 位置（前两个 true），不融合速度和姿态，因为 GPS 位置精度高但速度和姿态不如里程计+IMU。

4. 完整启动顺序 ​

加入 GPS 后需要 5 个终端，顺序严格按下面来：

终端 1：Gazebo 仿真 ​

bash
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

等 Gazebo 完全加载

终端 2：EKF（必须在 navsat 之前启动） ​

bash
ros2 run robot_localization ekf_node --ros-args \
  --params-file ~/carplaning/nav_ws/src/campus_nav/config/ekf.yaml \
  -p use_sim_time:=true

终端 3：navsat_transform（等 EKF 启动 5 秒后） ​

bash
ros2 run robot_localization navsat_transform_node --ros-args \
  --params-file ~/carplaning/nav_ws/src/campus_nav/config/navsat.yaml \
  -p use_sim_time:=true

正常输出：

bash
Datum (latitude, longitude, altitude) is (39.10, 117.20, 5.21)

Datum UTM coordinate is (50S, 517295.95, 4327893.33)

终端 4：Nav2 ​

bash
ros2 launch nav2_bringup navigation_launch.py \
  use_sim_time:=true \
  params_file:=$HOME/carplaning/nav_ws/src/campus_nav/config/nav2_params.yaml

终端 5：Rviz ​

bash
ros2 launch nav2_bringup rviz_launch.py use_sim_time:=true

5. 验证 GPS 融合是否成功 ​

验证 GPS 坐标转换话题 ​

bash
ros2 topic hz /odometry/gps      # 应为 10Hz

验证 EKF 融合输出 ​

bash
ros2 topic hz /odometry/filtered  # 应为 ~10-30Hz

验证 EKF 位置 ​

bash
ros2 topic echo /odometry/filtered --once | grep -A4 'position'

仿真阶段的预期现象：有无 GPS 对导航效果几乎没有区别，这是正常的。仿真里程计没有噪声，GPS 融合增益接近零。GPS 的真正价值在真实硬件户外长距离测试中体现。

6. 真实硬件替换（UM982）（未实践） ​

仿真链路打通后，替换真实硬件只需两步：

6.1 安装 UM982 驱动 ​

bash
cd ~/carplaning/nav_ws/src
git clone https://github.com/ironoa/um982_ros2_driver.git
sudo apt install -y python3-pyproj python3-serial
cd ~/carplaning/nav_ws
colcon build --packages-select um982_ros2_driver
source install/setup.bash

6.2 UM982 硬件配置 ​

用 UPrecise（Windows）连接 UM982，发送以下指令开启输出：

bash
config com3 115200
PVTSLNA com3 0.05
GPHPR com3 0.05
BESTNAVA com3 0.05

6.3 启动真实 UM982 驱动 ​

bash
ros2 launch um982_ros2_driver um982_ros2_driver.launch.py \
  port:=/dev/ttyUSB0 baud:=115200

验证话题（与仿真话题名一致）

bash
ros2 topic hz /gps/fix   # 应为 20Hz（0.05s输出频率）

替换完成后，navsat_transform 和 EKF 的配置完全不需要改动，因为话题名 /gps/fix 保持一致。这就是仿真阶段统一话题名的意义。

最近更新