Engineering Notebook // Build Log

2026.03.31

18:05:26

NOTEBOOK_ENTRY

GPS 定位导航使用指南

目录第一部分：基础知识——先搞懂 GPS 是什么 1. GNSS 卫星定位基础 1 gnss 卫星定位基础 2. 坐标系统与大地测量基础 2 坐标系统与大地测量基础 3. NMEA 协议——GPS 模块的"语言" 3 nmea 协议gps 模块的语言 4. GPS 精度与误差来源 4 gps 精度与误差来源 5. 坐标转换数学——从经纬度到平面米制坐标 5 坐标转换数学从经纬度到平面米制坐标 6. 传感器融合理论——为什么不能只靠 G…

Notebook Time

8 min

Image Frames

View Tracks

160

学习

FIELD_GUIDE

FIELD GUIDE

Use the guide rail to jump between sections.

第二部分：本项目 GPS 实现详解 8. 硬件连接与驱动层 9. GPS 数据流全景 10. GNSS 校准节点 —— gnss_calibration 11. PGO GPS 因子融合 —— 因子图优化中的 GPS 12. ENU→map 对齐 —— 连接 GPS 世界与 SLAM 世界 13. 三种运行模式中的 GPS 角色

第三部分：扩展开发——实现 GPS 远距离导航 14. 当前瓶颈分析 15. 拓展方向一：GPS 质量自适应融合 16. 拓展方向二：多点启动标定与地图锚定 17. 拓展方向三：GPS 全局路径规划闭环 18. 拓展方向四：RTK/差分 GPS 接入 19. 拓展方向五：GPS 辅助重定位与丢失恢复 20. 开发者实操 Cookbook

第一部分：基础知识——先搞懂 GPS 是什么

1. GNSS 卫星定位基础

1.1 什么是 GNSS

GNSS（Global Navigation Satellite System，全球导航卫星系统）是所有卫星定位系统的统称。GPS 只是其中之一：

系统	国家/地区	卫星数	说明
GPS	美国	31 颗	最早也最常用
GLONASS	俄罗斯	24 颗	与 GPS 互补
BeiDou（北斗）	中国	45+ 颗	亚太区精度更高
Galileo	欧洲	30 颗	精度最高的民用系统

日常说的"GPS"往往指整个 GNSS 定位功能，本项目使用的模块实际上同时接收多个系统的信号（NMEA 语句以 $GN 开头就表示多系统联合解算）。

1.2 定位原理——三球交汇

GPS 定位的核心原理非常直观：

卫星 A 说："你距我 20,000 km"  →  你在以 A 为圆心、20,000km 为半径的球上
卫星 B 说："你距我 21,000 km"  →  两个球的交线是一个圆
卫星 C 说："你距我 19,500 km"  →  三个球交于两个点
卫星 D 说："你距我 20,200 km"  →  四颗卫星精确确定唯一位置

为什么需要 4 颗而不是 3 颗？ 因为接收器的时钟不够精确，需要第 4 颗卫星解算时钟偏差。所以 GPS 实际上同时解算 4 个未知数：x, y, z, t（三维位置 + 时间）。

1.3 定位输出——"Fix" 的含义

GPS 模块输出的定位结果叫做 Fix（定位解）。Fix 有不同的质量等级：

Fix 类型	状态码	精度	说明
No Fix	-1	无	无法定位（室内、遮挡严重）
Standard (SPS)	0	2-10m	标准民用定位
DGPS	1	0.5-2m	差分校正后的定位
RTK Float	5	0.2-1m	RTK 浮点解
RTK Fixed	4	0.01-0.02m	RTK 固定解，厘米级

在 ROS 2 中，sensor_msgs/NavSatFix 消息的 status.status 字段就对应上表。本项目当前使用的是 Standard Fix (status=0)，精度约 2.5m。

1.4 关键质量指标

DOP（精度因子）

DOP 反映卫星几何分布对精度的影响。卫星分布越分散，DOP 越小，精度越高：

      好的几何分布（低 DOP）          差的几何分布（高 DOP）
          ★                              ★  ★
        /   \                              ||
      ★       ★                           ★  ★
       \     /
         📍                                📍
    精度: ~2m                           精度: ~8m

DOP 类型	全称	说明
HDOP	水平精度因子	水平方向的精度衰减，本项目主要看这个
VDOP	垂直精度因子	垂直方向的精度衰减
PDOP	位置精度因子	三维位置精度衰减
GDOP	几何精度因子	包含时间的总体精度衰减

经验值：HDOP < 1.5 优秀，< 3.0 可用，> 5.0 不可靠。本项目默认阈值 gps.quality_hdop_max: 3.0。

卫星数

可见卫星越多，定位越稳定。经验上：

< 4 颗：无法定位
4-6 颗：勉强可用，精度差
7-10 颗：正常工作
> 10 颗：精度良好

2. 坐标系统与大地测量基础

这是做 GPS 开发最容易搞混的部分。花 10 分钟理解，可以避免数天的调试。

2.1 WGS84——GPS 的原生坐标系

GPS 模块输出的经纬度基于 WGS84（World Geodetic System 1984）椭球体。

北极
|
|    纬度 (latitude): 赤道为 0°，北为正
|    范围: -90° ~ +90°
 $- - - - - - - + - - - - - - - \to 经度 (l o n g i t u d e) : 本初子午线为 0° ，东为正$ 
|          范围: -180° ~ +180°
|
南极

注意：经纬度是"角度"，不是长度！
1° 纬度 ≈ 111 km（处处相同）
1° 经度 ≈ 111 km × cos(纬度)（越靠近极点越短）
在苏州（纬度 ≈ 31°），1° 经度 ≈ 95 km

本项目中 ENU 原点：lat: 31.274927, lon: 120.737548，位于 XJTLU 校区附近。

2.2 大地坐标与笛卡尔坐标

