斯坦福大学AA274A机器人自主原理：轨迹优化（2019年秋季）

（原标题：Stanford AA274A Principles of Robotic Autonomy | Autumn 2019 | Trajectory Optimization）

🎯 轨迹优化核心定义与分类体系

• 轨迹优化的根本目标在于寻找一条连接初始状态与最终状态的时间历史轨迹，该轨迹必须严格满足系统动力学约束并最小化特定的成本函数。在机器人学语境下，若系统仅受动力学方程限制而无显式状态约束（如障碍物碰撞），该问题被定义为轨迹优化；一旦引入状态约束，则归类为运动规划范畴。
• 控制策略的设计分为开环与闭环两大范式。开环策略仅依赖时间参数生成控制输入，计算高效但缺乏对扰动的纠偏能力；闭环策略依赖当前状态与时间，具备更强的鲁棒性，但数学处理复杂且计算开销大。工程实践中常采用“参考信号加反馈跟踪”的折中方案，即先计算开环参考轨迹，再设计局部反馈控制器以应对实际执行中的偏差。
• 最优控制理论的历史渊源可追溯至冷战时期拦截导弹的需求。苏联学派发展出庞特里亚金极大值原理，侧重于推导最优性必要条件；美国学派由 Richard Bellman 主导动态编程方法，强调通过离散化状态空间求解全局最优解。这两种理论路径构成了现代轨迹优化算法的基础框架。

📊 数值离散化与成本函数结构

• 机器人运动方程表现为微分方程组，课程主要采用欧拉法进行数值离散化求解。在离散化过程中，步长 $\Delta t$ 的选择需在数值精度与计算耗时之间进行权衡：过小的步长导致计算量激增，过大的步长则可能引发数值不稳定或精度损失。
• 成本函数由两部分组成：终端成本 $H(x_{tf}, t_f)$ 和积分形式的运行成本 $\int g(x, u, t)dt$。其中，运行成本的加性结构（即总成本为各时刻局部成本之和）是应用动态规划等高效算法的前提条件。若成本函数不具备这种可分离的加性结构，则难以利用贝尔曼方程进行递归求解。
• 方法分类上，直接法先对连续时间问题离散化，将其转化为有限维非线性规划问题再行优化；间接法则先推导最优控制的必要条件（如哈密顿-雅可比方程或极大值原理），得到一组微分方程后再进行离散化求解。直接法实现简单且通用性强，而间接法在特定结构下可能具有更高的计算效率。

📐 微分平坦理论与平直输出定义

• “微分平坦”（Differential Flatness）是解决高维非线性系统轨迹优化的关键数学工具。其核心定义为：存在一组变量 $z$（称为平直输出），其数量等于控制输入数，且所有状态量和控制量均可由 $z$ 及其有限阶导数唯一确定，无需进行积分或求解微分方程。
• 对于具备微分平坦性的系统（如四旋翼、单摆模型、简单汽车模型），轨迹优化问题可转化为对低维平直输出曲线的规划。通过仅规划这些低维变量，即可快速生成满足运动学约束的可行轨迹，避免直接求解复杂的非线性动力学方程。
• 目前不存在通用的可计算准则来判定任意系统是否具有微分平坦性。发现新系统的平坦性是重要的学术贡献，通常依赖于对系统结构特性的深入分析。单摆模型（unicycle model）和简单汽车模型均被证明具备此性质，因此广泛适用于移动机器人的轨迹规划任务。

🚗 简单汽车案例与几何插值应用

• 以简单汽车模型为例，选取位置坐标 $(x, y)$ 作为平直输出。通过几何曲线插值确定 $(x(t), y(t))$ 后，可直接反推航向角 $\theta$、速度 $v$ 和转向角 $\phi$。这一过程将高维动力学约束简化为低维空间的路径规划问题。
• 该方法的计算优势显著：它将轨迹优化转化为低维空间中的曲线拟合或插值问题，支持高频（如 100Hz）实时重规划。这种特性使其特别适用于环境动态变化、需要快速响应的自主机器人系统。
• 在工程实践中，寻找“足够好”的可行轨迹往往比追求全局最优解更具现实意义。基于平直输出的多项式插值算法在计算效率与轨迹可行性之间取得了最佳平衡，是此类微分平坦系统的首选策略。

📈 多项式插值方法与系数优化

• 核心方法利用多项式插值连接初始与最终状态，将平输出变量表示为基函数（如 $1, t, t^2, t^3$）的线性组合。通过优化参数化函数的系数而非整个函数形状，显著降低了计算负载。该方法因可靠、可解释且实现简单而在机器人领域广泛流行。
• 约束处理机制将状态 $x$ 和输入 $u$ 的初始/最终条件转化为关于多项式系数的线性方程组。若边界条件不足以唯一确定所有系数（即解不唯一），则需引入最小化目标函数以确保轨迹平滑并减少控制努力。
• 常见的优化指标包括 jerk（三阶导数）或 snap（四阶导数）。通过最小化这些高阶导数的积分值，可以生成物理上可行且机械应力较小的轨迹。这种基于系数的优化方法使得实时计算成为可能，因为求解线性方程组或小型二次规划问题的复杂度远低于直接优化高维非线性系统。

📉 平滑性指标与行业实践标准

• 平滑性指标定义了轨迹各阶导数的物理意义：速度为一阶导数、加速度为二阶、jerk 为三阶、snap 为四阶、crackle 为五阶导数。高阶导数的幅值直接反映了控制输入的剧烈程度和机械结构的受力情况。
• 在行业实践中，最小化 snap 是计算四旋翼等空中车辆轨迹的事实标准。该策略能生成无极端振荡且控制效率高的可行轨迹，有效避免了因加速度突变导致的执行器饱和或结构疲劳。对于地面移动机器人，最小化 jerk 通常已足够满足平滑性要求。
• 针对速度上限（如 $5\text{ m/s}$）等输入边界问题，采用时间缩放技术进行处理。通过重新参数化轨迹的时间节奏（即调整时间映射函数），可以在不改变空间路径形状的前提下，确保控制输入始终处于允许的盒状约束范围内。

📚 理论局限、适用模型与学习资源

• 微分平坦性的判定缺乏通用算法，主要依赖人工推导和结构分析。这限制了该方法在未知复杂系统上的直接应用，但在已知经典模型（如四旋翼、差速驱动机器人）中表现优异。
• 推荐文献包括 Richard Murray 关于将微分平坦应用于项目优化的论文、Levine 的数学理论著作以及 Duigato 的入门书籍。课程重点章节为第 11 章，详细阐述了从平直输出定义到多项式插值实现的完整流程。
• 掌握微分平坦与数值积分是解决机器人自主性中轨迹优化问题的关键工具。实际应用中需根据模型不确定性选择控制策略：开环计算高效但缺乏纠偏能力，闭环鲁棒性强但数学处理复杂。结合两者优势的“参考跟踪”架构是当前工业界的主流解决方案。