本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

第 56 章：步态管理与接触序列¶

难度：⭐⭐⭐ | 预计学时：25-30 小时（1.5 周）| 前置：足式/70_腿足简化模型理论, 足式/80_接触力学与约束优化, 足式/110_OCS2完整栈与双线程MPC

一句话概要：步态是腿足机器人区别于一切其他机器人形态的核心——它把"哪只脚在什么时候踩地"编码为离散接触序列，驱动整个 MPC/WBC 栈的模式切换、约束启用、以及摆动腿轨迹规划。本章从步态的数学定义出发，逐步深入到 OCS2 的工业实现、摆动腿轨迹生成算法、以及前沿的接触序列优化方法。

56.0 前置自测¶

📋 答不出 ≥ 2 题 → 先回前置章节复习

[足式/70_腿足简化模型理论] 什么是 duty factor（占空比）？它与步态速度有什么关系？四足 trot 步态的 duty factor 典型值是多少？
[足式/80_接触力学与约束优化] 互补约束 $0 \le \lambda \perp d \ge 0$ 的物理含义是什么？OCS2 如何通过预定义 ModeSchedule 来规避互补约束的非光滑性？
[足式/110_OCS2完整栈与双线程MPC] OCS2 的 ModeSchedule 数据结构包含哪两个数组？mode = 9（二进制 1001）对应哪两条腿触地？
[足式/110_OCS2完整栈与双线程MPC] SwitchedModelReferenceManager::preSolverRun() 在每次 MPC 求解前做了哪三件事？
[控制] 什么是混合系统（Hybrid System）？为什么腿足运动天然是混合系统？

56.0.1 本章目标¶

学完本章，你应该能：

用数学语言精确定义步态——ModeSchedule 编码、接触状态位掩码、相位变量
区分并参数化 6 种经典步态——walk/trot/pace/bound/gallop/pronk 的时序图与 duty factor
理解 OCS2 GaitSchedule 的完整管线——从配置文件到 MPC horizon 内的 ModeSchedule 生成
掌握 OCS2 SwitchedModelReferenceManager 的模式依赖约束激活机制——ZeroForce/ZeroVelocity 的 isActive 逻辑
实现三种摆动腿轨迹生成算法——cubic spline、Bezier 曲线、cycloid
理解接触序列优化的前沿方法——从 TOWR、Contact-Implicit TO、MCTS+TO 到实时 CI-MPC 与 RL 步态发现
处理步态切换的工程问题——过渡稳定性、模式混合、安全检查

56.0.2 本章知识导航¶

本章要解决的根问题只有一句话：如何把"哪只脚什么时候踩地"这件离散的事，编码成一个连续运行的 MPC/WBC 控制栈能够消费的数据结构，并在运行时安全地改变它。围绕这个根问题，全章的知识点构成一棵清晰的知识树：

                        步态管理与接触序列
                              |
        +---------------------+---------------------+
        |                     |                     |
   [表示层]              [生成与调度层]          [消费与前沿层]
   §56.1 数学定义         §56.3 GaitSchedule      §56.4 约束激活
   §56.2 步态分类         §56.7 步态切换工程       §56.5 摆动腿轨迹
   (mode/相位)           §56.8 legged_control实现  §56.6 接触序列优化
        |                     |                     |
   "是什么"              "怎么算、怎么换"         "MPC怎么用、怎么自动发现"

三层之间是严格的**数据流依赖**关系，这也是建议的阅读顺序：

层	节	解决的问题	输出的"产物"	被谁消费
表示层	§56.1–56.2	步态如何被编码为数字	`mode`（4-bit）、`Gait`（相位空间）	调度层
生成调度层	§56.3、§56.7、§56.8	如何生成/切换绝对时间的序列	`ModeSchedule`（事件时刻+模式）	MPC 求解器
消费前沿层	§56.4–56.6	MPC 如何用它、能否自动优化它	约束激活逻辑、摆动轨迹、最优接触序列	SQP/WBC/RL

两条阅读路径：

工程实现路径（想把 OCS2 跑起来）：§56.1 → §56.3 → §56.4 → §56.5 → §56.7 → §56.8。跳过 §56.6（接触优化是研究内容，平地部署用不到）。
研究入门路径（想做接触序列优化）：§56.1 → §56.2 → §56.6（重点）→ §56.4（理解为什么 MPC 需要预定义序列）。

本质洞察：本章的全部复杂性都来自一个矛盾——机器人世界是连续的（力、速度、位置都是实数），但接触是离散的（脚要么在地上要么不在）。所有的步态表示方法（位掩码、相位、ModeSchedule）本质上都是在回答"如何在连续的优化框架里安放这个离散事实"。理解了这个矛盾，你就理解了为什么 OCS2 选择"预定义序列 + 约束激活"（把离散性外包给调度器）、为什么 CITO 选择"互补约束"（把离散性塞进约束）、为什么 RL 选择"相位信号"（把离散性交给网络去学）——三条路线是同一个矛盾的三种化解方式。

56.0.3 前置知识桥接¶

本章重度依赖三个前置章节。这里用 2-3 句话重新激活每个核心要点，让你不翻回去也能跟上：

回顾足式/70_腿足简化模型理论：我们在那里学了 duty factor（占空比）$\beta$——一条腿支撑相占整个步态周期的比例。$\beta > 0.5$ 意味着相邻腿的支撑相有重叠，机器人总有腿着地（walk）；$\beta < 0.5$ 则可能出现腾空相（run/gallop）。本章用 $\beta$ 来量化区分六种步态，并由它推导出每条腿的相位偏移。我们还学了 SRBD/Centroidal 简化模型——把机器人近似为一个集中质量的刚体，这是本章 Raibert 落脚点公式中"base 速度"的来源。
回顾足式/80_接触力学与约束优化：我们在那里学了**互补约束** $0 \le \lambda \perp d \ge 0$——接触力 $\lambda$ 和接触间隙 $d$ 不能同时为正（要么脚离地力为零，要么脚着地间隙为零）。这个约束是非光滑的，标准梯度法无法直接处理。本章揭示 OCS2 的应对策略：预先指定接触序列，从而把互补约束"降级"为分段的等式约束（触地段强制 $d=0$，摆动段强制 $\lambda=0$）——离散性被外包给了步态调度器。§56.6 则展示了不外包、直接硬啃互补约束的另一条路（CITO）。
回顾足式/110_OCS2完整栈与双线程MPC：我们在那里学了 OCS2 的 ModeSchedule 数据结构（eventTimes + modeSequence 两个数组）和 SwitchedModelReferenceManager 的接口，以及双线程架构（MRT 线程跑高频策略评估，MPC 线程跑低频重规划）。本章是那一章的"步态侧"展开：那里我们把 ModeSchedule 当作给定的输入，这里我们打开它，讲清楚它从哪来（§56.3 的 GaitSchedule）、MPC 求解器拿到它之后怎么用（§56.4 的 isActive 机制）。

56.0.4 如果跳过本章会怎样¶

场景	跳过本章的后果
配置 OCS2 跑 ANYmal/宇树	看到 `gait.info` 里的 `modeSequence = {6, 9}` 完全不知道 6 和 9 是什么，改步态只能瞎试；改坏了 MPC 直接发散
移植 MIT Cheetah 代码到 OCS2	不知道两者腿序约定不同（Cheetah `FR=0`，OCS2 `RH=0`），trot 写成 pace，机器人左右摇摆却查不出原因
调试"摆动腿擦地/着地砸地"	不知道摆动轨迹要 $C^1$ 连续、着地速度要趋零，乱改 swing_height 治标不治本
读接触序列优化论文（TOWR/CITO）	不理解"接触序列作为优化变量"相对"预定义步态"的本质区别，把两类方法混为一谈

56.0.5 预计阅读时间¶

模式	时长	覆盖范围
精读（推导+源码+练习全做）	25–30 小时	全章，含 §56.6 前沿与全部累积项目
速读（理解主线，跳过 §56.6 公式推导）	8–10 小时	§56.1–56.5、§56.7–56.8
速查（已学过，复习特定知识点）	1–2 小时	章末速查表 + API 速查表 + 故障排查手册

56.1 步态的数学定义 ⭐¶

这一节解决什么问题：给"步态"一个严格的数学定义。不是"机器人走路的样子"这种模糊描述，而是一个可以被计算机表示、被优化器使用、被调度器生成的精确数据结构。

56.1.1 动机：为什么需要形式化定义步态？¶

考虑一个实际场景：你在给四足机器人编写 MPC 控制器。MPC 需要在未来 1 秒的 horizon 内预测机器人的运动。但问题来了——在这 1 秒内，机器人的动力学方程会因为接触状态的改变而**多次变化**：

$t = 0.0$s 到 $t = 0.2$s：左前腿（LF）和右后腿（RH）触地，右前腿（RF）和左后腿（LH）摆动
$t = 0.2$s 到 $t = 0.4$s：RF 和 LH 触地，LF 和 RH 摆动
...如此交替

MPC 求解器需要**在求解前就知道**整个 horizon 内的接触序列。如果不提前告诉它，求解器要么需要自己决定接触时序（这是一个混合整数问题，NP-hard），要么只能按当前模式做一个保守的计划。

所以我们需要一个数据结构来精确描述："在什么时刻，哪些脚触地，哪些脚摆动"。这就是步态的数学定义。

56.1.2 如果不形式化定义会怎样¶

如果没有严格的数学编码，步态描述会退化为自然语言：

❌ 模糊描述："trot 步态就是对角线的脚交替运动"

问题 1：什么时候切换？不知道。
问题 2：每对脚触地多长时间？不知道。
问题 3：有没有四脚都腾空的瞬间？不知道。
问题 4：计算机怎么用这个信息？不能。

这种描述无法输入到 MPC 求解器中，也无法用于自动生成摆动腿轨迹。我们需要把它转化为数字。

56.1.3 接触状态与位掩码编码¶

核心思想：四足机器人有 4 条腿，每条腿在任意时刻只有两种状态——触地（stance, 1）或摆动（swing, 0）。因此，任意时刻的接触配置可以用一个 **4-bit 整数**表示。这好比交通信号灯系统：每个路口有红/绿两个状态，整个城市的信号配置可以用一个位向量编码。步态调度器的角色就相当于交通管控中心——按照预定时刻表切换各路口的信号状态，保证全局交通(机器人运动)的协调性。

OCS2 的腿序约定（从高位到低位）：

位	腿	含义
bit 3	LF（左前）	1=触地, 0=摆动
bit 2	RF（右前）	1=触地, 0=摆动
bit 1	LH（左后）	1=触地, 0=摆动
bit 0	RH（右后）	1=触地, 0=摆动

由此得到 $2^4 = 16$ 种可能的接触模式（mode），编号 0 到 15：

mode	二进制	触地腿	物理含义
0	`0000`	无	全腾空（flight phase）
3	`0011`	LH, RH	后两腿触地
5	`0101`	RF, RH	右侧触地（pace 的一半）
6	`0110`	RF, LH	对角触地（trot 的一半）
7	`0111`	RF, LH, RH	仅 LF 摆动
9	`1001`	LF, RH	对角触地（trot 的另一半）
10	`1010`	LF, LH	左侧触地（pace 的另一半）
11	`1011`	LF, LH, RH	仅 RF 摆动
12	`1100`	LF, RF	前两腿触地
13	`1101`	LF, RF, RH	仅 LH 摆动
14	`1110`	LF, RF, LH	仅 RH 摆动
15	`1111`	全触地	stance（站立）

提取第 $i$ 条腿的接触状态：

\[\text{isContact}(i, \text{mode}) = \left\lfloor \frac{\text{mode}}{2^i} \right\rfloor \mod 2 = (\text{mode} \gg i)\ \&\ 1\]

这个位运算在 OCS2 源码中随处可见：

// OCS2 legged_robot: check if leg legIndex is in contact under mode
bool isContactActive(size_t legIndex, size_t mode) {
  return (mode >> legIndex) & 1;
  // legIndex: 0=RH, 1=LH, 2=RF, 3=LF
}

56.1.4 ModeSchedule——接触序列的时间编码¶

有了 mode 编码，还需要知道每个 mode 持续多久。OCS2 用 ModeSchedule 数据结构来表示一段时间内的完整接触序列：

struct ModeSchedule {
  scalar_array_t eventTimes;    // mode switch instants [t_1, t_2, ..., t_{N-1}]
  size_array_t   modeSequence;  // mode sequence        [m_0, m_1, ..., m_{N-1}]
};

数学定义：给定 $N$ 个模式段，modeSequence 记录每段的接触配置 $m_i$，eventTimes 记录相邻段之间的切换时刻 $t_i$。在时间区间 $[t_{i-1}, t_i)$ 内，系统处于模式 $m_i$。

示例：trot 步态，周期 0.4s，从 t=0 开始

时间轴:  0.0     0.2     0.4     0.6     0.8     1.0
         |-------|-------|-------|-------|-------|
mode:      6       9       6       9       6
         (RF+LH) (LF+RH) (RF+LH) (LF+RH) (RF+LH)

eventTimes  = [0.2, 0.4, 0.6, 0.8]
modeSequence = [6,   9,   6,   9,   6]

注意：eventTimes 有 $N-1$ 个元素，modeSequence 有 $N$ 个元素，因为 $N-1$ 个切换时刻将时间分成 $N$ 段。

56.1.5 相位变量（Phase Variable）¶

在周期性步态中，用**绝对时间**描述接触序列不太方便——同一个步态在不同时刻开始，eventTimes 全都不同。更自然的表达是用**归一化相位** $\phi \in [0, 1)$：

\[\phi(t) = \frac{t - t_{\text{cycle\_start}}}{T} \mod 1\]

其中 $T$ 是步态周期。在相位空间中，步态的接触序列就变成了 $[0,1)$ 区间上的分段：

Trot 步态在相位空间:
phi:  0.0         0.5         1.0
      |-----------|-----------|
      mode = 6    mode = 9
      (RF+LH)     (LF+RH)

eventPhases = [0.5]
modeSequence = [6, 9]

OCS2 的 Gait 结构正是用相位空间定义步态：

struct Gait {
  scalar_t duration;                    // gait period T (seconds)
  std::vector<scalar_t> eventPhases;    // switch points in (0, 1)
  std::vector<size_t> modeSequence;     // mode for each phase segment
};

从 Gait 生成 ModeSchedule 的过程：给定当前时间 $t_0$ 和 horizon 终止时间 $t_f$，按步态周期 $T$ 平铺（tile）相位序列，计算绝对事件时刻：

\[t_{\text{event},k} = t_{\text{cycle\_start}} + T \times \phi_k\]

这个过程在 GaitSchedule::tileModeSequenceTemplate() 中实现（56.3 节详述）。

⚠️ 编程陷阱：eventPhases 不包含 0 和 1 eventPhases 只包含 $(0, 1)$ 之间的切换点，不包含 0.0 和 1.0。如果 trot 步态有两段（mode 6 和 mode 9），eventPhases = {0.5}，不是 {0.0, 0.5, 1.0}。把 0 和 1 加进去会导致 OCS2 生成多余的零长度模式段，MPC 求解器可能因为零时长约束段而数值奇异。

💡 概念误区：相位变量不是接触力的连续近似 初学者有时把相位变量 $\phi$ 和"软接触"（soft contact）的连续松弛混淆。相位变量只是时间的归一化表示，它本身不改变接触的离散本质。在 OCS2 的框架中，接触切换仍然是**硬切换**——在 eventTime 前后，约束集合突变。相位变量只是方便描述周期性步态的工具。

练习 56.1¶

[编码题] 写出 pace 步态的 mode 序列。pace 是"同侧腿交替"：第一阶段 RF+RH 触地，第二阶段 LF+LH 触地。用位掩码计算两个 mode 值。
[推导题] 如果一个四足机器人有 6 条腿（六足），接触模式需要多少 bit？总共有多少种可能的接触配置？

💡 概念澄清：步态 ≠ 速度命令

步态（Gait）定义的是腿部接触/摆动的时序模式，与机器人的行进速度和方向无关。一个机器人可以执行 trot 步态但保持原地踏步（速度为零），也可以在同一步态下以不同速度移动。

在控制架构中，步态管理、速度命令和落足点规划是三个独立模块： - 步态管理器：决定"哪条腿在什么时候抬起/放下" - 速度命令：决定"机器人整体往哪个方向以多快速度移动" - 落足点规划器：结合步态和速度，决定"每条腿放在哪里"