GPS 给出的是 大地坐标（经度、纬度、海拔），这是球面上的坐标，无法直接用于平面计算（比如求两点距离、求路径角度）。因此需要转换到某种 平面直角坐标系。

2.3 ENU 坐标系——本项目的核心中间坐标系

ENU（East-North-Up） 是以某个固定点为原点的局部笛卡尔坐标系：

        North (Y)
          ↑
          |
          |
          +———————→ East (X)
         /
        /
       ↓ 
      Up (Z)（垂直向上）

原点 = 你选择的固定参考点
单位 = 米

为什么选 ENU？

与 GPS 自然对接：经度增加 → East 增加，纬度增加 → North 增加
单位是米，可以直接计算距离
ROS 里许多 GPS 工具默认使用 ENU

本项目中 ENU 原点的配置（master_params.yaml）：

"gps.origin_lat": 31.274927
"gps.origin_lon": 120.737548
"gps.origin_alt": 0.0

至关重要：PGO、dispatcher、aligner 三个模块的 ENU 原点必须完全一致，否则整个 GPS 坐标系就是错位的。

2.4 map 坐标系——SLAM 的世界

FAST-LIO2 启动时，车头正对的方向和 IMU 初始姿态共同决定了 map 坐标系的朝向。每次启动，map 坐标系的"北"可能指向不同的方向。

这就是 GPS 融合最核心的问题：ENU 的"北"是固定的（地理北），而 map 的"北"是随机的。

ENU 坐标系 (固定)              map 坐标系 (每次不同)
North                          y_map
 $↑↑↗ 可能偏转了 θ 度$ 
|                              | /
 $+ —— \to E a s t + —— \to x_{m} a p$

所以需要估计一个 ENU → map 的刚体变换（旋转角 θ + 平移 tx, ty），这就是 gps_global_aligner 做的事。

2.5 完整坐标系链

WGS84 (经纬度,°)
│
 $│ p y p r o j / G eo g r a p hi c L ib 转换$ 
▼
ENU (东-北-上, 米) ←── 以固定原点为零点的平面坐标
│
 $│ E N U \to ma p 变换 (θ, t x, t y)$ 
│  由 aligner 或 PGO 估计
▼
map (SLAM 全局坐标系) ←── 方向取决于启动时 IMU 姿态
│
│  map→odom (由 PGO 发布的校正量)
▼
odom (里程计坐标系) ←── 高频精准但会漂移
│
 $│ o d o m \to ba s e_{l} ink (由 F A S T - L I O 2 发布)$ 
▼
 $ba s e_{l} ink (车体坐标系) \leftarrow ── 前方 = x ，左方 = y ，上方 = z$

3. NMEA 协议——GPS 模块的"语言"

3.1 NMEA 0183 格式

几乎所有消费级 GPS 模块都通过 NMEA 0183 协议输出数据。这是纯文本格式，通过串口传输。

一条典型的 NMEA 语句：

$GNGGA,073412.00,3116.4956,N,12044.2529,E,1,08,1.2,15.3,M,-9.0,M,,*6A

拆解如下：

字段	值	含义
`$GNGGA`		语句类型：GN=多系统，GGA=定位数据
`073412.00`		UTC 时间：07:34:12.00
`3116.4956,N`		纬度：31°16.4956'N = 31.274927°
`12044.2529,E`		经度：120°44.2529'E = 120.737548°
`1`		定位质量：1=GPS fix
`08`		使用卫星数：8 颗
`1.2`		HDOP：1.2（优秀）
`15.3,M`		海拔：15.3 米
`*6A`		校验和

3.2 常见 NMEA 语句类型

语句	说明	包含信息
GGA	定位数据	时间、经纬度、质量、卫星数、HDOP、海拔
RMC	推荐最小定位数据	时间、经纬度、速度、航向、日期
GSA	卫星状态	DOP 值、使用的卫星编号
GSV	可见卫星信息	每颗卫星的仰角、方位角、信噪比
VTG	地面速度和航向	真北航向、磁北航向、速度

在本项目中，nmea_navsat_driver 负责解析这些语句并转为 ROS 消息。它主要使用 GGA 语句。

3.3 纬度格式转换

NMEA 的纬度格式是 ddmm.mmmm（度分格式），而 ROS 和大多数编程接口使用 十进制度（decimal degrees）：

# NMEA: 3116.4956
# 拆分: 31 度 16.4956 分
# 转换: 31 + 16.4956 / 60 = 31.274927°

def nmea_to_decimal(nmea_value, direction):
    degrees = int(nmea_value / 100)
    minutes = nmea_value - degrees * 100
    decimal = degrees + minutes / 60.0
    if direction in ('S', 'W'):
        decimal = -decimal
    return decimal

4. GPS 精度与误差来源

4.1 误差分类

误差来源	量级	能否消除
电离层延迟	2-10m	差分/双频可消
对流层延迟	0.5-2m	模型可减小
多径效应（信号反射）	1-5m	选择空旷位置
卫星轨道误差	0.5-2m	差分可消
接收器噪声	0.3-1m	取决于硬件
卫星几何分布 (DOP)	倍增因子	等待或换位置

4.2 多径效应——室外最常见的干扰

         卫星
        / |  \
直达路径/  |   \反射路径
      /   |    \
     📍  🏢    📍
    接收器 建筑物 反射面

接收器同时收到直达信号和反射信号
→ 距离计算出现偏差
→ 位置抖动或系统性偏移

本项目在 XJTLU 校园内运行，建筑物密集，多径效应是 GPS 误差的主要来源。

4.3 为什么本项目的 GPS 精度是 ~2.5m

本项目使用的是 Wheeltec GPS 模块，属于消费级单频 GNSS 接收器：

单频（L1 频段）→ 无法消除电离层误差
无差分校正 → 保留所有系统误差
校园内建筑遮挡 → 可见卫星少 + 多径效应
9600 波特率串口 → 更新率约 1Hz

这些因素叠加，实测精度约 2.5m（CEP95，即 95% 的时间误差在 2.5m 以内）。

