4. 步态周期中单支撑与双支撑阶段的切换控制

任务目标

在双足机器人步态周期中，单支撑（SSP）与双支撑（DSP）阶段的切换是影响行走稳定性的关键。本案例实现一个相位切换控制器，基于归一化步态相位 φ∈[0,1) 计算各关节的目标角度与角速度，并在切换边界处采用五次多项式插值（quintic blend）保证关节位置和速度的连续过渡，避免冲击。

核心实现

import numpy as np

# 步态参数
T_ssp = 0.4   # 单支撑时长(s)
T_dsp = 0.2   # 双支撑时长(s)
T = T_ssp + T_dsp  # 完整周期

# 关节定义：髋关节、膝关节、踝关节（左右各3）
joint_names = ['L_hip', 'L_knee', 'L_ankle', 'R_hip', 'R_knee', 'R_ankle']

# 各阶段目标角度（弧度），行序：SSP起始、SSP结束、DSP结束
targets = {
    'L_hip':   [0.3, -0.2, 0.1],
    'L_knee':  [0.5, 0.8, 0.4],
    'L_ankle': [-0.2, -0.1, -0.15],
    'R_hip':   [-0.2, 0.3, 0.1],
    'R_knee':  [0.8, 0.5, 0.4],
    'R_ankle': [-0.1, -0.2, -0.15]
}

def phase_to_stage(phi):
    """返回 (阶段, 阶段内归一化时间t∈[0,1))"""
    if phi < T_ssp / T:
        return 'SSP', phi * T / T_ssp
    else:
        return 'DSP', (phi - T_ssp / T) * T / T_dsp

def quintic_blend(y0, y1, t):
    """五次多项式插值，保证位置和速度连续"""
    t2 = t * t
    t3 = t2 * t
    t4 = t3 * t
    t5 = t4 * t
    h = 6*t5 - 15*t4 + 10*t3
    dh = 30*t4 - 60*t3 + 30*t2
    return y0 + (y1 - y0) * h, (y1 - y0) * dh / 1.0  # 归一化时间导数

def get_joint_command(phi):
    """根据相位返回各关节目标角度与角速度"""
    stage, t = phase_to_stage(phi)
    cmd = {}
    for j in joint_names:
        if stage == 'SSP':
            # 从SSP起始插值到SSP结束
            theta, dtheta = quintic_blend(targets[j][0], targets[j][1], t)
        else:
            # 从SSP结束插值到DSP结束
            theta, dtheta = quintic_blend(targets[j][1], targets[j][2], t)
        cmd[j] = (theta, dtheta / (T_ssp if stage == 'SSP' else T_dsp))
    return cmd

# 验证：检查切换点连续性
def check_continuity():
    eps = 1e-6
    # SSP结束时刻（φ = T_ssp/T - eps）与DSP开始时刻（φ = T_ssp/T + eps）
    phi_boundary = T_ssp / T
    cmd_before = get_joint_command(phi_boundary - eps)
    cmd_after = get_joint_command(phi_boundary + eps)
    max_pos_err = 0.0
    max_vel_err = 0.0
    for j in joint_names:
        pos_err = abs(cmd_before[j][0] - cmd_after[j][0])
        vel_err = abs(cmd_before[j][1] - cmd_after[j][1])
        max_pos_err = max(max_pos_err, pos_err)
        max_vel_err = max(max_vel_err, vel_err)
    print(f"切换点最大位置误差: {max_pos_err:.2e} rad")
    print(f"切换点最大速度误差: {max_vel_err:.2e} rad/s")
    assert max_pos_err < 1e-4 and max_vel_err < 1e-4, "切换不连续！"
    print("连续性检查通过")

if __name__ == "__main__":
    check_continuity()
    # 示例输出：φ=0.3（SSP中段）和φ=0.7（DSP中段）的指令
    for phi in [0.3, 0.7]:
        cmd = get_joint_command(phi)
        print(f"\nφ={phi}: 阶段={phase_to_stage(phi)[0]}")
        for j in ['L_hip', 'R_knee']:
            print(f"  {j}: θ={cmd[j][0]:.3f} rad, ω={cmd[j][1]:.3f} rad/s")

运行与验证

依赖：仅需 Python 3 + NumPy。

运行：

python phase_switch_control.py

通过条件：程序输出切换点最大位置误差与速度误差均小于 1e-4（理论值为0），并打印“连续性检查通过”。示例输出展示不同相位下的关节指令，用于人工观察插值效果。

关键说明

• 五次插值保证在阶段边界处位置和速度均连续，因为 h(0)=0, h(1)=1, dh(0)=dh(1)=0。
• 角速度计算中，先对归一化时间求导，再除以实际阶段时长，得到物理角速度（rad/s）。
• 实际机器人应用中，将 get_joint_command(phi) 输出的角度与角速度作为底层PD控制器的前馈指令即可。