2. 数学基础

多约束轨迹规划的本质，是把“让机器人/车辆/飞行器从起点安全到达终点”这一工程诉求，翻译成一组可求解的数学问题。翻译过程依赖四类语言：线性代数描述空间与约束，微积分描述变化与灵敏度，凸优化给出可全局求解的骨架，非线性规划与最优控制处理真实世界中的非凸、连续时间耦合。本章不追求数学推导的完备性，而是建立符号约定与直觉，使后续章节的公式、算法与代码能够被一致地阅读。

一、核心原理：四层数学语言的职责分工

线性代数是轨迹规划的“坐标系与容器”。轨迹通常离散为 N 个路径点，每个点含位置、速度、加速度等状态量，于是整条轨迹可写成一个高维向量 x ∈ R^n。约束如“第 k 点速度不超过 v_max”可写成 a_k^T x ≤ v_max，多条约束堆叠为矩阵不等式 A x ≤ b。矩阵的秩、零空间、特征值分别对应约束是否冗余、可行方向是否存在、二次型代价是否正定。

微积分负责“变化率与灵敏度”。轨迹的平滑性由导数刻画：位置 p(t)、速度 ṗ(t)、加速度 p̈(t)、加加速度 p⃛(t)。代价函数常写成积分形式 J = ∫ (w_a‖p̈‖² + w_j‖p⃛‖²) dt，其离散化后成为二次型。梯度 ∇f 指出代价下降方向，Hessian ∇²f 决定曲率与收敛速度，雅可比矩阵 ∂g/∂x 则把约束的微小变化映射到状态空间。

凸优化提供“可证明最优”的基准。若目标函数为凸、可行域为凸集，则局部最优即全局最优，且可用内点法、有效集法稳定求解。轨迹规划中常见的二次规划（QP）形式为：

minimize    (1/2) x^T H x + f^T x
subject to  A_eq x = b_eq      // 等式约束：起终点、连续性
            A_ineq x ≤ b_ineq  // 不等式约束：速度、加速度、避障
            x_min ≤ x ≤ x_max  // 变量边界

非线性规划（NLP）与最优控制则处理凸优化覆盖不到的部分：障碍物距离约束 d(x) ≥ d_safe 通常非凸，动力学 ẋ = f(x,u) 往往非线性，时间变量 T 也可能作为决策变量。最优控制把轨迹规划写成连续时间问题，通过变分法或庞特里亚金极小值原理导出必要条件，再经直接法（打靶、配点）或间接法转为 NLP 求解。

二、方法流程：从符号约定到求解器输入

建立数学基础后，轨迹规划的标准流程可归纳为五步。第一步，定义决策变量：常见选择是路径点序列 x = [p_0, p_1, ..., p_N]，或多项式系数、B样条控制点。第二步，构造代价：平滑性用二阶/三阶导数的二次型，时间最优用总时间 T，跟踪误差用与参考轨迹的偏差。第三步，施加约束：等式约束保证起终点与连续性，不等式约束保证速度、加速度、避障与曲率限制。第四步，判断问题类型：若代价凸且约束凸，直接调用 QP/SOCP 求解器；若含非凸避障或非线性动力学，则用 SQP、内点法或序列凸化迭代。第五步，验证与后处理：检查约束违反量、求解时间与数值条件数。

三、案例/场景：避障轨迹的凸化与非线性修正

考虑一辆差速机器人在走廊中绕过两个圆形障碍物。若直接写“到障碍物中心距离 ≥ 半径 + 安全裕度”，该约束是非凸的，因为可行域是圆环外部。工程上常用两种处理：一是序列凸化，在当前轨迹附近将距离约束线性化，得到 d(x_k) + ∇d^T (x - x_k) ≥ d_safe，迭代求解并更新展开点；二是用安全走廊或凸多边形近似自由空间，把非凸避障转为多个凸约束的交集。前者保留原问题精度但需迭代，后者求解快但可能保守。

下表对比四类数学工具在轨迹规划中的典型角色与求解特性：

四、误区与复盘

复盘清单：① 决策变量是否覆盖所有自由度？② 代价函数是否凸？③ 约束是否完整表达物理限制？④ 初值是否可行或接近可行？⑤ 求解结果是否通过独立仿真验证？⑥ 求解时间是否满足实时性要求？

本章建立的符号与分层视角，将贯穿后续所有章节。线性代数提供容器，微积分提供方向，凸优化提供基准，NLP 与最优控制提供处理真实复杂性的手段。四者不是替代关系，而是同一求解流水线上不同阶段的工具。