56.2 经典步态分类与时序 ⭐⭐¶

这一节解决什么问题：把生物学上的步态分类翻译成工程参数——每种步态的 mode 序列、duty factor、phase offset 是什么？它们分别适合什么速度范围？

56.2.1 动机：为什么有这么多种步态？¶

观察自然界：马在不同速度下会自动切换步态——慢走（walk）-> 快步（trot）-> 跑步（canter）-> 飞奔（gallop）。这不是随意的选择，而是**能量最优**的结果——在每个速度区间，特定的步态最小化代谢成本（metabolic cost of transport）。

Hoyt & Taylor（1981）的经典实验测量了马在不同步态下的氧气消耗率，发现：

低速时 walk 最节能
中速时 trot 最节能
高速时 gallop 最节能

这意味着步态不仅是"脚的运动模式"，更是一个**能量优化问题**的解。

56.2.2 如果只用一种步态会怎样¶

❌ 场景：四足机器人在所有速度下都用 trot

问题 1：低速 trot → duty factor 不足 → 支撑相过短 → 不稳定
       解决方案：增加 duty factor → 但这已经接近 walk 了
问题 2：高速 trot → 步频过高 → 电机极限 → 速度封顶
       解决方案：增加步长 → 但腿的运动学范围有限
问题 3：非常高速 → trot 无法提供足够的推进力
       解决方案：需要 bound/gallop 的腾空相来增加步幅

结论：不同速度范围需要不同的步态，这不是人为规定，
     而是物理约束（电机极限、运动学范围、稳定性）决定的。

56.2.3 步态参数：duty factor、phase offset、stride frequency¶

**三个核心参数**完全参数化一个周期性步态：

参数	符号	定义	典型范围
Duty factor	$\beta$	每条腿的支撑相占步态周期的比例	0.3~0.8
Phase offset	$\phi_i$	第 $i$ 条腿相对于参考腿的相位延迟	$[0, 1)$
Stride period	$T$	一个完整步态周期的时长	0.3~1.2s

Duty factor 的物理意义：

$\beta > 0.5$：每条腿超过一半时间在地上 -> 总有至少一对腿同时触地 -> 无腾空相 -> 这是 walk
$\beta < 0.5$：每条腿超过一半时间在空中 -> 可能出现**腾空相**（所有腿离地）-> 这是 run/gallop
$\beta = 0.5$：边界情况，正好半触地半摆动 -> 这是 trot 的典型值

Phase offset 定义步态类型：以右后腿（RH）为参考（$\phi_{\text{RH}} = 0$），其他三条腿的相位偏移决定了步态类型。

56.2.4 六种经典步态详解¶

步态	$\phi_{\text{LF}}$	$\phi_{\text{RF}}$	$\phi_{\text{LH}}$	$\phi_{\text{RH}}$	duty factor	OCS2 mode 序列	速度范围
Walk	0.75	0.25	0.50	0.0	0.6~0.8	[14,13,11,7]	<1 m/s
Trot	0.5	0.0	0.0	0.5	0.4~0.6	[6, 9]	1~3 m/s
Pace	0.0	0.5	0.5	0.0	0.4~0.6	[10, 5]	1~3 m/s
Bound	0.5	0.5	0.0	0.0	0.3~0.5	[12, 3]	2~5 m/s
Pronk	0.0	0.0	0.0	0.0	0.3~0.5	[15, 0]	跳跃
Gallop	0.6	0.1	0.5	0.0	0.2~0.4	[7,14,13,11,...]	>5 m/s

时序图（Gait Diagram）：

Walk (beta=0.7, T=1.0s):
LF: ████████████████░░░░░░████████████████░░░░░░   duty=0.7
RF: ░░░░░░████████████████░░░░░░████████████████   offset=0.25
LH: ████░░░░░░████████████████░░░░░░████████████   offset=0.50
RH: ████████████████████░░░░░░████████████████░░   offset=0.0 (ref)
    |------ T=1.0s ------|------ T=1.0s ------|
    * = stance (contact)     - = swing (aerial)
    ^ at any instant >= 3 feet on ground -> statically stable

Trot (beta=0.5, T=0.4s):
LF: ██████████░░░░░░░░░░██████████░░░░░░░░░░     offset=0.5
RF: ░░░░░░░░░░██████████░░░░░░░░░░██████████     offset=0.0
LH: ░░░░░░░░░░██████████░░░░░░░░░░██████████     offset=0.0
RH: ██████████░░░░░░░░░░██████████░░░░░░░░░░     offset=0.5
    |--- T=0.4s ---|--- T=0.4s ---|
    ^ diagonal legs synchronous -> dynamically stable

Bound (beta=0.4, T=0.3s):
LF: ░░░░░░████████░░░░░░░░░░░░████████░░░░░░
RF: ░░░░░░████████░░░░░░░░░░░░████████░░░░░░     front legs sync
LH: ████████░░░░░░░░░░░░████████░░░░░░░░░░░░
RH: ████████░░░░░░░░░░░░████████░░░░░░░░░░░░     rear legs sync
    |--- T=0.3s ---|
    ^ front-rear alternation, possible flight phases

Pronk (beta=0.3, T=0.3s):
LF: ██████░░░░░░░░░░░░░░██████░░░░░░░░░░░░░░
RF: ██████░░░░░░░░░░░░░░██████░░░░░░░░░░░░░░     all synchronized
LH: ██████░░░░░░░░░░░░░░██████░░░░░░░░░░░░░░
RH: ██████░░░░░░░░░░░░░░██████░░░░░░░░░░░░░░
    ^ all four feet jump and land together, like a kangaroo

56.2.5 Gallop——最复杂的自然步态¶

Gallop（飞奔）是所有步态中最复杂的，因为它**打破了对称性**。马的 gallop 有两种变体：

Transverse gallop（横向飞奔）：前后腿着地顺序**同侧** - 着地顺序：RH -> LH -> RF -> LF（后腿先着地，同侧前腿后着地）

Rotary gallop（旋转飞奔）：前后腿着地顺序**对侧** - 着地顺序：RH -> LH -> LF -> RF（后腿先着地，对侧前腿后着地）

关键特征：gallop 通常包含一个或两个**腾空相**（flight phase, mode=0），在所有腿都离地时机器人完全处于抛体运动。这是 gallop 能达到最高速度的原因——腾空相中没有地面摩擦的减速。

Transverse Gallop (beta=0.3, T=0.35s):
LF: ░░░░░░░░░░░░████░░░░░░░░░░░░░░░░░░████░░
RF: ░░░░░░░░████░░░░░░░░░░░░░░░░░░████░░░░░░
LH: ░░░░████░░░░░░░░░░░░░░░░░░████░░░░░░░░░░
RH: ████░░░░░░░░░░░░░░░░░░████░░░░░░░░░░░░░░
    ^^^                   ^^^
    flight phase          flight phase
    (mode=0)              (mode=0)

56.2.6 Flying Trot——工程常用的带腾空 trot¶

在实际四足机器人工程中，flying trot 是除标准 trot 之外最常用的步态。它在两组对角腿切换时插入一个短暂的腾空相：

Flying Trot:
eventPhases = [0.27, 0.36, 0.91, 1.00]  (approximate)
# 注意：此处 1.00 表示周期末尾，在 OCS2 实现中会映射回 0.00（周期起点）
modeSequence = [6, 0, 9, 0]
              RF+LH  fly  LF+RH  fly

OCS2 gait.info configuration:
switchingTimes = {0.00, 0.15, 0.20, 0.50, 0.55}
modeSequence = {6, 0, 9, 0}

Flying trot 的优势在于腾空相让机器人的 CoM 有一个短暂的"自由飞行"阶段，减少了地面约束力对运动的限制，从而允许更大的步幅和更高的速度。代价是控制更难——着地时刻的冲击力更大。

从 mode 序列反推 duty factor（一个完整的手算示例）：很多人会"读" mode 表，但不会从 switchingTimes + modeSequence 反算每条腿的 duty factor。这里以 flying trot 为例走一遍完整计算，方法对任意步态通用。

给定 switchingTimes = {0.00, 0.15, 0.20, 0.50, 0.55}、modeSequence = {6, 0, 9, 0}，周期 $T = 0.55$s。各段时长和触地腿（用 §56.1 的 (mode >> i) & 1 读出，OCS2 腿序 RH=0, LH=1, RF=2, LF=3）：

段	时间区间	时长	mode	二进制	触地腿
1	[0.00, 0.15)	0.15s	6	`0110`	RF, LH
2	[0.15, 0.20)	0.05s	0	`0000`	无（腾空）
3	[0.20, 0.50)	0.30s	9	`1001`	LF, RH
4	[0.50, 0.55)	0.05s	0	`0000`	无（腾空）

对每条腿，把它**触地**的段时长加起来，除以周期 $T$：

\[\beta_{\text{LF}} = \frac{t_{\text{段3}}}{T} = \frac{0.30}{0.55} \approx 0.545, \quad \beta_{\text{RH}} = \frac{0.30}{0.55} \approx 0.545\]

\[\beta_{\text{RF}} = \frac{t_{\text{段1}}}{T} = \frac{0.15}{0.55} \approx 0.273, \quad \beta_{\text{LH}} = \frac{0.15}{0.55} \approx 0.273\]

得到一个反直觉的结论：这个"flying trot"的四条腿 duty factor 并不相等（LF/RH 约 0.55，RF/LH 约 0.27），因此它其实不是对称 trot，而是偏向某一对角腿的非对称步态。腾空相总占比 $\frac{0.05+0.05}{0.55} \approx 18\%$——这就是它能"飞"的原因。

本质洞察：duty factor 不是步态的"输入参数"，而是从接触序列**导出的统计量**。在 OCS2/MIT Cheetah 这类工程实现中，你直接写的是 switchingTimes（事件时刻），duty factor 是它的副产品。反过来，在 §56.6.6 的 DeepGait 这类参数化 RL 中，你直接给 duty factor，再由它反推切换时刻。两个方向都成立，但**永远只有一个是自由变量，另一个是导出量**——搞混"我在设定哪个、计算哪个"是步态调参最常见的概念混乱来源。

56.2.7 历史脉络：从 Muybridge 到现代机器人¶

步态研究的历史远早于机器人学：

年份	人物/事件	贡献
1878	Eadweard Muybridge	用连续摄影首次证明马在 gallop 时存在四脚腾空相
1899	Etienne-Jules Marey	用第一台电影摄像机系统性记录动物步态
1981	Hoyt & Taylor	用代谢率实验证明步态切换是能量最优的
1986	Marc Raibert (MIT)	出版 "Legged Robots That Balance"，首次实现机器人 trot/bound/pronk
2018	Di Carlo et al. (MIT)	用凸 MPC 实现 Cheetah 3 的实时 trot 控制
2022	Margolis & Agrawal	"Walk These Ways"——用 RL 学习任意步态参数化

🧠 思维陷阱：不是所有步态都对称 初学者容易假设步态一定是对称的——左右脚运动规律相同。这对 trot、pace、bound、pronk 是正确的，但对 walk 和 gallop 不成立。Walk 中四条腿的相位各不相同（0, 0.25, 0.5, 0.75），gallop 更是完全不对称。OCS2 的 Gait 数据结构通过自由设置 eventPhases 和 modeSequence，天然支持不对称步态——没有强制对称约束。

⚠️ 编程陷阱：mode 编码的腿序因项目而异 不同开源项目的腿序编号不同！OCS2 用 LF=bit3, RF=bit2, LH=bit1, RH=bit0，但 MIT Cheetah Software 用 FR=0, FL=1, HR=2, HL=3，legged_gym 又用另一种顺序。在移植代码时，必须先核实腿序约定，否则 trot 变 pace、walk 变乱序。这个错误在调试时非常隐蔽——机器人不会报错，只是"走路姿势奇怪"。

💡 概念误区：pace 和 trot 速度范围相同，但稳定性不同 Pace（同侧腿交替）和 trot（对角腿交替）的 duty factor 和速度范围确实相似，但它们的稳定性差异很大。Trot 的对角支撑提供了 roll 方向的稳定性（左前+右后 or 右前+左后形成对角支撑线），而 pace 的同侧支撑在 roll 方向是不稳定的——机器人容易左右摇摆。这就是为什么大多数四足机器人默认使用 trot 而不是 pace。

练习 56.2¶

[计算题] 对 flying trot 步态（modeSequence = {6, 0, 9, 0}，switchingTimes = {0.0, 0.15, 0.20, 0.50, 0.55}），计算每条腿的 duty factor。提示：需要分析每种 mode 中各腿的触地状态。
[设计题] 设计一种"tripod gait"（三足步态），使得任意时刻恰好 3 条腿触地、1 条腿摆动。写出 mode 序列和 switchingTimes（假设等时长切换，周期 1.0s）。
[思考题] 为什么 pronk 步态（四脚同时跳）很少用于实际四足机器人的行走？从稳定性和能量效率两个角度分析。

56.3 步态参数化与调度 ⭐⭐¶

上一节定义了步态的分类学——各种步态的时序图和参数。但仅有分类还不够:MPC 求解器不关心步态叫什么名字，它需要一段具体的、带有绝对时间戳的接触序列。从"步态定义"到"MPC 可用的 ModeSchedule",中间需要一个调度层来完成实时翻译。

这一节解决什么问题：OCS2 如何把一个步态定义（Gait 结构体）转化为 MPC 需要的 ModeSchedule？运行时如何切换步态？

56.3.1 动机：从 Gait 到 ModeSchedule 的鸿沟¶

前面我们定义了 Gait——一个周期性步态在相位空间中的描述。但 MPC 求解器需要的是 ModeSchedule——一段**绝对时间**上的模式序列。从 Gait 到 ModeSchedule 需要解决三个问题：

平铺（Tiling）：MPC horizon 可能跨越多个步态周期，需要把单周期 Gait 重复平铺
对齐（Alignment）：当前时刻可能不在步态周期的起点，需要找到正确的相位
切换（Switching）：用户可能在运行时改变步态（比如从 trot 切到 walk），需要平滑过渡

56.3.2 如果不用调度器，直接手写 ModeSchedule 会怎样¶

❌ 手动构建 ModeSchedule 的问题:

// trot, T=0.4s, horizon=2.0s -> 需要手写 10 个 mode 段
ModeSchedule ms;
ms.eventTimes = {0.2, 0.4, 0.6, 0.8, 1.0, 1.2, 1.4, 1.6, 1.8};
ms.modeSequence = {6, 9, 6, 9, 6, 9, 6, 9, 6, 9};

问题 1: 步态周期改变时需要重写所有时间点
问题 2: 运行时切换步态需要整段替换
问题 3: 多机器人/多步态时代码爆炸
问题 4: 容易写错 -> mode 数量和 eventTime 数量不匹配 -> 段错误

✅ 用 GaitSchedule 自动生成:
gaitSchedule.getModeSchedule(t_current, t_current + horizon);
// 自动处理平铺、对齐、切换

56.3.3 OCS2 的 GaitSchedule 类¶

GaitSchedule 是 OCS2 中管理步态生成的核心类。它的职责是：根据当前时间和 MPC horizon，生成覆盖整个 horizon 的 ModeSchedule。

class GaitSchedule {
public:
  // Constructor: initial ModeSchedule + gait template
  GaitSchedule(ModeSchedule initModeSchedule,
               ModeSequenceTemplate initModeSequenceTemplate,
               scalar_t phaseTransitionStanceTime);

  // Core method: generate ModeSchedule for a time window
  ModeSchedule getModeSchedule(scalar_t lowerBoundTime,
                                scalar_t upperBoundTime);

  // Runtime gait switching: insert a new gait template
  void insertModeSequenceTemplate(
      const ModeSequenceTemplate& newTemplate,
      scalar_t startTime,
      scalar_t finalTime);

private:
  // Tile the gait template across a time range
  void tileModeSequenceTemplate(scalar_t startTime,
                                scalar_t finalTime);

  ModeSchedule modeSchedule_;
  ModeSequenceTemplate modeSequenceTemplate_;
  scalar_t phaseTransitionStanceTime_;
};

ModeSequenceTemplate 是 GaitSchedule 内部使用的步态模板，用绝对时间差表示切换：

struct ModeSequenceTemplate {
  scalar_array_t switchingTimes;   // mode switch time offsets
  size_array_t modeSequence;       // mode sequence
};

56.3.4 平铺算法（Tiling）详解¶

