1. 课程导入
当一台无人机需要在楼宇间穿行、一辆自动驾驶车辆要在拥挤路口完成避让、一条机械臂要在狭小空间内抓取移动目标时，真正决定系统成败的往往不是单个控制器的性能，而是在多重约束下能否快速生成一条既安全又可行的轨迹。这正是本课程要解决的核心问题：多约束轨迹规划与实时优化。
所谓轨迹规划，是指在给定起点、终点以及环境与动力学条件下，求解一条随时间变化的位姿、速度、加速度序列；而多约束意味着这条轨迹必须同时满足几何避障、动力学极限、执行器饱和、舒适性、能耗、时间最优等多类限制；实时优化则要求上述求解在毫秒到几十毫秒级完成，并能随环境变化滚动重规划。三者叠加，构成了现代自主系统决策层的技术骨架。
一、核心原理：约束、目标与求解的三元结构
任何轨迹规划问题都可以抽象为三个要素：决策变量（轨迹参数或离散状态）、约束条件（等式与不等式）、目标函数（时间、能耗、平滑度等）。多约束轨迹规划的本质，是在高维非凸可行域中寻找满足实时性要求的近似最优解。
从数学形式看，典型问题可写为：
minimize J = ∫ (w_t + w_e·u² + w_j·jerk²) dt
subject to x(t+1) = f(x(t), u(t)) // 动力学约束
g(x(t)) ≤ 0 // 避障与边界约束
u_min ≤ u(t) ≤ u_max // 执行器饱和
v_min ≤ v(t) ≤ v_max // 速度约束
x(0)=x_start, x(T)=x_goal // 边值约束难点在于：避障约束通常是非凸的，动力学约束是非线性的，而实时性又要求求解器在有限算力下稳定收敛。因此，工程上常采用“前端搜索 + 后端优化”的两段式架构：前端用采样、搜索或图方法快速找到可行初值，后端用二次规划、非线性优化或模型预测控制进行精细化求解。
二、方法流程：从问题建模到实时部署
工程落地通常遵循一条清晰的流水线，每一步都对应本课程后续章节的核心内容：
- 问题建模：确定状态量、控制量、约束形式与目标权重，选择连续时间或离散时间描述。
- 前端搜索：用图搜索或采样方法生成满足避障的初始路径，必要时构建安全走廊。
- 后端优化：将非凸问题转化为可求解形式，如二次规划、序列二次规划或微分动态规划。
- 实时求解：利用稀疏结构、热启动、代码生成等手段压缩求解时间。
- 滚动执行：以模型预测控制方式滚动优化，结合状态估计形成闭环。
- 验证与调参：通过仿真、硬件在环与实车/实机测试验证约束满足度与实时性。
三、案例场景：一个典型的实时避障任务
假设一台四旋翼无人机需要在室内走廊中从 A 点飞到 B 点，走廊中存在静态柱子和一名走动的人员。系统每 50 ms 接收一次新的障碍物位置，要求轨迹平滑、不碰撞、速度不超过 2 m/s，且总飞行时间尽量短。
此时，前端可以用 RRT* 在三维栅格中快速找到一条绕开柱子的初始路径；后端将路径离散为若干段，构建以加速度为控制量的二次规划问题，把避障约束线性化为半平面约束，把速度与加速度限制写成箱式约束。求解器在 10 ms 内给出下一段轨迹，控制器跟踪前 50 ms 的轨迹片段，随后根据新的人员位置重新规划。这就是“感知—规划—优化—控制”闭环的典型形态。
四、常见误区与复盘要点
| 常见误区 | 后果 | 正确做法 |
|---|---|---|
| 只关注最优性，忽略约束可行性 | 求解失败或输出不可执行轨迹 | 先保证可行，再逐步优化目标 |
| 把非凸避障直接塞进 QP | 求解器不收敛或结果保守 | 用安全走廊或序列凸化处理 |
| 忽略计算耗时，只做离线仿真 | 实机运行时超时、抖动 | 统计最坏求解时间并留足余量 |
| 约束权重拍脑袋设定 | 轨迹忽快忽慢、能耗异常 | 结合物理量纲与场景标定权重 |
| 不做闭环反馈，只规划一次 | 环境变化后轨迹失效 | 采用滚动时域与热启动机制 |
五、学习目标与知识地图
完成本课程后，你应当能够：
- 用统一的数学语言描述多约束轨迹规划问题，并识别约束类型与求解难点；
- 独立实现前端搜索与后端优化的组合流程，并解释每一步的工程取舍；
- 针对具体场景选择合适的求解器与实时化手段，评估其计算与精度边界；
- 搭建仿真与硬件在环测试链路，定位规划失败、抖动、超时等典型问题；
- 阅读并复现该领域主流论文与开源框架中的核心模块。