02-03-05 GPS/北斗导航系统

1. 核心概念

1.1 GNSS系统概述

GNSS（Global Navigation Satellite System，全球导航卫星系统）通过卫星信号提供全球定位、导航、授时服务。

主要系统：

• GPS：美国，31颗卫星，全球覆盖
• 北斗（BDS）：中国，55颗卫星，全球覆盖
• GLONASS：俄罗斯，24颗卫星
• Galileo：欧盟，30颗卫星

定位原理：

三球交会定位：
- 需要至少4颗卫星
- 3颗卫星定位（X, Y, Z）
- 1颗卫星校时（时间误差）

距离测量：
距离 = 光速 × (接收时间 - 发射时间)

精度：
- 单点定位：5-10m
- 差分定位（DGPS）：1-3m
- RTK：厘米级

1.2 GPS数据格式

NMEA-0183协议：

$GPGGA：GPS定位信息
$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47

字段说明：
- 123519：UTC时间 12:35:19
- 4807.038,N：纬度 48°07.038' 北
- 01131.000,E：经度 11°31.000' 东
- 1：定位质量（0=无效，1=单点，2=差分）
- 08：使用卫星数
- 0.9：HDOP水平精度因子
- 545.4,M：海拔高度545.4米
- *47：校验和

$GPRMC：推荐最小定位信息
$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A

- A：数据有效
- 022.4：地面速率（节）
- 084.4：航向角
- 230394：日期 23/03/1994

1.3 坐标系统

WGS-84坐标系：

地心坐标系（ECEF）：
- 原点：地球质心
- X轴：赤道平面，指向本初子午线
- Y轴：赤道平面，X轴逆时针90°
- Z轴：指向北极

大地坐标系（LLA）：
- 纬度（Latitude）：-90° ~ +90°
- 经度（Longitude）：-180° ~ +180°
- 高度（Altitude）：海拔高度

转换公式：
X = (N + h) × cos(φ) × cos(λ)
Y = (N + h) × cos(φ) × sin(λ)
Z = (N×(1-e²) + h) × sin(φ)

其中：
- N = a / √(1 - e²×sin²(φ))
- a = 6378137m（长半轴）
- e² = 0.00669438（偏心率平方）

2. GPS模块集成

2.1 硬件连接

典型GPS模块（u-blox NEO-M8N）：

连接方式：
GPS模块 ←→ 飞控
VCC(3.3V/5V) ←→ VCC
GND         ←→ GND
TX          ←→ RX（UART）
RX          ←→ TX（UART）

波特率：9600/38400/115200 bps
数据格式：8N1（8数据位，无校验，1停止位）

2.2 NMEA数据解析

Python实现：

from dataclasses import dataclass
from typing import Optional

@dataclass
class GPSData:
    """GPS数据结构"""
    latitude: float = 0.0       # 纬度
    longitude: float = 0.0      # 经度
    altitude: float = 0.0       # 海拔（米）
    speed: float = 0.0          # 速度（m/s）
    course: float = 0.0         # 航向（度）
    satellites: int = 0         # 卫星数
    hdop: float = 99.9          # 水平精度因子
    fix_quality: int = 0        # 定位质量
    timestamp: str = ""         # UTC时间

