1. 课程导入
轨迹优化研究的是：在满足动力学、运动学、避障与执行器约束的前提下，寻找一条使某项性能指标最优的运动轨迹。它回答三个问题——去哪里（路径与目标）、怎么去（时间参数化与控制输入）、去得多好（代价函数与约束满足程度）。在机器人、自动驾驶与航空航天中，轨迹优化往往是从"能走"到"走得稳、走得快、走得省"的关键一步。
一、核心原理：轨迹优化问题的统一形式
绝大多数轨迹优化问题都可以写成如下形式：给定系统动力学 ẋ = f(x, u, t)，在时间区间 [t₀, t_f] 上寻找状态轨迹 x(t) 与控制输入 u(t)，使代价函数最小，同时满足边界条件与不等式约束。
min J = φ(x(t_f)) + ∫ L(x, u, t) dt
s.t. ẋ = f(x, u, t) 动力学约束
x(t₀) = x₀, x(t_f) ∈ X_f 边界条件
g(x, u, t) ≤ 0 避障/执行器/速度约束求解思路通常分为两大类：变分法/间接法把约束通过拉格朗日乘子并入代价，转化为两点边值问题，精度高但对初值敏感；直接法把连续问题离散为有限维非线性规划（NLP），鲁棒性好、易加约束，是工程主流。直接法又分为只离散控制（打靶法）与同时离散状态和控制（配点法、直接转录）。
二、发展历程：从最优控制到实时求解
- 1950s–1960s：庞特里亚金极大值原理与贝尔曼动态规划奠定最优控制理论基础，主要面向航天交会、登月等离线任务。
- 1970s–1990s：直接法与序列二次规划（SQP）发展，数值优化开始处理带约束轨迹问题。
- 2000s：内点法、自动微分与稀疏线性代数成熟，轨迹优化可处理上千变量规模。
- 2010s 至今：面向嵌入式与高频控制的实时求解器兴起，如基于 SQP 的实时迭代（RTI）、ADMM/算子分裂、GPU 并行与学习加速方法，把求解周期压到毫秒级。
三、应用领域与实时求解挑战
不同领域对轨迹优化的诉求差异明显，下表给出典型对比：
| 领域 | 典型任务 | 约束重点 | 实时性要求 |
|---|---|---|---|
| 机器人 | 机械臂抓取、移动底盘避障 | 关节限位、碰撞、力矩 | 1–10 ms 控制周期 |
| 自动驾驶 | 换道、跟车、路口通行 | 车辆动力学、交通规则 | 10–100 ms 规划周期 |
| 航空航天 | 火箭着陆、无人机穿越 | 推力幅值/方向、气动、热流 | 1–50 ms 制导周期 |
| 工业过程 | 能耗最优调度 | 设备能力、安全边界 | 秒级至分钟级 |
实时求解的核心挑战可归纳为四点：非凸性导致局部最优与初值敏感；规模与稀疏性要求高效线性代数；硬实时要求最坏情况可预测，而非平均快；资源受限要求算法在算力、内存、功耗受限的嵌入式平台上稳定运行。
四、课程整体框架与学习路径
本课程按"建模—求解—加速—落地"主线展开：先建立轨迹优化的问题建模与离散化能力，再掌握 SQP、内点法、ADMM 等求解方法，随后进入实时求解工程（热启动、稀疏结构、代码生成、嵌入式部署），最后通过机器人、自动驾驶与航空航天案例完成闭环。下图给出课程的知识结构与数据流关系。
学习路径建议按四步推进：第一步，用统一形式写出你所在领域的轨迹优化问题，明确状态、控制、代价与约束；第二步，选择离散方式并搭建可求解的 NLP；第三步，引入实时加速手段，测量单次求解时间分布；第四步，在真实或半实物平台上验证闭环稳定性与鲁棒性。
轨迹优化不是孤立的数学技巧，而是连接感知、决策与控制的桥梁。掌握它，意味着你能把"任务意图"翻译成"可执行、可验证、可实时求解"的运动指令。接下来的章节将沿着上述知识结构逐层展开，从问题建模开始，逐步走向可部署的实时求解系统。