🚶 第07课：双足步态基础

阶段二：步态控制 步态 相位 Python仿真

📚 课程目标

1. 人类步态分析

人类步行是一个周期性运动，一个完整步态周期（Gait Cycle）包含：

步态周期 (100%)
├── 支撑相 (60%) —— 脚在地面上
│   ├── 双支撑初期 (10%) —— 脚跟触地 → 全脚着地
│   ├── 单支撑 (40%) —— 一脚支撑，另一脚摆动
│   └── 双支撑末期 (10%) —— 全脚 → 脚尖离地
└── 摆动相 (40%) —— 脚在空中
    ├── 摆动初期 —— 脚尖离地 → 膝盖最大弯曲
    ├── 摆动中期 —— 脚从后方摆到前方
    └── 摆动末期 —— 伸膝准备触地

1.1 关键步态参数

2. 步态状态机

"""
双足步态有限状态机
状态：双支撑左→单支撑左→双支撑右→单支撑右→循环
"""
import numpy as np
from enum import Enum, auto
from dataclasses import dataclass
from typing import Dict, Optional

class GaitPhase(Enum):
    """步态相位枚举"""
    DOUBLE_SUPPORT_LEFT = auto()   # 双支撑，左脚在前
    SINGLE_SUPPORT_LEFT = auto()   # 单支撑，左脚
    DOUBLE_SUPPORT_RIGHT = auto()  # 双支撑，右脚在前
    SINGLE_SUPPORT_RIGHT = auto()  # 单支撑，右脚
    STOP = auto()                  # 停止

@dataclass
class GaitParams:
    """步态参数"""
    step_length: float = 0.30      # 步幅 (m)
    step_height: float = 0.05      # 步高 (m)
    step_time: float = 0.80        # 步时 (s)
    double_support_ratio: float = 0.2  # 双支撑占比
    step_width: float = 0.10       # 步宽 (m)

    @property
    def double_support_time(self) -> float:
        return self.step_time * self.double_support_ratio

    @property
    def single_support_time(self) -> float:
        return self.step_time * (1 - self.double_support_ratio)

    @property
    def walking_speed(self) -> float:
        return self.step_length / self.step_time


class BipedalGaitController:
    """双足步态控制器"""

    def __init__(self, params: GaitParams = None):
        self.params = params or GaitParams()
        self.phase = GaitPhase.DOUBLE_SUPPORT_LEFT
        self.phase_time = 0.0  # 当前相位已持续时间
        self.step_count = 0
        self.total_distance = 0.0

    def get_phase_progress(self) -> float:
        """当前相位的进度 (0-1)"""
        if self.phase in [GaitPhase.DOUBLE_SUPPORT_LEFT,
                          GaitPhase.DOUBLE_SUPPORT_RIGHT]:
            duration = self.params.double_support_time
        elif self.phase in [GaitPhase.SINGLE_SUPPORT_LEFT,
                            GaitPhase.SINGLE_SUPPORT_RIGHT]:
            duration = self.params.single_support_time
        else:
            return 0.0

        return min(self.phase_time / duration, 1.0) if duration > 0 else 1.0

    def update(self, dt: float) -> bool:
        """
        更新步态状态机
        返回: 是否发生了相位切换
        """
        self.phase_time += dt
        switched = False

        if self.phase == GaitPhase.DOUBLE_SUPPORT_LEFT:
            if self.phase_time >= self.params.double_support_time:
                self.phase = GaitPhase.SINGLE_SUPPORT_LEFT
                self.phase_time = 0.0
                switched = True

        elif self.phase == GaitPhase.SINGLE_SUPPORT_LEFT:
            if self.phase_time >= self.params.single_support_time:
                self.phase = GaitPhase.DOUBLE_SUPPORT_RIGHT
                self.phase_time = 0.0
                self.step_count += 1
                self.total_distance += self.params.step_length
                switched = True

        elif self.phase == GaitPhase.DOUBLE_SUPPORT_RIGHT:
            if self.phase_time >= self.params.double_support_time:
                self.phase = GaitPhase.SINGLE_SUPPORT_RIGHT
                self.phase_time = 0.0
                switched = True

        elif self.phase == GaitPhase.SINGLE_SUPPORT_RIGHT:
            if self.phase_time >= self.params.single_support_time:
                self.phase = GaitPhase.DOUBLE_SUPPORT_LEFT
                self.phase_time = 0.0
                self.step_count += 1
                self.total_distance += self.params.step_length
                switched = True

        return switched

    def get_foot_positions(self) -> Dict:
        """
        获取当前脚的位置参考
        左/右脚在世界坐标系中的位置
        """
        progress = self.get_phase_progress()
        p = self.params
        L = p.step_length
        H = p.step_height
        W = p.step_width

        if self.phase == GaitPhase.DOUBLE_SUPPORT_LEFT:
            # 双支撑：两脚都在地面，准备右脚摆动
            left_foot = np.array([0, W/2, 0])
            right_foot = np.array([0, -W/2, 0])

        elif self.phase == GaitPhase.SINGLE_SUPPORT_LEFT:
            # 左脚支撑，右脚摆动
            left_foot = np.array([0, W/2, 0])
            # 右脚轨迹：从后方到前方，经过最大高度
            x = -L/2 + L * progress
            z = H * np.sin(np.pi * progress)
            right_foot = np.array([x, -W/2, z])

        elif self.phase == GaitPhase.DOUBLE_SUPPORT_RIGHT:
            # 双支撑：准备左脚摆动
            left_foot = np.array([L/2, W/2, 0])
            right_foot = np.array([L/2, -W/2, 0])

        elif self.phase == GaitPhase.SINGLE_SUPPORT_RIGHT:
            # 右脚支撑，左脚摆动
            right_foot = np.array([L/2, -W/2, 0])
            x = L/2 + L * progress
            z = H * np.sin(np.pi * progress)
            left_foot = np.array([x, W/2, z])

        else:  # STOP
            left_foot = np.array([0, W/2, 0])
            right_foot = np.array([0, -W/2, 0])

        return {
            'left': left_foot,
            'right': right_foot,
            'support_foot': 'left' if self.phase in [
                GaitPhase.SINGLE_SUPPORT_LEFT,
                GaitPhase.DOUBLE_SUPPORT_LEFT
            ] else 'right',
        }

    def get_com_trajectory(self) -> np.ndarray:
        """
        获取CoM参考轨迹
        简化模型：CoM在支撑脚上方左右摆动
        """
        progress = self.get_phase_progress()
        p = self.params
        W = p.step_width

        # CoM横向摆动
        if self.phase in [GaitPhase.SINGLE_SUPPORT_LEFT,
                          GaitPhase.DOUBLE_SUPPORT_LEFT]:
            y_com = (W/2) * (1 - 2 * progress)  # 右→左
        else:
            y_com = -(W/2) * (1 - 2 * progress)  # 左→右

        # CoM纵向匀速前进
        x_com = self.total_distance + p.step_length * progress

        # CoM高度（近似恒定）
        z_com = 0.84

        return np.array([x_com, y_com, z_com])


