第二章：数据采集与预处理——传感器数据采集与坐标统一

各位同学，大家好。我是你们的老朋友，一个在自动驾驶仿真领域摸爬滚打了七八年的工程师。今天咱们聊聊数据采集与预处理。说实话，这一章是整个轨迹回放技术的基石。你想想看，如果原始数据都是错的，后面再怎么分析也是白搭。

我个人习惯把数据预处理比作「做饭前的洗菜切菜」。菜不洗干净，炒出来的菜再香也硌牙。传感器数据采集、时间戳对齐、坐标系转换，就是这三道工序。咱们一个一个来。

2.1 传感器数据采集：GPS/IMU/轮速计

先说说数据从哪来。自动驾驶车上最常见的定位传感器就这三样：GPS、IMU、轮速计。它们各有各的脾气。

2.1.1 GPS（全球定位系统）

GPS 提供绝对位置信息，说白了就是告诉你车在地球上的经纬度。但 GPS 有个毛病——更新频率低，一般只有 10Hz。而且信号容易被高楼、隧道遮挡。我在项目中遇到过，车辆进入地下车库后 GPS 直接「失联」，定位数据跳变好几米。

GPS 输出的原始数据长这样：

// GPS NMEA 协议原始数据示例
$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A

嗯，这里要注意，GPS 输出的经纬度是度分格式，需要转换成十进制才能用。

2.1.2 IMU（惯性测量单元）

IMU 提供加速度和角速度，更新频率高，通常 100Hz 以上。它不依赖外部信号，所以不会「失联」。但 IMU 有累积误差——时间一长，积分出来的位置就飘了。

我记得有一次做仿真回放，IMU 数据没做零偏校正，结果车辆轨迹在 30 秒后偏了 50 米。后来查了半天才发现是 IMU 的零偏没去掉。

IMU 原始数据格式：

// IMU 数据示例（ROS 标准格式）
timestamp: 1234567890.123
linear_acceleration:
  x: 0.12
  y: -0.03
  z: 9.81
angular_velocity:
  x: 0.001
  y: -0.002
  z: 0.015

2.1.3 轮速计

轮速计测量车轮转速，可以推算车速和行驶距离。它简单可靠，但受轮胎打滑影响大。雨雪天气下，轮速计的数据基本不能信。

三种传感器的特点对比：

传感器	优点	缺点	典型频率
GPS	绝对定位，无累积误差	更新慢，易受遮挡	10 Hz
IMU	更新快，短时精度高	有累积漂移	100-200 Hz
轮速计	成本低，可靠性高	受打滑影响大	50-100 Hz

我的经验： 实际项目中，千万别只依赖一种传感器。GPS+IMU+轮速计融合使用，才能得到稳定可靠的轨迹数据。

2.2 数据同步与时间戳对齐

三种传感器各说各话，时间基准还不一样。GPS 用 UTC 时间，IMU 用系统启动时间，轮速计可能用 CAN 总线时间。这就好比三个人对表，但各看各的表，你说乱不乱？

数据同步的核心就一句话：把所有传感器数据统一到同一个时间轴上。

2.2.1 时间戳对齐方法

常用的方法有两种：

最近邻插值：找到时间戳最接近的数据点直接使用。简单粗暴，适合频率高的传感器。
线性插值：根据前后两个数据点，按时间比例计算中间值。精度更高，但计算量大一些。

我曾经踩过一个坑：GPS 和 IMU 的时间戳差了 0.1 秒没对齐，结果回放出来的轨迹在转弯处出现了明显的「锯齿」。后来用线性插值对齐后，轨迹就平滑了。

代码示例：

def align_timestamps(gps_data, imu_data, target_freq=100):
    """
    将 GPS 数据插值对齐到 IMU 的时间轴上
    """
    import numpy as np
    from scipy import interpolate
    
    # 提取时间戳
    gps_time = np.array([d['timestamp'] for d in gps_data])
    gps_lat = np.array([d['latitude'] for d in gps_data])
    gps_lon = np.array([d['longitude'] for d in gps_data])
    
    # 创建插值函数
    f_lat = interpolate.interp1d(gps_time, gps_lat, kind='linear')
    f_lon = interpolate.interp1d(gps_time, gps_lon, kind='linear')
    
    # 在 IMU 时间戳上插值
    imu_time = np.array([d['timestamp'] for d in imu_data])
    aligned_lat = f_lat(imu_time)
    aligned_lon = f_lon(imu_time)
    
    return aligned_lat, aligned_lon

注意： 插值前一定要检查时间戳是否有重复或倒序。我遇到过数据采集时时钟回拨，导致时间戳乱序的情况。

2.3 坐标系统一与坐标系转换

GPS 输出的是经纬度（WGS84 坐标系），IMU 输出的是车体坐标系下的加速度，轮速计输出的是车轮坐标系下的转速。这三个坐标系如果不统一，你根本没法用。

2.3.1 常用坐标系

WGS84：地球坐标系，经纬度表示
UTM：投影坐标系，用米表示
车体坐标系：以车辆中心为原点，前向为 x 轴
局部坐标系：以起始点为原点，方便计算

我个人习惯把所有数据统一到 UTM 坐标系下。为什么？因为 UTM 用米做单位，计算距离、速度都方便。经纬度算距离还得用 Haversine 公式，麻烦。

2.3.2 坐标系转换流程

转换流程其实就三步：

GPS 经纬度 → UTM 坐标（米）
IMU 车体坐标系 → UTM 坐标系（通过旋转矩阵）
轮速计数据 → 车体坐标系下的速度

代码示例：

import utm
import numpy as np

def wgs84_to_utm(lat, lon):
    """将 WGS84 经纬度转换为 UTM 坐标"""
    easting, northing, zone_number, zone_letter = utm.from_latlon(lat, lon)
    return easting, northing

def body_to_utm(acc_body, yaw):
    """将车体坐标系下的加速度转换到 UTM 坐标系"""
    # 旋转矩阵（绕 Z 轴旋转 yaw 角）
    R = np.array([
        [np.cos(yaw), -np.sin(yaw)],
        [np.sin(yaw),  np.cos(yaw)]
    ])
    acc_utm = R @ acc_body[:2]  # 只转换 x, y 方向
    return acc_utm

  核心要点： 坐标系转换时，一定要搞清楚旋转顺序和方向定义。不同传感器厂商对「前向」的定义可能不一样，有的用右手系，有的用左手系。

知识体系总览

说了这么多，咱们用一张图把整个数据采集与预处理的知识体系串起来：

我的建议： 刚开始做数据预处理时，先把三种传感器的原始数据画出来看看。肉眼观察一下时间戳是否对齐、坐标是否有跳变。这一步虽然简单，但能帮你发现 80% 的数据问题。

好了，这一章的内容就到这里。数据采集与预处理看似基础，但做扎实了，后面的轨迹回放才能顺风顺水。下一章咱们聊聊轨迹插值与平滑，到时候见。

可可精品资料库

📚 课程目录

第二章：数据采集与预处理——传感器数据采集与坐标统一

2.1 传感器数据采集：GPS/IMU/轮速计

2.1.1 GPS（全球定位系统）

2.1.2 IMU（惯性测量单元）

2.1.3 轮速计

2.2 数据同步与时间戳对齐

2.2.1 时间戳对齐方法

2.3 坐标系统一与坐标系转换

2.3.1 常用坐标系

2.3.2 坐标系转换流程

知识体系总览