5. 坐标转换数学——从经纬度到平面米制坐标

5.1 Haversine 公式——球面两点距离

最基础的 GPS 计算：已知两个经纬度，求它们之间的地面距离。

import math

def haversine_m(lat1, lon1, lat2, lon2):
    """计算两个经纬度点之间的地表距离（米）"""
    R = 6371000.0  # 地球平均半径（米）
    dlat = math.radians(lat2 - lat1)
    dlon = math.radians(lon2 - lon1)
    a = (math.sin(dlat/2)**2 +
         math.cos(math.radians(lat1)) *
         math.cos(math.radians(lat2)) *
         math.sin(dlon/2)**2)
    return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))

本项目中多处使用这个函数，比如 gnss_calibration_node.py 里检查样本稳定性、collect_gps_scene.py 里检查点间距离。

5.2 WGS84 → ENU 转换

本项目使用了两种实现方式：

方式一：pyproj（Python 端）

from pyproj import CRS, Transformer

# 创建以固定原点为中心的等距方位投影
origin_lat, origin_lon = 31.274927, 120.737548
local_crs = CRS.from_proj4(
    f"+proj=aeqd +lat_0={origin_lat} +lon_0={origin_lon} "
    "+datum=WGS84 +units=m +no_defs"
)
transformer = Transformer.from_crs("EPSG:4326", local_crs, always_xy=True)

# 转换：经纬度 → ENU (x=东, y=北)
def latlon_to_enu(lat, lon):
    x, y = transformer.transform(lon, lat)  # 注意 pyproj 参数顺序: lon, lat
    return x, y  # x=East, y=North

这就是本项目 scene_runtime.py 中 FixedENUProjector 类的实现。

方式二：GeographicLib（C++ 端）

#include <GeographicLib/LocalCartesian.hpp>

// 以固定原点创建转换器
GeographicLib::LocalCartesian geo_converter(
    31.274927,   // 原点纬度
    120.737548,  // 原点经度
    0.0          // 原点海拔
);

// 转换：经纬度 → ENU
double enu_x, enu_y, enu_z;
geo_converter.Forward(lat, lon, alt, enu_x, enu_y, enu_z);
// enu_x = East, enu_y = North, enu_z = Up

这就是本项目 PGO 节点 (pgo_node.cpp) 中 GPS 因子计算的实现方式。

5.3 ENU → map 刚体变换

这是连接 GPS 世界和 SLAM 世界的关键变换。数学形式：

 $┌ ma p_{x} ┐ ┌ cos (θ) - s in (θ) ┐ ┌ e n u_{x} ┐ ┌ t x ┐$ 
│       │ = │                  │ × │       │ + │    │
 $└ ma p_{y} ┘ └ s in (θ) cos (θ) ┘ └ e n u_{y} ┘ └ t y ┘$

其中：

θ = ENU 坐标系与 map 坐标系之间的旋转角
tx, ty = 平移量
这三个参数就是 gps_global_aligner 在线估计的

估计方法——最小二乘配准：

已知 N 组配对 (enu_i, map_i)，需要找到最优的 (θ, tx, ty) 使得：

 $最小化 \sum ∥ R (θ) \times e n u_{i} + t - ma p_{i} ∥^{2}$

这是一个标准的 2D 刚体配准问题，有闭式解（不需要迭代）：

def solve_rigid_2d(enu_points, map_points):
 $""" 求解 E N U \to ma p 的 2 D 刚体变换 (θ, t x, t y) """$ 
# 1. 计算质心
 $e n u_{m} e an = n p . m e an (e n u_{p} o in t s, a x i s = 0)$ 
 $ma p_{m} e an = n p . m e an (ma p_{p} o in t s, a x i s = 0)$ 

# 2. 去质心
 $e n u_{c} e n t er e d = e n u_{p} o in t s - e n u_{m} e an$ 
 $ma p_{c} e n t er e d = ma p_{p} o in t s - ma p_{m} e an$ 

# 3. 计算旋转角（使用 SVD 或直接公式）
 $H = e n u_{c} e n t er e d . T @ ma p_{c} e n t er e d$ 
 $t h e t a = ma t h . a t an 2 (H [0, 1] - H [1, 0], H [0, 0] + H [1, 1])$ 

# 4. 计算平移
 $R = n p . a r r a y ([[cos (t h e t a), - s in (t h e t a)],$ 
[sin(theta),  cos(theta)]])
 $t = ma p_{m} e an - R @ e n u_{m} e an$ 

return theta, t[0], t[1]

5.4 方位角（Bearing）计算

从点 A 到点 B 的地理方位角（以正北为 0°，顺时针）：

def bearing_deg(lat1, lon1, lat2, lon2):
    """计算从 (lat1,lon1) 到 (lat2,lon2) 的方位角"""
    y = math.sin(math.radians(lon2 - lon1)) * math.cos(math.radians(lat2))
    x = (math.cos(math.radians(lat1)) * math.sin(math.radians(lat2)) -
         math.sin(math.radians(lat1)) * math.cos(math.radians(lat2)) *
         math.cos(math.radians(lon2 - lon1)))
    return (math.degrees(math.atan2(y, x)) + 360) % 360

注意：这个方位角是 罗盘方向（0°=北，90°=东），而 ENU 坐标系的 yaw 是 数学方向（0°=东，90°=北，逆时针正）。转换公式：enu_yaw = 90° - compass_heading。这就是项目中 compass_heading_to_enu_yaw_deg() 函数做的事。

6. 传感器融合理论——为什么不能只靠 GPS

6.1 各传感器对比

频率:   IMU 200Hz ████████████████████████████████████████
       LiDAR 10Hz ████
         GPS  1Hz █

精度:    GPS ██████████ 2.5m
       LiDAR ██ 0.05m (局部)
         IMU █ 0.01m (短期)

漂移:    GPS ✗ 不漂移（绝对坐标）
       LiDAR ✓ 长距离漂移
         IMU ✓ 快速漂移

