4. GPS在任务规划中的应用：数据采集、坐标转换与信号丢失处理

GPS这东西，说白了就是任务规划的眼睛。没有它，你的机器人连自己在哪都不知道，还谈什么规划？我最早接触GPS是在一个户外巡检项目上，当时觉得“不就是收个经纬度嘛”，结果踩了不少坑。今天我把这些经验掰开揉碎，跟你聊聊。

4.1 GPS数据采集与解析

GPS模块输出的原始数据，通常是NMEA-0183协议格式。你打开串口助手，看到的是一堆以$开头的字符串。别慌，核心就几条语句。

NMEA语句	含义	关键字段
$GPGGA	全球定位系统固定数据	时间、纬度、经度、质量因子、海拔
$GPRMC	推荐最小定位信息	时间、日期、速度、航向、定位状态
$GPGSA	精度因子与卫星编号	PDOP、HDOP、VDOP
$GPGSV	可见卫星信息	卫星编号、仰角、方位角、信噪比

我个人习惯优先解析$GPGGA。为什么？因为它直接告诉你定位质量——0表示无效，1表示单点定位，2表示差分定位。这个质量因子，我建议你每次都要检查。

来看一段解析代码。这是我在一个农业机器人项目里用过的，精简过，但骨架没变。

def parse_gpgga(sentence):
    if not sentence.startswith('$GPGGA'):
        return None
    fields = sentence.split(',')
    if len(fields) < 15:
        return None
    quality = int(fields[6])
    if quality == 0:
        return None  # 无效定位，直接扔掉
    lat = float(fields[2][:2]) + float(fields[2][2:]) / 60.0
    lon = float(fields[4][:3]) + float(fields[4][3:]) / 60.0
    if fields[3] == 'S':
        lat = -lat
    if fields[5] == 'W':
        lon = -lon
    altitude = float(fields[9]) if fields[9] else 0.0
    return {'lat': lat, 'lon': lon, 'alt': altitude, 'quality': quality}

嗯，这里要注意：NMEA里的纬度格式是ddmm.mmmm，经度是dddmm.mmmm。直接当浮点数用？那你就错了。我见过有人把4807.038当成48.07038度，结果机器人跑偏了200多公里。正确做法是：度取整数部分，分除以60，再加回去。

4.2 航点规划中的GPS坐标转换

拿到经纬度之后，你面临一个现实问题：任务规划需要的是平面坐标，比如米为单位。而GPS给的是球面坐标。这就涉及到坐标转换。

最常用的方案是UTM投影。UTM把地球分成60个带，每个带6度。在带内，坐标是米制的，误差在0.1%以内。对于大多数机器人任务规划来说，够用了。

我推荐用pyproj库来做转换。别自己写投影公式，容易出错。

import pyproj

# 定义WGS84和UTM投影
wgs84 = pyproj.Proj(proj='latlong', datum='WGS84')
utm_zone = pyproj.Proj(proj='utm', zone=50, datum='WGS84')  # 北京地区用zone 50

# 转换：经纬度 -> UTM
lon, lat = 116.397, 39.908  # 天安门坐标
x, y = pyproj.transform(wgs84, utm_zone, lon, lat)
print(f"UTM坐标: {x:.2f}, {y:.2f} 米")

但这里有个坑：UTM带号怎么确定？公式很简单：zone = int((lon + 180) / 6) + 1。我建议你在代码里动态计算，不要硬编码。否则机器人跨带作业时，坐标会跳变。

还有一个常见场景：航点规划时，你需要把GPS坐标转换成以起点为原点的局部坐标系。这样规划出来的路径才是直观的“向东10米，向北5米”。

def gps_to_local(ref_lat, ref_lon, target_lat, target_lon):
    """
    将GPS坐标转换为以参考点为原点的局部坐标（米）
    使用等距圆柱投影近似，适用于小范围（<10km）
    """
    R = 6371000  # 地球半径
    d_lat = (target_lat - ref_lat) * math.pi / 180.0
    d_lon = (target_lon - ref_lon) * math.pi / 180.0
    x = d_lon * R * math.cos(ref_lat * math.pi / 180.0)
    y = d_lat * R
    return x, y