tileModeSequenceTemplate() 的工作过程如下：

输入: gait template (switchingTimes=[0.0, 0.2, 0.4], modeSequence=[6, 9])
      startTime = 1.0, finalTime = 2.0

Step 1: compute gait period
  T = switchingTimes.back() = 0.4s

Step 2: tile template cycle by cycle
  cycle 1: eventTimes += [1.2, 1.4],  modes += [6, 9]
  cycle 2: eventTimes += [1.6, 1.8],  modes += [6, 9]
  cycle 3: eventTimes += [2.0, 2.2],  modes += [6, 9]

Step 3: truncate to finalTime
  keep eventTimes <= 2.0

Step 4: append terminal stance mode (15)
  ensures ModeSchedule ends in a safe stance phase

output: eventTimes = [1.2, 1.4, 1.6, 1.8, 2.0]
        modeSequence = [6, 9, 6, 9, 6, 15]

为什么要追加 stance mode？ 这是一个安全机制——如果 MPC 的 horizon 结束时恰好在摆动相，最后一段 stance 保证了终端状态是稳定的。MPC 的终端代价（terminal cost）通常也假设终端状态是 stance 模式。

56.3.5 运行时步态切换¶

当用户通过 ROS topic 发送"切换到 walk 步态"的命令时，insertModeSequenceTemplate() 被调用：

void GaitSchedule::insertModeSequenceTemplate(
    const ModeSequenceTemplate& newTemplate,
    scalar_t startTime, scalar_t finalTime) {
  // 1. Find insertion index: where startTime falls in current schedule
  auto insertIdx = findInsertionIndex(startTime);

  // 2. Erase everything after insertion point
  modeSchedule_.eventTimes.erase(/* from insertIdx */);
  modeSchedule_.modeSequence.erase(/* from insertIdx */);

  // 3. Optional: insert transition stance phase
  //    if current mode is not full stance (mode != 15),
  //    insert a brief all-contact phase as buffer
  if (currentMode != STANCE_MODE) {
    insertTransitionStance(phaseTransitionStanceTime_);
  }

  // 4. Update template to new gait
  modeSequenceTemplate_ = newTemplate;

  // 5. Tile the new gait from adjusted start time
  tileModeSequenceTemplate(adjustedStartTime, finalTime);
}

过渡 stance 相的物理意义：直接从 trot 切到 walk 可能导致某条腿从"摆动中途"突然被要求触地，这在物理上是危险的——腿还在空中呢！插入一个短暂的全触地 stance 相（典型持续 0.1~0.3s）让所有腿先落地，再开始新步态，大大提高了安全性。

步态切换时间轴 (trot -> walk):

旧步态:  ... mode=6 | mode=9 |
                             ↓ 插入过渡 stance
                       mode=15 (全触地, 0.2s)
                             ↓ 开始新步态
新步态:               mode=14 | mode=13 | mode=11 | mode=7 | ...

56.3.6 OCS2 gait.info 配置文件¶

OCS2 的步态通过配置文件 gait.info 定义，避免了修改代码的需要：

; OCS2 legged_robot gait.info (ANYmal example)
; Path: ocs2_legged_robot/config/gait/default.info

list
{
  [0] stance
  [1] trot
  [2] standing_trot
  [3] flying_trot
  [4] pace
  [5] dynamic_walk
  [6] static_walk
  [7] amble
  [8] lindyhop
  [9] bound
}

; Trot gait definition
trot
{
  modeSequence
  {
    [0] 6    ; RF+LH contact (0b0110)
    [1] 9    ; LF+RH contact (0b1001)
  }
  switchingTimes
  {
    [0] 0.00
    [1] 0.35
    [2] 0.70
  }
}

; Dynamic Walk gait definition
dynamic_walk
{
  modeSequence
  {
    [0] 7     ; RF+LH+RH (0b0111), LF swing
    [1] 14    ; LF+RF+LH (0b1110), RH swing
    [2] 13    ; LF+RF+RH (0b1101), LH swing
    [3] 11    ; LF+LH+RH (0b1011), RF swing
  }
  switchingTimes
  {
    [0] 0.00
    [1] 0.25
    [2] 0.50
    [3] 0.75
    [4] 1.00
  }
}

; Flying Trot (with aerial phases)
flying_trot
{
  modeSequence
  {
    [0] 6     ; RF+LH
    [1] 0     ; full flight
    [2] 9     ; LF+RH
    [3] 0     ; full flight
  }
  switchingTimes
  {
    [0] 0.00
    [1] 0.15
    [2] 0.20
    [3] 0.50
    [4] 0.55
  }
}

配置文件的设计思想：把步态参数从 C++ 代码中分离出来，使得用户可以在不重新编译的情况下修改步态周期、切换时序。对于实验调参来说这极其方便——修改 .info 文件，重启节点即可生效。

💡 概念误区：switchingTimes 和 eventTimes 不是同一个东西 switchingTimes 在 gait.info 中定义步态模板，第一个元素总是 0.0，最后一个元素是步态周期 T。它是**相对于周期起点**的时间偏移。eventTimes 在 ModeSchedule 中记录**绝对时间**的模式切换时刻。从 switchingTimes 到 eventTimes 的转换发生在 tiling 过程中。混淆二者是常见的 bug 来源。

⚠️ 编程陷阱：步态周期改变时忘记调整 MPC horizon 如果把步态从 trot（T=0.4s）切到 walk（T=1.0s），MPC 的 horizon 长度可能需要调整。OCS2 默认 horizon 是固定的（比如 1.0s），如果 walk 的周期也是 1.0s，那么 horizon 内只包含一个完整步态周期——对于 walk 来说可能不够（MPC 看不到下一个周期的计划）。一般建议 horizon 至少覆盖 1.5~2 个步态周期。

🧠 思维陷阱：认为 gait.info 是 OCS2 唯一的步态配置方式 gait.info 只是 ocs2_legged_robot 示例项目的配置方式。OCS2 的核心库（ocs2_oc, ocs2_mpc 等）对步态配置方式没有任何限制——你可以从 YAML、ROS parameter、甚至网络接口读取步态参数。gait.info 使用的是 OCS2 自带的 Boost property tree 解析器，与 ROS 的参数系统无关。

练习 56.3¶

[编程题] 给定一个 ModeSequenceTemplate（switchingTimes 和 modeSequence），写一个 C++ 函数 ModeSchedule tileTemplate(const ModeSequenceTemplate& tmpl, double t0, double tf) 实现平铺算法。注意处理 t0 不在周期起点的情况。
[分析题] 如果 MPC horizon 是 0.8s，trot 周期是 0.4s（switchingTimes = {0.0, 0.2, 0.4}），那么 ModeSchedule 会包含多少个 mode 段？多少个 eventTime？画出时间轴。
[思考题] 过渡 stance 相的持续时间 phaseTransitionStanceTime 设多长合适？太短和太长分别有什么问题？

56.4 OCS2 SwitchedModelReferenceManager ⭐⭐⭐¶

这一节解决什么问题：深入理解 OCS2 腿足控制的"大脑"——SwitchedModelReferenceManager。它如何把步态信息传递给 MPC 求解器？MPC 如何根据当前 mode 激活/禁用约束？

56.4.1 动机：MPC 需要知道未来的接触序列¶

回顾足式/110_OCS2完整栈与双线程MPC：OCS2 的 MPC 求解的是一个**分段最优控制问题**。在每一段内，动力学和约束是固定的；段与段之间，约束集合会因为接触模式的改变而突变。

MPC 求解器在求解前需要知道两件事： 1. 在哪些时刻发生模式切换？（eventTimes） 2. 每一段的约束集合是什么？（由 modeSequence 决定）

如果 MPC 不知道未来的接触序列，它就无法在摆动腿即将着地时提前规划接触力——这会导致着地瞬间出现巨大的力跳变。

56.4.2 SwitchedModelReferenceManager 的完整管线¶

User command (ROS Topic)       GaitSchedule
      |                          |
      v                          v
+------------------------------------------+
|  SwitchedModelReferenceManager           |
|                                          |
|  preSolverRun(t0, tf, x0):              |
|    (1) gaitSchedule_->getModeSchedule() |
|       -> generate ModeSchedule           |
|    (2) generateTargetTrajectories()      |
|       -> CoM reference from user command |
|    (3) updateSwingTrajectories()         |
|       -> swing foot trajectory per leg   |
|                                          |
|  getModeSchedule()  -> queried by MPC    |
|  getTargetTrajectories() -> queried      |
+------------------------------------------+
          |
          v
    MPC Solver (SQP/iLQR)
    - solves per-segment OCPs along ModeSchedule
    - queries isActive(mode) to activate constraints

步骤 1：生成 ModeSchedule

这一步调用 GaitSchedule::getModeSchedule()，按照 56.3 节描述的平铺算法生成覆盖整个 MPC horizon 的 ModeSchedule。OCS2 的实现中，请求范围稍大于实际 horizon——在 SwitchedModelReferenceManager::modifyReferences() 中，调用是：

// ocs2_legged_robot: SwitchedModelReferenceManager::modifyReferences
const auto modeSchedule = gaitSchedulePtr_->getModeSchedule(
    initTime - timeHorizon,        // 向过去扩 1 个 horizon
    finalTime + timeHorizon);      // 向未来扩 1 个 horizon

为什么要向两侧各扩一个 timeHorizon？ 这是一个容易被忽略但至关重要的工程细节。MPC 的实际求解窗口是 [initTime, finalTime]，按理只需生成这段的 ModeSchedule。但 OCS2 故意向两侧各扩展一个 timeHorizon，原因有三：

插值边界安全：SQP 求解器在做时间离散化时，靠近窗口端点的节点需要查询略微超出窗口的 mode（比如用前向差分计算端点导数）。如果 ModeSchedule 恰好在 finalTime 截断，端点查询会越界。向后扩展保证了端点附近的 getModeAtTime() 总能命中有效段。
时间推进的连续性：MPC 是滚动求解的——这一拍的 finalTime 是下一拍的窗口中部。预先生成更大范围避免了每拍都在窗口边缘重新平铺，减少了 eventTimes 序列的抖动。
向过去扩展处理"刚切换"：当步态刚被切换时，initTime 之前的模式信息（上一个 stance phase 的结束时刻）是计算当前摆动腿"起飞点"所必需的——摆动腿轨迹的起点是它上一次离地的位置。

OCS2 还提供了一个公开方法 setModeSchedule(const ModeSchedule&)，允许外部直接灌入一段完整的 ModeSchedule（绕过 GaitSchedule 的平铺逻辑）。这在两种场景下有用：(1) 接触序列优化器（§56.6）算出了一段非周期的最优序列，直接 setModeSchedule 注入；(2) 单元测试中构造确定性的 ModeSchedule 来验证约束激活逻辑。

⚠️ 编程陷阱：把 horizon 余量误当作 MPC 真正的预测长度 初学者看到 initTime - timeHorizon 到 finalTime + timeHorizon 这个 2 × timeHorizon + (finalTime - initTime) 的范围，常误以为 MPC 真的在这么长的窗口上求解，于是去调小 timeHorizon 想"减少计算量"。后果：MPC 的实际预测长度（finalTime - initTime）由 task.info 中的 mpc.timeHorizon 单独控制，与这里的余量无关；调小余量反而可能触发端点越界。生成范围 ≠ 求解范围——前者是为数值安全留的缓冲，后者才是 MPC 真正"看多远"。

步骤 2：生成参考轨迹

从用户的速度命令（通过 ROS topic 或 joystick 输入）生成 base 的参考位姿轨迹：

TargetTrajectories generateTrajectory(
    const vector_t& currentState,
    const vector_t& userCommand,     // [v_x, v_y, yaw_rate]
    scalar_t initTime, scalar_t finalTime) {
  // integrate velocity command to get position trajectory
  TargetTrajectories target;
  Eigen::Vector3d pos = currentState.head<3>();
  double yaw = currentState(3);
  for (auto t : linspace(initTime, finalTime, N)) {
    pos.head<2>() += userCommand.head<2>() * dt;
    yaw += userCommand(2) * dt;
    target.push_back(makeDesiredState(pos, yaw));
  }
  return target;
}

步骤 3：更新摆动腿轨迹

对每条在 MPC horizon 内处于摆动状态的腿，计算其足端参考轨迹（56.5 节详述）。这需要知道： - 摆动起点：上一个 stance phase 结束时的足端位置 - 摆动终点：下一个 stance phase 的目标落脚点（Raibert heuristic） - 摆动高度：用户配置的抬腿高度参数

56.4.3 模式依赖约束——isActive 机制¶

OCS2 的约束系统有一个关键特性：约束可以根据当前 mode 动态激活或禁用。这是通过 StateInputConstraint::isActive(t) 方法实现的。

ZeroForceConstraint——摆动腿零力约束：

class ZeroForceConstraint : public StateInputConstraint {
  bool isActive(scalar_t t) const override {
    // get mode at time t
    size_t mode = referenceManager_->getModeSchedule()
                      .getModeAtTime(t);
    // if this leg is NOT in contact -> constraint active
    return !isContactActive(legIndex_, mode);
    // constraint: contact force lambda_i must be zero
  }

  vector_t getValue(scalar_t t,
                    const vector_t& x,
                    const vector_t& u) const override {
    // extract force for leg legIndex_ from control input u
    return extractContactForce(u, legIndex_);
    // constraint: f_i = 0 (swing leg cannot exert force)
  }
};

ZeroVelocityConstraint——触地腿零速度约束：

class ZeroVelocityConstraint : public StateInputConstraint {
  bool isActive(scalar_t t) const override {
    size_t mode = referenceManager_->getModeSchedule()
                      .getModeAtTime(t);
    // if this leg IS in contact -> constraint active
    return isContactActive(legIndex_, mode);
    // constraint: foot-end velocity must be zero (no slip)
  }

  vector_t getValue(scalar_t t,
                    const vector_t& x,
                    const vector_t& u) const override {
    // compute foot velocity via kinematics
    return computeFootVelocity(x, u, legIndex_);
    // constraint: v_foot = J_foot * qdot = 0
  }
};

约束激活矩阵——不同 mode 下哪些约束激活：

mode	二进制	LF ZeroForce	RF ZeroForce	LH ZeroForce	RH ZeroForce	LF ZeroVel	RF ZeroVel	LH ZeroVel	RH ZeroVel
6	0110	ON	OFF	OFF	ON	OFF	ON	ON	OFF
9	1001	OFF	ON	ON	OFF	ON	OFF	OFF	ON
15	1111	OFF	OFF	OFF	OFF	ON	ON	ON	ON
0	0000	ON	ON	ON	ON	OFF	OFF	OFF	OFF

物理直觉： - 触地腿有两个约束：可以施力（ZeroForce OFF），但不能滑动（ZeroVelocity ON） - 摆动腿有两个约束：不能施力（ZeroForce ON），但可以自由移动（ZeroVelocity OFF） - 这两种约束是**互补的**——同一条腿的 ZeroForce 和 ZeroVelocity 永远不会同时激活

这个"按 mode 切换约束"的机制就是 OCS2 "Switched Systems" 抽象的精髓——不改变 OCP 的结构，只改变哪些约束参与求解。

本质洞察:OCS2 处理接触模式切换的精妙之处在于:它把离散的模式切换完全转化为约束的激活/休眠，而非切换不同的动力学方程。所有模式共享同一个 OCP 骨架(同样的决策变量、同样的动力学),区别仅在于哪些约束行是"活的"。这样做的好处是:SQP 求解器不需要知道"模式"这个概念——它只看到一个随时间变化的约束集合。模式切换的复杂性被封装在 isActive() 函数中，与求解器完全解耦。

56.4.4 摩擦锥约束也按 mode 切换¶

除了零力/零速度约束，摩擦锥约束（Friction Cone Constraint）也需要按 mode 动态切换：

class FrictionConeConstraint : public StateInputConstraint {
  bool isActive(scalar_t t) const override {
    size_t mode = referenceManager_->getModeSchedule()
                      .getModeAtTime(t);
    // only stance legs need to satisfy friction cone
    return isContactActive(legIndex_, mode);
  }

  // Constraint: ||f_tangential|| <= mu * f_normal
  // OCS2 uses analytic second-order cone (SOC), not linearized
};

这意味着在 mode=6（RF+LH 触地）时，只有 RF 和 LH 的摩擦锥约束激活；LF 和 RH 的摩擦锥约束不参与求解（因为它们的力已经被零力约束强制为零了）。

