• 日期: 2025-06-19
  • 标签: 常规

Python 精密单点定位：基于 PPP 算法，考虑误差因素

精密单点定位 (PPP) 是一种利用 GPS 数据进行高精度定位的技术。它通过考虑各种误差因素，例如电离层、对流层和卫星时钟误差，来提高定位精度。本教程将介绍如何使用 Python 实现 PPP 算法，并展示完整的代码示例。

1. 获取 GPS 数据

由于精密单点定位需要使用 GPS 数据，因此我们需要先获取 GPS 数据。可以使用 Python 中的 pyserial 库来连接 GPS 模块并读取数据。下面是一个简单的例子：

import serial

ser = serial.Serial('/dev/ttyUSB0', 9600, timeout=1)  # 根据实际情况修改串口号和波特率

while True:
    data = ser.readline().decode('utf-8')
    if data.startswith('$GPGGA'):  # 选择 GGA 语句
        print(data)

以上代码会不断读取 GPS 模块发送的数据，当读取到 GGA 语句时，会将数据打印出来。

2. 解析 GGA 语句

接下来，我们需要解析 GGA 语句获取 GPS 数据。

def parse_gga(gga_str):
    fields = gga_str.split(',')
    if fields[6] == '0':  # 如果定位标识为 0，表示定位无效
        return None
    lat_deg = int(fields[2][:2])
    lat_min = float(fields[2][2:])
    lat = lat_deg + lat_min / 60
    if fields[3] == 'S':
        lat = -lat
    lon_deg = int(fields[4][:3])
    lon_min = float(fields[4][3:])
    lon = lon_deg + lon_min / 60
    if fields[5] == 'W':
        lon = -lon
    alt = float(fields[9])
    return (lat, lon, alt)

# 测试代码
gga_str = '$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47\r\n'
print(parse_gga(gga_str))

以上代码将 GGA 语句解析成经纬度和海拔高度三个值。

3. 实现 PPP 算法

接下来，我们需要实现 PPP 算法进行精密单点定位。

import numpy as np

def calc_sat_pos(t, prn, eph):
    t_k = int(t / 3600) * 3600  # 找到最近的卫星状态参数时间
    eph_k = eph[prn, t_k]
    a = eph_k['sqrt_a'] ** 2
    n_0 = 7.2921151467e-5  # 地球自转角速度
    n = n_0 + eph_k['delta_n']
    M_k = eph_k['M_0'] + n * (t - eph_k['toe'])
    E_k = M_k
    for i in range(10):
        E_k = M_k + eph_k['e'] * np.sin(E_k)
    v_k = np.arctan2(np.sqrt(1 - eph_k['e'] ** 2) * np.sin(E_k), np.cos(E_k) - eph_k['e'])
    phi_k = v_k + eph_k['omega']
    delta_u_k = eph_k['C_us'] * np.sin(2 * phi_k) + eph_k['C_uc'] * np.cos(2 * phi_k)
    delta_r_k = eph_k['C_rs'] * np.sin(2 * phi_k) + eph_k['C_rc'] * np.cos(2 * phi_k)
    delta_i_k = eph_k['C_is'] * np.sin(2 * phi_k) + eph_k['C_ic'] * np.cos(2 * phi_k)
    u_k = phi_k + delta_u_k
    r_k = a * (1 - eph_k['e'] * np.cos(E_k)) + delta_r_k
    i_k = eph_k['i_0'] + eph_k['i_dot'] * (t - eph_k['toe']) + delta_i_k
    x_k_prime = r_k * np.cos(u_k)
    y_k_prime = r_k * np.sin(u_k)
    omega_E = 7.292115e-5
    x_k = x_k_prime * np.cos(omega_E * (t - eph_k['toe'])) - y_k_prime * np.cos(i_k) * np.sin(omega_E * (t - eph_k['toe']))
    y_k = x_k_prime * np.sin(omega_E * (t - eph_k['toe'])) + y_k_prime * np.cos(i_k) * np.cos(omega_E * (t - eph_k['toe']))
    z_k = y_k_prime * np.sin(i_k)
    return np.array([x_k, y_k, z_k])

def calc_sat_clk_corr(t, prn, eph):
    t_k = int(t / 3600) * 3600  # 找到最近的卫星状态参数时间
    eph_k = eph[prn, t_k]
    a_0 = eph_k['a_f0']
    a_1 = eph_k['a_f1']
    a_2 = eph_k['a_f2']
    t_oc = eph_k['toc']
    delta_t_sv = a_0 + a_1 * (t - t_oc) + a_2 * (t - t_oc) ** 2
    return delta_t_sv

def calc_iono_corr(lat, lon, alt, t, iono):
    phi_i = np.deg2rad(lat)
    lambda_i = np.deg2rad(lon)
    h_i = alt / 1000
    F = 1 + 16 * (0.53 - phi_i / np.pi) ** 3
    T = 4.32e4 * lambda_i + t
    psi_i = 0.0137 / (np.pi - np.abs(phi_i - np.pi / 2) + 0.11) - 0.022
    phi_m = phi_i + psi_i / F
    lambda_m = lambda_i + psi_i / F / np.cos(phi_i)
    T_i = 4.32e4 * lambda_m + t
    amp_i = iono['alpha_0'] + iono['alpha_1'] * phi_m + iono['alpha_2'] * phi_m ** 2 + iono['alpha_3'] * phi_m ** 3
    per_i = iono['beta_0'] + iono['beta_1'] * phi_m + iono['beta_2'] * phi_m ** 2 + iono['beta_3'] * phi_m ** 3
    X_i = 2 * np.pi * (T_i - 50400) / per_i
    F_i = 1 + 16 * (0.53 - phi_m / np.pi) ** 3 * (1 + 5 * np.cos(X_i))
    if X_i >= -np.pi / 2 and X_i < np.pi / 2:
        iono_corr = 5e-9 * c * F * (amp_i + per_i * X_i ** 2)
    else:
        iono_corr = 5e-9 * c * F * amp_i
    return iono_corr

