2. 数学基础回顾

局部规划算法看似是“几何+控制”的组合，但其稳定性、最优性与鲁棒性，几乎都由背后的数学工具决定。轨迹点、障碍物、约束、代价、噪声、拓扑关系，最终都会被翻译成向量、矩阵、概率分布、优化问题或图结构。理解这些数学语言，才能在复杂场景中判断：为什么某个规划器会抖动、为什么会陷入局部极小、为什么换一个代价函数就变得不可解。

一、核心原理：五类数学工具在规划中的角色

线性代数负责“空间与变换”。位姿、速度、障碍物边界、传感器观测都可以写成向量；旋转矩阵、齐次变换、协方差矩阵描述坐标系之间的映射与不确定性传播。规划中常见的二次型代价 J = xᵀQx + uᵀRu，本质就是线性代数中的正定矩阵度量。

概率统计负责“不确定性与噪声”。激光雷达有测距噪声，定位有漂移，行人运动有随机性。贝叶斯滤波、卡尔曼滤波、粒子滤波、高斯过程回归，都是把“不确定”变成可计算分布的工具。局部规划中的碰撞概率、置信椭圆、风险代价，都来自概率统计。

凸优化负责“可证明全局最优”的一类问题。当目标函数是凸函数、约束是凸集时，局部最优即全局最优。二次规划（QP）、线性规划（LP）、二阶锥规划（SOCP）是局部规划中最常用的凸模型，例如模型预测控制中的QP求解、避障中的半平面约束。

非线性优化负责“真实但难解”的问题。车辆运动学、避障距离、非完整约束通常是非凸的。梯度下降、高斯牛顿、Levenberg-Marquardt、序列二次规划（SQP）、内点法，都是在非凸地形中寻找可行下降方向。局部规划器中的轨迹优化、图优化SLAM，都依赖这些方法。

图论负责“离散结构与拓扑关系”。道路网络、拓扑地图、行为树、状态机、任务分配，都可以抽象为图。Dijkstra、A*、RRT、PRM 本质上是在图上搜索或采样。图论还用于判断场景的连通性、死锁与拓扑同伦类。

二、方法流程：从数学形式到规划求解

在工程中，一个局部规划问题通常按以下流程被“数学化”：

1. 状态与坐标定义：用向量 x = [px, py, θ, v, ω]ᵀ 描述车辆状态，用齐次变换矩阵 T 统一各传感器坐标系。
2. 不确定性建模：用协方差矩阵 Σ 表示定位与感知误差，将障碍物膨胀为概率安全域。
3. 代价函数构造：用二次型或非线性项描述跟踪误差、控制量、舒适性与风险。
4. 约束建模：运动学约束、避障约束、速度与加速度约束，分别写成等式或不等式。
5. 选择求解器：凸问题用QP/SOCP，非凸问题用SQP/内点法，离散搜索用A*/RRT。
6. 滚动执行与反馈：MPC在每个周期重新求解，用最新状态修正偏差。

三、案例/场景：一个避障局部规划问题的数学拆解

假设车辆在狭窄通道中避让一个移动行人。规划器需要同时处理：车辆非完整约束、行人预测不确定性、通道边界约束、舒适性代价。

状态: x = [px, py, θ, v]ᵀ
控制: u = [a, ω]ᵀ
离散模型: x_{k+1} = f(x_k, u_k)
代价: J = Σ (x_k - x_ref)ᵀ Q (x_k - x_ref) + u_kᵀ R u_k + λ·risk_k
约束:
  |px - ox| + |py - oy| ≥ d_safe + 3σ   (概率安全约束)
  v_min ≤ v ≤ v_max
  |a| ≤ a_max, |ω| ≤ ω_max
  x_0 = x_current

其中 risk_k 来自概率统计，Q、R 来自线性代数，d_safe + 3σ 是概率约束，f 是非线性运动学，通道连通性则由图论判断。若把避障约束线性化为半平面，问题可近似为QP；若保留精确距离，则变成非凸问题，需要SQP或序列凸化。

四、误区与复盘

复盘时建议按以下清单检查：