💡 概念误区：以为约束切换导致 QP 问题维度改变 在 OCS2 的 SQP 实现中，约束是否激活不改变 QP 的维度——非激活约束的残差被设为零、雅可比被设为零矩阵。这样 HPIPM 的问题结构保持不变，避免了运行时动态分配内存。这是一个重要的工程优化——动态改变 QP 维度意味着每次切换都要重新分配矩阵，在 1kHz 控制循环中这是不可接受的。

🧠 思维陷阱：认为 SwitchedModelReferenceManager 只管步态 SwitchedModelReferenceManager 管理的不只是步态，还有**参考轨迹**和**摆动腿轨迹**。这三者是紧密耦合的：步态决定了哪些腿什么时候摆动，摆动腿轨迹需要步态信息来确定起止时间，参考轨迹需要步态信息来确定支撑力的分配。把它们放在同一个 Manager 里是合理的架构决策。

⚠️ 编程陷阱：getModeAtTime 在 eventTime 边界的行为 当查询时间 $t$ 恰好等于某个 eventTime 时，它属于前一段还是后一段？OCS2 的约定是 eventTime 属于**后一段**——即区间是**左闭右开** $[t_{i-1}, t_i)$。如果你在调试时发现某个时间点的 mode 与预期不符，检查是否是边界条件问题。

练习 56.4¶

[源码阅读] 在 OCS2 仓库中找到 ZeroForceConstraint 和 ZeroVelocityConstraint 的完整实现。回答：它们是 equality constraint 还是 inequality constraint？为什么？
[分析题] 在 flying trot（modeSequence = [6, 0, 9, 0]）的腾空相（mode=0）中，有多少个约束被激活？列出所有激活的约束。这对 MPC 求解有什么影响？
[思考题] 如果 OCS2 不使用 isActive 机制，而是在每个 mode 段构建不同维度的 QP，会有什么工程问题？从内存分配和 HPIPM 的角度分析。

56.5 摆动腿轨迹生成 ⭐⭐¶

这一节解决什么问题：当一条腿处于摆动相时，它的末端应该沿什么轨迹运动？如何生成既平滑又避免擦地的摆动轨迹？

56.5.1 动机：摆动腿不是"自由"的¶

直觉上，摆动腿已经离开地面，似乎可以"随便动"。但实际上，摆动腿轨迹的设计对运动质量有巨大影响：

太低：脚擦地 -> 绊倒 -> 摔倒
太高：能量浪费 -> 关节力矩过大 -> 电机过热
不平滑：加速度突变 -> 整机振动 -> IMU 数据恶化 -> 状态估计误差增大
落点不对：与 Raibert heuristic 的期望落脚点不一致 -> 下一个支撑相失去平衡

56.5.2 如果不精心设计摆动轨迹会怎样¶

❌ 最简单的摆动轨迹: 直线插值

z(t) = h * (1 - |2t/T - 1|)   <- 三角形轨迹

问题 1: 起飞瞬间加速度无穷大 (导数不连续)
        -> 巨大的关节力矩脉冲 -> 电机过流保护 -> 步态中断
问题 2: 着地瞬间加速度无穷大
        -> 冲击力传导到 base -> IMU 数据跳变
问题 3: 顶点处加速度突变
        -> 振动

✅ 需要至少 C1 连续 (一阶导连续) 的轨迹
   理想情况下 C2 连续 (加速度连续)

56.5.3 Raibert Heuristic——确定落脚点¶

在生成摆动轨迹之前，需要先确定**落脚点**——这条腿应该在哪里着地。Raibert（1986）给出了一个至今仍广泛使用的启发式公式：

\[\boldsymbol{p}_{\text{foot}}^{\text{target}} = \boldsymbol{p}_{\text{hip}} + \frac{T_{\text{stance}}}{2} \boldsymbol{v}_{\text{base}} + k_p (\boldsymbol{v}_{\text{base}} - \boldsymbol{v}_{\text{ref}})\]

三项的物理推导：

第一项 $\boldsymbol{p}_{\text{hip}}$——髋关节在地面上的投影。如果机器人静止不动，最稳定的脚位置就是正下方——重力方向投影。这是**零速平衡点**。

第二项 $\frac{T_{\text{stance}}}{2} \boldsymbol{v}_{\text{base}}$——前馈项。考虑匀速运动的情况：在支撑相 $T_{\text{stance}}$ 内，base 移动了 $\boldsymbol{v} \cdot T_{\text{stance}}$。如果脚在支撑相中点时恰好在 base 正下方（对称摆动），那么支撑相开始时脚应该在 base 后方 $\frac{T_{\text{stance}}}{2} \boldsymbol{v}$，结束时在前方同样距离。这意味着落脚点应该比当前髋关节位置前方 $\frac{T_{\text{stance}}}{2} \boldsymbol{v}$。

Raibert foothold geometry for constant velocity motion:

      base motion direction -->
      +===============+
      |     base      |
      +===============+
           |
      -----+--------- stance midpoint: foot under base
     p_hip - v*T/2     p_hip + v*T/2
      (start)            (end = foothold target)

第三项 $k_p (\boldsymbol{v}_{\text{base}} - \boldsymbol{v}_{\text{ref}})$——反馈修正项。如果实际速度大于目标速度（$v > v_{\text{ref}}$），需要多迈一步来减速——落脚点往前移；如果实际速度小于目标速度，少迈一步来加速——落脚点往后缩。$k_p$ 是反馈增益，典型值 0.03~0.1（取决于步态周期和机器人尺寸）。

为什么这个简单公式如此有效？ 这相当于自行车骑行中的直觉：如果你骑太快，就把车把往前多打一点(多迈步减速);如果骑太慢，少打一点(少迈步加速)。Raibert heuristic 本质上是一个关于**Capture Point**（捕获点）概念的线性近似。Capture Point 理论（Pratt 2006, Koolen 2012）表明，机器人为了不摔倒，脚必须踩到一个特定的位置使得倒立摆的发散运动被抑制。

Capture Point 的严格定义（与 Raibert 公式的精确联系）：把机器人近似为线性倒立摆（Linear Inverted Pendulum, LIP）——质心高度恒为 $h$，则质心水平动力学是 $\ddot{x} = \omega^2 (x - x_{\text{foot}})$，其中

\[\omega = \sqrt{\frac{g}{h}}\]

是 LIP 的**自然频率**（注意这是发散系统——特征根 $\pm\omega$ 中有一个正根，所以倒立摆"自然倒"）。Pratt（2006）定义的**瞬时捕获点**（Instantaneous Capture Point, ICP），又称运动的**发散分量**（Divergent Component of Motion, DCM），为：

\[\boldsymbol{\xi} = \boldsymbol{x} + \frac{\dot{\boldsymbol{x}}}{\omega}\]

它的物理含义是：如果机器人立刻把脚踩到 $\boldsymbol{\xi}$ 这个位置（让 ZMP 与 $\boldsymbol{\xi}$ 重合），质心就会沿稳定的指数衰减轨迹 $\dot{\boldsymbol{x}} = \omega(\boldsymbol{\xi} - \boldsymbol{x})$ 渐近停下——发散的那一项被精确抵消了。换句话说，$\boldsymbol{\xi}$ 是"要想刹住，脚该踩哪"的解析答案。

现在对比 Raibert 公式的前馈项 $\frac{T_{\text{stance}}}{2}\boldsymbol{v}$ 和 DCM 的速度项 $\frac{\dot{\boldsymbol{x}}}{\omega}$：两者都是"在静止落脚点（hip 正下方）基础上，沿速度方向前移一段正比于速度的距离"。差别只在系数——Raibert 用 $\frac{T_{\text{stance}}}{2}$（半个支撑相时长），DCM 用 $\frac{1}{\omega} = \sqrt{h/g}$（LIP 时间常数）。对典型四足机器人（$h \approx 0.4$m，$T_{\text{stance}} \approx 0.2$s），$\frac{1}{\omega} = \sqrt{0.4/9.81} \approx 0.20$s，而 $\frac{T_{\text{stance}}}{2} = 0.1$s——同一量级。这说明 Raibert 的前馈项就是 DCM 速度项的一个工程化近似：Raibert 用容易测量的步态周期参数 $T_{\text{stance}}$ 替代了需要知道质心高度的 LIP 时间常数 $\frac{1}{\omega}$。

本质洞察：Raibert heuristic 不是最优的，而是 Capture Point 理论的"穷人版"。Capture Point 给出了基于物理（LIP 动力学）的精确落脚点，但需要在线估计质心高度和速度；Raibert 用步态周期 $T_{\text{stance}}$ 这个**预先已知的常数**替代了 LIP 时间常数 $1/\omega$，牺牲了一点精度，换来了零在线计算和对模型误差的鲁棒性。这正是工程中"足够好 vs 理论最优"权衡的经典案例——1986 年的简单公式至今仍是无数四足机器人的默认落脚点策略，恰恰因为它"够用且不挑模型"。

💡 概念误区：把 $\omega = \sqrt{g/h}$ 当成步态频率 LIP 自然频率 $\omega = \sqrt{g/h}$（单位 rad/s）和步态的角频率 $2\pi/T$（步态周期的倒数）是**两个无关的量**。前者由机器人的质心高度和重力决定，描述"倒立摆倒得多快"；后者由步态参数决定，描述"腿迈得多快"。初学者容易因为符号都叫 $\omega$ 而混淆。在 DCM 公式里，$\omega$ 永远指 LIP 自然频率。

// Raibert heuristic implementation
Eigen::Vector3d computeRaibertFoothold(
    const Eigen::Vector3d& hipPosition,      // hip position (ground proj)
    const Eigen::Vector3d& baseVelocity,     // current base velocity
    const Eigen::Vector3d& refVelocity,      // desired velocity
    double stanceDuration,                    // stance phase duration
    double feedbackGain) {                    // feedback gain kp
  Eigen::Vector3d foothold = hipPosition;
  foothold.head<2>() += 0.5 * stanceDuration
                         * baseVelocity.head<2>();       // feedforward
  foothold.head<2>() += feedbackGain
      * (baseVelocity.head<2>() - refVelocity.head<2>()); // feedback
  foothold.z() = 0.0;  // project to ground (flat terrain assumption)
  return foothold;
}

56.5.4 方法一：三次样条（Cubic Spline）——OCS2 的选择¶

OCS2 的摆动腿轨迹使用**分段三次样条**（piecewise cubic spline）。水平方向（x, y）用单段线性插值（从起点到 Raibert 落脚点），垂直方向（z）用**三段三次多项式**：

z-axis trajectory:
height
  |        .-----.  peak (max height)
  |       /       \
  |      /         \
  |     /           \
  |    /             \
  |   /               \
  ---.-----------------.---- ground
    t_liftoff  t_mid  t_touchdown
    seg1:rise  seg2:flat seg3:fall

三段划分的原因： - 段 1（上升）：从地面到抬腿高度 $h_{\text{peak}}$，确保脚迅速离开地面 - 段 2（峰值）：在最高点附近保持一段，给脚留出越过障碍物的余量 - 段 3（下降）：从 $h_{\text{peak}}$ 到地面，确保着地时垂直速度较小

