2. 数学基础
轨迹预测不是“把历史坐标往后画一条线”，而是要在不确定性中估计未来分布、在时序中传播状态、在高维特征中压缩信息，并通过优化让模型输出既符合观测又符合物理约束。概率论、线性代数、微积分与优化理论分别承担“不确定性建模”“空间变换与降维”“连续变化与梯度传播”“参数学习与约束求解”四类核心职责。理解它们的接口关系，比单独记忆公式更重要。
p(Y | X, C)，其中 X 为历史轨迹与场景观测，C 为地图、交通规则等上下文，Y 为未来多模态轨迹。四类数学工具分别回答：Y 如何分布、X 如何表示、梯度如何回传、参数如何更新。
一、概率论：把“未来”从单点变成分布
真实道路中，同一历史轨迹可能对应直行、变道、减速让行等多种未来。概率论的第一作用是承认多模态：用混合模型表示 p(Y|X)=Σ_k π_k N(Y; μ_k, Σ_k)，其中 π_k 是第 k 个意图的概率，μ_k 是该意图下的均值轨迹，Σ_k 描述该意图内部的位置不确定性。训练时常用负对数似然：L = -log Σ_k π_k N(y; μ_k, Σ_k)，它比均方误差更能容忍多峰标签。
第二作用是条件概率与贝叶斯更新。当车辆打转向灯、刹车灯亮起或地图显示前方施工时，后验 p(意图|观测) 会发生变化。工程上常把先验意图概率与观测似然相乘再归一化，得到更新后的模态权重。第三作用是评价：ADE/FDE 只衡量单条轨迹误差，而 NLL、Brier 分数、校准曲线才能衡量“概率是否可信”。若模型给出 0.9 概率的轨迹却频繁失败，即使 ADE 很低，下游规划也不敢使用。
二、线性代数：坐标、特征与注意力的共同语言
轨迹点、车道线、交通参与者最终都要变成向量与矩阵。线性代数提供三件事：坐标变换、特征压缩、关系计算。坐标变换用旋转矩阵 R 与平移向量 t 把世界坐标转到自车坐标系：x' = R x + t。多传感器融合时，外参标定本质上就是求 R 与 t，标定误差会直接放大为轨迹预测偏差。
特征压缩依赖矩阵分解。对高维轨迹片段做 PCA，等价于对协方差矩阵 C = (1/N) Σ (x_i-μ)(x_i-μ)^T 做特征分解，取前 d 个特征向量作为主方向。这样既降维又保留主要运动模式。Transformer 中的注意力 Attention(Q,K,V)=softmax(QK^T/√d_k)V 也是线性代数：QK^T 计算相似度，softmax 归一化为权重，再对 V 加权求和。理解这一点，就能明白为什么缩放因子 √d_k 能防止点积过大导致梯度消失。
三、微积分：梯度如何穿过时间与空间
轨迹预测模型通常包含递归或注意力结构，反向传播依赖链式法则。以简单状态更新 h_t = f(W h_{t-1} + U x_t) 为例，损失对早期状态的梯度需要连乘多个雅可比矩阵 ∂h_t/∂h_{t-1}。若这些矩阵的谱半径小于 1，梯度会指数衰减，导致长时依赖学不到；若大于 1，则可能梯度爆炸。LSTM 的门控、残差连接、LayerNorm 都是在改善这条梯度路径。
微积分还用于连续时间建模。当轨迹用微分方程描述时，dx/dt = f(x,u)，预测等价于数值积分。常见做法是欧拉法 x_{t+1}=x_t+Δt·f(x_t,u_t)，或更稳定的龙格-库塔法。若把物理约束写成可微损失，例如 L_phys = ||a_t - a_{t-1}||^2 惩罚加速度突变，就能通过梯度直接约束输出平滑性。
四、优化理论：从损失到可部署参数
训练阶段的核心是求解 min_θ L(θ)。随机梯度下降用 θ ← θ - η ∇L 更新，Adam 进一步用一阶矩和二阶矩估计自适应调整每个参数的学习率。轨迹预测中常见的优化难点有三类：多模态导致的非凸性、长尾场景导致的样本不均衡、以及物理约束导致的可行域限制。
对于约束，可用拉格朗日乘子把硬约束转为软惩罚：L_total = L_data + λ L_rule，其中 L_rule 可编码“不撞车”“不越界”“满足最小跟车距离”。推理阶段则常转化为带约束的优化问题，例如在预测分布上求最小风险轨迹：y* = argmin_y E_{Y~p}[cost(y,Y)]。此时概率论提供分布，优化理论提供决策。
| 数学分支 | 核心作用 | 典型公式 | 轨迹预测中的落点 |
|---|---|---|---|
| 概率论 | 建模不确定性与多模态 | p(Y|X)=Σ_k π_k N(Y;μ_k,Σ_k) |
多意图预测、NLL 损失、概率校准 |
| 线性代数 | 坐标变换、降维与关系计算 | x'=Rx+t；Attention=softmax(QK^T/√d_k)V |
自车坐标系、PCA 压缩、Transformer 注意力 |
| 微积分 | 梯度传播与连续变化 | ∂L/∂θ = Σ ∂L/∂h_t · ∂h_t/∂θ |
反向传播、LSTM 门控、物理平滑损失 |
| 优化理论 | 参数学习与约束求解 | θ ← θ - η∇L；L_total=L_data+λL_rule |
Adam 训练、规则惩罚、风险最小化推理 |
五、案例：一个变道预测中的数学链路
假设自车在高速上行驶，右侧车辆持续靠近车道线。输入编码阶段，线性代数把该车历史轨迹从世界坐标转到自车坐标系，并用 PCA 提取“横向偏移—纵向速度”主成分；概率论根据转向灯与车道线距离给出变道先验概率；时序模型用注意力计算该车与自车、与车道线的交互权重；微积分把预测误差与平滑约束的梯度回传；优化器更新参数，使模型在类似场景中提高变道模态概率。最终输出不是一条线，而是“直行 0.35、变道 0.55、减速 0.10”的分布。
六、常见误区与复盘
- 只优化 ADE/FDE，忽略概率校准。 单轨迹误差低不代表分布可信，下游规划需要的是可靠的概率。
- 把线性代数只当计算工具。 坐标变换、特征分解和注意力权重都直接影响空间关系理解，标定错误会系统性偏移预测。
- 忽视梯度路径。 长时轨迹预测中，梯度消失会让模型只关注最近几帧，必须用门控、残差或注意力缓解。
- 把约束全部硬编码。 硬约束可能不可微，工程上更常用软惩罚加投影，兼顾可训练性与可行性。
# 轨迹预测中常见的多模态 NLL 损失（示意）
def nll_loss(y_true, pi, mu, sigma):
# y_true: [B, T, 2]
# pi: [B, K] 模态权重
# mu: [B, K, T, 2] 各模态均值轨迹
# sigma: [B, K, T, 2] 各模态标准差
log_prob = gaussian_log_prob(y_true, mu, sigma) # [B, K]
log_mix = log(pi + 1e-8) + log_prob
return -logsumexp(log_mix, axis=1).mean()
总结来看，概率论回答“未来有多少种可能、各自多可信”，线性代数回答“场景如何表示与交互”，微积分回答“误差如何传回每个参数”，优化理论回答“如何更新参数并满足约束”。四者共同构成轨迹预测从数据到决策的数学底座。掌握它们的接口关系，才能在模型设计、损失函数选择和推理策略上做出有依据的工程判断。