3、轨迹数据格式：常见轨迹格式（KML、GPX、CSV、ROS Bag）、轨迹数据字段定义、数据压缩与采样策略

做仿真回放，第一关就是数据格式。说实话，我见过不少团队，算法调得挺漂亮，结果卡在数据读取上——格式不对、字段缺失、采样率太高导致回放卡顿。今天咱们就把这块硬骨头啃下来。

3.1 常见轨迹格式：各有各的脾气

市面上轨迹格式少说十几种，但真正在自动驾驶领域常用的，也就下面这四种。我按使用频率排个序，你心里有个数。

CSV 是我个人最常用的格式。为什么？因为简单。你想想看，一个文本编辑器就能打开，Python里pandas一行代码就读进来了。我在项目中经常用CSV做快速验证——先跑通逻辑，再换其他格式。

ROS Bag 是实车数据的标配。它不光存轨迹，还能把激光雷达点云、摄像头图像、CAN总线信号一股脑儿打包。我记得第一次处理100GB的bag文件时，差点把笔记本跑死。后来学乖了，先做数据裁剪再回放。

GPX 和 KML 更多用于后处理展示。比如你跑完一次仿真，想看看车辆实际走了哪条路，导出KML扔到Google Earth里，一目了然。但要说做算法分析，这两个格式字段太少，不够用。

3.2 轨迹数据字段定义：别漏了关键信息

一个完整的轨迹点，到底该包含哪些字段？我见过有人只存了经纬度和时间，结果回放时发现速度、航向全没有，根本没法做碰撞检测。

下面是我在项目中总结的「黄金字段集」，你直接拿去用：

你可能会问：为什么 altitude 不是必须的？嗯，这里要注意——很多GPS模块在高度上的误差能达到几十米，你存了反而可能误导算法。我一般只在做坡道场景时才启用高度字段。

下面是一个标准的CSV轨迹文件示例：

timestamp,latitude,longitude,altitude,speed,heading
1630000000.000,31.2304,121.4737,10.5,0.0,90.0
1630000000.100,31.2305,121.4738,10.5,1.2,90.5
1630000000.200,31.2306,121.4739,10.6,2.5,91.2
1630000000.300,31.2307,121.4740,10.6,3.8,92.0

3.3 数据压缩与采样策略：别让数据撑爆内存

实车采集的数据，动辄几十GB。你想想看，如果直接全部加载到内存里回放，再好的机器也得跪。所以压缩和采样是必修课。

3.3.1 时间域采样：降采样

最常见的做法是降低采样率。比如原始数据是100Hz，回放时其实10Hz就够了——人眼根本看不出区别。我一般用线性插值重采样：

import numpy as np
import pandas as pd

def downsample_trajectory(df, target_freq=10):
    """
    将轨迹数据降采样到目标频率
    df: 包含timestamp和位置字段的DataFrame
    target_freq: 目标频率，单位Hz
    """
    # 计算目标时间间隔
    dt = 1.0 / target_freq
    
    # 生成新的时间轴
    t_start = df['timestamp'].min()
    t_end = df['timestamp'].max()
    new_timestamps = np.arange(t_start, t_end, dt)
    
    # 线性插值
    df_downsampled = pd.DataFrame({'timestamp': new_timestamps})
    for col in ['latitude', 'longitude', 'altitude', 'speed', 'heading']:
        if col in df.columns:
            df_downsampled[col] = np.interp(
                new_timestamps, 
                df['timestamp'], 
                df[col]
            )
    
    return df_downsampled

3.3.2 空间域压缩：道格拉斯-普克算法

如果车辆长时间直线行驶，中间的点其实全是冗余。这时候用道格拉斯-普克（Douglas-Peucker）算法，只保留关键拐点。我曾在一条10公里的直道上，把10万个点压缩到200个，回放效果完全不受影响。

def douglas_peucker(points, epsilon=0.0001):
    """
    道格拉斯-普克轨迹压缩
    points: [(lat, lon), ...]
    epsilon: 距离阈值，单位度（约11米）
    """
    if len(points) <= 2:
        return points
    
    # 找到距离首尾连线最远的点
    dmax = 0
    index = 0
    for i in range(1, len(points) - 1):
        d = perpendicular_distance(points[i], points[0], points[-1])
        if d > dmax:
            index = i
            dmax = d
    
    # 如果最大距离大于阈值，递归压缩
    if dmax > epsilon:
        left = douglas_peucker(points[:index+1], epsilon)
        right = douglas_peucker(points[index:], epsilon)
        return left[:-1] + right
    else:
        return [points[0], points[-1]]

3.3.3 混合策略：先时间后空间

我个人的最佳实践是两步走：先做时间域降采样到10Hz，再做空间域压缩去掉冗余点。这样既保证了时间精度，又大幅减小了数据量。

3.4 知识体系总览

下面这张图，把本章的核心逻辑串起来了。你一看就明白：

从数据采集开始，根据场景选择合适的格式，定义好字段，再经过压缩采样，最终实现高效回放。每一步都有坑，但只要你按这个流程走，基本不会出大问题。

好了，轨迹数据格式这块就聊到这儿。记住一句话：格式是死的，人是活的。别被工具束缚，选最适合你场景的方案就行。