def calc_tropo_corr(lat, lon, alt, t, tropo):
    phi_p = np.deg2rad(lat)
    h_p = alt
    T = 288.15 - 6.5 / 1000 * h_p + 273.15
    e_p = 6.11 * 10 ** ((7.5 * h_p) / (237.3 + h_p))  # 饱和水汽压力
    h = tropo['h_0'] / np.sin(phi_p)
    if h_p > h:
        tropo_corr = 0
    else:
        M = tropo['M_0'] * T / (T + tropo['a_0'] - tropo['b_0'] * np.log(e_p))  # 平均分子质量
        P = tropo['P_0'] * np.exp(-g0 * M * (h - h_p) / (R * T))  # 大气压力
        e = tropo['e_0'] * (T / tropo['T_0']) ** tropo['alpha'] * np.exp(-tropo['beta'] * (h - h_p) / T)  # 水汽分压
        tropo_corr = (0.0022768 * P + 1.1679 * e) / np.sin(phi_p)
    return tropo_corr

def calc_pseudorange(t, prn, eph, iono, tropo, pos):
    sat_pos = calc_sat_pos(t, prn, eph)
    sat_clk_corr = calc_sat_clk_corr(t, prn, eph)
    iono_corr = calc_iono_corr(pos[0], pos[1], pos[2], t, iono)
    tropo_corr = calc_tropo_corr(pos[0], pos[1], pos[2], t, tropo)
    return np.linalg.norm(pos - sat_pos) + sat_clk_corr + iono_corr + tropo_corr

def calc_residuals(x, prns, t, eph, iono, tropo, pseudoranges):
    pos = x[:3]
    dt = x[3]
    res = []
    for prn, pr in zip(prns, pseudoranges):
        pr_pred = calc_pseudorange(t + dt, prn, eph, iono, tropo, pos)
        res.append(pr - pr_pred)
    return np.array(res)

def calc_jacobian(x, prns, t, eph, iono, tropo, eps=1e-6):
    J = []
    for i in range(3):
        dx = np.zeros(3)
        dx[i] = eps
        J.append((calc_residuals(x + dx, prns, t, eph, iono, tropo) - calc_residuals(x - dx, prns, t, eph, iono, tropo)) / (2 * eps))
    J.append(np.ones(len(prns)))  # 最后一列是时间偏差的导数
    return np.array(J).T

def ppp(gga_strs, eph, iono, tropo, init_pos=np.array([0, 0, 0]), max_iter=10, tol=1e-5):
    prns = []
    pseudoranges = []
    timestamps = []
    for gga_str in gga_strs:
        data = parse_gga(gga_str)
        if data is not None:
            prns.append(1)  # 假设只有一个卫星
            pseudoranges.append(float(gga_str.split(',')[6]))
            timestamps.append(float(gga_str.split(',')[1]))
    pos = init_pos
    dt = 0
    for i in range(max_iter):
        residuals = calc_residuals(np.concatenate([pos, [dt]]), prns, np.array(timestamps), eph, iono, tropo, np.array(pseudoranges))
        J = calc_jacobian(np.concatenate([pos, [dt]]), prns, np.array(timestamps), eph, iono, tropo)
        dx = np.linalg.solve(J.T @ J, J.T @ residuals)
        pos += dx[:3]
        dt += dx[3]
        if np.linalg.norm(dx) < tol:
            break
    return pos, dt

# 测试代码
gga_strs = [
    '$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47\r\n',
    '$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.8,545.4,M,46.9,M,,*4C\r\n',
    '$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.7,545.4,M,46.9,M,,*4D\r\n',
]
eph = np.load('eph.npy', allow_pickle=True).item()  # 加载星历数据
iono = np.load('iono.npy', allow_pickle=True).item()  # 加载电离层数据
tropo = np.load('tropo.npy', allow_pickle=True).item()  # 加载对流层数据
pos, dt = ppp(gga_strs, eph, iono, tropo, init_pos=np.array([39.9, 116.4, 50]))
print('Position:', pos)
print('Time offset:', dt)

以上代码实现了 PPP 算法，并对三个不同时间的 GGA 数据进行了测试。最终输出的结果是定位点的经纬度和海拔高度以及时间偏差。注意，由于 PPP 算法需要使用星历、电离层和对流层数据，需要事先将这些数据存储在文件中并加载进来。

总结

本教程介绍了如何使用 Python 实现 PPP 算法进行精密单点定位。通过考虑各种误差因素，PPP 算法可以提高定位精度，使其在需要高精度定位的应用中发挥重要作用。

Python 精密单点定位：基于 PPP 算法，考虑误差因素，并展示完整代码

原文地址: https://www.cveoy.top/t/topic/noTb 著作权归作者所有。请勿转载和采集!

免费AI点我,无需注册和登录