1. 项目概述：当Cassie机器人遇上动态规划

如果你关注过双足机器人领域，那么对Cassie这个名字一定不陌生。它那对标志性的、像鸵鸟腿一样的机械结构，以及在各种复杂地形上展现出的惊人稳定性，让它成为了机器人学研究的明星平台。但今天我们不聊它的硬件设计，也不聊那些复杂的控制算法，我们来聊一个更具体、也更“接地气”的问题： Cassie如何自主、稳定地爬楼梯？

这听起来像是一个纯粹的机器人运动控制问题，对吧？但当你深入其核心决策层，你会发现一个老朋友的身影—— 动态规划 。没错，就是那个在算法面试里让人又爱又恨，用来解决“爬楼梯”、“01背包”问题的动态规划。当这个经典的算法思想，从LeetCode的虚拟世界，走进Cassie的“大脑”去规划每一步的落脚点时，事情就变得非常有趣了。

想象一下，Cassie站在一段楼梯前。它需要决定：第一步迈多大？脚掌落在台阶的哪个位置？身体重心如何转移？如何应对可能出现的打滑或台阶高度不一致？这绝不是一个简单的“抬腿-放下”的循环。它需要一套能在瞬间评估未来几步、甚至十几步可能性的决策系统，从中选出一条能耗最低、稳定性最高、动作最平滑的“最优路径”。这个过程，本质上就是一个 多阶段决策优化问题 ，而动态规划，正是解决这类问题的“屠龙刀”。

所以，“Cassie: Dynamic Planning on Stairs”这个项目，其核心就是探讨如何将动态规划这一数学工具，应用于双足机器人爬楼梯这一复杂的、连续的、高维度的现实任务中。它不是简单地套用“dp[i] = dp[i-1] + dp[i-2]”来计算有多少种爬法，而是要将机器人的状态（姿态、速度、关节角度）、动作（电机扭矩）、以及楼梯的环境模型（台阶高度、深度、摩擦系数）全部纳入一个庞大的“状态空间”中，通过动态规划来搜索最优的动作序列。这背后，是 最优控制理论 、 强化学习 与经典算法的深度融合。

接下来，我将为你层层拆解，如何为Cassie构建这样一个在楼梯上“动态规划”的大脑。我们会从最根本的思路设计开始，深入到状态与动作的定义、代价函数的设计这些核心细节，再通过一个简化的仿真示例，让你看到代码是如何落地的。最后，我会分享在实际研究和仿真中必然会遇到的“坑”以及如何绕过它们。无论你是机器人领域的研究者，还是对算法如何解决实际问题充满好奇的开发者，这篇文章都将为你提供一个清晰的、可操作的视角。

2. 核心思路：从离散算法到连续控制世界的桥梁

把动态规划直接“扔”给Cassie去爬楼梯，肯定会碰一鼻子灰。经典教材里的动态规划，比如爬楼梯问题，状态是离散的（第几级台阶），决策也是离散的（爬1阶或2阶）。但Cassie的世界是连续的：它的身体有无数种可能的姿态，关节电机可以输出连续的扭矩，楼梯的几何尺寸也是连续值。我们的首要任务，就是在这道“离散”与“连续”的鸿沟上，架起一座坚固的桥梁。

2.1 问题重构：定义状态、动作与代价

动态规划的核心是 贝尔曼方程 ，它要求我们明确定义三个要素： 状态（State） 、 动作（Action） 和 代价函数（Cost Function） ，有时也叫奖励函数。对于Cassie爬楼梯，我们需要将它们具体化。

状态（State） ：描述Cassie在某一时刻的“全身快照”。这绝不仅仅是“在第几级台阶上”。一个典型的状态向量可能包括：

• 整体运动状态 ：躯干在三维空间中的位置（x, y, z）、姿态（滚转角、俯仰角、偏航角）、线速度、角速度。
• 腿部关节状态 ：左右腿髋、膝、踝等所有驱动关节的角度和角速度。
• 足端接触状态 ：左脚和右脚是否与地面（或台阶）接触，以及接触点的位置和估计的接触力。
• 环境信息 ：相对于当前躯干坐标系，下一级、下两级台阶的边缘位置和高度。这需要依赖机载传感器（如激光雷达、深度相机）的实时感知。

