v2_humanoid_navigation_tutorial

第 12 章 — 规划与控制（算法实现，不涉硬件）

开篇段落

本章是连接导航算法“大脑”与物理世界“身体”的关键桥梁。前述章节产出的宏观计划（如 VLA 的子目标序列）或几何路径（如 A* 的航点），在质上仍是离散和抽象的。本章的目标，是系统性地阐述如何将这些指令，转化为人形机器人可以精确、平滑、安全执行的时空连续运动轨迹。我们将从“行动原语”这一核心抽象层入手，探讨其设计哲学与实现模式；随后深入剖E析轨迹生成的数学本质——一个带约束的多目标优化问题，并对比不同的参数化和求解方法；接着，我们将重点介绍现代机器人控制的基石——模型预测控制（MPC），特别是其在处理复杂动力学和安全约束上的优势；最后，我们将讨论平顺性控制和系统级的故障安全（Fail-Safe）策略，它们是决定用户体验和系统可靠性的最后一道防线。学完本章，您将具备设计一个完整、鲁棒且高效的机器人运动规划与控制栈的算法能力。

12.1 行动原语库（Action Primitives Library）

行动原语（Action Primitives）是高层规划与底层控制之间的“合约”或API”。它们将复杂的连续控制问题，封装成一系列离散、可参数化、有明确执行语义的“技能”。这使得 VLA 等“大脑”可以专注于任务层面的逻辑推理，而无需关心动力学的细枝末节。

12.1.1 设计哲学与粒度权衡

12.1.2 核心原语详解

12.2 轨迹生成：多目标约束优化

全局规划器输出的路径是几何和静态的，而机器人需在动态世界中移动。轨迹生成（Trajectory Generation）的目标是，为这条路径赋予时间信息，并将其优化成一条平滑、安全、且动力学可行的时空曲线。

12.2.1 问题形式化

寻找一条由时间 $t$ 参数化的状态轨迹 $X(t) = [p(t), v(t), a(t), …]$，其中 $p(t) \in SE(3)$ 是机器人的位姿，该轨迹需要最小化一个加权代价函数 $J_{total}$：

$ \min_{X(t)} J_{total} = \int_{0}^{T} ( w_{s} L_{smooth} + w_{o} L_{obs} + w_{e} L_{energy} ) dt + w_t L_{time}(T) $

12.2.2 轨迹参数化

为了将无限维的优化问题转为有限维，需要对轨迹进行参数化。

12.2.3 求解方法

      (Obstacle Field - High Cost)
      /========================\
     |       ESDF < d_max       |
      \========================/
               ^
               | Cost Gradient Pushes Trajectory Away
               |
Initial Path: A------------------>B
               |
Optimized Traj: A~~~~~~~~~~~~~~~>B (Bends around obstacle)

12.3 鲁棒控制：MPC 深入、动力学模型与约束处理

有了理想轨迹 $X_{ref}(t)$，控制器的任务是计算出电机指令 $u(t)$，使得机器人的实际状态 $X_{actual}(t)$ 尽可能精确地跟踪 $X_{ref}(t)$，同时应对各种扰动。模型预测控制（MPC）因其前瞻性和处理约束的能力，已成为事实上的标准。

12.3.1 MPC 核心循环

在每个控制周期（例如 $t_k$，周期为 $\Delta t$），MPC 执行以下操作：

  1. 状态估计: 获取当前机器人状态 $X_{actual}(t_k)$。
  2. 构建优化问题: 在未来 $N$ 个时间步（预测时域 $T_p = N \cdot \Delta t$）内，求解一个最优控制序列 $U^* = { u_k^, u_{k+1}^, …, u_{k+N-1}^* }$。 $ \min_{U} \sum_{i=0}^{N-1} ( ||X_{k+i} - X_{ref, k+i}||^2Q + ||u{k+i}||^2R ) + ||X{k+N} - X_{ref, k+N}||^2_P $ subject to:
    • $X_{k+i+1} = f(X_{k+i}, u_{k+i})$ (动力学模型约束)
    • $X_{min} \le X_{k+i} \le X_{max}$ (状态约束，如关节角度)
    • $u_{min} \le u_{k+i} \le u_{max}$ (输入约束，如电机力矩)
    • $g(X_{k+i}) \le 0$ (其他约束，如与障碍物的安全距离)
  3. 应用控制: 仅将计算出的最优控制序列的第个元素 $u_k^*$ 应用到机器人。
  4. 滚动时域: 在下一个时间步 $t_{k+1}$，重复整个过程。这种“滚动优化”策略使得 MPC 能够持续地对新信息做出反应。

12.3.2 动力学模型的选择

MPC 的性能关键取决于内部预测模型 $f(X, u)$ 的好坏。

12.3.3 约束处理

MPC 最大的优势在于其显式处理约束的能力。

12.4 抖动与迟滞的消除：根源分析与系统级解决方案

不平滑、犹豫的运动是机器人导航中最影响用户体验的问题之一。

12.5 安全停止与分层降级策略 (Fail-Safe)

一个鲁棒的系统必须假设任何组件都可能失败，并为此设计预案。

本章小结

本章详细阐述了从抽象计划到具体运动的算法全链路，这是决定机器人导航性能和安全性的核心。

常见陷阱与错误 (Gotchas)

  1. 控制器与规划器打架: 局部规划器（轨迹优化器）和 MPC 控制器都在进行优化，如果它们的目标或模型不一致，可能会导致系统振荡。调试建议: 确保 MPC 的目标是紧密跟踪轨迹优化器生成的轨迹，而不是自己重新规划。MPC 应该扮演一个“高质量的轨迹跟踪器”角色。
  2. MPC 求解时间不稳定: MPC 的计算时间取决于问题的复杂度和状态。在接近大量障碍物时，约束增多，求解时间可能飙升，超过控制周期，导致系统失稳。调试建议: 使用实时性更好的求解器（如 qpOASES），设置求解超时，如果超时则复用上一周期的解或执行安全停止。
  3. 忽略状态估计的延迟和噪声: 控制器依赖的状态（位置、速度）本身是估计出来的，有延迟和噪声。直接将带噪的速度信号用于控制会引起剧烈抖动。调试建议: 控制器应使用经过卡尔曼滤波或类似滤波器平滑后的状态估计。同时，在 MPC 模型中应考虑并补偿已知的系统延迟。
  4. 原语的死锁或活锁: 在复杂的状态机设计中，可能会出现状态之间来回跳转（活锁）或卡在某个状态无法退出（死锁）的情况。调试建议: 为每个状态设置一个超时定时器。对状态转移的条件进行形式化验证或充分的压力测试，确保逻辑的完备性。
  5. 不切实际的轨迹: 轨迹优化器如果使用了过于简化的模型，可能会生成一条机器人动力学上无法跟踪的轨迹，导致巨大的跟踪误差和不稳定。调试建议: 确保轨迹生成器的动力学约束至少与（或比）MPC 控制器使用的模型同样严格。轨迹的生成和执行之间应该有“可行性握手”。