核心矛盾：精度高的传感器会漂移，不漂移的传感器精度低。融合的目的就是取长补短。

6.2 因子图优化（Factor Graph）——本项目的融合方法

本项目使用 GTSAM 库进行因子图优化。这是当前机器人定位领域最主流的融合框架。

因子图的直觉理解：

变量节点（位姿）:   X0 ——— X1 ——— X2 ——— X3 ——— X4
                    |       |       |       |       |
因子（约束）:       里程计  里程计  里程计  里程计  里程计
                    |               |               |
额外因子:          GPS因子         GPS因子         GPS因子
                                    |
                               回环检测因子

每个因子 = 一条"弹簧"，附带噪声模型（弹簧的弹性系数）
优化 = 找到让所有弹簧能量总和最小的位姿序列

GPS 因子的数学定义（GTSAM 中的 GPSFactor）：

给定关键帧 i 的位姿 Xi
GPS 在该时刻观测到 ENU 坐标 (gps_x, gps_y, gps_z)

 $GPS 因子 = \|Xi.translation - (gps_x, gps_y, gps_z)\|^2_\sum$ 

 $其中 \sum 是噪声协方差矩阵 :$ 
 $\sum = d ia g (σ_{x} y^{2}, σ_{x} y^{2}, σ_{z}^{2})$ 

 $σ_{x} y = g p s . n o i s e_{x} y (本项目默认 2.5 m)$ 
 $σ_{z} = g p s . n o i s e_{z} (本项目默认 5.0 m)$

关键理解：noise_xy 越大，GPS 因子的"弹簧"越松，对最终优化结果的影响越小。这就是为什么 RTK GPS (σ=0.02m) 的约束力远大于普通 GPS (σ=2.5m)。

6.3 紧耦合 vs 松耦合

	紧耦合	松耦合
数据层级	原始观测量（伪距、载波相位）	定位结果（经纬度）
实现复杂度	高	低
精度潜力	更高	受限于 GPS 模块自身解算
本项目	✗ 未使用	✓ 直接使用 NavSatFix

本项目使用的是松耦合方案：GPS 模块自行解算出经纬度，PGO 把它作为一个位置约束加入因子图。这种方案实现简单，对当前的硬件精度来说已经足够。

7. ROS 2 GPS 生态——消息类型与工具链

7.1 核心消息类型

`sensor_msgs/msg/NavSatFix`

这是 GPS 数据的标准 ROS 消息，本项目所有 GPS 数据都用这个类型：

# 消息结构
 $Header header # 时间戳 + 坐标系 ID$ 
NavSatStatus status        # 定位状态
float64 latitude           # 纬度（十进制度）
float64 longitude          # 经度（十进制度）
float64 altitude           # 海拔（米）
float64[9] position_covariance    # 3×3 协方差矩阵（行主序）
uint8 position_covariance_type    # 协方差类型

协方差矩阵的含义：

position_covariance = [
σ_east²,  0,       0,       # [0] = 东向方差
0,        σ_north², 0,       # [4] = 北向方差
0,        0,       σ_up²    # [8] = 垂直方差
]

在代码中取水平精度：

horizontal_sigma = math.sqrt(max(msg.position_covariance[0],
                                  msg.position_covariance[4]))

`sensor_msgs/msg/NavSatStatus`

int8 STATUS_NO_FIX  = -1   # 无法定位
int8 STATUS_FIX     =  0   # 标准定位
int8 STATUS_SBAS_FIX =  1  # 差分定位
int8 STATUS_GBAS_FIX =  2  # 地面增强

int8 status                # 当前状态
uint16 service             # 使用的卫星系统

7.2 本项目使用的关键 ROS 2 包

包	作用	本项目中的位置
`nmea_navsat_driver`	NMEA 串口解析 → NavSatFix	`src/sensor_drivers/gnss/nmea_navsat_driver/`
`nmea_msgs`	NMEA 消息定义	`src/sensor_drivers/gnss/nmea_msgs/`
`tf2_ros`	坐标变换广播与监听	ROS 2 内置
`nav2_msgs`	导航 Action 接口	Nav2 内置

7.3 常用调试命令

# 查看 GPS 话题是否存在
ros2 topic list | grep -E "fix|gnss|gps"

# 实时查看 GPS 数据
ros2 topic echo /fix

# 查看 GPS 发布频率
ros2 topic hz /fix

# 查看 GPS 协方差
ros2 topic echo /fix --field position_covariance

# 查看 TF 链是否完整
ros2 run tf2_ros tf2_echo map base_link

# 查看所有 TF 关系
ros2 run tf2_tools view_frames

第二部分：本项目 GPS 实现详解

8. 硬件连接与驱动层

8.1 硬件规格

项目	值
GPS 模块	Wheeltec GPS（多系统 GNSS 接收器）
连接方式	USB-UART 串口
设备文件	`/dev/wheeltec_gps`
波特率	9600
更新率	~1 Hz
支持系统	GPS + BeiDou + GLONASS
精度等级	~2.5m (CEP95)

8.2 驱动节点：nmea_navsat_driver

nmea_navsat_driver 是一个标准的 ROS 2 GPS 驱动，它做的事情：

串口 → 读取 NMEA 语句 → 解析 GGA/RMC → 发布 /fix (NavSatFix)
                                       → 发布 /heading (如有)
                                       → 发布 /time_reference

配置在 master_params.yaml：

/nmea_navsat_driver:
  ros__parameters:
    port: /dev/wheeltec_gps    # 串口设备路径
    baud: 9600                 # 波特率
    frame_id: gps              # TF 坐标系名
    useRMC: false              # 是否优先用 RMC 语句

9. GPS 数据流全景