动作（Action） ：Cassie能做什么来改变状态？通常，我们控制的是各个关节电机的期望扭矩或位置。但在高层规划中，“动作”可以更抽象，比如：“在接下来0.1秒内，将身体重心向前移动5厘米，同时将摆动腿的足端轨迹规划到下一个台阶的特定位置”。这个高层指令会被底层的控制器（如PD控制、模型预测控制）解析为具体的电机命令。

代价函数（Cost Function） ：这是动态规划的“指挥棒”，它告诉Cassie什么是“好”，什么是“坏”。一个精心设计的代价函数是成功的关键。它通常是多项代价的加权和：

1. 跟踪代价 ：鼓励Cassie跟踪一条预设的、理想的爬楼梯步态轨迹（如躯干高度、速度）。
2. 能量代价 ：惩罚关节扭矩的大小， cost_energy = Σ(τ_i²) ，以最小化能耗。
3. 稳定性代价 ：惩罚躯干姿态（俯仰、翻滚）偏离竖直方向的角度， cost_stability = θ_pitch² + θ_roll² 。
4. 足端滑移代价 ：惩罚足端在接触点上的水平速度，鼓励稳定踩踏。
5. 动作平滑代价 ：惩罚相邻时刻动作的剧烈变化，使运动看起来更自然。
6. 终端代价 ：在规划周期的末尾，特别强调稳定性（如要求躯干速度接近零，双脚平稳支撑）。

注意 ：代价函数中各项的权重系数需要精心调节。例如，过于强调能量节省可能导致动作迟缓、容易失稳；过于强调稳定性则可能让机器人动作僵硬、能耗激增。这通常需要通过大量的仿真实验来“调参”。

2.2 离散化与简化：让连续问题可计算

连续的状态和动作空间是无限大的，我们无法直接计算。因此，必须进行 离散化 和 模型简化 。

1. 时间离散化 ：将连续的爬行过程切成一个个小的时间步，例如每0.01秒或0.05秒一个步长。动态规划就在这个离散的时间轴上向前“看”N步（即规划时域）。
2. 状态空间采样/参数化 ：我们无法枚举所有连续状态。常用的方法是结合 模型预测控制（MPC） 的思想。我们不直接搜索所有状态，而是将问题转化为：给定当前状态，寻找未来N个时间步的 最优动作序列 。通过机器人的动力学模型，我们可以预测执行该动作序列后会到达哪些状态。这样，决策变量就从庞大的状态空间，缩减为相对较小的动作序列。
3. 简化动力学模型 ：Cassie的全身动力学模型非常复杂。为了满足动态规划实时计算的要求（通常需要在几毫秒内解算），我们必须使用简化模型。最经典、最有效的就是 线性倒立摆（LIP）模型 或 弹簧负载倒立摆（SLIP）模型 。这些模型将机器人的复杂躯体简化为一个集中在躯干的质量点，腿部则简化为无质量的弹簧或连杆。在这个简化模型上进行动态规划，计算量大大降低，得出的“重心运动轨迹”和“足踏点位置”规划结果，再交给底层的全身控制器去跟踪实现。

思路总结 ：我们的核心思路是 分层规划与控制 。高层（动态规划层）在简化的模型（如LIP）和离散的时间轴上，以较低的频率（如每秒50次）解决“下一步脚该踩哪里，重心该怎么移动”的全局优化问题。底层（关节控制层）则以更高的频率（如每秒2000次），接收高层的目标，利用Cassie的完整动力学模型，计算出具体的关节扭矩，实现精准跟踪。动态规划在这里扮演了高层“战略家”的角色。

3. 核心细节：代价函数设计与动力学模型嵌入

理解了分层框架，我们现在要钻到动态规划这个“战略家”的脑子里，看看它到底是怎么做决策的。这其中的魔鬼，全在细节里。

3.1 代价函数的艺术：平衡多个竞争目标

设计代价函数就像为Cassie制定一套“行为准则”。我们之前列出了多种代价，这里详细解释两个最关键也是最容易出问题的：

稳定性代价的陷阱 ：最简单的稳定性代价是直接惩罚躯干倾角。但这样够吗？不够。因为当Cassie单脚支撑时，它的稳定性很大程度上取决于 零力矩点（ZMP） 是否落在支撑脚的多边形区域内。因此，一个更专业的稳定性代价会包含ZMP的约束。我们可以将理想的ZMP设定在支撑脚中心，然后计算实际ZMP（由动力学模型根据状态和动作估计）与理想位置的偏差，并将其平方后计入代价。这比单纯惩罚角度更贴近物理本质。