每段是一个三次多项式 $z(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3$，通过边界条件确定系数：

\[z(t_0) = z_0, \quad z(t_1) = z_1, \quad \dot{z}(t_0) = v_0, \quad \dot{z}(t_1) = v_1\]

四个方程、四个未知数 $(a_0, a_1, a_2, a_3)$，唯一确定。展开求解：

\[\begin{aligned} a_0 &= z_0 \\ a_1 &= v_0 \\ a_2 &= \frac{3(z_1 - z_0) - \Delta t(2v_0 + v_1)}{\Delta t^2} \\ a_3 &= \frac{-2(z_1 - z_0) + \Delta t(v_0 + v_1)}{\Delta t^3} \end{aligned}\]

其中 $\Delta t = t_1 - t_0$。

// OCS2 CubicSpline core implementation
struct CubicSpline {
  scalar_t t0, t1;              // start/end time
  scalar_t a0, a1, a2, a3;     // polynomial coefficients

  static CubicSpline create(scalar_t t0, scalar_t z0, scalar_t v0,
                            scalar_t t1, scalar_t z1, scalar_t v1) {
    scalar_t dt = t1 - t0;
    scalar_t dt2 = dt * dt;
    scalar_t dt3 = dt2 * dt;
    CubicSpline spline;
    spline.t0 = t0;  spline.t1 = t1;
    spline.a0 = z0;
    spline.a1 = v0;
    spline.a2 = (3*(z1-z0) - dt*(2*v0+v1)) / dt2;
    spline.a3 = (-2*(z1-z0) + dt*(v0+v1)) / dt3;
    return spline;
  }

  scalar_t position(scalar_t t) const {
    scalar_t s = t - t0;
    return a0 + a1*s + a2*s*s + a3*s*s*s;
  }

  scalar_t velocity(scalar_t t) const {
    scalar_t s = t - t0;
    return a1 + 2*a2*s + 3*a3*s*s;
  }
};

56.5.5 方法二：Bezier 曲线——MIT Cheetah 的选择¶

MIT Cheetah 使用 12 控制点的 Bezier 曲线 来生成摆动轨迹。Bezier 曲线的优势是：通过调整控制点可以精确控制轨迹形状，且端点处的速度/加速度可以直接通过控制点间距控制。

$n$ 阶 Bezier 曲线的定义：

\[\boldsymbol{B}(s) = \sum_{i=0}^{n} \binom{n}{i} (1-s)^{n-i} s^i \boldsymbol{P}_i, \quad s \in [0, 1]\]

其中 $\boldsymbol{P}_i$ 是控制点，$s$ 是归一化参数。

MIT Cheetah 的 z 方向 Bezier 控制点设计（12 个控制点）：

Control:  P0  P1  P2  P3  P4  P5  P6  P7  P8  P9 P10 P11
z value:   0   0   h   h   h   h   h   h   h   h   0   0
           ^^             ^^^^                      ^^
         liftoff       peak plateau              landing
         (v=0)         (maintain height)          (v=0)

Bezier 端点速度性质：对于 $n$ 阶 Bezier 曲线，起点和终点的导数由前两个和后两个控制点决定：

\[\boldsymbol{B}'(0) = n(\boldsymbol{P}_1 - \boldsymbol{P}_0), \quad \boldsymbol{B}'(1) = n(\boldsymbol{P}_n - \boldsymbol{P}_{n-1})\]

当 $\boldsymbol{P}_0 = \boldsymbol{P}_1$ 时，$\boldsymbol{B}'(0) = 0$，即起点速度为零。这正是 MIT Cheetah 设计中 P0 = P1 = 0 和 P10 = P11 = 0 的原因——确保脚在抬起和着地时刻的垂直速度为零，减少冲击。

56.5.6 方法三：摆线（Cycloid）——传统方案¶

摆线是一个圆沿直线滚动时圆上一点的轨迹。它被广泛用于早期四足机器人的摆动腿轨迹设计，因为它有一个天然的优良特性：起止点速度为零。

x 方向（水平）：

\[x(s) = x_0 + (x_f - x_0) \left( s - \frac{1}{2\pi} \sin(2\pi s) \right)\]

z 方向（垂直）：

\[z(s) = h \cdot \frac{1 - \cos(2\pi s)}{2}\]

其中 $s = (t - t_{\text{liftoff}}) / T_{\text{swing}} \in [0, 1]$ 是归一化时间参数。

验证端点条件：$x(0) = x_0$, $x(1) = x_f$, $z(0) = z(1) = 0$, $\dot{x}(0) = \dot{x}(1) = 0$, $\dot{z}(0) = \dot{z}(1) = 0$。

56.5.7 三种方法对比¶

方法	典型使用者	参数数量	连续性	形状灵活性	计算量	地形适应
Cubic Spline	OCS2, legged_control	每段 4 个	$C^1$	中	低	容易（改端点高度）
Bezier	MIT Cheetah	12 控制点	$C^{n-1}$	高	中	中等
Cycloid	传统四足	2 个 (h, L)	$C^\infty$	低	最低	困难
5 次多项式	ANYmal parkour	每段 6 个	$C^2$	高	中	容易
优化生成	TOWR	优化变量	取决于基函数	最高	最高	最好

如何选择？

通用四足行走：Cubic spline 足够，OCS2 默认方案
高速运动/特技：Bezier 或 5 次多项式，需要精细控制加速度
简单原型验证：Cycloid 最快实现
不确定地形：优化生成，但计算代价最高

56.5.8 地形自适应摆动¶

在非平坦地形上，摆动轨迹需要**根据地形调整**：

Flat terrain:          Uneven terrain:
     .---.                .---.
    /     \              /     \      <- raised to clear obstacle
---.-------.--  --------.-------.--.--.--
                                 ^
                           terrain height known
                           (from perception / elevation map)

OCS2 的 SwingTrajectoryPlanner 支持传入 terrainHeight：

void SwingTrajectoryPlanner::update(
    const ModeSchedule& modeSchedule,
    scalar_t terrainHeight) {
  // For each swing leg:
  // 1. touchdown z = terrainHeight (not 0)
  // 2. swing height = terrainHeight + clearance
  // 3. regenerate cubic spline with new endpoints
}

⚠️ 编程陷阱：落脚点高度未考虑地形 在 Raibert heuristic 中，最后一步 foothold.z() = 0.0 假设了平地。如果地形不平，需要从高程图（elevation map）或深度相机获取落脚点处的地面高度，替换这个硬编码的 0。忽略地形高度是四足机器人在非平坦地面上绊倒的最常见原因之一。

💡 概念误区：摆动轨迹的终端速度应该为零吗？ 大多数实现都要求着地时刻的垂直速度为零（$\dot{z}(t_{\text{touchdown}}) = 0$），但这不一定是最优的。Bledt et al.（2018, MIT Cheetah 3）的实验表明，允许一个小的负向着地速度（约 -0.1 m/s）可以**主动建立接触**，减少着地时刻的不确定性。但代价是增加了冲击力。这是一个工程权衡——安全性 vs. 接触可靠性。

🧠 思维陷阱：忽视摆动腿动力学对 base 的反作用 摆动腿不是"零质量"的——它的加速运动会产生**反作用力矩**作用在 base 上。快速的摆腿动作会导致 base 的 roll/pitch 扰动。这就是为什么高频 trot 更难控制——摆腿频率越高，加速度越大，对 base 的扰动越强。MPC 在规划 base 轨迹时需要考虑这个效应，而 Centroidal Model（足式/110_OCS2完整栈与双线程MPC 的 OCS2 默认模型）通过质心动量矩阵已经隐式包含了这个耦合。

练习 56.5¶

[编程题] 实现一个 cycloid 摆动腿轨迹生成器（C++ 或 Python）。输入：起点 $(x_0, 0)$，终点 $(x_f, 0)$，最大抬腿高度 $h$，摆动时长 $T$。输出：给定时间 $t$ 的 $(x, z)$ 位置和速度。
[对比题] 在同一组参数下（$h=0.1$m, 步长 0.2m, $T=0.2$s），分别用 cubic spline 和 cycloid 生成摆动轨迹。用 Python matplotlib 绘制两条轨迹的 $z(t)$、$\dot{z}(t)$、$\ddot{z}(t)$ 曲线，比较最大加速度。
[思考题] 为什么 OCS2 选择 cubic spline 而不是 Bezier 或 cycloid？从 MPC 对轨迹梯度信息的需求角度分析（提示：MPC 需要摆动腿轨迹对时间的导数来计算约束雅可比）。

56.6 接触序列优化 ⭐⭐⭐⭐¶

这一节解决什么问题：在 OCS2 等框架中，步态是预定义的参数——MPC 不优化"什么时候踩地"。但如果地形未知、任务复杂，我们希望优化器**自己发现**最优的接触序列。这是腿足运动规划的前沿问题。

56.6.1 动机：为什么需要优化接触序列？¶

预定义步态（56.3 节的 GaitSchedule）在平地行走时工作良好，但面对以下场景会力不从心：

跳跃上台阶：需要从四脚 stance 切换到四脚腾空再到着陆，这不是标准步态中的模式
不规则地形：某些落脚位置不可用（有坑洞/障碍），步态时序需要调整
推动物体：loco-manipulation 任务中，可能需要一条前腿始终用于推动，剩余三腿维持平衡
能量最优步态发现：如何证明 trot 在某个速度范围内确实是最优步态？

56.6.2 如果不优化接触序列会怎样¶

Scenario: quadruped facing a 0.3m wide ditch

Predefined trot gait:
  trot period 0.4s, step length 0.15m
  -> step length insufficient to cross
  -> robot stops at ditch edge, unable to proceed

Required strategy:
  1. Slow down, increase step length
  2. Switch to a different gait with longer stance
  3. Or: use a leap (all legs push off, flight, land)
  -> All three require modifying the contact sequence
  -> Predefined gaits cannot adapt

56.6.3 路线一：TOWR——接触时序作为优化变量¶

Winkler et al.（2018, RA-L, "Gait and Trajectory Optimization for Legged Systems Through Phase-Based End-Effector Parameterization"）提出的 TOWR 框架是接触序列优化的里程碑工作。

核心思想：不把接触时序作为固定参数，而是把每条腿的"支撑相持续时间"和"摆动相持续时间"作为连续优化变量。

TOWR 的参数化：

\[\text{For each leg } i: \quad \boldsymbol{\tau}_i = [\Delta t_{\text{stance}}^1, \Delta t_{\text{swing}}^1, \Delta t_{\text{stance}}^2, \Delta t_{\text{swing}}^2, \ldots]\]

其中 $\Delta t_{\text{stance}}^k$ 和 $\Delta t_{\text{swing}}^k$ 分别是第 $k$ 个支撑相和摆动相的持续时间。

优化问题：

\[\min_{\boldsymbol{x}(t), \boldsymbol{u}(t), \boldsymbol{\tau}} \int_0^T \|\boldsymbol{u}(t)\|^2 dt\]

subject to: - 动力学约束：$\dot{\boldsymbol{x}} = f(\boldsymbol{x}, \boldsymbol{u})$ - 接触约束：触地时速度为零，摆动时力为零 - 运动学约束：足端在可达范围内 - 时序约束：$\Delta t_{\text{stance}}^k, \Delta t_{\text{swing}}^k > 0$

TOWR 的关键创新：把离散的接触模式切换问题转化为**连续**优化问题（通过相位持续时间参数化），使得标准的 NLP 求解器（如 IPOPT）可以直接求解。

TOWR optimization variable hierarchy:
+-------------------------------------+
| Base motion: position, orientation  | <- dynamics constraints
| (polynomial coefficients)           |
+-------------------------------------+
| Foot forces: GRF per leg            | <- friction cone constraints
| (polynomial coefficients)           |
+-------------------------------------+
| Foot motion: position per leg       | <- kinematics constraints
| (polynomial coefficients)           |
+-------------------------------------+
| Contact timing: stance/swing durations | <- non-negativity
+-------------------------------------+

56.6.4 路线二：Contact-Implicit TO——让优化器自己"发现"接触¶

Posa, Cantu & Tedrake（2014, IJRR, "A Direct Method for Trajectory Optimization of Rigid Bodies Through Contact"）提出了 Contact-Implicit Trajectory Optimization（CITO）的概念。

核心思想：不预定义接触序列，也不参数化接触时序。而是把**接触力**和**互补约束**直接嵌入优化问题：

\[\min_{\boldsymbol{x}, \boldsymbol{u}, \boldsymbol{\lambda}} \int_0^T \ell(\boldsymbol{x}, \boldsymbol{u}) dt\]

subject to: - 动力学：$M\ddot{q} = h + J_c^T \lambda + Bu$ - 互补约束：$0 \le \lambda_n \perp d(q) \ge 0$ - 摩擦约束：$\|\lambda_t\| \le \mu \lambda_n$

其中互补约束 $\lambda_n \cdot d(q) = 0$ 的含义是：要么接触力为零（脚离地），要么接触距离为零（脚着地），不可能两者都非零。

CITO 的数学处理：互补约束是非光滑的，标准 NLP 求解器无法直接处理。两种常见的松弛方法：

Fischer-Burmeister 函数：把 $\lambda \perp d$ 替换为 $\phi(\lambda, d) = \sqrt{\lambda^2 + d^2} - \lambda - d = 0$
松弛互补：$\lambda \cdot d \le \epsilon$，其中 $\epsilon > 0$ 是小的松弛参数

为什么标准 NLP 求解器啃不动互补约束？ 关键在于它的可行集形状。把 $\lambda_n \ge 0$、$d \ge 0$、$\lambda_n \cdot d = 0$ 三条画在 $(\lambda_n, d)$ 平面上，可行集是**两条相互垂直的半轴**——一个"L 形"（或叫"非凸的角"）：

   d (接触间隙)
   ^
   |
   * (脚离地: d>0, λ=0)        <- 整条正 d 轴
   *
   *
   +--*--*--*--*--*--> λ_n (法向接触力)
      (脚着地: λ>0, d=0)        <- 整条正 λ 轴

   可行集 = 正 d 轴 ∪ 正 λ 轴（L 形）
   角点 (0,0): 脚"刚好接触、力刚好为零"的临界态

两个致命问题：(1) 可行集**非凸**（两条轴的并集，中间整个第一象限内部都不可行），凸优化的所有保证全部失效；(2) 在**角点 $(0,0)$ 处约束梯度退化**——LICQ（线性独立约束规范）不成立，KKT 条件的拉格朗日乘子可能不存在或发散，导致 SQP 在这点附近反复横跳不收敛。这就是为什么必须用 §上面的两种松弛——把尖锐的"L 形"磨成一条光滑曲线（FB 函数）或一个有厚度的带状区域（松弛 $\le \epsilon$），代价是引入了一点点"既离地又有力"的非物理松弛。

💡 概念误区：以为松弛参数 $\epsilon$ 越小越好 直觉上 $\epsilon \to 0$ 才能精确恢复真实的互补约束，所以"越小越好"。但 $\epsilon$ 越小，可行集越接近尖锐的 L 形，优化问题越病态、越难收敛。实践中 $\epsilon$ 要在"物理精确性"和"数值可解性"之间折中——通常用**同伦延拓（homotopy）**：先用较大的 $\epsilon$ 求一个粗解，再逐步减小 $\epsilon$ 用上一步的解 warm-start，渐进逼近。一上来就设极小的 $\epsilon$ 几乎必然不收敛。

CITO vs TOWR 对比：

方面	TOWR	CITO
接触序列	优化变量（时序持续时间）	隐式由互补约束决定
需要预设步数	是（指定每条腿走几步）	否
可发现新步态	有限	可以
求解难度	较低（连续 NLP）	很高（非光滑/组合）
求解时间	秒级	秒~分钟级
实时性	离线	离线
代表实现	TOWR (ETH, github.com/ethz-adrl/towr)	Drake (MIT)

56.6.5 路线三：MCTS + TO——把接触序列当作博弈树来搜（2024-2025）¶

为什么用树搜索？ 回顾 §56.1：四足机器人任意时刻的接触配置是一个 4-bit 整数，共 16 种 mode。如果把规划 horizon 切成 $H$ 个时间步，每步选一个 mode，那么可能的接触序列共有 $16^H$ 种——$H=10$ 时就是 $16^{10} \approx 10^{12}$。这是一个**组合爆炸**的离散搜索空间，穷举不可能。但它有一个关键结构：序列是逐步展开的（第 $k$ 步的选择限制第 $k+1$ 步的合理选项，比如不能让一条刚触地的腿下一步立刻腾空又触地）。这种"逐步决策、每步若干离散选项"的结构，恰好就是棋类游戏的博弈树结构——于是 AlphaGo 用的 Monte-Carlo Tree Search（MCTS，蒙特卡洛树搜索） 被自然地借用过来。

接触模式树的结构：

              root (current contact: 1111 stance)
            /        |         \
      mode=6      mode=9      mode=15      <- depth 1: next contact choice
     /  |  \      /  |  \      ...
   ...  ...  ...                          <- depth 2: 16-way branching
   |
  leaf: a full contact sequence of length H
        -> evaluated by Trajectory Optimization (TO)

每个**树节点**是"到目前为止的部分接触序列"，每条**边**是"下一步选哪个 mode"，每个**叶子**是一条完整的长度 $H$ 序列。MCTS 不展开整棵树（那就是穷举），而是用四个阶段的循环**有偏采样**地生长树：

阶段	做什么	关键
Selection（选择）	从根沿"最有前途"的边下行到叶	用 UCB1 平衡探索/利用
Expansion（扩展）	在叶节点新增一个未访问的子节点	加入一个新的 mode 选项
Simulation（模拟/rollout）	从新节点随机走到完整序列	这里用 TO 检验动力学可行性作为 rollout
Backpropagation（回传）	把 TO 的代价/可行性作为 reward 回传到路径上所有节点	更新各节点的访问次数和平均回报

UCB1 选择公式——MCTS 的灵魂在 Selection 阶段。在节点选择子节点时，最大化：

\[\text{UCB1}(i) = \underbrace{\bar{Q}_i}_{\text{利用：平均回报}} + c\underbrace{\sqrt{\frac{\ln N_{\text{parent}}}{N_i}}}_{\text{探索：访问越少越鼓励}}\]

其中 $\bar{Q}_i$ 是子节点 $i$ 的平均回报（TO 算出的轨迹代价的负值），$N_i$ 是它的访问次数，$N_{\text{parent}}$ 是父节点访问次数，$c$ 是探索系数。第一项倾向于已知好的接触序列（利用），第二项倾向于尝试少访问的接触序列（探索）——这正是组合搜索中"既要利用已发现的好步态、又不能错过潜在更好步态"的形式化。

MCTS + TO 的分工：MCTS 负责**离散的接触模式序列搜索**（搜哪些腿什么时候触地），TO 负责**连续的轨迹可行性检验**（给定接触序列，能否找到满足动力学/摩擦锥的力和轨迹）。两者解耦——MCTS 在离散空间剪枝，TO 在连续空间验证——避开了直接求解混合整数最优控制（MINLP）的 NP-hard 难题。

代表工作： - Tonneau et al.（2024, arXiv）"Non-Gaited Legged Locomotion with MCTS and Supervised Learning"——用监督学习训练一个 value function 来**预测节点回报**，替代昂贵的 TO rollout，把 MCTS 搜索加速若干数量级（思路与 AlphaGo 用价值网络替代随机 rollout 一致）。 - Liu et al.（2025, arXiv）"Simultaneous Contact Sequence and Patch Planning for Dynamic Locomotion"——首次在全身动力学模型上实现 MCTS + 全身 TO 的联合优化，同时搜索接触**时序**和落脚**位置块（patch）**。 - Chen et al.（2025, Advanced Intelligent Systems）"Contact-Implicit TO without Fixed Contact Sequences"——基于 ACAL-iLQR 的无固定接触序列优化（与 MCTS 路线互补，走的是 §56.6.4 的互补约束方向）。

💡 概念误区：MCTS 直接输出关节力矩/轨迹 MCTS 本身**只搜索离散的接触模式序列**，它不产生连续的力或轨迹——那是 TO 的工作。把 MCTS 当成"端到端运动规划器"是常见误解。正确的图景是：MCTS 是上层的"接触序列决策者"，TO 是下层的"给定序列求轨迹的执行者"，二者通过 reward（TO 的可行性/代价）耦合。这与 §56.6.7 的 RL 路线形成对比——RL 是用一个网络同时端到端地决定接触和动作。

56.6.6 实时 Contact-Implicit MPC——把接触发现搬进控制循环 ⭐⭐⭐⭐¶

这一节解决什么问题：前面讲的 CITO（§56.6.4）是**离线轨迹优化**——求解一次要几十秒到几分钟，只能事先算好一条轨迹再去跟踪。但 2024–2025 年的突破是：把"接触发现"从离线规划器搬进**在线 MPC 循环**，做到几十到几百 Hz 实时重规划。这模糊了"规划"和"控制"的边界，也修正了"接触优化必然离线"这个曾被广泛接受的观念。

为什么这一步如此困难，又为什么 2024 年才做到？ 回顾 §56.6.4：互补约束 $0 \le \lambda \perp d \ge 0$ 的可行集是 $(\lambda, d)$ 平面上的两条半轴（一个"L 形"角点集合），在角点处梯度不存在。离线 CITO 可以容忍求解器花几分钟在这个病态landscape里挣扎，但 MPC 要求每个控制周期（比如 20ms）就得吐出一个解——根本没时间做几十次非光滑迭代。2024–2025 的几项工作通过**重构问题结构**而非**改进求解器**绕过了这个障碍，这是关键的范式转变。

路线 A：Consensus Complementarity Control（C3，Aydinoglu & Posa, 2022→2024）

C3 的核心思想是用 ADMM（交替方向乘子法） 把"轨迹优化"和"接触投影"**解耦**成两个交替求解的子问题：

C3 的 ADMM 分解（每个 MPC 周期内迭代若干次）:
+----------------------------------------+
| 子问题 1: 光滑 QP（不含互补约束）         |   <- 标准 QP，快
|   min 二次代价 s.t. 线性动力学            |
+----------------------------------------+
              | 交换变量 (consensus)
              v
+----------------------------------------+
| 子问题 2: 逐时间步的接触投影（并行）       |   <- 每个时间步独立，可并行
|   把力/间隙投影到互补集 {λ⊥d}            |
+----------------------------------------+

关键洞察：互补约束是**逐时间步局部**的——第 $k$ 步的 $\lambda_k \perp d_k$ 与第 $j$ 步无关。因此子问题 2 可以把 horizon 内所有时间步的接触投影**并行**求解（每个是一个小的低维投影，甚至有闭式解）。ADMM 在"全局光滑 QP"和"局部接触投影"之间交替，直到两者对接触力达成一致（consensus）。这把一个大的非光滑问题拆成了一个大光滑问题 + 一堆小投影，前者用成熟 QP 求解器、后者高度并行，于是整体能跑到上百 Hz。

路线 B：Inverse-Dynamics CI-MPC（Kim et al., 2025, IJRR / KAIST HUBO Lab）

Kim 等人 2025 年发表在 IJRR 的工作 "Contact-Implicit Model Predictive Control: Controlling Diverse Quadruped Motions Without Pre-Planned Contact Modes or Trajectories" 是一个里程碑——它在**真实宇树 Go1 硬件**上实现了在线接触发现。两个关键技术：

硬接触模型 + 光滑梯度：不像传统 CITO 那样用 Fischer-Burmeister 软化互补约束，而是直接用硬接触模型做前向仿真，再为这个仿真过程计算**光滑的梯度**（通过对接触求解器的隐函数微分）。这样既保留了接触的物理精确性，又给了 MPC 求解器可用的下降方向。
逆动力学形式（Inverse-Dynamics TO）：以广义加速度和接触力为决策变量，动力学约束 $M\dot{v} + h = J^T\lambda + Bu$ 变成**显式可解**的（给定加速度直接算力矩），避免了正向动力学积分的数值刚性。这一形式被 Le Cleac'h 等人的 "Inverse Dynamics Trajectory Optimization for CI-MPC"（2023）系统化，是当前实时 CI-MPC 的主流骨架。

实时 CI-MPC vs 离线 CITO vs 预定义步态——三者在"接触决定权"上的根本差异：

维度	预定义步态（§56.3）	离线 CITO（§56.6.4）	实时 CI-MPC（本节）
谁决定接触序列	人（写 gait.info）	离线优化器（算一次）	在线 MPC（每周期重算）
接触序列能否在线变	否（除非手动切换）	否（事先固定）	是（实时涌现）
求解频率	N/A（查表）	一次性，秒~分钟	50–500 Hz
处理互补约束的方式	外包给调度器（降级为等式）	软化（FB/松弛）	ADMM 分解 / 隐函数光滑梯度
硬件验证	全部商用四足	多为仿真	Go1 等硬件已验证
适用任务	平地行走	离线特技/攀爬规划	在线多接触（推门、爬乱石）

本质洞察：实时 CI-MPC 的出现，让 §56.4 讲的"OCS2 把离散性外包给调度器"不再是唯一的工程可行解。OCS2 路线的本质是**用人类先验（预定义步态）换取实时性**；实时 CI-MPC 路线则证明了——只要把互补约束重构成"光滑大问题 + 并行小投影"（C3）或"硬接触 + 隐函数梯度"（Kim 2025），机器可以在控制循环内自己决定接触，不再需要人类预先指定步态。这不是说 OCS2 过时了——预定义步态在平地上仍然更简单更可靠——而是说"接触必须预定义"这个长期假设被打破了。判断一个领域是否成熟，一个标志就是看曾经的"必须"变成了"可选"。

💡 概念误区：以为 CI-MPC 完全不需要任何接触先验 即便是 Kim 2025 这样的在线接触发现，实践中仍然需要一个**参考轨迹或代价塑形**来引导优化（否则容易收敛到"原地不动、接触力为零"这个互补约束的平凡解，§56.6.8 会再强调这点）。区别在于：预定义步态把接触**时序**完全钉死，而 CI-MPC 只给一个**软引导**（如期望速度、期望落脚区域），把具体的接触时刻和顺序留给优化器。"不需要预定义接触模式"指的是不钉死时序，不等于零先验。

⚠️ 编程陷阱：在实时 CI-MPC 中沿用离线 CITO 的收敛判据 离线 CITO 通常跑到约束残差 $< 10^{-6}$ 才停。把这个判据搬到实时 MPC 会导致单周期求解超时——MPC 宁可要一个"不完全收敛但及时"的解，也不要一个"精确但迟到"的解。正确做法是**固定迭代次数**（如 ADMM 跑固定 5–10 轮）而非固定残差阈值，用 warm-start（上一周期的解）保证即使少量迭代也足够好。这是实时优化与离线优化在工程上的分水岭。

56.6.7 路线四:RL 步态发现——从数据中涌现接触序列 ⭐⭐⭐¶

2020 年以来，强化学习(RL)在步态发现领域取得了突破性进展。与 TOWR/CITO 通过数学优化"推导"最优接触序列不同,RL 通过海量试错"涌现"出有效的步态策略。这种范式转变值得深入理解。

为什么 RL 能发现步态?¶

RL 步态发现的核心假设是:如果给定足够多的训练经验和恰当的奖励信号，策略网络能学会在不同条件下选择合适的接触时序。这和生物进化中步态的产生过程有类比关系——自然界的动物步态不是被"设计"的，而是在运动效率和稳定性的选择压力下"演化"出来的。RL 用梯度下降替代了自然选择，用神经网络替代了基因，用奖励函数替代了适应度，但核心逻辑相同。

这与 CITO 的区别是本质性的:CITO 在**单个轨迹**上做优化，找到特定初始条件下的最优接触序列;RL 在**状态空间的分布**上做优化，学到一个能在多种条件下泛化的策略。类比建筑学:CITO 是为每块地皮定制设计蓝图(精确但耗时),RL 是训练一个建筑师，让他能应对各种地形和需求(灵活但需要大量训练)。

Curriculum Learning 步态发现¶

AMP (Adversarial Motion Priors, Peng et al. 2021, SIGGRAPH)

AMP 的核心思想是用**对抗训练**替代手工奖励设计。传统 RL 需要精心设计奖励函数来引导策略发现"自然"步态(跟踪速度、最小化能耗、保持平衡等),稍有不慎就会产生"reward hacking"——策略找到高奖励但不自然的运动(如滑行、抖动)。

AMP 的解决方案：训练一个判别器(Discriminator)来区分"参考运动"和"策略产生的运动"。策略的奖励来自判别器——如果策略的运动能"骗过"判别器(让判别器认为是参考运动),就获得高奖励。

\[r_{\text{AMP}}(s, s') = -\log(1 - D(s, s'))\]