class GPSParser:
    """GPS NMEA解析器"""

    def __init__(self, port: str = '/dev/ttyUSB0', baudrate: int = 9600):
        self.serial = serial.Serial(port, baudrate, timeout=1)
        self.gps_data = GPSData()

    def parse_nmea_line(self, line: str):
        """解析NMEA语句"""
        if not line.startswith('$'):
            return

        # 校验和验证
        if not self._verify_checksum(line):
            return

        # 解析不同类型的NMEA语句
        if line.startswith('$GPGGA') or line.startswith('$GNGGA'):
            self._parse_gga(line)
        elif line.startswith('$GPRMC') or line.startswith('$GNRMC'):
            self._parse_rmc(line)
        elif line.startswith('$GPGSA') or line.startswith('$GNGSA'):
            self._parse_gsa(line)

    def _parse_gga(self, line: str):
        """解析GGA语句（定位信息）"""
        fields = line.split(',')
        if len(fields) < 15:
            return

        try:
            # UTC时间
            self.gps_data.timestamp = fields[1]

            # 纬度
            if fields[2] and fields[3]:
                lat_deg = int(fields[2][:2])
                lat_min = float(fields[2][2:])
                self.gps_data.latitude = lat_deg + lat_min / 60.0
                if fields[3] == 'S':
                    self.gps_data.latitude = -self.gps_data.latitude

            # 经度
            if fields[4] and fields[5]:
                lon_deg = int(fields[4][:3])
                lon_min = float(fields[4][3:])
                self.gps_data.longitude = lon_deg + lon_min / 60.0
                if fields[5] == 'W':
                    self.gps_data.longitude = -self.gps_data.longitude

            # 定位质量
            self.gps_data.fix_quality = int(fields[6]) if fields[6] else 0

            # 卫星数
            self.gps_data.satellites = int(fields[7]) if fields[7] else 0

            # HDOP
            self.gps_data.hdop = float(fields[8]) if fields[8] else 99.9

            # 海拔
            self.gps_data.altitude = float(fields[9]) if fields[9] else 0.0

        except (ValueError, IndexError):
            pass

    def _parse_rmc(self, line: str):
        """解析RMC语句（推荐最小定位）"""
        fields = line.split(',')
        if len(fields) < 12:
            return

        try:
            # 地面速率（节 → m/s）
            if fields[7]:
                self.gps_data.speed = float(fields[7]) * 0.514444

            # 航向角
            if fields[8]:
                self.gps_data.course = float(fields[8])

        except (ValueError, IndexError):
            pass

    def _verify_checksum(self, line: str) -> bool:
        """验证NMEA校验和"""
        if '*' not in line:
            return False

        data, checksum = line.rsplit('*', 1)
        data = data[1:]  # 移除$符号

        # 计算校验和
        calc_checksum = 0
        for char in data:
            calc_checksum ^= ord(char)

        try:
            return calc_checksum == int(checksum[:2], 16)
        except ValueError:
            return False

    def read(self) -> GPSData:
        """读取GPS数据"""
        try:
            line = self.serial.readline().decode('ascii', errors='ignore').strip()
            self.parse_nmea_line(line)
        except Exception as e:
            print(f"GPS读取错误: {e}")

        return self.gps_data

    def get_position(self) -> tuple:
        """获取当前位置"""
        return (self.gps_data.latitude,
                self.gps_data.longitude,
                self.gps_data.altitude)

    def is_fixed(self) -> bool:
        """检查是否定位"""
        return self.gps_data.fix_quality > 0 and self.gps_data.satellites >= 4

# 使用示例
if __name__ == '__main__':
    gps = GPSParser('/dev/ttyUSB0', 9600)

    print("等待GPS定位...")

    while True:
        data = gps.read()

        if gps.is_fixed():
            print(f"位置: {data.latitude:.6f}°N, {data.longitude:.6f}°E")
            print(f"海拔: {data.altitude:.1f}m")
            print(f"速度: {data.speed:.1f}m/s")
            print(f"航向: {data.course:.1f}°")
            print(f"卫星: {data.satellites}, HDOP: {data.hdop}")
            print("-" * 50)

        time.sleep(1)

2.3 STM32集成（C实现）

// gps.h - GPS模块驱动
#include <stdint.h>
#include <stdbool.h>

typedef struct {
    double latitude;       // 纬度
    double longitude;      // 经度
    float altitude;        // 海拔（米）
    float speed;           // 速度（m/s）
    float course;          // 航向（度）
    uint8_t satellites;    // 卫星数
    float hdop;            // 水平精度因子
    uint8_t fix_quality;   // 定位质量
    char timestamp[12];    // UTC时间
} gps_data_t;

// GPS解析器
typedef struct {
    gps_data_t data;
    char rx_buffer[256];
    uint16_t rx_index;
} gps_parser_t;

// 初始化GPS
void gps_init(gps_parser_t* gps);

// 解析NMEA语句
void gps_parse_nmea(gps_parser_t* gps, const char* line);

// 获取GPS数据
gps_data_t* gps_get_data(gps_parser_t* gps);

// 检查是否定位
bool gps_is_fixed(gps_parser_t* gps);

// gps.c
#include "gps.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

void gps_init(gps_parser_t* gps) {
    memset(gps, 0, sizeof(gps_parser_t));
    gps->data.hdop = 99.9f;
}

static bool verify_checksum(const char* line) {
    const char* asterisk = strrchr(line, '*');
    if (!asterisk) return false;

    uint8_t checksum = 0;
    for (const char* p = line + 1; p < asterisk; p++) {
        checksum ^= *p;
    }

    uint8_t expected = (uint8_t)strtol(asterisk + 1, NULL, 16);
    return checksum == expected;
}