能量代价的考量 ： Σ(τ_i²) 是最简单的形式，但它假设所有关节的“耗能成本”相同。实际上，驱动髋关节和驱动踝关节所需的能量特性可能不同。更精细的做法是为不同关节的扭矩赋予不同的权重系数。此外，还可以加入对关节速度的惩罚，因为高速运动通常也意味着高功率消耗。

实操心得：代价函数的调试是一场马拉松，不是冲刺 。不要试图一次性调好所有权重。一个有效的方法是 分阶段调试 ：

1. 第一阶段 ：只启用“跟踪代价”和很小的“动作平滑代价”，在平地上让Cassie行走。目标是让它能跟上预设的步态。
2. 第二阶段 ：引入“稳定性代价”（ZMP相关）的权重，慢慢增大，观察机器人在受到微小扰动时的恢复能力。
3. 第三阶段 ：引入“能量代价”权重，在稳定行走的基础上，观察步态是否变得怪异（如拖着脚走路），找到稳定与节能的平衡点。
4. 第四阶段（爬楼梯） ：将“足端位置跟踪代价”的目标轨迹，从平地改为上一级台阶的序列。此时可能需要微调所有权重，因为楼梯引入了新的高度变化和碰撞动力学。

3.2 动力学模型：简化与精度之间的权衡

动态规划需要有一个“预言”能力：给定当前状态和未来一系列动作，它能预测出未来的状态。这个预言家就是 动力学模型 。对于Cassie，我们有几种选择：

1. 完整刚体动力学模型 ：最精确，但计算量巨大，即使离散化后，用于在线动态规划也几乎不可能。它通常只用于事后的分析和仿真验证。
2. 线性倒立摆（LIP）模型 ：这是最流行的选择。它假设：
   • 总质量集中在躯干一点（质心）。
   • 腿无质量，且始终伸直。
   • 质心在一个恒定高度上运动。
   • 支撑点可以瞬时切换（即脚可以瞬间抬起和放下）。 在这个模型下，动力学方程被简化为一个线性微分方程，这使得动态规划中的状态转移计算变得极其快速。 它的核心输出是：在未来的步态周期中，质心应该如何运动，以及支撑脚应该在何时、何地切换。 对于爬楼梯，我们需要将楼梯的几何（台阶高度h，深度d）作为约束条件融入LIP的足端位置规划中。
3. 弹簧负载倒立摆（SLIP）模型 ：比LIP更复杂一点，它把腿看作弹簧，可以更好地捕捉奔跑或弹跳步态中的能量储存与释放。但对于Cassie以行走为主的爬楼梯，LIP通常已足够。

如何将楼梯“告诉”模型？ 这是关键一步。在LIP的规划中，足端位置不再是平面上的点，而是一个三维序列 [x_i, y_i, z_i] 。其中 z_i 由楼梯的几何决定： z_i = floor(i / 2) * h （假设左右脚交替上楼，每上一级台阶高度增加h）。同时，在规划足端水平位置 (x_i, y_i) 时，必须确保它落在目标台阶的矩形区域内，这是一个 不等式约束 。动态规划算法（如DDP）需要能够处理这种带约束的优化。

注意 ：使用简化模型必然带来 模型失配 。即，规划器基于LIP模型算出的“最优”动作，交给Cassie的真实身体执行时，效果会打折扣，因为真实身体有质量分布、关节柔性、延迟等。因此，底层控制器必须足够鲁棒，能够抵抗这种模型误差。同时，规划器也需要具备一定的 反馈 机制，即根据当前真实的传感器状态（而不是模型预测状态）来重新规划，这就是迭代线性二次型调节器（iLQR）或模型预测控制（MPC）的思想，它们本质上是带反馈的动态规划。

4. 实操推演：一个高度简化的仿真示例

理论说了这么多，我们来点“看得见摸得着”的。由于真实的Cassie仿真环境（如MuJoCo, PyBullet）配置复杂，我将用一个极度简化的2D LIP模型，在Python中演示动态规划（具体来说，是迭代线性二次型调节器iLQR，一种高效求解非线性动态规划的方法）如何规划爬楼梯的质心轨迹。

这个示例的目标是：一个质点在恒定高度上运动（LIP假设），规划它在跨越三个台阶时的水平运动轨迹，使其运动平滑且最终停在顶部。