┌──────────────────────────────────────────────────────────────────────────┐
│                                                                          │
│   GPS 卫星 → GPS 模块 → /dev/wheeltec_gps (NMEA 串口)                   │
│                              │                                           │
│                              ▼                                           │
│                    ┌─────────────────────┐                               │
│                    │ nmea_navsat_driver   │                               │
│                    │ (NMEA → NavSatFix)   │                               │
│                    └─────────┬───────────┘                               │
│                              │                                           │
│                              ▼                                           │
│                       话题: /fix                                         │
│                   (原始 GPS: 经纬度+状态)                                │
│                     ┌────────┼──────────┐                                │
│                     │        │          │                                 │
│                     ▼        ▼          ▼                                 │
│            ┌────────────┐ ┌──────┐ ┌──────────────┐                      │
│            │gnss_calibra│ │ PGO  │ │gps_global_   │                      │
│            │tion        │ │(直接 │ │aligner       │                      │
│            │(偏移校准)  │ │消费) │ │(独立对齐)    │                      │
│            └──────┬─────┘ └──┬───┘ └──────┬───────┘                      │
│                   │          │            │                               │
│                   ▼          ▼            ▼                               │
│             话题: /gnss   GPS Factor   ENU→map 变换                      │
│             (校准后)     (GTSAM)     (/gps_corridor/enu_to_map)          │
│                   │          │            │                               │
│                   ▼          ▼            ▼                               │
│              PGO GPS     map→odom    gps_route_runner                    │
│              Factor       TF 校正     (GPS 路点→Nav2 目标)              │
│                                          │                               │
│                                          ▼                               │
│                                   NavigateToPose                         │
│                                   → /cmd_vel → 电机                     │
│                                                                          │
└──────────────────────────────────────────────────────────────────────────┘

10. GNSS 校准节点 ── gnss_calibration

10.1 为什么需要校准

低成本 GPS 模块有固定的系统偏移。每次开机偏移量可能不同，但同一次运行期间偏移相对稳定。

10.2 校准算法

# 核心逻辑 (gnss_calibration_node.py)

# 1. 已知参考点坐标（从 calibration_points.yaml 读取）
ref_lat, ref_lon = 31.274927, 120.737548

# 2. 收集 5 个 GPS 样本
samples = [(31.274903, 120.737530), (31.274910, 120.737525), ...]

# 3. 检查稳定性（任意两点距离 < 1m）
for sample in samples:
    if haversine_m(samples[0], sample) > 1.0:
        重新采集

# 4. 计算偏移量
avg_lat = mean(s.lat for s in samples)  # GPS 平均读数
avg_lon = mean(s.lon for s in samples)
lat_offset = ref_lat - avg_lat          # 偏移 = 真实 - 观测
lon_offset = ref_lon - avg_lon

# 5. 后续所有 GPS 数据加上偏移量
calibrated_lat = raw_lat + lat_offset
calibrated_lon = raw_lon + lon_offset

10.3 关键限制

启动时车必须停在校准点上——如果车不在，偏移量计算就是错的
只做平移校正，不做旋转——假设 GPS 误差在局部区域内是均匀偏移
单次校准，不持续更新——不适合长时间运行（偏移可能随大气条件变化）

11. PGO GPS 因子融合 ── 因子图优化中的 GPS

11.1 GPS 因子添加流程

每收到一帧关键帧:
  key_pose_count++
  if key_pose_count % gps_factor_interval == 0:
    从 GPS 缓存队列中找时间最近的 GPS 消息
    if 时间差 < 1秒:
      经纬度 → GeographicLib → ENU (x,y,z)
      if ENU→map 变换已知:
        将 ENU 转为 map 坐标
      创建 GTSAM::GPSFactor(关键帧索引, map坐标, 噪声模型)
      加入因子图
      触发 GTSAM 优化

11.2 噪声模型如何工作

// pgo_node.cpp 中的核心代码逻辑
gtsam::Vector3 noise_vec;
noise_vec << gps_noise_xy, gps_noise_xy, gps_noise_z;
auto gps_noise = gtsam::noiseModel::Diagonal::Sigmas(noise_vec);

// GPS 因子只约束位置 (x,y,z)，不约束旋转
// 这意味着车的朝向完全由 LiDAR + IMU 决定
gtsam::GPSFactor gps_factor(key_index, gps_point, gps_noise);

热身机制：对齐刚建立时的前 N 个 GPS 因子使用更大的 sigma（更松的约束），避免不准确的对齐"硬拉"位姿图。

"gps.alignment_warmup_factors": 5     # 前 5 个 GPS 因子用松约束
"gps.alignment_warmup_sigma": 10.0    # 热身期 sigma = 10m（比正常的 2.5m 松很多）

11.3 ENU→map 对齐估计（PGO 内部）

PGO 在积累 GPS 因子的同时，也会缓存 (ENU, map) 配对。当配对数 ≥ alignment_min_points 且空间展幅 ≥ alignment_min_spread_m 时，执行 2D 刚体配准解算 ENU→map 变换。

12. ENU→map 对齐 ── 连接 GPS 世界与 SLAM 世界

12.1 为什么需要独立 aligner

最初的设计是让 PGO 同时做 loop closure 和 GPS 对齐。但实车发现 PGO 在 loop closure 时会调整所有位姿，导致 ENU→map 变换跟着"晃"。于是架构演进为：

v1: PGO 同时做 GPS 对齐 + loop closure → 对齐不稳定
v2: 独立 gps_global_aligner，与 PGO 解耦 → 对齐平滑

12.2 gps_global_aligner 的核心机制

# 简化版运行逻辑

# 1. Bootstrap: 启动时立即估计初始 ENU→map
yaw0 = 从 TF 读取 map→base_link 的朝向
launch_yaw_deg = 从 route 文件读取（车的地理朝向）
theta_bootstrap = yaw0 - radians(launch_yaw_deg)

