3. 轨迹数据预处理

轨迹数据在采集阶段往往来自不同传感器、不同车辆平台和不同时间基准：GNSS 输出经纬高，IMU 输出局部姿态，激光雷达与摄像头输出相对坐标，CAN 总线输出车速与轮速。若直接把这些原始点拼接成一条轨迹，后续的标签工程会因坐标不一致、时间错位和噪声抖动而失效。因此，轨迹预处理是标签工程的第一道质量闸门，其目标是把多源、异步、带噪的原始观测，整理成统一坐标系、统一时间轴、几何连续且语义可用的轨迹序列。

一、坐标系统转换：从经纬高到局部平面

自动驾驶轨迹通常需要在地图、感知和规划模块之间流转，涉及三类坐标系：地心坐标系（ECEF）、地理坐标系（WGS84 经纬高）和局部平面坐标系（ENU 或 UTM）。GNSS 原始输出为 WGS84 经纬高，而路径规划、碰撞检测和标签标注更习惯使用以自车或路段起点为原点的 ENU 平面坐标。转换的核心步骤是：先将经纬高转为 ECEF 直角坐标，再以参考原点做平移和旋转，得到东-北-天三轴局部坐标。

工程中常用 PROJ、pyproj 或自研转换库完成这一过程。需要特别注意的是，参考原点一旦选定，整条轨迹必须使用同一原点，否则会出现轨迹整体漂移。对于跨区长轨迹，建议按路段分段设置原点，并在标签中记录每段的原点信息。

# 以 pyproj 为例：WGS84 经纬高 -> ENU 局部平面
from pyproj import Transformer
transformer = Transformer.from_crs("EPSG:4326", "EPSG:32650", always_xy=True)
x, y = transformer.transform(lon, lat)   # UTM 东向、北向
# 再以参考点 (x0, y0) 做平移，得到局部 ENU
enu_x, enu_y = x - x0, y - y0

二、时间同步：把异步观测对齐到统一时间轴

多传感器的时间戳来源不同：GNSS 通常带 UTC 时间，IMU 使用内部晶振计数，摄像头使用帧触发时间，CAN 报文使用总线时间。时间同步要解决两个问题：一是时钟域统一，二是采样时刻对齐。常用做法是先用 PTP 或 GPS 秒脉冲做硬件级时钟同步，再在软件层用线性插值或最近邻匹配，把各传感器观测映射到统一的时间栅格上。

对于轨迹标签，推荐以 10 Hz 或 20 Hz 作为统一时间栅格。若某传感器频率低于栅格频率，则采用零阶保持或线性插值补齐；若高于栅格频率，则做降采样并保留最近时刻值。时间同步误差应控制在半个采样周期以内，否则急加速、急转向场景下的轨迹点会出现明显错位。

三、噪声过滤与轨迹平滑：抑制抖动，保留真实机动

原始轨迹噪声主要来自 GNSS 多路径、IMU 零偏和感知框抖动。噪声过滤的目标是去掉高频抖动，同时保留真实的加减速和转向行为。常见方法包括：滑动平均、中值滤波、卡尔曼滤波、Savitzky-Golay 滤波和小波去噪。滑动平均实现简单但会模糊急弯；中值滤波对脉冲噪声鲁棒；卡尔曼滤波适合融合多源观测并输出平滑状态；Savitzky-Golay 在保留峰值形状方面表现较好。

轨迹平滑通常与噪声过滤配合使用。对于已经过滤波的轨迹，可用三次样条、B 样条或贝塞尔曲线做几何平滑，使曲率连续，便于后续计算航向角和曲率。平滑强度需要根据场景调节：高速公路可适当加大平滑窗口，城市路口则应减小窗口，避免把真实的转向动作抹平。

四、插值与重采样：让轨迹点“等距又等时”

不同来源的轨迹点间距差异很大：GNSS 在 10 Hz 下约 1~3 m 一个点，而视觉里程计可能每帧输出一个点。标签工程通常要求轨迹点具有稳定的空间或时间间隔，因此需要插值和重采样。插值解决“点不够密”的问题，重采样解决“点分布不均”的问题。

常用插值方法包括线性插值、三次样条插值和 Hermite 插值。线性插值计算快，但曲率不连续；三次样条平滑性好，但可能产生过冲；Hermite 插值可同时利用位置和速度信息，适合有 IMU 速度观测的场景。重采样则分为等时间重采样和等距离重采样：等时间重采样便于与时间序列标签对齐，等距离重采样便于计算曲率和横向偏差。

五、案例与复盘：城市路口场景的预处理链路

以一段城市路口左转轨迹为例：原始数据包含 10 Hz GNSS、100 Hz IMU 和 15 Hz 视觉里程计。预处理时，先将 GNSS 经纬高转为以路口中心为原点的 ENU 坐标，再用 GPS 秒脉冲对齐 IMU 和视觉时间戳，统一到 20 Hz 栅格。随后用中值滤波去除 GNSS 多路径造成的跳点，再用卡尔曼滤波融合 IMU 速度，最后用三次样条平滑并做等距离重采样，点距设为 0.5 m。

复盘时发现两个典型误区：一是平滑窗口过大，把左转的曲率峰值削平，导致后续“急左转”标签漏标；二是重采样时只保留位置而丢弃时间戳，使速度标签无法与轨迹点对齐。修正方法是按场景自适应调整平滑窗口，并在重采样输出中同时保留原始时间戳和插值时间戳。

综上，轨迹预处理是一条从坐标到时间、从噪声到几何、从插值到重采样的完整链路。每一步都会影响后续标签的准确性和一致性，因此需要把参数、版本和中间结果记录下来，形成可追溯的数据处理流水线。