import numpy as np
import matplotlib.pyplot as plt

# ==================== 1. 参数定义 ====================
dt = 0.05  # 时间步长 (秒)
N = 100    # 总时间步数 (总时间 = N*dt = 5秒)
n_state = 2  # 状态维度 [位置x, 速度v]
n_ctrl = 1   # 控制维度 [加速度u]

# 楼梯参数
step_height = 0.15  # 米 (台阶高度，这里仅用于可视化，LIP模型高度恒定)
step_depth = 0.25   # 米 (台阶深度)
num_steps = 3
# 足端落脚点序列 (简化：假设脚步瞬时切换，且准确踩在台阶边缘)
foot_placements = [0.0, step_depth, 2*step_depth, 3*step_depth]  # 每一步的支撑点x坐标

# LIP参数 (质心高度恒定)
z0 = 0.8  # 米
g = 9.81
omega = np.sqrt(g / z0)  # LIP的特征频率

# ==================== 2. 动力学模型 (离散时间线性系统) ====================
# 状态方程: x_{t+1} = A * x_t + B * u_t
# 对于LIP在水平面的运动，我们简化为双积分器模型: 位置x, 速度v, 控制是加速度u
# 这忽略了LIP真正的摆动动力学，但用于演示规划流程足够了。
A = np.array([[1, dt],
              [0, 1]])
B = np.array([[0.5*dt*dt],
              [dt]])

# ==================== 3. 代价函数定义 ====================
# 我们使用线性二次型代价: J = sum_{t=0}^{N} (x_t^T Q_t x_t + u_t^T R_t u_t) + x_N^T Q_N x_N
Q = np.diag([1.0, 0.1])    # 状态代价权重 [位置, 速度]
R = np.array([[0.01]])      # 控制代价权重 [加速度]
Q_final = np.diag([10.0, 1.0]) # 终端状态代价权重 (希望最终停稳)

# ==================== 4. iLQR 算法核心 (简化版) ====================
def ilqr_simple(A, B, Q, R, Q_final, x0, N):
    """
    简化的iLQR算法求解线性二次型问题（即LQR）。
    对于非线性问题，需要在每个点进行动力学线性化和代价二次展开，这里省略。
    """
    # 倒推计算增益矩阵 K
    P = [None] * (N + 1)
    K = [None] * N
    P[N] = Q_final.copy()
    
    for t in range(N-1, -1, -1):
        # 离散时间代数Riccati方程
        K[t] = -np.linalg.inv(R + B.T @ P[t+1] @ B) @ (B.T @ P[t+1] @ A)
        P[t] = Q + A.T @ P[t+1] @ A + A.T @ P[t+1] @ B @ K[t]
    
    # 前向滚动应用控制律
    x = np.zeros((N + 1, n_state))
    u = np.zeros((N, n_ctrl))
    x[0] = x0
    
    for t in range(N):
        u[t] = K[t] @ x[t]
        x[t+1] = A @ x[t] + B @ u[t]
    
    return x, u

# ==================== 5. 设置问题并求解 ====================
# 初始状态：从原点开始，速度为零
x0 = np.array([0.0, 0.0])
# 期望的终端状态：到达第三个台阶的位置，速度为零
x_des = np.array([num_steps * step_depth, 0.0])

# 为了使用LQR，我们需要将代价函数重新表述为跟踪误差的代价。
# 定义参考轨迹：期望状态随时间变化（这里简单地从x0线性插值到x_des）
ref_states = np.zeros((N + 1, n_state))
for t in range(N + 1):
    alpha = t / N
    ref_states[t] = (1 - alpha) * x0 + alpha * x_des

# 在实际iLQR中，会在参考轨迹附近迭代优化。这里我们用LQR来近似。
# 我们求解一个围绕参考轨迹的调节器问题（假设参考控制输入为0）。
x, u = ilqr_simple(A, B, Q, R, Q_final, x0, N)

# ==================== 6. 可视化结果 ====================
time = np.arange(N + 1) * dt

fig, axes = plt.subplots(3, 1, figsize=(10, 8))

# 图1：水平位置轨迹
axes[0].plot(time, x[:, 0], 'b-', linewidth=2, label='CoM Position (Planned)')
# 画出楼梯和足端位置
for i in range(num_steps + 1):
    axes[0].axvline(x=foot_placements[i], color='gray', linestyle='--', alpha=0.5, label='Foot Placement' if i==0 else "")
    axes[0].scatter([foot_placements[i]], [0], color='red', s=50, zorder=5)
