数据预处理：时间戳同步、坐标系转换、异常值剔除、数据插值与降采样

数据预处理这步，说实话，是组合导航里最容易被低估的环节。很多人一上来就调滤波参数，结果数据本身就有问题，那调来调去都是白费功夫。我个人习惯是，拿到原始数据后，先花至少三分之一的时间做预处理。你想想看，地基没打好，房子能稳吗？

这一章我们重点讲四个核心操作：时间戳同步、坐标系转换、异常值剔除、数据插值与降采样。它们各自解决不同的问题，但最终目标只有一个——让数据变得干净、对齐、可用。

核心原则：预处理不是可有可无的步骤，而是决定定位精度的关键。数据质量差，再牛的算法也救不回来。

数据预处理 时间戳同步 坐标系转换 异常值剔除 插值与降采样 四个步骤环环相扣，顺序执行才能得到干净的数据

3.1 时间戳同步：让所有传感器说同一种时间语言

组合导航里，GNSS 和 IMU 的数据频率不一样，这是常识。GNSS 一般是 1Hz 到 10Hz，IMU 通常是 100Hz 到 200Hz。但问题不在于频率差异，而在于它们的时间基准可能完全不同。

我在项目中遇到过这样的情况：GNSS 用的是 GPS 周秒（GPST），IMU 用的是计算机系统时间，两者差了十几秒。你想想看，这要是直接丢进滤波器，结果能对吗？

时间戳同步的核心就两件事：统一时间基准和对齐时间戳。

统一时间基准

我建议把所有传感器的时间都转换到同一个参考系。常用的参考系有：

  • GPS 周秒（GPST）：从 1980 年 1 月 6 日 0 时开始计数，不考虑闰秒
  • UTC 时间：世界协调时，考虑了闰秒
  • Unix 时间戳：从 1970 年 1 月 1 日 0 时开始计数

我个人习惯用 GPS 周秒，因为 GNSS 数据本身就是这个格式，省去一次转换。但如果你用的是 ROS 系统，那 Unix 时间戳可能更方便。选哪个不重要，重要的是所有传感器必须用同一个。

小技巧：如果你不确定时间基准是否统一，可以打印一段数据看看。GNSS 的时间戳通常是连续的整数或浮点数，IMU 的时间戳如果出现跳跃或重复，那多半是有问题。

对齐时间戳

统一基准之后，还要对齐。GNSS 和 IMU 的采样时刻不可能完全重合，我们需要找到最接近的匹配。

最简单的做法是最近邻匹配：对于每个 GNSS 观测值，找到时间上最接近的 IMU 数据。但这样做有个问题——如果 GNSS 频率低，IMU 数据会被浪费掉。

更好的做法是线性插值。举个例子：

def sync_timestamps(gnss_data, imu_data):
    """
    将 IMU 数据插值到 GNSS 时间戳上
    gnss_data: [(t_gnss, lat, lon, h), ...]
    imu_data:  [(t_imu, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z), ...]
    """
    synced_data = []
    for t_gnss, lat, lon, h in gnss_data:
        # 找到 t_gnss 前后两个 IMU 数据点
        idx = np.searchsorted(imu_data[:, 0], t_gnss)
        if idx == 0 or idx == len(imu_data):
            continue  # 超出范围，跳过
        t0, *val0 = imu_data[idx - 1]
        t1, *val1 = imu_data[idx]
        # 线性插值
        alpha = (t_gnss - t0) / (t1 - t0)
        interp_vals = [v0 + alpha * (v1 - v0) for v0, v1 in zip(val0, val1)]
        synced_data.append([t_gnss, lat, lon, h] + interp_vals)
    return np.array(synced_data)

注意：插值的前提是 IMU 数据本身是连续的、无跳变的。如果 IMU 数据有丢包，先做异常值剔除，再做插值。顺序别搞反了。

3.2 坐标系转换：别让坐标系的坑坑了你