# 2. 持续采集配对
while running:
    fix = 最新 /fix 消息
    enu = WGS84_to_ENU(fix.lat, fix.lon)
    map_xy = 从 TF 读取当前 map→base_link 位置

    # 只有移动了足够距离才添加配对
    if distance(enu, last_enu) > pair_min_spacing_m:
        pairs.append((enu, map_xy))

    # 3. 在线解算
    if len(pairs) >= alignment_min_pairs:
        raw_alignment = solve_rigid_2d(pairs)

        # 4. 限速平滑——防止跳变
        if |raw.theta - current.theta| > max_theta_step:
            theta_step = clamp(delta, -max_step, +max_step)
        if |raw.translation - bootstrap.translation| > max_delta:
            reject!  # 偏离 bootstrap 太远，可能是 GPS 跳变

        current_alignment += step  # 渐进更新

12.3 Waypoint 内冻结机制

当 runner 在执行一个 waypoint 到下一个 waypoint 的导航时，冻结当前 alignment 不更新。只在 waypoint 边界处才吸收新的 alignment。这防止了"走到一半目标点突然跳了"的问题。

13. 三种运行模式中的 GPS 角色

	Explore GPS	Nav GPS	Corridor
启动命令	`make launch-explore-gps`	`make launch-nav-gps`	`make launch-corridor`
GPS 校准	`gnss_calibration`	`gps_anchor_localizer`	无（直接用 `/fix`）
GPS→PGO	✓ 通过 `/gnss`	✓ 通过 `/gnss`	✗ PGO GPS 关闭
全局对齐	PGO 内部估计	PGO 内部估计	独立 `gps_global_aligner`
导航目标	手动（RViz）	按名字选择（`goto_name`）	自动分段（route 文件）
启动位置	校准点附近	任意 anchor 附近	固定 Launch Pose

第三部分：扩展开发——实现 GPS 远距离导航

14. 当前瓶颈分析

基于项目文档和实车验证记录，当前 GPS 远距离导航存在以下核心问题：

14.1 GPS 启动锚定误差（2.5-4.75m）

单次启动采样的 GPS 坐标本身就有 ~2.5m 误差。整条路线的起点就已经偏了，后续所有路点都会系统性偏移。

14.2 ENU→map 对齐精度不足

因为 GPS 精度有限 + 采集配对需要足够空间展幅，对齐在运行初期（展幅不足 20m）几乎没有纠错能力。

14.3 FAST-LIO2 odom 发散

在持续 recovery/backup 动作后，FAST-LIO2 的 odom → base_link 出现跳变（单步 2.43m/0.11s），导致 Nav2 控制器混乱。这不是 GPS 的问题，但会影响 GPS 纠偏的效果。

14.4 PGO 稳定性

PGO 节点偶现段错误（exit code -11），导致 map → odom TF 消失，整个导航链断裂。

15. 拓展方向一：GPS 质量自适应融合

15.1 问题

当前 GPS 因子使用固定噪声模型（noise_xy = 2.5m），不区分 GPS 信号好坏。

15.2 解决思路

根据 GPS 消息自带的协方差动态调整因子噪声：

# 概念代码
def adaptive_gps_noise(fix_msg, base_noise_xy=2.5):
"""根据 GPS 协方差动态计算噪声"""
# 从消息中提取水平 sigma
 $i f f i x_{m} s g . p os i t i o n_{c} o v a r ian c e_{t} y p e! = C O V A R I A N C E_{T} Y P E_{U} N K N O W N :$ 
 $r e p or t e d_{s} i g ma = ma t h . s q r t ($ 
max(fix_msg.position_covariance[0],   # σ_east²
fix_msg.position_covariance[4])    # σ_north²
)
else:
 $r e p or t e d_{s} i g ma = ba s e_{n} o i s e_{x} y$ 

# 限幅：不低于 0.3m（防止过度信任），不超过 10m
 $s i g ma = ma x (0.3, min (10.0, r e p or t e d_{s} i g ma))$ 

return sigma

15.3 在项目中的实现位置

修改 pgo_node.cpp 中的 tryAddGPSFactor() 函数：

// 当前: 固定噪声
gtsam::Vector3 noise_vec;
noise_vec << m_node_config.gps_noise_xy,
             m_node_config.gps_noise_xy,
             m_node_config.gps_noise_z;

// 改为: 自适应噪声
double reported_sigma_xy = std::sqrt(
    std::max(gps_msg->position_covariance[0],
             gps_msg->position_covariance[4]));
double clamped_sigma_xy = std::clamp(reported_sigma_xy, 0.3, 10.0);
noise_vec << clamped_sigma_xy, clamped_sigma_xy, m_node_config.gps_noise_z;

15.4 进阶：基于 HDOP 的因子间隔调整

# GPS 信号好的时候多加因子，差的时候少加
if hdop < 1.5:
    factor_interval = 3   # 密集
elif hdop < 3.0:
    factor_interval = 5   # 正常
else:
    factor_interval = 15  # 稀疏，甚至可以跳过

16. 拓展方向二：多点启动标定与地图锚定

16.1 问题

当前启动标定依赖单点 GPS，误差 ~2.5m 直接传导到整条路线。

16.2 解决思路：启动阶段短程移动+多点标定

1. 车辆上电，原地采集 GPS 样本 → 粗略锚定
2. 车辆沿已知路径移动 10-20m → 采集第二组 GPS 样本
3. 用两组 GPS + 对应 map 位姿做刚体配准
4. 多点标定的精度远高于单点

16.3 代码框架

class MultiPointCalibrator:
    """启动阶段多点标定器"""

    def __init__(self, projector: FixedENUProjector, min_spread_m=10.0):
        self.projector = projector
        self.min_spread = min_spread_m
        self.calibration_pairs = []  # [(enu_x, enu_y, map_x, map_y), ...]

    def add_pair(self, lat, lon, map_x, map_y):
        enu_x, enu_y = self.projector.forward(lat, lon)
        self.calibration_pairs.append((enu_x, enu_y, map_x, map_y))

    def can_solve(self) -> bool:
        if len(self.calibration_pairs) < 3:
            return False
        enu_points = [(p[0], p[1]) for p in self.calibration_pairs]
        spread = max_pairwise_distance(enu_points)
        return spread >= self.min_spread

    def solve(self) -> tuple[float, float, float]:
        """返回 (theta, tx, ty)"""
        enu = np.array([(p[0], p[1]) for p in self.calibration_pairs])
        map_ = np.array([(p[2], p[3]) for p in self.calibration_pairs])
        return solve_rigid_2d(enu, map_)