其中 $D(s, s')$ 是判别器对状态转移 $(s, s')$ 的输出(0=策略生成,1=参考运动)。

AMP 在步态发现中的应用:用不同动物的运动捕捉数据(mocap)作为参考,RL 策略可以学会 trot、pace、gallop 等步态——无需手工编码步态参数。甚至可以用狗、马的 mocap 数据训练四足机器人，让它模仿动物的步态风格。

DeepGait (Da et al. 2020, RA-L / Tsounis et al. 2020)

DeepGait 的思路更直接：用 RL 直接优化步态参数(步频、duty factor、相位偏移),而非让策略输出关节力矩。

参数	范围	含义
步频 $f$	1-4 Hz	步态周期 $T = 1/f$
占空比 $\beta$	0.3-0.8	支撑相占比
相位偏移 $\phi_i$	0-1	各腿间的时序关系
抬腿高度 $h$	0.02-0.15 m	摆动腿最大高度

RL 在训练过程中探索这些参数的组合，学会在不同速度/地形下选择最优步态配置。这种"参数化 RL"比"端到端 RL"更容易迁移到真实机器人——因为底层仍然使用经过验证的 MPC/WBC 控制栈,RL 只调节步态参数。

Walk These Ways (Margolis & Agrawal, CoRL 2022)

这是步态参数化 RL 的里程碑工作。核心创新：把步态参数(步频、步幅、抬腿高度、体高、俯仰角等)作为**策略的条件输入**,训练一个能响应任意步态命令的通用策略。

训练流程: 1. 在每个 episode 开始时，随机采样一组步态参数(从均匀分布) 2. 策略以当前状态 + 步态参数为输入，输出关节角度 3. 奖励函数包含速度跟踪 + 步态参数匹配(实际步频接近命令步频等) 4. 经过数千万步训练，策略学会在任意步态参数组合下稳定行走

部署时，操作员可以用手柄实时调节步态参数——从慢速 walk 平滑过渡到高速 gallop,无需预定义切换逻辑。这实质上用一个连续策略替代了传统的步态状态机。

Robot Parkour Learning (Zhuang et al., CoRL 2023) & ANYmal Parkour (Hoeller et al., Science Robotics 2024)

这两项工作代表了 RL 步态发现的最新高度。机器人不仅学会了常规步态，还学会了**跳跃、攀爬、钻洞、跨沟**等非周期性的复杂接触行为。

关键技术: - Curriculum Learning(课程学习): 训练环境的难度逐步提升——先在平地上学走路，再加小台阶，再加大台阶，最后是连续障碍。环境难度根据策略的成功率自动调节。 - Privileged Learning(特权学习): 教师网络可以"看到"仿真中的所有信息(精确地形、接触力、摩擦系数),学生网络只能看到真实传感器数据(本体感知 + 深度图像)。学生通过模仿教师的行为来学习。 - 域随机化(Domain Randomization): 在训练中随机化摩擦系数、质量分布、电机延迟、传感器噪声等参数，使策略对 sim-to-real gap 鲁棒。

这些方法产生的策略能执行的接触序列远超人类预定义的步态库——例如，跨越宽 0.5m 的沟渠时，策略自动发现了"前腿先跳到对岸 → 后腿加速蹬地 → 身体跃过"的非对称接触序列。这种接触序列用 TOWR 或 CITO 也许能规划出来，但 RL 的优势在于**实时决策**:策略推理时间 < 1 ms,而 CITO 需要秒级。

Contact Schedule Optimization 前沿¶

2024-2025 年，学界开始将 RL 的探索能力与传统优化的精确性结合:

RL 热启动 CITO: 用 RL 策略生成粗糙的接触序列作为 CITO 的初始猜测。这解决了 CITO 对初值敏感的问题——RL 提供"大致正确"的初始解,CITO 再精化为"精确最优"的解。实验表明,RL warm-start 可以让 CITO 的收敛时间减少 5-10 倍。

可微仿真 + 策略梯度: 利用可微物理引擎(如 MuJoCo MJX、Brax、Dojo)直接对接触序列的性能做梯度下降。可微仿真提供了精确的梯度信息，避免了 RL 的高方差采样问题，但对接触的处理仍然是核心挑战——接触力的不连续性会导致梯度爆炸或消失。

扩散模型生成接触计划: 2025 年的最新尝试是用扩散模型(Diffusion Model)生成接触序列。训练数据来自 CITO 或人类示教，扩散模型学习接触序列的分布，推理时可以在毫秒内采样出多样化的接触计划。这种方法的优势是**多样性**——一次采样可以生成多个候选方案，再用 MPC 选择最优的。

方法	求解时间	最优性	泛化能力	物理保证
TOWR	1-10 s	局部最优	低(需重新优化)	强(约束满足)
CITO	10-100 s	局部最优	低	强
RL 端到端	< 1 ms	次优但鲁棒	高(泛化到新地形)	弱(无显式约束)
RL + CITO	0.1-1 s	近似最优	中	强
扩散模型	10-100 ms	分布最优	中-高	中(需后验检查)

本质洞察:步态发现方法的演进反映了机器人学中"规则"与"学习"的辩证关系。预定义步态是纯规则(100% 人类知识,0% 数据),CITO 是规则+优化(人类定义目标，优化器搜索解),RL 是学习+弱规则(人类定义奖励，策略从数据中涌现)。没有哪种范式是绝对最优的——关键在于任务的复杂度和部署约束的匹配。平地行走用预定义步态，复杂地形离线规划用 CITO,需要实时自适应用 RL+MPC 混合。工程的智慧不在于选择最"先进"的方法，而在于选择最"合适"的方法。

56.6.8 六条路线的统一视角¶

把前面讨论的所有思路放到同一张坐标系里，一共有六条路线：作为基线的**预定义步态**（OCS2），四条主线优化/学习方法——TOWR（路线一）、CITO（路线二）、MCTS+TO（路线三）、RL+MPC（路线四），以及由 CITO 实时化衍生出的**实时 CI-MPC**（§56.6.6）。它们沿"灵活性""实时性""地形适应""规则↔学习"四个维度铺开如下：

          Six approaches to contact sequence

  Predefined -- TOWR -- CITO -- CI-MPC -- MCTS+TO -- RL+MPC
  (OCS2)       (2018)  (2014)  (2024-25) (2024)    (2022+)
   |            |       |        |         |          |
  Simple <---------------- Flexibility ----------------> Flexible
  Real-time <---- Computation ----> Offline | back to Real-time
  Flat ground <----------- Terrain -----------> Arbitrary
  Rules <-------------- Paradigm ----> Optimization -> Learning

  Note: CI-MPC (real-time) and RL+MPC sit at both ends —
        flexible AND real-time — the 2024-25 frontier closed
        the long-standing "flexible XOR real-time" trade-off.

工程选择指南：

场景	推荐方法	原因
平地行走/trot	预定义步态	简单、实时、可靠
已知台阶/斜坡	预定义 + 参数调整	调整 duty factor 和步幅
复杂地形（离线规划）	TOWR	秒级求解，可优化时序
未知接触模式（离线研究）	CITO / MCTS+TO	可发现新策略，不要求实时
在线多接触（推门/乱石，前沿）	实时 CI-MPC（C3 / Kim 2025）	控制循环内在线发现接触，硬件已验证
实时自适应步态（前沿）	RL + MPC	足式/190_腿足RL训练栈将展示如何用强化学习训练自适应步态策略:RL 智能体在数千种随机地形上探索步态参数，学会根据感知信息实时切换步态类型和时序，相当于用数据驱动替代人工设计的步态状态机

🧠 思维陷阱：认为"接触优化一定比预定义步态好" 优化确实更灵活，但它的计算成本高出几个数量级。在大多数实际部署场景中（平地、缓坡、已知台阶），预定义步态 + 少量在线调整已经足够。不要为了"高级"而过度工程化——选择最简单的足以解决问题的方法。

💡 概念误区：CITO 可以"自动发现"任意步态 理论上 CITO 确实可以发现任意接触序列，但实际中它高度依赖初始猜测和参数调节。没有好的初始解，优化器往往收敛到局部最优（比如始终站在原地——接触力为零也是互补约束的一个解！）。所以实践中 CITO 通常需要用一个粗糙的预定义步态作为初始化。

练习 56.6¶

[论文阅读] 阅读 Winkler et al. 2018 "Gait and Trajectory Optimization..." 的 Section III。回答：TOWR 如何保证优化后的 stance/swing 时间不会出现负值？（提示：bound constraint on timing variables）
[思考题] 互补约束 $0 \le \lambda \perp d \ge 0$ 对应的可行集是什么形状？画在 $(\lambda, d)$ 平面上。为什么标准 NLP 求解器难以处理这个约束？（提示：可行集的角点处梯度不存在）

56.7 步态切换的工程问题 ⭐⭐¶

这一节解决什么问题：从一种步态切换到另一种步态时，有哪些工程陷阱？如何保证切换过程中机器人不摔倒？

56.7.1 动机：步态切换不是"瞬间替换"¶

在理想的数学世界里，步态切换只是把 modeSequence = [6, 9] 替换为 modeSequence = [14, 13, 11, 7]。但在物理世界中，这种瞬间替换可能导致灾难：

Scenario: trot -> walk switch occurs mid-swing

trot: ... mode=6 (RF+LH stance) | mode=9 (LF+RH stance) ...
                                  ^
                            switch happens here
                            LF and RH are in the air!

