7. 轨迹预测算法

本文描述 Prediction 模块规划中的轨迹预测算法：从卡尔曼跟踪到多步前向传播。

7.1 预测流水线

检测输入 z_k
    │
    ▼
数据关联（匈牙利 + 门控）
    │
    ▼
卡尔曼更新 → 状态估计 x̂_k|k
    │
    ▼
运动模型多步 predict → {x̂_{k+1|k}, ..., x̂_{k+K|k}}
    │
    ▼
PredictedTrajectory 输出

7.2 跟踪与关联

7.2.1 轨迹管理

每个动态障碍维护一条轨迹：

字段	说明
`id`	唯一标识
`state`	卡尔曼状态 \(\hat{\mathbf{x}}\)
`covariance`	\(P\)
`age`	存活帧数
`coast_count`	连续无观测帧数

7.2.2 关联算法

构造代价矩阵 \(C_{ij}\)（IoU 或马氏距离）
匈牙利算法求最优匹配
未匹配检测 → 新轨迹；未匹配轨迹 → coast_count++，超阈值删除

7.2.3 门控

马氏距离：

\[ d_M^2 = (\mathbf{z} - H\hat{\mathbf{x}})^\top S^{-1} (\mathbf{z} - H\hat{\mathbf{x}}) < \chi^2_{0.95, 2} \approx 5.99 \]

7.3 恒速预测器（CV Predictor）

算法：

用 CV 运动模型 + 卡尔曼滤波估计 \([x, y, v_x, v_y]\)
对 \(k = 1, \ldots, K\)，\(\hat{\mathbf{x}}_{k+j|k} = F^j \hat{\mathbf{x}}_{k|k}\)

输出：直线轨迹，不确定性椭圆随 \(j\) 增大。

公式：见 03_math.md §3.2。

7.4 CTRV 预测器

算法：

状态 \([x, y, v, \theta, \omega]\)
解析积分（\(\omega \neq 0\)）或 CV 退化（\(\omega \approx 0\)）

输出：圆弧或直线轨迹，适合车辆。

公式：见 03_math.md §3.3。

7.5 多模态预测

对意图不确定的障碍，输出 \(L\) 条轨迹：

模式	概率	运动
直行	\(\pi_1\)	CV 沿当前方向
左转	\(\pi_2\)	CTRV \(\omega > 0\)
右转	\(\pi_3\)	CTRV \(\omega < 0\)
停车	\(\pi_4\)	Stationary

约束：\(\sum_l \pi_l = 1\)。

规划器可取 \(\arg\max_l \pi_l\) 或鲁棒优化（考虑所有模式）。

7.6 预测时域与步长

参数	典型值	说明
\(T_p\)	3–8 s	预测时域
\(\Delta t\)	0.1 s	离散步长
\(K = T_p / \Delta t\)	30–80	轨迹点数

实时性：\(K\) 不宜过大；10 Hz 预测频率下，单障碍 CV 预测 \(< 1\) ms。

7.7 不确定性传播

协方差传播：

\[ P_{k+j|k} = F^j P_{k|k} (F^j)^\top + \sum_{i=0}^{j-1} F^i Q (F^i)^\top \]

2D 位置不确定性椭圆供规划器做 chance-constraint 避障。

7.8 与 Planning 的对接

struct PredictedTrajectory {
    int id;
    double probability;
    std::vector<Pose2D> poses;      // t, x, y, theta
    std::vector<Eigen::Matrix2d> cov_xy;  // 可选
};

// Planning 查询 t 时刻障碍占用
bool occupied = false;
for (const auto& traj : trajectories) {
    auto pose = Interpolate(traj, t);
    if (Distance(ego_pose, pose) < r_safe) occupied = true;
}

7.9 实现优先级

优先级	算法	依赖
P0	CV + 卡尔曼	motion_model ✅
P1	CTRV	需新增 CTRV 类
P2	多模态（规则）	地图/车道
P3	学习型（VectorNet）	训练数据、ONNX