16.4 在项目中如何集成

修改 gps_global_aligner_node.py，在 bootstrap 阶段增加一个"移动标定"步骤
或者新建一个 multi_point_bootstrap_node.py，在 corridor runner 之前执行

17. 拓展方向三：GPS 全局路径规划闭环

17.1 当前状态

项目中有一个实验包 gnss_global_path_planner，支持 GeoJSON 路网 + A* 规划，但还没有接入主运行链。

17.2 完整闭环的缺失环节

已有:
  ✓ GPS 采点脚本 (collect_gps_scene.py)
  ✓ GeoJSON 路网文件
  ✓ A* 全局规划器 (global_path_planner.py)
  ✓ 坐标转换节点 (global2local_tf.py)

缺失:
  ✗ GPS 路径 → Nav2 全局路径的实时对接
  ✗ 规划路径的 costmap 感知（当前 A* 只看 GPS 图，不看障碍物）
  ✗ 失败后的重规划机制

17.3 实现建议

方案 A：GPS 路径作为 Nav2 全局路径

# 新节点: gps_path_to_nav2.py
class GPSPathToNav2(Node):
    """将 GPS 全局路径转换为 Nav2 可执行路径"""

    def __init__(self):
        # 订阅 GPS 全局规划器的路径
        self.sub = self.create_subscription(
            Path, '/path4global', self.on_global_path, 10)
        # 使用 Nav2 的 FollowPath action
        self.follow_path_client = ActionClient(
            self, FollowPath, '/follow_path')

    def on_global_path(self, global_path: Path):
        # 1. GPS 路径 (经纬度) → ENU → map
        map_path = self.convert_to_map_frame(global_path)

        # 2. 沿路径采样点，检查 costmap 中是否有障碍
        checked_path = self.validate_against_costmap(map_path)

        # 3. 发送给 Nav2 执行
        self.follow_path_client.send_goal(checked_path)

方案 B：GPS 只做"远处目标选择"，局部路径完全交给 Nav2

这是更保守但更可靠的方案。GPS 路网只决定"下一个要去的路口节点"，Nav2 负责"怎么去那个节点"：

# 类似当前 gps_waypoint_dispatcher 的思路
while not reached_final_destination:
    next_node = gps_planner.get_next_waypoint()
    map_goal = enu_to_map(latlon_to_enu(next_node.lat, next_node.lon))
    nav2.navigate_to_pose(map_goal)
    wait_until_reached_or_failed()

18. 拓展方向四：RTK/差分 GPS 接入

18.1 RTK 是什么

RTK（Real-Time Kinematic）利用载波相位测量 + 基站差分，可以达到 厘米级精度。

                    RTK 工作原理

  GPS 卫星 ──→ 基站 (已知坐标) ──→ 计算校正量
      │                                │
      │                                │ RTCM 数据流
      │                                │ (通过网络/电台)
      ▼                                ▼
  GPS 卫星 ──→ 移动站 (你的车) + 校正量 = 厘米级定位

18.2 如果接入 RTK，需要改什么

硬件层：

更换为支持 RTK 的 GNSS 模块（如 u-blox F9P、华测 CGI-410）
配置 NTRIP 客户端接收基站校正数据

软件层：

# master_params.yaml 调整
/nmea_navsat_driver:
  ros__parameters:
    port: /dev/rtk_gnss       # 新设备路径
    baud: 115200               # RTK 模块通常用 115200
    frame_id: gps

/pgo:
  pgo_node:
    ros__parameters:
      "gps.noise_xy": 0.05    # RTK Fixed 精度 → 大幅减小噪声
      "gps.noise_z": 0.10
      "gps.factor_interval": 2 # 可以更频繁地加入 GPS 因子

同时需要根据 fix status 动态切换噪声：

if fix.status == STATUS_RTK_FIXED:      # status = 4
    noise_xy = 0.02  # 2cm
elif fix.status == STATUS_RTK_FLOAT:    # status = 5
    noise_xy = 0.5   # 50cm
elif fix.status == STATUS_DGPS:         # status = 1
    noise_xy = 1.0   # 1m
else:
    noise_xy = 2.5   # 2.5m

18.3 NTRIP 客户端接入

# 安装 NTRIP 客户端
sudo apt install str2str  # RTKLIB 工具

# 从千寻位置等服务接收 RTCM 校正数据
str2str -in ntrip://username:password@rtk.ntrip.qxwz.com:8002/RTCM32_GGB \
        -out serial:///dev/rtk_gnss:115200

19. 拓展方向五：GPS 辅助重定位与丢失恢复

19.1 场景

长距离导航中，LiDAR 定位可能因为环境变化（比如进入一个没有特征的空旷区域）而丢失。此时需要 GPS 辅助恢复。

19.2 实现思路

class GPSRelocalizationNode(Node):
    """GPS 辅助重定位节点"""

    def __init__(self):
        self.localization_quality_sub = self.create_subscription(...)
        self.fix_sub = self.create_subscription(NavSatFix, '/fix', ...)

    def check_localization_health(self):
        """监控定位质量"""
        # 指标 1: PGO optimized_odom 发布频率下降
        # 指标 2: map→odom TF 时间戳过旧
        # 指标 3: Nav2 controller 频繁报 patience exceeded
        pass

    def gps_relocalize(self):
        """用 GPS 重新初始化位置"""
        # 1. 获取当前 GPS 位置
        gps_enu = self.get_stable_gps_enu(n_samples=10)

        # 2. 用已有的 ENU→map 对齐转为 map 坐标
        map_x = cos(theta) * gps_enu[0] - sin(theta) * gps_enu[1] + tx
        map_y = sin(theta) * gps_enu[0] + cos(theta) * gps_enu[1] + ty

        # 3. 发布初始位姿给 AMCL 或直接重置 PGO
        initial_pose = PoseWithCovarianceStamped()
        initial_pose.pose.pose.position.x = map_x
        initial_pose.pose.pose.position.y = map_y
        # 朝向用 GPS 航向或上一次已知朝向
        self.initial_pose_pub.publish(initial_pose)