walk: ... mode=14 (LF+RF+LH) -> mode=13 (LF+RF+RH) -> ...
           ^
    walk requires LF to be in stance!
    But LF is still in the air,
    hasn't finished swinging!
    -> constraint violation -> MPC fails -> fall

56.7.2 过渡 stance 相——OCS2 的解决方案¶

OCS2 的 GaitSchedule::insertModeSequenceTemplate() 在步态切换时会插入一个**过渡 stance 相**（transition stance phase）：

trot:  ... mode=6 | mode=9 |
                           v insert transition stance
                     mode=15 (all contact, 0.2s)
                           v start new gait
walk:                 mode=14 | mode=13 | mode=11 | mode=7 | ...

过渡 stance 相的持续时间 phaseTransitionStanceTime_ 是一个可配置参数（典型值 0.1~0.3s）。这段时间内四脚全触地，所有摆动腿有时间安全着陆，然后再开始新步态。

56.7.3 步态切换的状态机¶

在工程实践中，步态切换通常由一个**有限状态机**（FSM）管理：

enum class LocomotionState {
  IDLE,       // motors enabled but no motion
  STAND,      // standing (full stance PD control)
  WALK,       // walk gait + MPC
  TROT,       // trot gait + MPC
  BOUND,      // bound gait + MPC
  RECOVERY    // fall recovery
};

class LocomotionFSM {
  LocomotionState current_;

  void handleEvent(LocomotionEvent event) {
    switch (current_) {
      case LocomotionState::STAND:
        if (event == LocomotionEvent::CMD_TROT) {
          gaitSchedule_->insertModeSequenceTemplate(
              trotTemplate_, currentTime_, horizonEnd_);
          current_ = LocomotionState::TROT;
        }
        break;

      case LocomotionState::TROT:
        if (event == LocomotionEvent::CMD_WALK) {
          // TROT -> WALK: needs transition stance
          gaitSchedule_->insertModeSequenceTemplate(
              walkTemplate_, currentTime_, horizonEnd_);
          current_ = LocomotionState::WALK;
        }
        if (event == LocomotionEvent::FALL_DETECTED) {
          disableMPC();
          enableRecoveryController();
          current_ = LocomotionState::RECOVERY;
        }
        break;

      case LocomotionState::RECOVERY:
        if (event == LocomotionEvent::UPRIGHT) {
          current_ = LocomotionState::STAND;
        }
        break;
    }
  }
};

状态转移矩阵：

当前状态	CMD_TROT	CMD_WALK	CMD_STOP	FALL	UPRIGHT
IDLE	STAND->TROT	STAND->WALK	-	-	-
STAND	TROT	WALK	IDLE	RECOVERY	-
TROT	-	WALK	STAND	RECOVERY	-
WALK	TROT	-	STAND	RECOVERY	-
RECOVERY	-	-	-	-	STAND

56.7.4 速度自适应步态切换¶

更高级的系统不需要用户手动选择步态，而是根据**速度命令自动切换**：

LocomotionState selectGaitBySpeed(double cmdSpeed) {
  if (cmdSpeed < 0.3)  return LocomotionState::STAND;
  if (cmdSpeed < 0.8)  return LocomotionState::WALK;
  if (cmdSpeed < 2.5)  return LocomotionState::TROT;
  return LocomotionState::BOUND;
}

legged_gym (RL) 的做法更优雅：不硬编码速度阈值，而是训练一个神经网络来预测最优步态参数。Margolis et al.（2022, CoRL）"Walk These Ways" 提出了一种参数化步态空间，让 RL 策略输出步态参数（frequency, duty factor, phase offsets），实现了连续的步态过渡——不需要离散的 FSM 切换。

56.7.5 安全检查清单¶

步态切换前应检查的安全条件：

检查项	条件	原因
关节限位	所有关节在安全范围内	避免切换时关节超限
足端力	触地腿 $f_z > f_{\min}$	确保有足够的地面支撑
base 倾角	$	\text{roll}
速度匹配	当前速度在新步态的可行范围内	从 stand 直接切 gallop 会摔
MPC 状态	MPC 求解正常（非异常退出）	MPC 求解失败时不应切换

⚠️ 编程陷阱：步态切换后 MPC 的第一次求解可能失败 步态切换导致 ModeSchedule 突变，MPC 的 warm start（用上一次的解作为初始猜测）可能与新步态不兼容。常见的修复方法是在步态切换后重置 MPC 的初始猜测——但这会降低第一次求解的质量。更好的方案是用过渡 stance 相给 MPC 足够的时间重新收敛。

🧠 思维陷阱：认为步态切换只需要改 ModeSchedule 步态切换不仅要改 ModeSchedule，还需要：(1) 更新摆动腿轨迹的目标落脚点；(2) 可能需要调整 MPC 的代价权重（walk 和 trot 的跟踪权重可能不同）；(3) 可能需要调整 MPC 的 horizon 长度（walk 周期更长）。只改 ModeSchedule 而不更新这些配套参数是常见的 bug 来源。

练习 56.7¶

[设计题] 设计一个从 trot 切换到 bound 的过渡策略。画出包含过渡 stance 相的完整 ModeSchedule 时间轴，标注每段的 mode 和持续时间。
[思考题] 在跌倒恢复（RECOVERY）状态中，机器人应该使用什么控制策略？为什么不能直接用 MPC？（提示：跌倒后机器人的姿态远离 MPC 的线性化点）

56.8 legged_control 的步态实现 ⭐⭐¶

这一节解决什么问题：legged_control（Qiayuan Liao 开发）是 OCS2 在实际机器人上部署时最常用的开源框架。它如何封装 OCS2 的步态管理？与 MIT Cheetah 的做法有什么本质区别？

56.8.1 legged_control 的定位¶

legged_control（github.com/qiayuanl/legged_control）是一个基于 OCS2 + ros-control 的**完整腿足控制栈**，包含 NMPC、WBC、状态估计（ESKF）和 Sim2Real 框架。它的步态管理建立在 OCS2 的 SwitchedModelReferenceManager 之上，但做了一些工程简化。

56.8.2 GaitReceiver——ROS 步态切换接口¶

legged_control 用 GaitReceiver 类通过 ROS topic 接收步态切换命令：

class GaitReceiver {
  ros::Subscriber gaitSub_;

  void gaitCallback(const std_msgs::String::ConstPtr& msg) {
    auto gaitName = msg->data;  // e.g. "trot", "walk"
    auto newTemplate = gaitLibrary_.at(gaitName);
    gaitSchedule_->insertModeSequenceTemplate(
        newTemplate, currentTime_, horizonEnd_);
  }
};

使用方式：

# Switch to trot
rostopic pub /gait_command std_msgs/String "trot"
# Switch to walk
rostopic pub /gait_command std_msgs/String "walk"
# Switch to stance (stop)
rostopic pub /gait_command std_msgs/String "stance"

56.8.3 与 MIT Cheetah 步态管理的本质对比¶

MIT Cheetah Software 的步态管理哲学与 OCS2/legged_control 根本不同：

// MIT Cheetah: continuous phase + runtime query
class GaitScheduler {
  double period_;         // gait period
  double dutyCycle_;      // duty factor
  double phaseOffset_[4]; // per-leg phase offset
  double phase_[4];       // per-leg current phase

  void step(double dt) {
    for (int leg = 0; leg < 4; ++leg) {
      phase_[leg] += dt / period_;
      if (phase_[leg] > 1.0) phase_[leg] -= 1.0;
    }
  }

  bool isStance(int leg) const {
    return phase_[leg] < dutyCycle_;
  }
  // Key difference: contact schedule is generated from phase/gait table
  // rather than represented as an OCS2 ModeSchedule object.
};

关键差异：

方面	OCS2/legged_control	MIT Cheetah
步态表示	ModeSchedule（离散事件序列）	连续相位变量
MPC 对步态的感知	知道整个 horizon 的接触序列	用 phase/contact table 固定预测时域内的接触序列
步态切换	替换模板 + 过渡 stance	修改 phase offset/duty cycle
MPC 类型	NMPC（非线性，显式处理模式序列）	凸 MPC（在线性化模型中使用给定 contact table）
适用场景	复杂步态、非平坦地形	简单步态、平地高速

为什么 MIT Cheetah 不需要 OCS2 这种 ModeSchedule？ 因为它使用**凸 MPC**（Di Carlo 2018）和连续相位生成的 gait/contact table。每次 MPC 求解时，预测时域内每条腿哪些节点可施加接触力是已知的，但这个信息以轻量 contact table 进入 QP，而不是以 OCS2 的 ModeSchedule/跳变系统形式进入 NMPC。差异在于数据结构和动力学近似，而不是“完全不知道未来接触”。

56.8.4 RL 路线的步态表达¶

legged_gym（NVIDIA Isaac Gym）中的步态管理采用了截然不同的范式：

# legged_gym: gait is part of RL observation/action
class LeggedRobot(BaseEnv):
    def _compute_gait_phase(self):
        self.gait_phase += self.dt / self.gait_period
        self.gait_phase %= 1.0
        self.desired_contact = (
            self.gait_phase < self.duty_factor
        ).float()

    def _compute_observation(self):
        obs = torch.cat([
            self.base_ang_vel,
            self.projected_gravity,
            self.commands,
            self.dof_pos - self.default_dof_pos,
            self.dof_vel,
            self.actions,
            self.gait_phase,          # phase signal
            self.desired_contact,     # desired contact state
        ], dim=-1)

在 RL 范式中，步态不是由调度器显式管理的，而是作为策略网络的**输入信号**——网络根据相位信号和期望接触状态来决定关节力矩。

三种哲学的总结：

             Three philosophies of gait management

  OCS2             MIT Cheetah         legged_gym (RL)
  -------          -----------         ---------------
  Declarative      Imperative          Learned
  "Here is the     "At this instant,   "Given phase signal,
   full schedule"   are you stance?"    figure it out"

  MPC sees future  MPC sees now        No MPC, just policy

  + Complex gaits  + Simple, fast      + End-to-end
  + Terrain-aware  + High frequency    + Can discover gaits
  - Slower MPC     - No future info    - Needs training
  - Fixed schedule - Flat terrain only - Less interpretable

💡 概念误区：认为 RL 不需要步态定义 即使在端到端 RL 中，步态信息也没有完全消失。legged_gym 的标准做法是把**步态相位**作为 observation 的一部分输入到策略网络。不提供步态信号的 RL 策略虽然也能学会行走，但通常会产生**非周期性**的、看起来不自然的步态。步态相位信号起到了"节拍器"的作用——引导策略学习周期性运动模式。

⚠️ 编程陷阱：legged_control 和原版 OCS2 的配置路径不同 legged_control 对 OCS2 的文件结构做了重组。原版 OCS2 的步态配置在 ocs2_legged_robot/config/gait/default.info，而 legged_control 可能把它放在 legged_interface/config/ 或机器人特定的 description 包中。如果切换框架后步态加载失败，首先检查配置文件路径是否正确。

练习 56.8¶

[代码对比] 下载 OCS2 legged_robot 和 MIT Cheetah Software，分别找到步态定义的核心文件。列出两者的 API 差异（函数名、参数类型、调用方式）。
[思考题] 如果要在 legged_control 中支持"根据速度自动切换步态"，需要修改哪些模块？画出架构图。

本章常见误解汇总¶

下表汇总全章出现的高频误解，供快速自检。每一条都对应正文中的某个概念误区或思维陷阱——如果某条让你"咦，我一直以为不是这样"，回到对应小节重读。

常见误解	正确理解	出处
"步态就是机器人走路的样子"	步态是把"哪只脚何时触地"编码的离散接触序列（`mode` + 时序），是可被 MPC 消费的数据结构，与"样子"无关	§56.1
"步态 = 速度命令"	步态、速度命令、落脚点规划是三个独立模块。同一步态可原地踏步也可高速移动	§56.1
"相位变量 $\phi$ 是接触力的连续近似/软接触"	$\phi$ 只是时间的归一化表示，不改变接触的离散本质；OCS2 中接触切换仍是硬切换	§56.1
"`switchingTimes` 和 `eventTimes` 是一回事"	`switchingTimes` 是相对周期起点的模板偏移（首元素 0、末元素 T）；`eventTimes` 是绝对时间的切换时刻。转换发生在 tiling	§56.3
"所有步态都左右对称"	trot/pace/bound/pronk 对称，但 walk（相位 0/0.25/0.5/0.75）和 gallop 不对称	§56.2
"mode 编码的腿序是通用标准"	腿序因项目而异！OCS2 `RH=0…LF=3`，MIT Cheetah `FR=0…HL=3`。移植必须先核实	§56.2
"约束按 mode 切换会改变 QP 维度"	不会。非激活约束残差置零、雅可比置零矩阵，QP 结构不变，避免运行时重分配内存	§56.4
"`getModeSchedule` 的请求范围就是 MPC 预测长度"	生成范围（含两侧 `timeHorizon` 余量）≠ 求解范围。后者由 `mpc.timeHorizon` 单独控制	§56.4
"$\omega=\sqrt{g/h}$ 是步态频率"	它是 LIP 自然频率（质心高度决定），与步态角频率 $2\pi/T$ 无关	§56.5
"Raibert 落脚点是最优的"	它是 Capture Point/DCM 的工程近似——用步态周期 $T_s$ 替代 LIP 时间常数 $1/\omega$，牺牲精度换鲁棒	§56.5
"摆动腿是零质量、可以随便动"	摆动腿加速会对 base 产生反作用力矩；Centroidal Model 通过质心动量矩阵隐式包含此耦合	§56.5
"接触优化一定比预定义步态好"	优化更灵活但计算贵几个数量级。平地/缓坡用预定义步态足够，不要过度工程化	§56.6
"CITO 可以全自动发现任意步态"	高度依赖初值，无好初值会收敛到平凡解（原地不动、力为零）。常需预定义步态做初始化	§56.6
"接触优化必然离线"	2024–25 的实时 CI-MPC（C3 / Kim 2025）已在 Go1 硬件上把接触发现搬进控制循环	§56.6
"RL 端到端不需要步态定义"	步态相位通常仍作为 observation 输入，起"节拍器"作用；不给会产生非周期不自然步态	§56.8

56.9 本章小结¶

知识点总结表¶

知识点	核心内容	难度	关键公式/概念
56.1 步态数学定义	位掩码编码、ModeSchedule、相位变量	⭐	$\text{mode} = \sum 2^i \cdot c_i$
56.2 经典步态分类	walk/trot/pace/bound/gallop/pronk	⭐⭐	duty factor $\beta$, phase offset $\phi_i$
56.3 步态参数化调度	GaitSchedule、平铺算法、运行时切换	⭐⭐	tileModeSequenceTemplate
56.4 SwitchedModelRefMgr	模式依赖约束、isActive 机制	⭐⭐⭐	ZeroForce/ZeroVelocity 按 mode 激活
56.5 摆动腿轨迹	Cubic spline/Bezier/Cycloid/Raibert	⭐⭐	$p_f = p_h + \frac{T_s}{2}v + k(v-v_r)$
56.6 接触序列优化	TOWR/CITO/MCTS+TO	⭐⭐⭐⭐	$0 \le \lambda \perp d \ge 0$
56.7 步态切换工程	过渡 stance、FSM、安全检查	⭐⭐	phaseTransitionStanceTime
56.8 legged_control	GaitReceiver、与 Cheetah/RL 对比	⭐⭐	三种步态管理哲学

关键术语¶

英文	中文	首次出现
Mode	接触模式	56.1
ModeSchedule	模式调度序列	56.1
Contact bitmask	接触位掩码	56.1
Phase variable	相位变量	56.1
Duty factor	占空比	56.2
Phase offset	相位偏移	56.2
Stride period	步态周期	56.2
GaitSchedule	步态调度器	56.3
Tiling	平铺	56.3
SwitchedModelReferenceManager	切换模型参考管理器	56.4
isActive	约束激活判断	56.4
ZeroForceConstraint	零力约束	56.4
ZeroVelocityConstraint	零速度约束	56.4
Raibert heuristic	Raibert 落脚点启发式	56.5
Cubic spline	三次样条	56.5
Bezier curve	Bezier 曲线	56.5
Cycloid	摆线	56.5
TOWR	轨迹优化框架	56.6
Contact-Implicit TO	接触隐式轨迹优化	56.6
MCTS	蒙特卡罗树搜索	56.6
Transition stance	过渡站立相	56.7

