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

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

难度：⭐⭐⭐ | 预计学时：25-30 小时（1.5 周）| 前置：Ch51, Ch52, Ch55

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

56.0 前置自测¶

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

[Ch51] 什么是 duty factor（占空比）？它与步态速度有什么关系？四足 trot 步态的 duty factor 典型值是多少？
[Ch52] 互补约束 $0 \le \lambda \perp d \ge 0$ 的物理含义是什么？OCS2 如何通过预定义 ModeSchedule 来规避互补约束的非光滑性？
[Ch55] OCS2 的 ModeSchedule 数据结构包含哪两个数组？mode = 9（二进制 1001）对应哪两条腿触地？
[Ch55] 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
处理步态切换的工程问题——过渡稳定性、模式混合、安全检查

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 有一个短暂的"自由飞行"阶段，减少了地面约束力对运动的限制，从而允许更大的步幅和更高的速度。代价是控制更难——着地时刻的冲击力更大。

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 需要知道未来的接触序列¶

回顾 Ch55：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（initTime - timeHorizon 到 finalTime + timeHorizon），以确保边界处有足够的余量。

步骤 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）表明，机器人为了不摔倒，脚必须踩到一个特定的位置使得倒立摆的发散运动被抑制。Raibert 公式中的前馈项 $\frac{T}{2}v$ 就是对 Capture Point 的一阶近似。

// 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（Ch55 的 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$ 是小的松弛参数

CITO vs TOWR 对比：

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

56.6.5 路线三：MCTS + TO——最新前沿（2024-2025）¶

2024-2025 年的最新研究开始结合 Monte-Carlo Tree Search (MCTS) 和 Trajectory Optimization (TO) 来求解接触序列优化：

流程： 1. 用 MCTS 在接触模式的离散空间中搜索——每个树节点是一种接触配置 2. 对每个候选接触序列，用 TO 检验其动力学可行性 3. MCTS 用 TO 的结果作为 reward signal 来指导搜索

代表工作： - Tonneau et al.（2024, arXiv）"Non-Gaited Legged Locomotion with MCTS and Supervised Learning"——用学到的 value function 加速 MCTS 搜索 - Liu et al.（2025, arXiv）"Simultaneous Contact Sequence and Patch Planning for Dynamic Locomotion"——首次在全身动力学模型上实现 MCTS + 全身 TO 的联合优化 - Chen et al.（2025, Advanced Intelligent Systems）"Contact-Implicit TO without Fixed Contact Sequences"——基于 ACAL-iLQR 的无固定接触序列优化

56.6.6 三条路线的统一视角¶

          Three approaches to contact sequence

  Predefined gait --- TOWR ------- CITO ------- MCTS+TO
  (OCS2, 2020)      (2018)        (2014)        (2024)

  Simple <--------- Flexibility ---------> Flexible
  Real-time <------- Computation ---------> Offline
  Flat ground <----- Terrain ---------> Arbitrary

工程选择指南：

场景	推荐方法	原因
平地行走/trot	预定义步态	简单、实时、可靠
已知台阶/斜坡	预定义 + 参数调整	调整 duty factor 和步幅
复杂地形（离线）	TOWR	秒级求解，可优化时序
未知接触模式（研究）	CITO / MCTS+TO	可发现新策略
实时自适应（前沿）	RL + MPC	Ch63 将展示如何用强化学习训练自适应步态策略: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 中支持"根据速度自动切换步态"，需要修改哪些模块？画出架构图。

常见故障与排查¶

现象	可能原因	排查方法
步态切换瞬间机器人剧烈抖动或摔倒	接触模式突变导致 MPC 约束集突变,QP 解不连续	1. 检查模式切换时刻的接触力是否平滑过渡到零 2. 在 `eventTimes` 前后各加 10-20ms 的力衰减窗口 3. 确认 WBC 的 warm-start 在模式切换时正确重置
摆动腿触地时足端力出现尖峰(冲击)	摆动腿轨迹末段速度不为零,着地瞬间产生碰撞冲量	1. 打印摆动腿 touchdown 时刻的足端速度,确认 $\dot{z}$ 接近零 2. 检查 cubic spline 终点的速度边界条件是否设为零 3. 适当降低最后 10% 相位的下降速度
步态频率与实际运动速度不匹配,机器人前倾或后仰	Raibert 落脚点公式中的速度增益 $k$ 不当,或步态周期 $T$ 与速度命令不协调	1. 打印 Raibert 公式的各项:$\frac{T_s}{2}v$, $k(v-v_r)$——哪一项异常? 2. 检查速度命令滤波器是否引入过大延迟 3. 尝试减小增益 $k$ 或增大步态周期
相位变量 $\phi$ 在步态切换后跳变或回绕异常	步态切换时新旧步态的 `eventTimes` 拼接不连续,相位计算出现模运算错误	1. 打印切换前后的 `eventTimes` 序列,检查是否严格递增 2. 确认 `tileModeSequenceTemplate()` 的 startTime 参数正确 3. 检查相位归一化公式的分母是否可能为零(stance duration = 0)
trot 步态中对角腿不同步,出现"跛行"	对角腿的 phase offset 不精确为 0.5,或两侧摆动腿轨迹生成器的时间戳不一致	1. 检查 ModeSchedule 中对角腿的 eventTimes 是否精确对称 2. 确认摆动腿参考轨迹的起始时间来自同一个时钟源 3. 对比左前-右后 vs 右前-左后的接触力曲线

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 累积项目：本章新增模块¶

足式累积项目进度：

章节	模块	内容
Ch51	简化模型	SRBD 动力学 + Centroidal Model
Ch55	OCS2 MPC	SQP + HPIPM + 双线程架构
Ch56	步态管理	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.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 步态训练

与其他章节衔接¶

向前承接：

前置章节	本章使用的知识
Ch51 腿足简化模型	步态分类（trot/walk/bound 等）、duty factor 概念
Ch52 互补约束	接触模式预定义是处理互补约束的路线之一
Ch55 OCS2 完整栈	ModeSchedule 数据结构、SwitchedModelReferenceManager 接口、isActive 机制

向后指向：

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

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

知识点	核心内容	难度	关键公式/概念
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 对比	⭐⭐	三种步态管理哲学