5. 步态控制
上一章 Pupper 已经能站起来。这一章我们让它走起来，暂时不动强化学习，只用手写的步态生成器。这样我们能清楚知道机器人为什么会走成这样，也能为第 6 章的 RL 策略提供一个对比基线。
本章走的是 model-based（基于模型） 路线：我们事先用运动学/动力学模型把"腿应该怎么动"显式写成公式——支撑相-摆动相节奏、足端轨迹、IK 反解关节角，全部由人手工设计，控制器内部没有任何需要训练的参数。它的对立面是第 6 章的 learning-based（基于学习） 路线，那里步态由神经网络在仿真里试错学出来。model-based 的好处是行为可解释、不需要数据、改一行公式就能预测效果；代价是地形/扰动一旦超出建模假设就容易崩，因此通常用作 baseline 与安全兜底。
本章目标
- 理解支撑相（stance）与摆动相（swing）的划分
- 能写一个最简单的 trot 步态发生器（四条腿两两同相）
- 能用 IK 把"末端足端轨迹"转成"关节角序列"
- 能让 Pupper 在仿真里走出 1 米直线
前置阅读
5.1 常见步态
四足步态可以先按速度理解：walk 最稳、trot 最常用、bound/gallop 更快但更难控。四种典型步态的着地节奏、占空比和适用场景如 图 1 所示。
本章从 trot（小跑） 入门，原因很简单：
- 好写：对角腿同步，FL+RR 一组，FR+RL 一组；
- 够稳：左右对称，支撑线穿过机身附近，不容易横向偏；
- 效果明显：速度通常在 0.3–0.5 m/s，比 walk 更容易看出前进，又不需要 bound/gallop 的腾空和落地缓冲。
补充一点：pace（侧对步） 也是占空比约 0.5 的两两同步步态，但同步的是同侧腿（FL+RL、FR+RR），转弯和侧倾问题更明显，不作为本章重点。
CS123 Lab 4 slides 直接挑明："Pupper is NOT expected to walk straight from this simple gait." 后面 5.7 节会让你亲眼看到这一点，并把它当作第 6 章 RL 的动机。
5.2 步态节奏图
步态图（gait diagram）用于描述各条腿在一个步态周期内的支撑相（stance）与摆动相（swing）分布。对本章的 trot 步态而言，主要关注以下三个参数：
- 周期
：一次完整步态循环的持续时间，trot 中通常取 0.3–0.5 s。 - 占空比
：单条腿在一个周期内处于支撑相的时间比例；trot 中通常取 。 - 相位偏移
：第 条腿相对于参考腿的相位延迟，单位为周期，取值范围为 。
trot 的对角腿相位关系如 图 2 所示。FL 与 RR 同相，FR 与 RL 同相，两组对角腿之间相差半个周期。
因此，trot 步态的相位偏移可以写为：
phi_FL = 0.0, phi_RR = 0.0 # 对角线 1
phi_FR = 0.5, phi_RL = 0.5 # 对角线 2
实现时可使用一个循环计数器
当
import numpy as np
PHASE_OFFSETS = {'FL': 0.0, 'FR': 0.5, 'RL': 0.5, 'RR': 0.0}
DUTY = 0.5
def leg_phase(t, leg, T_cycle):
"""返回 (in_stance, s),s 是当前 stance 或 swing 内的归一化进度 ∈ [0, 1)。"""
t_global = (t / T_cycle) % 1.0
t_local = (t_global + PHASE_OFFSETS[leg]) % 1.0
if t_local < DUTY:
return True, t_local / DUTY # stance, s 从 0 走到 1
else:
return False, (t_local - DUTY) / (1 - DUTY) # swing, 同样 0..1
这段实现的核心是将步态时序表示为相位计算：控制器只维护一个全局循环相位，每条腿通过 PHASE_OFFSETS 得到局部相位，再由 DUTY 判定其处于支撑相还是摆动相。
5.3 足端轨迹
在确定各腿的支撑/摆动节奏后，还需要为每条腿生成足端在身体坐标系下的目标轨迹。CS123 Lab 4 slides 将单条腿一个周期内的足端运动划分为四个关键相位，如 图 3 所示。
| 相位 | 时机 | 足端位置 | 物理意义 |
|---|---|---|---|
| Touch down | swing → stance 切换 | 身体前方 + 着地高度 0 | 落脚瞬间 |
| Standing | stance 中段 | 身体正下方 + 高度 0 | 重心接近支撑足上方，推进力较大 |
| Lift off | stance → swing 切换 | 身体后方 + 高度 0 | 抬腿瞬间 |
| Mid swing | swing 中段 | 身体正下方 + 最高点 | 越过障碍/避免拖地 |
基于上述相位，可以构造一个简化的足端轨迹生成器。这里的足端位置定义在身体坐标系下，并相对于对应腿髋关节的零位表示：
def foot_trajectory(s, in_stance, step_length, step_height, stand_height):
"""
s: 当前相位段内的进度 (0..1),由 5.2 节的 leg_phase 给出
返回 (x, z) — 身体系下足端目标。x 沿前进方向,z 向上。
"""
if in_stance:
# Stance: 从 +L/2 (touch down) 线性走到 -L/2 (lift off)
x = step_length * (0.5 - s)
z = -stand_height
else:
# Swing: 从 -L/2 抬起,经 mid swing 最高点,落回 +L/2
x = step_length * (s - 0.5)
z = -stand_height + step_height * np.sin(np.pi * s)
return np.array([x, z])
实现时需要注意以下三点：
- 支撑相中
保持常量：支撑腿的足端应保持在地面高度附近，主要通过 方向的相对后移表示身体向前运动。如果在支撑相中额外引入下压运动，容易造成机身垂向扰动。 - 摆动相中使用
sin(πs)生成抬脚高度：该函数在和 处高度为 0，在 处达到最高点，能够形成平滑的抬脚-落脚过程。 step_length表示身体坐标系下的足端前后摆幅：实现时需要保持坐标方向一致，避免将身体相对足端的运动方向与足端目标轨迹方向混淆。
5.4 轨迹到关节角
5.3 节生成的是身体坐标系下的足端目标位置，而底层执行器接收的是关节空间中的控制输入。因此，需要先通过逆运动学（IK）将足端目标转换为目标关节角，再由 PD 控制器根据当前关节状态计算力矩或控制量，并送入 MuJoCo 仿真。这条执行管线如 图 4 所示。
在实现上，第 3 章的单腿 IK 会对四条腿分别调用一次，并将每条腿的 3 个目标关节角拼接成 12 维目标关节向量：
from ik_pupper import ik_leg_3dof # 第 3 章解析法的四足版本
def gait_step(t, T_cycle, step_length, step_height, stand_height):
"""对每条腿算一次目标关节角,返回 12 维向量。"""
target_q = np.zeros(12)
for k, leg in enumerate(['FL', 'FR', 'RL', 'RR']):
in_stance, s = leg_phase(t, leg, T_cycle)
foot_xy = foot_trajectory(s, in_stance,
step_length, step_height, stand_height)
# foot_xy 是 (x, z),y 维度由 HAA 关节决定,这里 hold 在 stance 时的横向偏置
foot_xyz = np.array([foot_xy[0], LEG_Y_OFFSET[leg], foot_xy[1]])
target_q[3*k : 3*k+3] = ik_leg_3dof(foot_xyz, leg=leg)
return target_q
接入第 4 章的 PD 回路后，target_q 作为期望关节角，与仿真中读取到的当前关节角 q 和关节速度 dq 一起用于计算控制输入。
频率分层：步态生成器和 IK 通常可以以 50–100 Hz 更新；PD 控制器则跟随 MuJoCo 仿真步长运行，常见频率为 500 Hz–2 kHz。这与第 3 章 IK + PD 的结构一致，只是足端参考轨迹从单腿测试轨迹扩展为四条腿同步的步态轨迹。
5.5 CPG 简述
5.2 节使用的模 1 相位计数器可以理解为一种全局时钟：控制器维护一个统一的全局相位
全局时钟的优点是实现简单、参数直观，并且可以直接与 5.2 节的步态表和相位偏移定义对应起来。本章的手写 trot 控制器采用这种方式，便于调试和理解。
CPG 的主要优势在于节奏具有一定的柔性：当机器人受到外界扰动、局部接触状态发生变化时，耦合振荡器有机会通过相位调整逐步恢复协调节奏。代价是实现复杂度更高，参数也更难调试。
在现代四足机器人中，纯 CPG 控制并不是唯一主流方案；工程上常见做法包括基于全局相位的手写步态、模型预测控制，以及第 6 章将介绍的强化学习步态策略。不过，CPG 强调的相位表示仍然非常重要：许多学习型策略会将全局相位或腿部相位作为额外观测输入，用来帮助策略学习周期性运动结构。
如果希望进一步实现 Hopf 振荡器形式的 trot，可以参考 Margolis 2022 的 "Walk These Ways" 第 4 节。
5.6 实验 1：原地踏步
最稳的"步态生成器有没有写对"自检——不让它前进：
import mujoco
model = mujoco.MjModel.from_xml_path('pupper.xml')
data = mujoco.MjData(model)
T_cycle = 0.4
step_length = 0.0 # 关键:不前进
step_height = 0.04
stand_height = 0.18
Kp = np.full(12, 30.0); Kd = np.full(12, 1.0)
while data.time < 10.0:
target_q = gait_step(data.time, T_cycle,
step_length, step_height, stand_height)
q, dq = data.qpos[7:], data.qvel[6:]
data.ctrl[:] = Kp * (target_q - q) - Kd * dq
mujoco.mj_step(model, data)
上述参数下的原地 trot 踏步效果如 图 6 所示。该实验主要用于检查对角腿同步关系、抬腿高度和机身垂向稳定性。
观察重点：
- 对角腿是不是同步：FL + RR 一起抬、FR + RL 一起抬。不同步说明
PHASE_OFFSETS写反了。 - 抬腿高度对不对：viewer 里目测约 4 cm，相机俯视看不到地面闪光就是没抬起来。
- base.z 抖动幅度：原地踏步时身体应该上下小幅起伏 < 1 cm；抖得很厉害多半是
step_height太大或stand_height太低。
通过这一关，至少证明 5.2 + 5.3 + 5.4 这三步串起来没有 bug。
5.7 前进实验
把 step_length 改成"周期内身体应该走的距离"：
接触系数经验上取 1.0~1.2（补偿 stance 末期略微滑动）。脚本几乎和实验 1 一样，只改一行：
v_cmd = 0.3 # m/s
step_length = v_cmd * T_cycle * 1.1 # ≈ 0.13 m
vx_log = []
while data.time < 10.0:
target_q = gait_step(data.time, T_cycle,
step_length, step_height, stand_height)
q, dq = data.qpos[7:], data.qvel[6:]
data.ctrl[:] = Kp * (target_q - q) - Kd * dq
mujoco.mj_step(model, data)
vx_log.append((data.time, data.qvel[0]))
给定前进速度后的开环 trot 效果如 图 7 所示。该实验用于观察手写开环步态在前进任务中的速度误差和走偏现象。
10 秒下来期望走出约 3 m（命令 0.3 m/s × 10 s）。但你大概率会看到三种"问题"现象——这些就是 CS123 Lab 4 slides 那句"NOT expected to walk straight"的具体表现：
- 走偏 1–3 度：开环控制对左右惯量不对称、四条腿摩擦不一致非常敏感。
- 实际速度比命令低 10–20%：触地瞬间足端有微滑，能量被摩擦带走。
- 跑得越快越歪：
v_cmd > 0.6 m/s时 stance 阶段太短，足端来不及"压实"地面就被抬起来。
把 (time, vx_actual) 画成曲线和 v_cmd 横线对比，就是动手任务里"和 RL 步态对比"的素材。这条曲线就是第 6 章 RL 替代手写 trot 的最直接动机。
5.8 常见失败模式
按"症状 → 原因 → 怎么调"列：
| 症状 | 多半的原因 | 怎么修 |
|---|---|---|
| 滑步：触地瞬间足端横向窜出 | swing 末速度不为 0（写成抛物线了） | 改成 sin(π s) 让端点速度归零 |
| 踩空：摆动腿擦着地走 | step_height 太小，或 stand_height 没对齐第 4 章站姿 | step_height 调到 4–6 cm,stand_height 和 4.6 节那个 0.18 m 一致 |
| 对角腿不同步：四条腿乱晃 | PHASE_OFFSETS 写错或 % 1.0 漏写 | 单步打印 (t_global, t_local for each leg) 一目了然 |
| 走偏：直线命令但越走越斜 | 开环控制本身的痼疾 | 模型驱动方法里加 yaw 反馈 PI（base 朝向 → 前后腿步长差） |
| 翻车：高速下身体前栽 | T_cycle 太长或 stance 太短 | 提高步频（T_cycle 0.4 → 0.3）让四足"踏踏地"更频繁 |
| NaN 飞天 | step_length > 工作空间，IK 返回 NaN | IK 调用前 clip 一下 foot 目标到工作空间内 |
调参顺序（推荐）：实验 1 先把 4 条腿对齐 → 实验 2 用
v_cmd = 0.2跑出"看着像走"的视频 → 再逐渐加速看哪条症状先出来 → 针对症状调一项参数，每次只调一项。
小结
- trot 是手写步态的入门级：对角腿成对抬落、占空比 0.5、相位偏移 (0, 0.5, 0.5, 0) 是它的全部规则。
- 步态生成器 = 三步管线：循环相位计数器
→ 四相位足端轨迹（Touch down / Standing / Lift off / Mid swing）→ 第 3 章 IK 转关节角 → 第 4 章 PD 转力矩。 - 频率分层一脉相承：步态 + IK 跑 50–100 Hz，PD 跟仿真步长跑 500 Hz–2 kHz，和第 3 章 IK+PD 完全同构。
- 开环 trot 注定走不直：开环对建模误差/摩擦/惯量不对称非常敏感，这正是第 6 章 RL 步态要解决的问题——模型驱动给基线，数据驱动给鲁棒。
- 调试有顺序：先原地踏步验证步态生成、再低速前进验证 IK 闭环、最后才提速找极限——别一上来就 1 m/s。
动手任务
§5.6 让 trot 原地踏步、§5.7 把它推上 ground 看到了走偏。但 §5.1 表里 trot 之外的另两种步态（pace、bound）一行代码也没跑过——它们到底比 trot 难在哪？本章动手任务把 §5.2 的 leg_phase、§5.3 的 foot_trajectory、§5.4 的 gait_step 抽成一个 gait factory，trot / pace / bound 仅在 (offsets, duty) 上不同；三只 Pupper 在同一份 ground + PD + IK 下并排跑，"哪几条腿同相"这件事直接变成可视证据。三种步态的并排对照如 图 8 所示。
三只 Pupper 的 base 都用 <weld> 焊在空中（shared/models/pupper_v3_on_stand.xml），模型 / Kp / Kd / 步频 / 步长 / 抬腿高度全部相同，单变量只有 (offsets, duty)。把 base 焊住是刻意选择——pace 和 bound 接到地面上 12 秒之内就会侧翻或前栽，注意力立刻从 phase pattern 滑到摔倒姿态去（参见 §5.8 的"翻车"症状）。welded 之后摔不倒，但每只 Pupper 关节层面的"对角同抬 / 同侧同抬 / 前后同抬"差异看得清清楚楚：左 trot 对角腿成对抬落，机身几乎纹丝不动；中 pace 同侧腿同相，base 沿前进方向有明显的横滚激励（tests.py 里 pace 的 roll std ≈ 1.0、trot ≈ 0）；右 bound 前两腿与后两腿成对抬落，俯仰激励大于 trot。把 base 解开焊住、接到地面上是 Lab 6 端到端 RL 要面对的下一关。
要做的四件事：
- 把三种步态写成
GAITSdict，差别只在offsets和duty；leg_phase(t, leg, offsets, duty, T_cycle)单元测试覆盖 stance/swing 边界 -
foot_trajectory(s, in_stance, ...)：stance 直线、swing 用step_height * sin(πs)，三种步态共用同一份；端点垂直速度为 0 -
gait_step(t)：四条腿轮一遍 phase → traj →shared.kinematics.ik_pupper_leg，吐 12 维目标角 -
make_artifacts.py：复用shared/models/pupper_v3_on_stand.xml（base welded）+Kp=15, Kv=0.5, period=1.2 s, stride=7 cm, swing_z=5 cm，offscreen 渲三只 Pupper 拼成 1280×480 GIF；附带 4 条腿的 stance/swing Gantt 图——trot 呈 X、pace 呈 =、bound 呈 Z，把 §5.1 的分类落到数据上
完整 starter / 测试 / 交付清单（含 Stretch:三种步态的 base z FFT 频谱叠图）见 exercises/lab_5_gait_zoo/。
参考资料
- CS123 Lecture 5: Model-based Gait Control
- Raibert, "Legged Robots That Balance"