坐标系转换这块，说白了就是让不同传感器的数据在同一个空间参考系下说话。GNSS 给的是经纬高（WGS-84），IMU 给的是载体坐标系（b系），这俩根本不在一个频道上。

我记得刚入行时，有一次把 IMU 的加速度直接当成导航系下的数据用，结果算出来的位置偏了十几公里。嗯，从那以后我再也不敢跳过坐标系转换了。

常见的坐标系

坐标系 说明 常见来源
WGS-84（经纬高） 大地坐标系，用纬度、经度、海拔表示位置 GNSS 原始输出
ECEF（地心地固系） 以地球质心为原点，X轴指向本初子午线 GNSS 转换后
ENU（站心坐标系） 以某点为原点，东-北-天方向 局部导航计算
b系（载体坐标系） 固定在载体上，前-右-下或前-左-上 IMU 原始输出
n系（导航坐标系） 通常取 ENU 或 NED 组合导航解算

核心转换流程

我一般按这个顺序做：

  1. WGS-84 → ECEF：用公式把经纬高转成地心地固坐标
  2. ECEF → ENU：选定一个参考点（比如起点），转成局部坐标系
  3. b系 → n系：用姿态矩阵（方向余弦矩阵）把 IMU 数据转到导航系

这里有个细节：IMU 的 b系定义各家不一样。有的用前-右-下，有的用前-左-上。我曾经被这个坑过——某款 IMU 的 Z 轴朝下，我按朝上处理，结果加速度符号全反了。

避坑指南：拿到 IMU 数据后，先静态放一会儿。如果 Z 轴加速度接近 +9.8 m/s²，说明 Z 轴朝上；如果是 -9.8 m/s²，说明朝下。这是最直接的验证方法。

def wgs84_to_ecef(lat, lon, h):
    """WGS-84 转 ECEF"""
    a = 6378137.0  # 地球长半轴
    e2 = 0.00669437999014  # 第一偏心率平方
    lat_rad = np.radians(lat)
    lon_rad = np.radians(lon)
    N = a / np.sqrt(1 - e2 * np.sin(lat_rad)**2)
    x = (N + h) * np.cos(lat_rad) * np.cos(lon_rad)
    y = (N + h) * np.cos(lat_rad) * np.sin(lon_rad)
    z = (N * (1 - e2) + h) * np.sin(lat_rad)
    return x, y, z

def ecef_to_enu(x, y, z, lat0, lon0, h0):
    """ECEF 转 ENU，参考点为 (lat0, lon0, h0)"""
    x0, y0, z0 = wgs84_to_ecef(lat0, lon0, h0)
    dx, dy, dz = x - x0, y - y0, z - z0
    lat0_rad, lon0_rad = np.radians(lat0), np.radians(lon0)
    # 旋转矩阵
    R = np.array([
        [-np.sin(lon0_rad), np.cos(lon0_rad), 0],
        [-np.sin(lat0_rad)*np.cos(lon0_rad), -np.sin(lat0_rad)*np.sin(lon0_rad), np.cos(lat0_rad)],
        [np.cos(lat0_rad)*np.cos(lon0_rad), np.cos(lat0_rad)*np.sin(lon0_rad), np.sin(lat0_rad)]
    ])
    e, n, u = R @ np.array([dx, dy, dz])
    return e, n, u

3.3 异常值剔除：把脏数据挡在门外

数据回放时，你经常会看到一些离谱的跳点。比如 GNSS 位置突然跳出去几百米，或者 IMU 加速度飙到 50 m/s²。这些异常值如果不处理，滤波器会直接发散。

为什么会这样？原因很多：多路径效应、卫星失锁、IMU 受到冲击、数据传输丢包……但不管原因是什么，我们的原则只有一个——宁可丢掉，不可错用。

常用的剔除方法

我常用的方法有三种，按复杂度排序：

  • 阈值法：设定物理上限。比如加速度不超过 20 m/s²，角速度不超过 500°/s。超出直接丢弃。
  • 3σ 准则：计算滑动窗口内的均值和标准差，超出均值 ±3σ 的视为异常。适合处理缓慢变化的信号。
  • 中值滤波：用滑动窗口的中值代替当前值，对孤立跳点特别有效。我处理 GNSS 高度跳变时常用这个。