# === 仿真验证 ===
if __name__ == "__main__":
    params = GaitParams(step_length=0.30, step_height=0.05,
                         step_time=0.80, double_support_ratio=0.2)
    controller = BipedalGaitController(params)

    print("=" * 60)
    print("双足步态仿真")
    print("=" * 60)
    print(f"步态参数:")
    print(f"  步幅: {params.step_length}m")
    print(f"  步高: {params.step_height}m")
    print(f"  步时: {params.step_time}s")
    print(f"  双支撑比: {params.double_support_ratio*100:.0f}%")
    print(f"  行进速度: {params.walking_speed:.2f} m/s")

    # 仿真5步
    dt = 0.01
    n_steps = int(5 * params.step_time / dt)

    phase_log = []
    foot_log = []
    com_log = []

    for i in range(n_steps):
        switched = controller.update(dt)
        phase = controller.phase
        progress = controller.get_phase_progress()
        feet = controller.get_foot_positions()
        com = controller.get_com_trajectory()

        phase_log.append({
            'phase': phase.name,
            'progress': progress,
            'switched': switched,
            'step_count': controller.step_count,
        })
        foot_log.append({
            'left': feet['left'].copy(),
            'right': feet['right'].copy(),
        })
        com_log.append(com.copy())

    # 统计
    phase_counts = {}
    for p in phase_log:
        phase_counts[p['phase']] = phase_counts.get(p['phase'], 0) + 1

    print(f"\n5步仿真结果:")
    print(f"  总步数: {controller.step_count}")
    print(f"  总距离: {controller.total_distance:.2f}m")
    print(f"  相位分布:")
    for phase_name, count in phase_counts.items():
        pct = count / len(phase_log) * 100
        print(f"    {phase_name}: {count}帧 ({pct:.1f}%)")

    # 脚轨迹分析
    left_x = [f['left'][0] for f in foot_log]
    left_z = [f['left'][2] for f in foot_log]
    right_x = [f['right'][0] for f in foot_log]
    right_z = [f['right'][2] for f in foot_log]

    print(f"\n  左脚 x范围: [{min(left_x):.3f}, {max(left_x):.3f}]m")
    print(f"  左脚 z范围: [{min(left_z):.3f}, {max(left_z):.3f}]m (最大离地={max(left_z):.3f}m)")
    print(f"  右脚 x范围: [{min(right_x):.3f}, {max(right_x):.3f}]m")
    print(f"  右脚 z范围: [{min(right_z):.3f}, {max(right_z):.3f}]m (最大离地={max(right_z):.3f}m)")

    # ZMP稳定性分析
    com_arr = np.array(com_log)
    zmp_x = com_arr[:, 0]
    zmp_y = com_arr[:, 1]
    foot_width = params.step_width
    # 简化：ZMP在双脚之间的区域内即稳定
    stable = all(abs(zmp_y[i]) < foot_width for i in range(len(zmp_y)))
    print(f"\n  ZMP横向范围: [{min(zmp_y):.4f}, {max(zmp_y):.4f}]m")
    print(f"  稳定性（ZMP在支撑面内）: {'✅' if stable else '❌'}")

    print("\n✅ 双足步态仿真验证完成！")

