3. 数学原理
本文以 Step-by-Step 方式推导 Prediction 模块涉及的核心公式。运动模型实现见 06_motion_models.md；算法综述见 09_survey.md。
3.1 问题形式化
Step 1：定义障碍状态
时刻 \(t\) 第 \(i\) 个动态障碍的状态：
\[
\mathbf{x}_i(t) = \big(x, y, v_x, v_y, \theta, \omega, \ldots\big)^\top \in \mathbb{R}^n
\]
Step 2：预测目标
给定历史观测 \(\mathcal{H}_t = \{\mathbf{z}_{i,t-k}, \ldots, \mathbf{z}_{i,t}\}\) 与预测时域 \(T_p\)，求未来轨迹：
\[
\hat{\mathbf{x}}_i(t+\tau) = f\big(\mathbf{x}_i(t), \tau, \mathcal{H}_t\big), \quad \tau \in [0, T_p]
\]
Step 3：输出形式
离散预测轨迹：
\[
\hat{\mathcal{T}}_i = \big\{ \hat{\mathbf{x}}_i(t + k \Delta t) \big\}_{k=0}^{K}, \quad K = T_p / \Delta t
\]
3.2 恒速模型（Constant Velocity, CV）
Step 1：状态向量
\[
\mathbf{x} = [x, y, v_x, v_y]^\top
\]
Step 2：连续时间动力学
\[
\dot{x} = v_x, \quad \dot{y} = v_y, \quad \dot{v}_x = 0, \quad \dot{v}_y = 0
\]
Step 3：状态转移矩阵
\[\begin{split}
F = \begin{bmatrix}
1 & 0 & \Delta t & 0 \\
0 & 1 & 0 & \Delta t \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
Step 4：离散预测
\[
\mathbf{x}_{k+1} = F \mathbf{x}_k
\]
展开：
\[\begin{split}
\begin{aligned}
x_{k+1} &= x_k + v_{x,k} \Delta t \\
y_{k+1} &= y_k + v_{y,k} \Delta t \\
v_{x,k+1} &= v_{x,k} \\
v_{y,k+1} &= v_{y,k}
\end{aligned}
\end{split}\]
3.3 恒转弯率模型（CTRV）
Step 1：状态向量
\[
\mathbf{x} = [x, y, v, \theta, \omega]^\top
\]
其中 \(v\) 为线速度，\(\theta\) 为航向，\(\omega\) 为角速度。
Step 2：非线性动力学
\[\begin{split}
\begin{aligned}
\dot{x} &= v \cos\theta \\
\dot{y} &= v \sin\theta \\
\dot{v} &= 0 \\
\dot{\theta} &= \omega \\
\dot{\omega} &= 0
\end{aligned}
\end{split}\]
Step 3：解析积分（\(\omega \neq 0\)）
\[\begin{split}
\begin{aligned}
x(t+\Delta t) &= x + \frac{v}{\omega}\big(\sin(\theta + \omega\Delta t) - \sin\theta\big) \\
y(t+\Delta t) &= y + \frac{v}{\omega}\big(-\cos(\theta + \omega\Delta t) + \cos\theta\big) \\
\theta(t+\Delta t) &= \theta + \omega \Delta t
\end{aligned}
\end{split}\]
Step 4：\(\omega \to 0\) 极限（退化为 CV）
\[
x(t+\Delta t) = x + v\cos\theta \cdot \Delta t, \quad
y(t+\Delta t) = y + v\sin\theta \cdot \Delta t
\]
3.4 恒加速度模型（CA）
Step 1：状态向量
\[
\mathbf{x} = [x, y, v_x, v_y, a_x, a_y]^\top
\]
Step 2：状态转移矩阵
\[\begin{split}
F = \begin{bmatrix}
1 & 0 & \Delta t & 0 & \frac{\Delta t^2}{2} & 0 \\
0 & 1 & 0 & \Delta t & 0 & \frac{\Delta t^2}{2} \\
0 & 0 & 1 & 0 & \Delta t & 0 \\
0 & 0 & 0 & 1 & 0 & \Delta t \\
0 & 0 & 0 & 0 & 1 & 0 \\
0 & 0 & 0 & 0 & 0 & 1
\end{bmatrix}
\end{split}\]
Step 3：预测公式
\[\begin{split}
\begin{aligned}
x_{k+1} &= x_k + v_{x,k}\Delta t + \frac{1}{2}a_{x,k}\Delta t^2 \\
v_{x,k+1} &= v_{x,k} + a_{x,k}\Delta t
\end{aligned}
\end{split}\]
3.5 卡尔曼滤波预测步
Step 1：预测（Prior）
\[\begin{split}
\begin{aligned}
\hat{\mathbf{x}}_{k|k-1} &= F \hat{\mathbf{x}}_{k-1|k-1} \\
P_{k|k-1} &= F P_{k-1|k-1} F^\top + Q
\end{aligned}
\end{split}\]
Step 2：更新（Posterior）
\[\begin{split}
\begin{aligned}
K_k &= P_{k|k-1} H^\top (H P_{k|k-1} H^\top + R)^{-1} \\
\hat{\mathbf{x}}_{k|k} &= \hat{\mathbf{x}}_{k|k-1} + K_k (\mathbf{z}_k - H \hat{\mathbf{x}}_{k|k-1}) \\
P_{k|k} &= (I - K_k H) P_{k|k-1}
\end{aligned}
\end{split}\]
Step 3：多步前向预测
对 \(j = 1, \ldots, K\)：
\[
\hat{\mathbf{x}}_{k+j|k} = F^j \hat{\mathbf{x}}_{k|k}, \quad
P_{k+j|k} = F^j P_{k|k} (F^j)^\top + \sum_{i=0}^{j-1} F^i Q (F^i)^\top
\]
3.6 数据关联（匈牙利算法）
Step 1：代价矩阵
\(N\) 个轨迹、\(M\) 个检测，代价：
\[
C_{ij} = 1 - \mathrm{IoU}(\mathrm{track}_i, \mathrm{det}_j)
\]
或采用欧氏距离代价：
\[
C_{ij} = \|\hat{\mathbf{x}}_i - \mathbf{z}_j\|_2
\]
Step 2：最优分配
求排列 \(\pi\) 使 \(\sum_i C_{i,\pi(i)}\) 最小，满足一对一匹配。
Step 3：门控（Gating）
马氏距离门控，剔除不可能的关联：
\[
d_M^2 = (\mathbf{z} - H\hat{\mathbf{x}})^\top S^{-1} (\mathbf{z} - H\hat{\mathbf{x}}) < \chi^2_{\alpha, df}
\]
其中 \(S = H P H^\top + R\)。
3.7 轨迹不确定性表示
3.7.1 高斯椭圆
2D 位置协方差 \(P_{xy}\) 的特征值 \(\lambda_1, \lambda_2\) 定义 \(k\sigma\) 置信椭圆：
\[
(\mathbf{p} - \boldsymbol{\mu})^\top P_{xy}^{-1} (\mathbf{p} - \boldsymbol{\mu}) = k^2
\]
3.7.2 多模态轨迹
对 \(L\) 条假设轨迹 \(\{\hat{\mathcal{T}}^{(l)}, \pi_l\}\)，\(\sum_l \pi_l = 1\)：
\[
P(\mathcal{T}^{(l)}) = \pi_l, \quad \hat{\mathbf{x}}^{(l)}(t+\tau) = f_l(\mathbf{x}, \tau)
\]
3.8 碰撞风险评估
Step 1：自车预测轨迹 \(\hat{\mathcal{T}}_{ego}\)
Step 2：障碍预测轨迹 \(\hat{\mathcal{T}}_i\)
Step 3：时空重叠
\[
\mathrm{risk} = \max_{k} \mathbf{1}_{\left\{\|\hat{\mathbf{p}}_{ego,k} - \hat{\mathbf{p}}_{i,k}\| < r_{\mathrm{safe}}\right\}}
\]
或基于 TTC（Time To Collision）：
\[
\mathrm{TTC} = \min_{\tau > 0} \big\{ \tau : \|\mathbf{p}_{ego}(\tau) - \mathbf{p}_i(\tau)\| < r_{safe} \big\}
\]
3.9 符号表
符号 |
含义 |
|---|---|
\(T_p\) |
预测时域 |
\(\Delta t\) |
预测/滤波时间步 |
\(F\) |
状态转移矩阵 |
\(H\) |
观测矩阵 |
\(Q, R\) |
过程/观测噪声协方差 |
\(\omega\) |
角速度（CTRV） |
\(\pi_l\) |
多模态轨迹概率 |