我的经验：阈值法最粗暴，但也最可靠。3σ 准则对缓慢漂移的传感器有效，但如果数据本身有趋势（比如车辆加速），3σ 会误删正常数据。中值滤波是折中方案，但会引入延迟。

def remove_outliers_3sigma(data, window_size=10, sigma=3):
    """滑动窗口 3σ 异常值剔除"""
    filtered = np.copy(data)
    for i in range(len(data)):
        start = max(0, i - window_size // 2)
        end = min(len(data), i + window_size // 2)
        window = data[start:end]
        mean = np.mean(window)
        std = np.std(window)
        if abs(data[i] - mean) > sigma * std:
            filtered[i] = np.nan  # 标记为缺失
    return filtered

注意：异常值剔除后，数据会出现空洞。别急着填，先看看空洞的分布。如果空洞太多，说明传感器本身有问题，插值也救不了。

3.4 数据插值与降采样：让数据节奏一致

经过前面三步，数据已经干净了、对齐了、坐标统一了。但还有一个问题——不同传感器的数据频率不一样。IMU 可能 200Hz，GNSS 只有 5Hz，这怎么融合？

答案就是插值和降采样。说白了，就是让所有数据在同一个时间轴上「步调一致」。

插值：把低频数据变密

插值通常是对 GNSS 数据做的。因为 GNSS 频率低，我们需要把它插到 IMU 的时间戳上。常用的方法有：

  • 线性插值：简单快速，适合位置和速度这种变化平缓的量
  • 三次样条插值：更平滑，适合姿态角这种需要连续导数的量
  • 最近邻插值：不做平滑，直接用最近的值。适合离散状态量

我个人习惯：位置用线性插值，姿态用三次样条。为什么？因为位置变化相对平滑，线性插值误差可控；姿态变化可能很快（比如车辆转弯），三次样条能更好地保持连续性。

降采样：把高频数据变疏

降采样是对 IMU 数据做的。如果 IMU 是 200Hz，而你的滤波器只需要 100Hz，那就降采样。但注意，降采样不是简单抽稀，而是先滤波再抽稀。

为什么？因为直接抽稀会引起混叠。比如 IMU 数据里有 50Hz 的振动噪声，直接抽稀到 100Hz，这个噪声会被折叠到低频段，影响定位精度。

正确做法：先做低通滤波（截止频率设为降采样频率的一半），再等间隔抽取。我常用的是 FIR 低通滤波器，延迟可控。

from scipy import signal

def downsample_imu(imu_data, original_fs=200, target_fs=100):
    """降采样 IMU 数据，先滤波后抽取"""
    cutoff = target_fs / 2 * 0.9  # 截止频率设为目标频率的一半的 90%
    nyquist = original_fs / 2
    normal_cutoff = cutoff / nyquist
    # 设计 FIR 低通滤波器
    b = signal.firwin(31, normal_cutoff, window='hamming')
    # 滤波
    filtered = signal.filtfilt(b, 1, imu_data, axis=0)
    # 抽取
    step = original_fs // target_fs
    downsampled = filtered[::step]
    return downsampled

嗯，到这里数据预处理的核心内容就讲完了。你可能会问：这四个步骤的顺序能调换吗？我的建议是：先同步时间戳，再做坐标系转换，然后剔除异常值，最后插值和降采样。顺序调了可能会引入新的问题，比如先插值再剔除异常值，插值会把异常值的影响扩散到周围数据点。

数据预处理做好了，后面的滤波和优化才能事半功倍。别嫌麻烦，这一步省下的时间，后面会加倍还给你。

一句话总结：预处理不是技术活，是态度活。认真对待每一帧数据，它才会认真对待你的定位结果。

可可精品资料库

← 上一章 📖 返回目录 下一章 →