✅ 验证通过：步态状态机正确切换，脚轨迹含摆动和支撑，步高0.05m，ZMP始终在支撑面内。

3. 步态参数对稳定性的影响

"""
步态参数敏感性分析
研究步长、步速、步高对ZMP稳定性的影响
"""
import numpy as np

class GaitStabilityAnalyzer:
    """步态稳定性分析器"""

    def __init__(self):
        self.g = 9.81

    def analyze_step_length(self, step_lengths, step_time=0.8):
        """分析步长对稳定性的影响"""
        results = []
        for L in step_lengths:
            # CoM前进速度
            v = L / step_time
            # CoM加速度（假设正弦轨迹）
            a_max = v * 2 * np.pi / step_time  # 最大加速度近似

            # ZMP偏移
            z_c = 0.84  # CoM高度
            zmp_offset = z_c / self.g * a_max

            # 支撑面余量
            foot_length = 0.25
            support_margin = foot_length / 2 - abs(zmp_offset)

            results.append({
                'step_length': L,
                'speed': v,
                'max_acc': a_max,
                'zmp_offset': zmp_offset,
                'margin': support_margin,
                'stable': support_margin > 0,
            })
        return results

    def analyze_step_frequency(self, frequencies, step_length=0.3):
        """分析步频对稳定性的影响"""
        results = []
        for freq in frequencies:
            step_time = 1.0 / freq
            v = step_length / step_time
            a_max = v * 2 * np.pi / step_time

            z_c = 0.84
            zmp_offset = z_c / self.g * a_max
            foot_length = 0.25
            support_margin = foot_length / 2 - abs(zmp_offset)

            results.append({
                'frequency': freq,
                'step_time': step_time,
                'speed': v,
                'zmp_offset': zmp_offset,
                'margin': support_margin,
                'stable': support_margin > 0,
            })
        return results

    def find_max_speed(self, foot_length=0.25, z_c=0.84):
        """找到最大稳定行走速度"""
        margin = foot_length / 2
        # zmp_offset = z_c/g * a_max < margin
        # a_max = v * 2π/T, T = L/v
        # 简化：zmp_offset = z_c/g * 2πv/L * v = 2πz_cv²/(gL)
        # 对于L=0.3: zmp_offset = 2π*0.84*v²/(9.81*0.3)
        # v² < margin * g * L / (2π * z_c)
        v_max = np.sqrt(margin * self.g * 0.3 / (2 * np.pi * z_c))
        return v_max


# === 仿真 ===
if __name__ == "__main__":
    analyzer = GaitStabilityAnalyzer()

    print("=" * 60)
    print("步态参数敏感性分析")
    print("=" * 60)

    # 步长分析
    print("\n--- 步长影响 (步时=0.8s) ---")
    step_lengths = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.8, 1.0]
    results = analyzer.analyze_step_length(step_lengths)
    print(f"{'步长(m)':<10} {'速度(m/s)':<12} {'ZMP偏移(m)':<14} {'余量(m)':<12} {'稳定'}")
    print("-" * 60)
    for r in results:
        print(f"{r['step_length']:<10.1f} {r['speed']:<12.3f} {r['zmp_offset']:<14.4f} "
              f"{r['margin']:<12.4f} {'✅' if r['stable'] else '❌'}")

    # 步频分析
    print("\n--- 步频影响 (步幅=0.3m) ---")
    frequencies = [0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0]
    results = analyzer.analyze_step_frequency(frequencies)
    print(f"{'步频(Hz)':<10} {'步时(s)':<10} {'速度(m/s)':<12} {'ZMP偏移(m)':<14} {'稳定'}")
    print("-" * 60)
    for r in results:
        print(f"{r['frequency']:<10.1f} {r['step_time']:<10.3f} {r['speed']:<12.3f} "
              f"{r['zmp_offset']:<14.4f} {'✅' if r['stable'] else '❌'}")

    # 最大速度
    v_max = analyzer.find_max_speed()
    print(f"\n最大稳定行走速度: {v_max:.3f} m/s")
    print(f"对应步频(步幅0.3m): {v_max/0.3:.2f} Hz")

    print("\n✅ 步态参数分析完成！")

4. 练习题