4、轨迹表示：轨迹的数学表示
各位同学，今天我们来聊聊轨迹的数学表示。说实话，这是整个回放测试的基石。你想想看，如果连轨迹怎么描述都搞不清楚，后面的回放、分析、评价就全是空中楼阁。
我在做自动驾驶测试的头两年，就吃过这个亏。当时拿到一批路测数据，直接用离散点去算曲率，结果算出来的值忽大忽小，根本没法用。后来才明白——嗯，轨迹的表示方式，直接决定了后续所有算法的精度和稳定性。
离散点序列：最朴素的表示方式
最直接的方式，就是用一串点来描述轨迹。每个点包含位置信息（x, y）和时间戳 t。说白了就是：
// 离散点序列示例
struct TrajectoryPoint {
double x; // 东向位置，单位米
double y; // 北向位置，单位米
double t; // 时间戳，单位秒
double v; // 速度，单位m/s
double yaw; // 航向角，单位弧度
};
std::vector<TrajectoryPoint> trajectory;这种方式的优点很明显——简单、直观、不丢信息。传感器采集到的原始数据就是这种格式。但问题也来了：
- 数据量大：10Hz的采样率，跑一小时就是36000个点
- 噪声敏感：GPS抖动、IMU漂移都会让点序列变得毛糙
- 插值困难：两个点之间怎么走？线性插值？那曲率就断了
注意：直接用离散点计算曲率，结果会非常不稳定。我曾经在高速场景下试过，相邻两个点的曲率能差出3-4倍，根本没法用来做评价指标。
样条曲线：让轨迹变得光滑
为了解决离散点的问题，我们引入了样条曲线。我个人最常用的是三次样条（Cubic Spline）。为什么是三次？因为二阶导数连续，曲率自然就连续了。
三次样条的核心思想很简单：
- 把轨迹分成若干段
- 每段用一个三次多项式拟合
- 在连接点处保证一阶、二阶导数连续
// 三次样条插值示例（简化版）
// 输入：离散点序列 points
// 输出：样条曲线参数
// 每段曲线形式：S_i(t) = a_i + b_i*t + c_i*t^2 + d_i*t^3
// 约束条件：
// 1. S_i(t_i) = points[i].x
// 2. S_i(t_{i+1}) = points[i+1].x
// 3. S'_i(t_{i+1}) = S'_{i+1}(t_{i+1})
// 4. S''_i(t_{i+1}) = S''_{i+1}(t_{i+1})
我的经验：在高速场景（时速100km/h以上）下，建议使用五次样条。虽然计算量大了点，但曲率变化更平滑，回放时车辆控制更稳定。我在京沪高速的测试中就吃过这个亏——三次样条在急弯处还是会有轻微抖动。
轨迹长度计算
轨迹长度，说白了就是路径的总里程。对于离散点序列，直接累加相邻点距离就行：
double calcTrajectoryLength(const std::vector<TrajectoryPoint>& pts) {
double length = 0.0;
for (size_t i = 1; i < pts.size(); ++i) {
double dx = pts[i].x - pts[i-1].x;
double dy = pts[i].y - pts[i-1].y;
length += std::sqrt(dx*dx + dy*dy);
}
return length;
}对于样条曲线，就需要对弧长进行积分了。这里有个小技巧——我一般用数值积分，比如高斯-勒让德求积法，精度够用，速度也快。
曲率计算：关键中的关键
曲率决定了车辆能不能安全通过。你想想看，如果轨迹曲率超过了车辆的最小转弯半径，那肯定要出问题。
对于参数曲线 (x(t), y(t))，曲率公式是：
κ = |x'y'' - y'x''| / (x'² + y'²)^(3/2)
其中：
x' = dx/dt, x'' = d²x/dt²
y' = dy/dt, y'' = d²y/dt²实际工程中的坑：
- 离散点求导时，差分步长不能太大也不能太小。我一般用5个点的中心差分，效果不错
- 曲率突变点要特别关注。我曾经在高速匝道入口处发现曲率从0.001突变到0.05，这种地方最容易出事故
- 曲率符号表示转向方向：正为左转，负为右转
知识体系总览
下面这张图，是我自己总结的轨迹表示知识体系。你看一眼就能明白各个模块之间的关系：
实际工程中的选择建议
| 场景 | 推荐表示方式 | 原因 |
|---|---|---|
| 低速泊车（<10km/h） | 离散点序列 | 精度要求不高，计算简单 |
| 城市道路（30-60km/h） | 三次样条 | 平衡精度与计算量 |
| 高速场景（>80km/h） | 五次样条 | 曲率连续性好，控制稳定 |
| 极端工况（紧急避障） | B样条 | 局部可控，便于实时调整 |
我的个人习惯：在做轨迹回放测试时，我会同时保留离散点和样条曲线两种表示。离散点用于快速定位和可视化，样条曲线用于精确的曲率分析和控制评价。两者互补，缺一不可。
好了，关于轨迹的数学表示就讲到这里。记住一句话：选对表示方式，后续工作事半功倍；选错了，后面全是坑。我在实际项目中见过太多因为轨迹表示不当导致的分析偏差，希望大家能少走这些弯路。
可可精品资料库