20. 开发者实操 Cookbook

20.1 如何新增一个 GPS 相关节点

第 1 步：在合适的包下创建 Python 节点（遵循项目约定用 ament_python）：

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import NavSatFix

class MyGPSNode(Node):
    def __init__(self):
        super().__init__('my_gps_node')
        self.sub = self.create_subscription(
            NavSatFix, '/fix', self.fix_callback, 10)

    def fix_callback(self, msg: NavSatFix):
        if msg.status.status < 0:
            return  # 无效定位，跳过
        self.get_logger().info(
            f'GPS: lat={msg.latitude:.7f}, lon={msg.longitude:.7f}')

def main(args=None):
    rclpy.init(args=args)
    node = MyGPSNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

第 2 步：在 setup.py 中注册可执行文件

第 3 步：构建和测试

colcon build --packages-select <your_package> --symlink-install --parallel-workers 1
source install/setup.bash
ros2 run <your_package> my_gps_node

20.2 如何使用 FixedENUProjector 做坐标转换

from gps_waypoint_dispatcher.scene_runtime import FixedENUProjector

# 创建投影器（使用项目统一的 ENU 原点）
projector = FixedENUProjector(
    origin_lat=31.274927,
    origin_lon=120.737548,
    origin_alt=0.0
)

# 经纬度 → ENU 坐标 (x=东, y=北, 单位=米)
enu_x, enu_y = projector.forward(31.275000, 120.738000)
print(f"ENU: ({enu_x:.2f}, {enu_y:.2f}) m")

20.3 如何读取当前 TF 位姿

from tf2_ros import Buffer, TransformListener

tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer, self)

# 获取 map→base_link 变换
try:
    t = tf_buffer.lookup_transform('map', 'base_link', rclpy.time.Time())
    x = t.transform.translation.x
    y = t.transform.translation.y
    # 提取 yaw
    q = t.transform.rotation
    yaw = math.atan2(2*(q.w*q.z + q.x*q.y), 1 - 2*(q.y**2 + q.z**2))
except TransformException as e:
    self.get_logger().warn(f'TF lookup failed: {e}')

20.4 如何发送 Nav2 导航目标

from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient

nav_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
nav_client.wait_for_server()

goal = NavigateToPose.Goal()
goal.pose.header.frame_id = 'map'
goal.pose.header.stamp = self.get_clock().now().to_msg()
goal.pose.pose.position.x = target_x
goal.pose.pose.position.y = target_y
goal.pose.pose.orientation.z = math.sin(target_yaw / 2)
goal.pose.pose.orientation.w = math.cos(target_yaw / 2)

future = nav_client.send_goal_async(goal)

20.5 如何调试 GPS 融合效果

# 1. 录制 bag（包含所有 GPS 相关话题）
ros2 bag record /fix /gnss /fastlio2/lio_odom /pgo/optimized_odom /tf /tf_static

# 2. 回放分析
ros2 bag play <bag_directory>

# 3. 在 RViz 中对比
#    - 添加 /pgo/optimized_odom 的 Odometry 显示
#    - 添加 /fix 的 NavSatFix 显示（需要 mapviz 或自定义 marker）

# 4. 数值对比
ros2 topic echo /fix --field latitude --field longitude
ros2 topic echo /pgo/optimized_odom --field pose.pose.position

20.6 关键文件位置速查

功能	文件路径
GPS 驱动配置	`src/bringup/config/master_params.yaml` (`/nmea_navsat_driver` 段)
PGO GPS 因子参数	`src/bringup/config/master_params.yaml` (`/pgo` 段 `gps.*`)
GPS 校准节点	`src/sensor_drivers/gnss/gnss_calibration/gnss_calibration/gnss_calibration_node.py`
GPS 校准点	`src/sensor_drivers/gnss/gnss_calibration/config/calibration_points.yaml`
PGO C++ 核心	`src/perception/pgo_gps_fusion/src/pgo_node.cpp`
GTSAM 因子图	`src/perception/pgo_gps_fusion/src/pgos/simple_pgo.cpp`
独立 Aligner	`src/navigation/gps_waypoint_dispatcher/gps_waypoint_dispatcher/gps_global_aligner_node.py`
Route Runner	`src/navigation/gps_waypoint_dispatcher/gps_waypoint_dispatcher/gps_route_runner_node.py`
坐标转换工具	`src/navigation/gps_waypoint_dispatcher/gps_waypoint_dispatcher/scene_runtime.py`
GPS 采场景脚本	`scripts/collect_gps_scene.py`
GPS 采路线脚本	`scripts/collect_gps_route.py`
Explore GPS Launch	`src/bringup/launch/system_explore_gps.launch.py`
Corridor Launch	`src/bringup/launch/system_gps_corridor.launch.py`
Nav GPS Launch	`src/bringup/launch/system_nav_gps.launch.py`
Nav2 GPS 参数	`src/bringup/config/nav2_gps.yaml`
运行时 GPS 数据	`~/XJTLU-autonomous-vehicle/runtime-data/gnss/`

总结：要做好 GPS 远距离导航，核心是理解三件事：

坐标转换链（WGS84 → ENU → map）和每一步的误差传播

因子图融合的噪声模型如何平衡 GPS 与 LiDAR 的信任度

ENU→map 对齐是整个系统的薄弱环节，改进它（多点标定、自适应更新、RTK 接入）是提升精度的最直接路径