static double parse_coord(const char* str, char dir) {
    if (!str || !*str) return 0.0;

    double deg_part = atof(str);
    int degrees = (int)(deg_part / 100);
    double minutes = deg_part - (degrees * 100);
    double decimal = degrees + minutes / 60.0;

    if (dir == 'S' || dir == 'W') {
        decimal = -decimal;
    }

    return decimal;
}

static void parse_gga(gps_parser_t* gps, char* line) {
    char* token = strtok(line, ",");
    int field = 0;

    while (token != NULL && field < 15) {
        switch (field) {
            case 1:  // UTC时间
                strncpy(gps->data.timestamp, token, sizeof(gps->data.timestamp) - 1);
                break;

            case 2:  // 纬度
                if (token[0]) {
                    char* dir_token = strtok(NULL, ",");
                    gps->data.latitude = parse_coord(token, dir_token ? dir_token[0] : 'N');
                    field++;
                }
                break;

            case 4:  // 经度
                if (token[0]) {
                    char* dir_token = strtok(NULL, ",");
                    gps->data.longitude = parse_coord(token, dir_token ? dir_token[0] : 'E');
                    field++;
                }
                break;

            case 6:  // 定位质量
                gps->data.fix_quality = atoi(token);
                break;

            case 7:  // 卫星数
                gps->data.satellites = atoi(token);
                break;

            case 8:  // HDOP
                gps->data.hdop = atof(token);
                break;

            case 9:  // 海拔
                gps->data.altitude = atof(token);
                break;
        }

        token = strtok(NULL, ",");
        field++;
    }
}

void gps_parse_nmea(gps_parser_t* gps, const char* line) {
    if (!line || line[0] != '$') return;
    if (!verify_checksum(line)) return;

    char buffer[256];
    strncpy(buffer, line, sizeof(buffer) - 1);

    if (strncmp(line, "$GPGGA", 6) == 0 || strncmp(line, "$GNGGA", 6) == 0) {
        parse_gga(gps, buffer);
    }
    // 可添加其他NMEA语句解析...
}

gps_data_t* gps_get_data(gps_parser_t* gps) {
    return &gps->data;
}

bool gps_is_fixed(gps_parser_t* gps) {
    return gps->data.fix_quality > 0 && gps->data.satellites >= 4;
}

// 主程序示例
int main(void) {
    HAL_Init();
    SystemClock_Config();

    // 初始化UART（用于GPS）
    MX_USART1_UART_Init();

    gps_parser_t gps;
    gps_init(&gps);

    printf("等待GPS定位...\r\n");

    while (1) {
        // 接收NMEA数据（假设已通过UART中断接收到rx_buffer）
        if (uart_line_available()) {
            char line[128];
            uart_read_line(line, sizeof(line));
            gps_parse_nmea(&gps, line);

            if (gps_is_fixed(&gps)) {
                printf("位置: %.6f°N, %.6f°E\r\n",
                       gps.data.latitude, gps.data.longitude);
                printf("海拔: %.1fm, 卫星: %d\r\n",
                       gps.data.altitude, gps.data.satellites);
            }
        }

        HAL_Delay(1000);
    }
}

3. 位置融合算法

3.1 GPS/IMU融合（EKF）

状态向量：

x = [位置x, 位置y, 位置z, 速度vx, 速度vy, 速度vz]^T

预测（IMU）：
x(k|k-1) = F×x(k-1|k-1) + B×u(k)

更新（GPS）：
x(k|k) = x(k|k-1) + K×(z - H×x(k|k-1))

代码实现：