这个近似方法在10公里范围内误差可以忽略。你想想看，大多数机器人任务规划，范围也就几百米到几公里，完全够用。

4.3 GPS信号丢失时的处理策略

这是最让人头疼的问题。GPS信号说丢就丢——进隧道、过立交桥、树荫太密、高楼遮挡。我做过一个城市配送项目，信号丢失率高达15%。不处理？机器人直接原地懵圈。

我的策略分三级：

1. 短期丢失（<5秒）：用航位推算（Dead Reckoning）顶住。利用上一时刻的速度和航向，推算当前位置。
2. 中期丢失（5-30秒）：结合IMU和轮式里程计，做多传感器融合。这时候卡尔曼滤波就派上用场了。
3. 长期丢失（>30秒）：触发安全策略——减速、停车、或者沿原路返回直到重新捕获信号。

航位推算的代码实现其实不复杂：

def dead_reckoning(last_pos, velocity, heading, dt):
    """
    航位推算：根据上一位置、速度、航向和时间间隔推算当前位置
    last_pos: (x, y) 单位米
    velocity: 速度 单位米/秒
    heading: 航向 单位弧度（0=正东，逆时针为正）
    dt: 时间间隔 单位秒
    """
    dx = velocity * math.cos(heading) * dt
    dy = velocity * math.sin(heading) * dt
    current_pos = (last_pos[0] + dx, last_pos[1] + dy)
    return current_pos

但航位推算有个致命问题：误差会累积。你想想看，每推一步都有微小误差，10秒后可能偏了1米，30秒后可能偏了5米。所以它只能作为短期应急。

对于中期丢失，我建议用扩展卡尔曼滤波（EKF）融合GPS、IMU和里程计。这里给一个简化的融合框架：

# 伪代码：EKF融合框架
def ekf_predict(state, covariance, control_input, dt):
    # 状态预测：根据运动模型
    state_pred = motion_model(state, control_input, dt)
    # 协方差预测
    covariance_pred = A @ covariance @ A.T + Q
    return state_pred, covariance_pred

def ekf_update(state_pred, covariance_pred, measurement, measurement_type):
    if measurement_type == 'gps':
        # GPS更新：直接修正位置
        H = measurement_matrix_gps()
        R = measurement_noise_gps()
    elif measurement_type == 'imu':
        # IMU更新：修正姿态和角速度
        H = measurement_matrix_imu()
        R = measurement_noise_imu()
    # 卡尔曼增益计算
    K = covariance_pred @ H.T @ inv(H @ covariance_pred @ H.T + R)
    state_updated = state_pred + K @ (measurement - H @ state_pred)
    covariance_updated = (I - K @ H) @ covariance_pred
    return state_updated, covariance_updated

嗯，这里要注意：GPS信号恢复后，不要立刻把融合结果跳变到GPS值。我见过有人直接赋值，结果机器人猛地一抖，差点翻车。正确做法是：用渐近融合，比如alpha = 0.1，让融合结果慢慢靠拢GPS。

最后，我建议你建立一个“定位健康度”指标。它综合了卫星颗数、HDOP值、定位质量因子、以及最近一次信号丢失的时间间隔。当健康度低于阈值时，任务规划器应该主动降低速度、缩小转弯半径、或者请求人工干预。

def compute_health_score(satellites, hdop, quality, time_since_last_loss):
    score = 0
    if satellites >= 8:
        score += 30
    elif satellites >= 5:
        score += 20
    elif satellites >= 3:
        score += 10
    if hdop < 1.0:
        score += 40
    elif hdop < 2.0:
        score += 25
    elif hdop < 5.0:
        score += 10
    if quality >= 2:
        score += 20
    elif quality == 1:
        score += 10
    # 时间惩罚：离上次信号丢失越近，分数越低
    score -= max(0, 10 - time_since_last_loss) * 2
    return max(0, min(100, score))

这个分数你可以直接喂给任务规划器。分数低于40，我建议强制停车检查。别觉得夸张——机器人定位丢了还硬跑，撞了东西修起来更贵。