axes[0].set_ylabel('X Position (m)')
axes[0].set_title('LIP Model CoM Trajectory Planning for Stair Climbing')
axes[0].legend()
axes[0].grid(True)

# 图2：速度轨迹
axes[1].plot(time, x[:, 1], 'g-', linewidth=2)
axes[1].axhline(y=0, color='k', linestyle=':', alpha=0.3)
axes[1].set_ylabel('Velocity (m/s)')
axes[1].grid(True)

# 图3：控制输入（加速度）
axes[2].plot(time[:-1], u, 'r-', linewidth=2)
axes[2].set_xlabel('Time (s)')
axes[2].set_ylabel('Control Accel (m/s²)')
axes[2].grid(True)

plt.tight_layout()
plt.show()

# 输出关键指标
print(f"初始位置: {x[0, 0]:.3f} m, 初始速度: {x[0, 1]:.3f} m/s")
print(f"最终位置: {x[-1, 0]:.3f} m, 最终速度: {x[-1, 1]:.3f} m/s")
print(f"目标位置: {x_des[0]:.3f} m")
print(f"位置误差: {abs(x[-1,0] - x_des[0]):.4f} m")

代码解读与实操要点：

1. 极度简化 ：这个例子为了清晰，省略了真正的LIP摆动动力学和脚切换约束。它用一个双积分器模型（位置、速度受加速度控制）来模拟质心水平运动，并用经典的 线性二次型调节器（LQR） 来求解最优轨迹。LQR可以看作是线性动力学和二次代价函数下的动态规划解析解。
2. 核心思想展示 ：虽然简化，但它展示了动态规划/最优控制的核心流程：
   • 定义模型 ( A , B 矩阵)。
   • 定义代价 ( Q , R , Q_final 矩阵)。这里 Q 矩阵中位置权重(1.0)远大于速度权重(0.1)，表示我们更看重到达准确位置； R 很小(0.01)，表示我们允许较大的控制量以实现快速跟踪。
   • 逆向求解 ：代码中的 ilqr_simple 函数实际上是在倒推求解Riccati方程，得到最优反馈增益矩阵 K[t] 。这个过程就是动态规划中的“值迭代”或“策略迭代”在线性二次型情况下的高效实现。
   • 正向滚动 ：从初始状态开始，应用计算出的最优控制律 u = K * x ，生成整条状态轨迹。
3. 从仿真到现实的鸿沟 ：
   • 真实环境中， A 和 B 矩阵不会是固定的，它们会随着机器人姿态（单支撑/双支撑）和楼梯几何变化。
   • 代价函数远非二次型这么简单，会包含复杂的非线性项和不等式约束（如足端必须在台阶上）。
   • 因此，真实的实现会使用 迭代LQR（iLQR） 或 微分动态规划（DDP） ，它们能处理非线性动力学和非二次代价。其核心是在一个初始猜测轨迹附近反复进行线性化和二次展开，迭代求解局部最优解。

尽管这个示例简单，但它清晰地揭示了将动态规划应用于运动规划的本质： 在一个时间轴上，逆向推导出“最优策略”（增益矩阵K），然后正向执行这个策略，同时根据实际状态进行反馈调整。

5. 常见问题与工程实践中的“坑”

在实际研究和工程化中，理论到实践的路径布满荆棘。以下是我在相关工作中总结的几个关键挑战和应对策略。

5.1 计算实时性：如何在毫秒内完成规划？

动态规划，尤其是处理非线性、高维状态空间时，计算量巨大。Cassie需要每秒进行数十次规划（与控制器频率同步），每次规划必须在几毫秒内完成。

解决方案：

1. 简化模型是王道 ：坚持使用LIP等简化模型。这是平衡精度与速度最关键的一步。
2. 降低规划维度 ：不要规划所有关节的轨迹。高层规划只输出 足端位置轨迹 和 质心运动轨迹 。关节轨迹由底层的逆运动学或任务空间控制器实时计算。
3. 缩短规划时域 ：不必规划完整的爬楼过程。通常规划未来1-2步（约0.5-1秒）就足够了。采用 滚动时域 的方式，每执行一步，就基于新的传感器数据重新规划下一步。
4. 利用高效求解器 ：使用为实时优化定制的库，如 OSQP （用于二次规划）、 ACADOS 或 CasADi （用于非线性优化）。这些工具提供了高效的数值算法实现。
5. 代码高度优化与硬件加速 ：在C++中实现核心算法，并利用SIMD指令或多线程进行并行计算。对于最耗时的部分（如矩阵运算），可以考虑使用GPU加速。

