2. 数学基础

轨迹规划表面上是“给机器人或车辆找一条路”，但真正决定算法能否稳定、快速、可解释地工作的，是背后的数学工具。线性代数负责描述空间与约束，微积分负责刻画运动与变化，概率统计负责处理不确定性，图论负责组织搜索空间，优化理论负责在众多可行解中选出最优解。理解这五类数学工具在轨迹规划中的分工，才能避免把调参当成算法设计。

一、线性代数：状态、变换与约束的统一语言

在轨迹规划中，位置、速度、加速度、姿态、控制量通常被组织成向量，系统演化被写成矩阵形式。例如离散线性系统可表示为 x_{k+1} = A x_k + B u_k，其中 x 是状态向量，u 是控制向量，A 和 B 描述系统动力学。线性代数让“多变量耦合”变成可计算的对象：矩阵乘法表示状态转移，特征值分析判断系统稳定性，奇异值分解用于分析约束方向的可控性。

在几何层面，旋转矩阵、齐次变换矩阵用于描述车辆或机械臂在不同坐标系下的位姿。轨迹点从局部坐标系变换到全局坐标系，本质上是矩阵作用。若忽略坐标系变换，规划出的轨迹可能在局部看起来合理，却在全局发生碰撞。因此，线性代数不是抽象符号，而是空间一致性的保障。

二、微积分：连续运动与平滑性的数学描述

轨迹的本质是时间函数：p(t) 表示位置，v(t)=p'(t) 表示速度，a(t)=p''(t) 表示加速度，j(t)=p'''(t) 表示加加速度。微积分把“运动是否平滑”转化为导数连续性问题。例如，多项式轨迹常用五次多项式，因为可以同时约束位置、速度、加速度，使轨迹在连接点处保持 C² 连续。

在动态场景中，障碍物也在运动，因此需要处理时间维度上的变化率。若障碍物速度估计存在延迟，轨迹的可行性不仅取决于当前距离，还取决于相对速度与相对加速度。微积分中的极值、梯度和积分，分别用于寻找最优切换点、计算代价函数梯度以及评估累计风险。

三、概率统计：不确定性下的风险量化

真实感知存在噪声：障碍物位置有协方差，速度估计有方差，预测轨迹有置信区间。概率统计把这些不确定性建模为分布，而不是单一确定值。例如，障碍物未来位置可表示为高斯分布 N(μ, Σ)，轨迹碰撞概率可通过积分或采样估计。若只使用均值预测，规划器可能过于激进；若使用过大的安全边界，又会导致轨迹保守甚至不可行。

贝叶斯滤波、卡尔曼滤波和粒子滤波用于状态估计与预测，蒙特卡洛采样用于评估轨迹风险。概率统计的核心作用是把“安全”从布尔判断转化为可量化的风险指标，使规划器能够在效率与安全之间做权衡。

四、图论与优化理论：搜索空间与最优决策

图论把连续或离散的规划问题组织成节点与边。栅格地图中，每个栅格是节点，相邻栅格之间的连接是边；道路网络中的车道段、路口和换道点也可构成图。Dijkstra、A*、RRT 等算法本质上是在图上搜索可行路径。图论决定了搜索的完备性、最优性和复杂度。

优化理论则进一步回答“哪条轨迹最好”。常见形式包括二次规划、非线性规划、模型预测控制。以模型预测控制为例，其滚动时域优化可写为：

min  Σ (x_k - x_ref)^T Q (x_k - x_ref) + u_k^T R u_k
s.t. x_{k+1} = A x_k + B u_k
     x_min ≤ x_k ≤ x_max
     u_min ≤ u_k ≤ u_max
     g(x_k, u_k) ≤ 0   // 避障、动力学等约束

其中 Q 和 R 是权重矩阵，g 表示避障与动力学约束。优化理论提供了约束处理、梯度计算、凸性分析和收敛性保证。若问题非凸，还需要初值策略、松弛方法或多起点搜索。

五、案例：动态避障中的数学协同

假设一辆自动驾驶车辆在高速公路上行驶，前方车辆突然减速，右侧车道有来车。规划器需要完成以下步骤：

1. 状态表示：用线性代数建立自车与障碍物的状态向量，包括位置、速度、加速度。
2. 运动预测：用概率统计估计前方车辆和右侧来车的未来位置分布。
3. 轨迹生成：用微积分构造满足加速度和加加速度约束的候选轨迹。
4. 离散搜索：用图论在车道、换道点和时间维度上组织候选决策。
5. 优化求解：用二次规划或非线性规划选择代价最低且满足避障约束的轨迹。

如果缺少概率统计，右侧来车的速度估计误差可能被忽略；如果缺少微积分，轨迹可能在连接点出现加速度突变；如果缺少优化理论，规划器只能“找到一条路”，而无法保证舒适性和安全性最优。

六、常见误区与复盘

总结来看，线性代数提供空间与状态的统一表示，微积分提供连续运动的描述能力，概率统计提供不确定性下的风险量化，图论提供离散搜索的结构，优化理论提供最优决策的求解框架。五者不是并列的知识点，而是轨迹规划从建模到落地的完整数学链条。掌握这条链条，才能在动态场景中设计出既安全又高效的轨迹重规划算法。