56.10 累积项目：本章新增模块¶

足式累积项目进度：

章节	模块	内容
足式/70_腿足简化模型理论	简化模型	SRBD 动力学 + Centroidal Model
足式/110_OCS2完整栈与双线程MPC	OCS2 MPC	SQP + HPIPM + 双线程架构
足式/120_步态管理与接触序列	步态管理	GaitSchedule + 摆动腿轨迹 + 步态切换

本章新增任务：

任务 56-A：添加 transverse gallop 步态

在 OCS2 legged_robot 的 gait.info 中添加 transverse gallop 步态定义。设计 modeSequence 和 switchingTimes，使其包含两个腾空相。在仿真中运行并观察效果。

任务 56-B：摆动轨迹对比可视化

用 Python + matplotlib 实现三种摆动轨迹生成器（cubic spline, Bezier, cycloid），在相同参数下绘制对比图，包含位置、速度、加速度曲线。分析哪种方法的最大加速度最小。

任务 56-C：Raibert heuristic 可视化

在 legged_control 或 OCS2 中实现 Raibert heuristic，在 RViz 中用 Marker 可视化计算出的落脚点位置。修改速度命令，观察落脚点如何响应。

[跨章综合题] 任务 56-D：步态管理 + MPC + 状态估计闭环

本题需要综合足式/110_OCS2完整栈与双线程MPC(OCS2 MPC 架构)、足式/130_腿足状态估计(状态估计)和本章(步态管理)的知识。

场景: 在 OCS2 + legged_control 环境中，实现从 trot 到 walk 的步态切换，并在切换过程中观察状态估计器的表现。

(本章 56.7) 在 OCS2 的 gait.info 中定义 trot 和 walk 两种步态，设计一个 2 秒的过渡 stance 相。解释为什么直接"瞬间切换"可能导致机器人失稳。
(足式/110_OCS2完整栈与双线程MPC) 步态切换时,ModeSchedule 的更新通过哪个组件传递到 SQP 求解器?画出从 GaitSchedule::tileModeSequenceTemplate() 到 SqpSolver::run() 的数据流路径。
(足式/130_腿足状态估计) 步态切换的过渡 stance 相中，四只脚全部触地。状态估计器的接触检测模块会如何响应?如果 Schmitt 触发器的 contactThreshold 设置不当(太高),会导致什么问题?
(综合) 实际运行实验，记录步态切换前后 5 秒内的 MPC 求解时间、状态估计位置误差、接触力分布的变化曲线。分析切换过程中的暂态行为。

56.11 延伸阅读¶

必读文献¶

文献	内容	难度
Raibert, "Legged Robots That Balance" (1986, MIT Press)	步态控制的奠基之作，Raibert heuristic 的原始出处	⭐⭐
Di Carlo et al., "Dynamic Locomotion in the MIT Cheetah 3 through Convex MPC" (2018, IROS)	MIT Cheetah 的凸 MPC + 步态管理	⭐⭐
Winkler et al., "Gait and Trajectory Optimization for Legged Systems" (2018, RA-L)	TOWR 框架，接触时序作为优化变量	⭐⭐⭐

进阶文献¶

文献	内容	难度
Posa et al., "A Direct Method for TO of Rigid Bodies Through Contact" (2014, IJRR)	Contact-Implicit TO 的经典方法	⭐⭐⭐⭐
Bledt et al., "MIT Cheetah 3: Design and Control of a Robust, Dynamic Quadruped" (2018, IROS)	Cheetah 3 的完整控制系统	⭐⭐⭐
Margolis & Agrawal, "Walk These Ways" (2022, CoRL)	RL 学习步态参数化，多行为泛化	⭐⭐⭐
Jenelten et al., "TAMOLS: Terrain-Aware Motion Optimization" (2022, T-RO)	地形感知的步态规划	⭐⭐⭐⭐

前沿文献（2024-2025）¶

文献	内容	难度
Tonneau et al., "Non-Gaited Locomotion with MCTS" (2024, arXiv 2408.07508)	MCTS + 学习 value function 用于步态搜索	⭐⭐⭐⭐
Liu et al., "Simultaneous Contact Sequence and Patch Planning" (2025, arXiv 2508.12928)	MCTS + 全身 TO 联合优化	⭐⭐⭐⭐
Chen et al., "Contact-Implicit TO without Fixed Sequences" (2025, Adv. Intell. Syst.)	基于 ACAL-iLQR 的新方法	⭐⭐⭐⭐

开源代码¶

项目	地址	关注点
OCS2	github.com/leggedrobotics/ocs2	GaitSchedule, SwitchedModelReferenceManager
legged_control	github.com/qiayuanl/legged_control	OCS2 + ros-control 集成
TOWR	github.com/ethz-adrl/towr	接触时序优化
MIT Cheetah Software	github.com/mit-biomimetics/Cheetah-Software	连续相位步态管理
legged_gym	github.com/leggedrobotics/legged_gym	RL 步态训练

🔧 故障排查手册¶

按"症状→可能原因→排查步骤→相关章节"组织。排查步骤按从易到难、从高频到低频排序，优先验证最可能的原因。

症状	可能原因	排查步骤	相关章节
步态切换瞬间机器人剧烈抖动或摔倒	接触模式突变导致 MPC 约束集突变，QP 解不连续；或切换发生在摆动中途	1. 检查模式切换时刻的接触力是否平滑过渡到零 2. 在 `eventTimes` 前后各加 10–20ms 的力衰减窗口 3. 确认 WBC 的 warm-start 在模式切换时正确重置 4. 确认切换前插入了过渡 stance 相（§56.7.2）	§56.7
摆动腿触地时足端力出现尖峰(冲击)	摆动腿轨迹末段速度不为零，着地瞬间产生碰撞冲量	1. 打印摆动腿 touchdown 时刻的足端速度，确认 $\dot{z}$ 接近零 2. 检查 cubic spline 终点的速度边界条件是否设为零 3. 适当降低最后 10% 相位的下降速度	§56.5
摆动腿刮地/绊倒	足端轨迹抬腿高度不足；或非平坦地形未做地形补偿	1. 增大 swing_height 参数 2. 检查 `SwingTrajectoryPlanner` 的 `terrainHeight` 是否接入高程图 3. 确认 Raibert 落脚点的 `foothold.z()` 没有硬编码为 0（§56.5.8）	§56.5
步态频率与实际运动速度不匹配，机器人前倾或后仰	Raibert 落脚点公式中的速度增益 $k$ 不当，或步态周期 $T$ 与速度命令不协调	1. 打印 Raibert 公式的各项 $\frac{T_s}{2}v$、$k(v-v_r)$——哪一项异常? 2. 检查速度命令滤波器是否引入过大延迟 3. 尝试减小增益 $k$ 或增大步态周期	§56.5
相位变量 $\phi$ 在步态切换后跳变或回绕异常	步态切换时新旧步态的 `eventTimes` 拼接不连续，相位计算出现模运算错误	1. 打印切换前后的 `eventTimes` 序列，检查是否严格递增 2. 确认 `tileModeSequenceTemplate()` 的 startTime 参数正确 3. 检查相位归一化公式的分母是否可能为零（stance duration = 0）	§56.3
trot 步态中对角腿不同步，出现"跛行"	对角腿的 phase offset 不精确为 0.5，或两侧摆动腿轨迹生成器的时间戳不一致	1. 检查 ModeSchedule 中对角腿的 eventTimes 是否精确对称 2. 确认摆动腿参考轨迹的起始时间来自同一个时钟源 3. 对比 LF-RH vs RF-LH 的接触力曲线	§56.2、§56.4
支撑相时间不准，接触检测时序漂移	时钟漂移/传感器延迟，软件时间戳与真实接触时刻错位	1. 用硬件时间戳替代软件时间戳 2. 增加 phase margin 3. 核对状态估计的接触检测阈值（与步态期望接触做一致性检查）	足式/130
RL 训练出的步态不自然（滑步/抖动）	reward 中缺少步态正则项，策略 reward-hacking	1. 添加 AMP style reward 或左右对称惩罚 2. 把步态相位信号加入 observation（§56.8.4）3. 检查是否提供了期望接触状态信号	足式/190
trot→gallop 切换震荡（来回跳变）	速度阈值的状态机 hysteresis 不足，临界速度附近反复触发	1. 增大切换阈值的死区（hysteresis band）2. 检查过渡 stance 相设计是否合理 3. 对速度命令做低通滤波	§56.7

56.12 API 速查表¶

本章涉及的 OCS2 核心 API 签名与说明，按数据流顺序（步态定义 → 调度 → 约束）组织。所有签名对应 ocs2_legged_robot 主分支，可作为读源码和写集成代码的索引。

数据结构¶

结构/类型	字段	含义
`ModeSchedule`	`scalar_array_t eventTimes`	模式切换的绝对时刻，长度 $N-1$
	`size_array_t modeSequence`	各段模式编号（4-bit 整数），长度 $N$
`ModeSequenceTemplate`	`scalar_array_t switchingTimes`	模板内的相对时间偏移（首元素 0、末元素周期 $T$）
	`size_array_t modeSequence`	模板模式序列
`Gait`（相位空间）	`scalar_t duration`	步态周期 $T$（秒）
	`std::vector<scalar_t> eventPhases`	$(0,1)$ 内的切换相位（不含 0 和 1）
	`std::vector<size_t> modeSequence`	各相位段模式

GaitSchedule 类¶

方法	签名	说明
构造	`GaitSchedule(ModeSchedule init, ModeSequenceTemplate tmpl, scalar_t phaseTransitionStanceTime)`	初始调度 + 模板 + 过渡 stance 时长
生成调度	`ModeSchedule getModeSchedule(scalar_t lb, scalar_t ub)`	平铺模板生成 $[lb, ub]$ 的 ModeSchedule（实际调用时两侧各扩 `timeHorizon`）
注入调度	`void setModeSchedule(const ModeSchedule&)`	直接灌入完整 ModeSchedule，绕过平铺（用于接触优化器输出/单测）
运行时切换	`void insertModeSequenceTemplate(const ModeSequenceTemplate& tmpl, scalar_t start, scalar_t final)`	在 `start` 处替换为新步态模板，可插入过渡 stance
平铺（私有）	`void tileModeSequenceTemplate(scalar_t start, scalar_t final)`	周期平铺模板，截断到 `final`，追加终端 stance

SwitchedModelReferenceManager 类¶

方法	签名	说明
求解前回调	`void modifyReferences(scalar_t initTime, scalar_t finalTime, ...)`	MPC 每次求解前调用：生成 ModeSchedule + 参考轨迹 + 摆动轨迹
查询调度	`const ModeSchedule& getModeSchedule() const`	供 MPC 求解器查询当前接触序列
查询参考	`const TargetTrajectories& getTargetTrajectories() const`	供 MPC 查询 base 参考轨迹
查询某时刻模式	`size_t ModeSchedule::modeAtTime(scalar_t t) const`	返回时刻 $t$ 的 `mode`，区间左闭右开 $[t_{i-1}, t_i)$

模式依赖约束（StateInputConstraint 派生）¶

约束类	`isActive(t)` 条件	约束内容
`ZeroForceConstraint`	`!isContactActive(leg, mode)`（摆动腿）	接触力 $\lambda_i = 0$（等式）
`ZeroVelocityConstraint`	`isContactActive(leg, mode)`（触地腿）	足端速度 $v_{\text{foot}} = 0$（等式，防滑移）
`FrictionConeConstraint`	`isContactActive(leg, mode)`（触地腿）	$\\|\lambda_t\\| \le \mu\lambda_n$（解析二阶锥，不等式）

腿索引位运算速记：isContactActive(i, mode) = (mode >> i) & 1，OCS2 腿序 RH=0, LH=1, RF=2, LF=3。常用 mode：6=0b0110（RF+LH，trot 半周期）、9=0b1001（LF+RH，trot 另半周期）、15=0b1111（全触地 stance）、0=0b0000（全腾空 flight）。

legged_control ROS 接口¶

接口	用法	说明
步态切换 topic	`rostopic pub /gait_command std_msgs/String "trot"`	`GaitReceiver` 订阅，从 gait library 取模板并 `insertModeSequenceTemplate`
步态配置文件	`gait.info`（Boost property tree 格式）	定义 `modeSequence` 与 `switchingTimes`，不重新编译即可改步态

与其他章节衔接¶

向前承接：

前置章节	本章使用的知识
足式/70_腿足简化模型理论腿足简化模型	步态分类（trot/walk/bound 等）、duty factor 概念
足式/80_接触力学与约束优化互补约束	接触模式预定义是处理互补约束的路线之一
足式/110_OCS2完整栈与双线程MPC OCS2 完整栈	ModeSchedule 数据结构、SwitchedModelReferenceManager 接口、isActive 机制

向后指向：

后续章节	本章提供的基础
足式/130_腿足状态估计腿足状态估计	步态提供的接触状态信息用于约束状态估计器（接触腿的速度为零可作为观测）
足式/140_落脚点规划经典方法落脚点规划	从预定义步态扩展到感知驱动/优化驱动的落脚点选择
足式/190_腿足RL训练栈 RL 步态策略	从手工步态到学习步态的演进
足式/230_Perceptive_MPC Perceptive MPC	步态+地形感知 -> 自适应步态切换

参数	符号	定义	典型范围
Duty factor	\(\beta\)	每条腿的支撑相占步态周期的比例	0.3~0.8
Phase offset	\(\phi_i\)	第 \(i\) 条腿相对于参考腿的相位延迟	\([0, 1)\)
Stride period	\(T\)	一个完整步态周期的时长	0.3~1.2s

参数	范围	含义
步频 \(f\)	1-4 Hz	步态周期 \(T = 1/f\)
占空比 \(\beta\)	0.3-0.8	支撑相占比
相位偏移 \(\phi_i\)	0-1	各腿间的时序关系
抬腿高度 \(h\)	0.02-0.15 m	摆动腿最大高度

知识点	核心内容	难度	关键公式/概念
56.1 步态数学定义	位掩码编码、ModeSchedule、相位变量	⭐	\(\text{mode} = \sum 2^i \cdot c_i\)
56.2 经典步态分类	walk/trot/pace/bound/gallop/pronk	⭐⭐	duty factor \(\beta\), phase offset \(\phi_i\)
56.3 步态参数化调度	GaitSchedule、平铺算法、运行时切换	⭐⭐	tileModeSequenceTemplate
56.4 SwitchedModelRefMgr	模式依赖约束、isActive 机制	⭐⭐⭐	ZeroForce/ZeroVelocity 按 mode 激活
56.5 摆动腿轨迹	Cubic spline/Bezier/Cycloid/Raibert	⭐⭐	\(p_f = p_h + \frac{T_s}{2}v + k(v-v_r)\)
56.6 接触序列优化	TOWR/CITO/MCTS+TO	⭐⭐⭐⭐	\(0 \le \lambda \perp d \ge 0\)
56.7 步态切换工程	过渡 stance、FSM、安全检查	⭐⭐	phaseTransitionStanceTime
56.8 legged_control	GaitReceiver、与 Cheetah/RL 对比	⭐⭐	三种步态管理哲学

结构/类型	字段	含义
`ModeSchedule`	`scalar_array_t eventTimes`	模式切换的绝对时刻，长度 \(N-1\)
	`size_array_t modeSequence`	各段模式编号（4-bit 整数），长度 \(N\)
`ModeSequenceTemplate`	`scalar_array_t switchingTimes`	模板内的相对时间偏移（首元素 0、末元素周期 \(T\)）
	`size_array_t modeSequence`	模板模式序列
`Gait`（相位空间）	`scalar_t duration`	步态周期 \(T\)（秒）
	`std::vector<scalar_t> eventPhases`	\((0,1)\) 内的切换相位（不含 0 和 1）
	`std::vector<size_t> modeSequence`	各相位段模式

约束类	`isActive(t)` 条件	约束内容
`ZeroForceConstraint`	`!isContactActive(leg, mode)`（摆动腿）	接触力 \(\lambda_i = 0\)（等式）
`ZeroVelocityConstraint`	`isContactActive(leg, mode)`（触地腿）	足端速度 \(v_{\text{foot}} = 0\)（等式，防滑移）
`FrictionConeConstraint`	`isContactActive(leg, mode)`（触地腿）	\(\\|\lambda_t\\| \le \mu\lambda_n\)（解析二阶锥，不等式）