class GPSIMUFusion:
    """GPS/IMU融合（简化EKF）"""

    def __init__(self):
        # 状态向量：[x, y, z, vx, vy, vz]
        self.x = np.zeros(6)

        # 误差协方差矩阵
        self.P = np.eye(6) * 10

        # 过程噪声协方差
        self.Q = np.diag([0.1, 0.1, 0.1, 0.5, 0.5, 0.5])

        # GPS测量噪声协方差（5m精度）
        self.R_gps = np.diag([25, 25, 25])

    def predict(self, acc, dt):
        """预测步骤（IMU）"""
        # 状态转移矩阵
        F = np.eye(6)
        F[0:3, 3:6] = np.eye(3) * dt

        # 控制输入矩阵
        B = np.zeros((6, 3))
        B[3:6, :] = np.eye(3) * dt

        # 状态预测
        self.x = F @ self.x + B @ acc

        # 协方差预测
        self.P = F @ self.P @ F.T + self.Q

    def update_gps(self, gps_position):
        """更新步骤（GPS）"""
        # 测量矩阵（只测量位置）
        H = np.zeros((3, 6))
        H[0:3, 0:3] = np.eye(3)

        # 卡尔曼增益
        S = H @ self.P @ H.T + self.R_gps
        K = self.P @ H.T @ np.linalg.inv(S)

        # 状态更新
        y = gps_position - H @ self.x
        self.x = self.x + K @ y

        # 协方差更新
        self.P = (np.eye(6) - K @ H) @ self.P

    def get_position(self):
        """获取融合后的位置"""
        return self.x[0:3]

    def get_velocity(self):
        """获取融合后的速度"""
        return self.x[3:6]

# 使用示例
fusion = GPSIMUFusion()

# 模拟数据融合
for i in range(100):
    # IMU加速度（m/s²）
    acc = np.array([0.1, 0.05, 0.0])

    # 预测（100Hz）
    fusion.predict(acc, dt=0.01)

    # GPS更新（1Hz）
    if i % 100 == 0:
        gps_pos = np.array([10.0 + np.random.randn()*5,
                           20.0 + np.random.randn()*5,
                           100.0])
        fusion.update_gps(gps_pos)

    # 获取融合结果
    pos = fusion.get_position()
    vel = fusion.get_velocity()

    print(f"位置: {pos}, 速度: {vel}")

4. RTK高精度定位

4.1 RTK原理

差分定位：

基站（Base）：
- 已知精确坐标
- 计算卫星误差
- 发送差分数据

移动站（Rover）：
- 接收差分数据
- 修正自身误差
- 达到厘米级精度

精度提升：
- 单点定位：5-10m
- DGPS：1-3m
- RTK（固定解）：1-2cm

4.2 RTCM数据解析

RTCM3协议：

class RTCMParser:
    """RTCM3数据解析"""

    def parse_rtcm3(self, data: bytes):
        """解析RTCM3消息"""
        if len(data) < 3:
            return None

        # 同步字节（0xD3）
        if data[0] != 0xD3:
            return None

        # 消息长度
        msg_len = ((data[1] & 0x03) << 8) | data[2]

        # 消息类型
        msg_type = (data[3] << 4) | (data[4] >> 4)

        if msg_type == 1005:
            return self._parse_1005(data[3:])  # 基站坐标
        elif msg_type == 1077:
            return self._parse_1077(data[3:])  # GPS MSM7观测值
        elif msg_type == 1087:
            return self._parse_1087(data[3:])  # GLONASS MSM7

        return None

    def _parse_1005(self, data: bytes):
        """解析基站坐标"""
        # 简化示例
        station_id = int.from_bytes(data[0:2], 'big') >> 4
        return {'type': 1005, 'station_id': station_id}

5. 行业应用

案例1：精准农业

DJI Agras农业无人机：

• GPS+RTK双模
• 定位精度：±2cm
• 喷洒精度：误差<10cm
• 应用：精准施肥、植保

案例2：无人机送货

亚马逊Prime Air：

• GPS+视觉融合
• 定位精度：±0.5m
• 降落精度：±10cm（视觉辅助）

6. 故障诊断

6.1 常见问题

现象	原因	解决方法
无法定位	卫星信号弱	室外空旷环境
精度差	HDOP过高	等待更多卫星
飘移	多径效应	远离建筑物
高度跳变	气压干扰	GPS/气压融合

6.2 诊断代码

def gps_diagnose(gps_data: GPSData):
    """GPS健康诊断"""
    issues = []

    if gps_data.fix_quality == 0:
        issues.append("无GPS定位")
    elif gps_data.satellites < 6:
        issues.append(f"卫星数不足({gps_data.satellites}<6)")

    if gps_data.hdop > 2.0:
        issues.append(f"HDOP过高({gps_data.hdop:.1f}>2.0)")

    if gps_data.altitude < -100 or gps_data.altitude > 10000:
        issues.append(f"海拔异常({gps_data.altitude}m)")

    if issues:
        print("GPS问题：")
        for issue in issues:
            print(f"  - {issue}")
        return False

    print("GPS状态正常")
    return True