5.2 状态估计与感知误差：规划基于错误的地图怎么办？

动态规划严重依赖于当前状态和对环境的感知。如果Cassie错误地估计了自己的姿态，或者深度相机把台阶边缘看错了10厘米，规划出的落脚点可能就是“万丈深渊”。

应对策略：

1. 多传感器融合 ：结合IMU、关节编码器、足底力传感器和视觉/激光雷达，通过卡尔曼滤波器或状态观测器，鲁棒地估计机器人的本体状态（姿态、速度）。
2. 感知不确定性建模 ：在规划中考虑感知误差。例如，不是把台阶边缘视为一个确定点，而是视为一个概率分布（高斯分布）。规划时可以选择落脚在概率分布的中心（最可能的位置），或者甚至优化一个期望代价，将踩空的风险纳入考量。
3. 基于触觉的快速反应 ：当规划失败，足端接触感觉与预期不符（例如，预计踩到台阶但实际踩空了），底层需要有一个 反射层 。这通常是一套预设的、快速的平衡恢复策略（如跨步反射、踝关节策略），它不经过复杂的重新规划，直接触发保护性动作，为高层规划争取重新计算的时间。

5.3 模型失配与扰动：现实世界总是不完美的

简化模型（LIP）与真实机器人之间的差异，以及未知的地面扰动（如楼梯表面轻微不平、有杂物），都会导致规划失效。

解决方案：

1. 鲁棒优化与随机动态规划 ：在优化问题中考虑模型不确定性或可能的扰动。例如，使用 最小-最大优化 （Min-Max Optimization）来寻找在最差扰动下仍然表现良好的策略，或者使用 随机动态规划 ，假设扰动服从某种分布，优化期望性能。
2. 在线模型自适应 ：让机器人一边走一边学习。可以设计一个在线参数估计器，实时更新简化模型中的关键参数（如等效质心高度、地面摩擦系数估计），使模型不断逼近当前环境的真实动力学。
3. 分层控制中的阻抗控制 ：在底层关节控制器中采用阻抗控制策略。这意味着控制器不严格跟踪位置，而是模拟一个弹簧阻尼系统。当脚落地位置有偏差或地面有凸起时，阻抗控制允许腿“柔顺”地压缩和调整，吸收冲击，而不是硬邦邦地对抗，这极大地增强了对外部扰动的鲁棒性。

5.4 足端碰撞与摆动腿轨迹规划

爬楼梯时，摆动腿需要从一个台阶提升并落到下一个台阶，这个过程必须避免与楼梯边缘发生碰撞。

实操心得：

1. 轨迹参数化 ：摆动腿足端的轨迹通常用一个参数化的曲线来表示，例如三次样条曲线或贝塞尔曲线。曲线的起点是当前支撑点，终点是下一个目标落脚点。
2. 添加航点与约束 ：为了避开楼梯竖板（riser），可以在轨迹中间添加一个或多个“航点”，强制足端在抬起时有一个足够高的抬脚高度。这个高度需要作为优化问题的一个约束。
3. 在优化中直接处理碰撞约束 ：在动态规划/优化问题中，可以将楼梯的几何模型表示为一系列不等式约束。例如，要求摆动腿足端在运动过程中的任何时刻，其z坐标都必须高于由当前x坐标决定的楼梯轮廓线。这会将问题变成一个带约束的非线性优化，求解更复杂，但更安全。

一个典型的避障约束可以表示为： foot_z(t) > stair_height_at(foot_x(t)) + clearance_margin 其中 stair_height_at(x) 是一个根据x坐标查询台阶高度的函数， clearance_margin 是一个安全裕度（如5厘米）。这个约束需要施加在从抬脚到落脚的整个时间区间内。

将动态规划应用于Cassie爬楼梯，是一个融合了算法思想、物理建模、控制理论和工程实践的精彩案例。它告诉我们，最优雅的算法往往需要最接地气的适配和打磨，才能从论文公式走向现实世界的楼梯。这个过程没有银弹，有的只是对细节的不断打磨和对物理规律的深刻尊重。