3、数据预处理：时间戳对齐、坐标系转换（WGS84转UTM）、数据插值与去噪
说实话，很多刚入行的朋友觉得轨迹回放嘛，把数据录下来，再放一遍不就完了？
嗯，真这么简单就好了。我见过太多案例——回放出来的轨迹要么跳来跳去，要么跟实际道路对不上。问题出在哪？十有八九是数据预处理没做好。
这一节，我就把我在高速场景下积累的预处理经验，掰开了讲给你听。
3.1 时间戳对齐——让所有传感器说同一种“时间语言”
你想想看，一辆自动驾驶车上，GPS、IMU、摄像头、激光雷达，每个传感器都有自己的时钟。GPS可能用的是UTC时间，IMU用的是板载晶振，相机是触发时间戳。
这些时间如果不统一，回放出来的轨迹就是“各说各话”。
我个人习惯的做法是：
- 先找主时钟——通常以GPS时间作为基准，因为它精度高、无漂移。
- 计算时间偏移——把IMU、相机的时间戳减去GPS时间戳，得到一个固定的偏移量。
- 线性插值对齐——比如IMU是100Hz，GPS是10Hz，那就把GPS的位姿插值到每个IMU时间点上。
我在项目中遇到过一个问题：某款IMU的晶振受温度影响，时间漂移能达到几十毫秒。高速上100km/h，几十毫秒就是将近1米的误差。后来我加了个滑动窗口的时间校准，才把这个问题压下去。
3.2 坐标系转换——WGS84转UTM，把地球“拍平”
GPS给的是经纬度（WGS84坐标系），但我们的轨迹计算、碰撞检测，都是在平面直角坐标系里做的。所以必须把经纬度转成米为单位的UTM坐标。
为什么选UTM？说白了，UTM把地球分成60个带，每个带内变形很小，高速场景下精度足够。
转换公式我就不手写了，直接用现成的库。Python里我常用 pyproj 或 utm 库。
import utm
# 示例：WGS84转UTM
lat, lon = 31.2304, 121.4737 # 上海某点
easting, northing, zone_number, zone_letter = utm.from_latlon(lat, lon)
print(f"UTM坐标: ({easting:.2f}, {northing:.2f})")
print(f"UTM带: {zone_number}{zone_letter}")
这里有个坑，我曾经踩过——UTM带号必须一致。如果轨迹跨越了两个UTM带（比如从上海开到新疆），直接转换会导致坐标跳变。我的做法是：统一使用起始点的UTM带，或者用自定义的局部坐标系。
3.3 数据插值与去噪——让轨迹“顺滑如丝”
原始数据往往有两个问题：一是时间戳不均匀，二是噪声大。尤其是GPS，在城市峡谷或隧道里，信号跳变几米是常事。
3.3.1 插值：填补时间缝隙
我常用的插值方法有三种：
| 方法 | 适用场景 | 优缺点 |
|---|---|---|
| 线性插值 | 匀速直线运动 | 简单快速，但弯道误差大 |
| 三次样条插值 | 弯道、变道场景 | 平滑性好，但可能过冲 |
| 阿克曼模型插值 | 车辆动力学约束 | 最符合实际，但计算量大 |
我个人习惯：高速直线段用线性插值，弯道用三次样条。如果计算资源允许，阿克曼模型插值效果最好。
3.3.2 去噪：剔除“野点”
去噪的方法很多，我推荐一个组合拳：
- 中值滤波——对位置序列做窗口滤波，窗口大小取5~11帧。能有效剔除孤立跳变点。
- 速度/加速度阈值——计算相邻点的速度和加速度，如果超过物理极限（比如加速度 > 5m/s²），就标记为异常点。
- RANSAC拟合——对局部轨迹做RANSAC直线或曲线拟合，剔除离群点。
我曾经在一条高速上遇到GPS信号被高架遮挡，轨迹出现了连续十几米的跳变。单纯用中值滤波根本拉不回来，后来我加了“道路约束”——把轨迹投影到高精地图的车道线上，才把问题解决。
3.4 知识体系总览
下面这张图，是我自己总结的数据预处理流程。你可以把它当作一个检查清单：
嗯，数据预处理这部分，说难不难，说简单也不简单。关键是要理解每一步“为什么这么做”，而不是“怎么调API”。
我见过太多人拿着现成的库一顿调，结果轨迹回放出来全是问题。记住：预处理的质量，直接决定了回放测试的可信度。
可可精品资料库