主题044：双足机器人的步态动力学

1. 引言

双足机器人是人类模仿自身运动而设计的机器人系统，具有在复杂环境中行走、跨越障碍、上下楼梯等能力。与轮式或履带式机器人相比，双足机器人在人类环境中具有更好的适应性和灵活性。然而，双足行走涉及复杂的动力学问题，包括周期性步态生成、稳定性控制、地面接触力处理等。

本教程将介绍双足机器人的步态动力学基础，包括步行周期分析、零力矩点（ZMP）稳定性判据、倒立摆模型、以及典型的步态生成与控制方法。

2. 双足机器人的运动学结构

2.1 人体下肢运动学模型

双足机器人的腿部通常采用仿人设计，主要关节包括：

• 髋关节：3自由度（屈伸、外展内收、内外旋）
• 膝关节：1自由度（屈伸）
• 踝关节：2自由度（屈伸、内外翻）

简化模型通常采用6自由度每条腿（髋关节3DOF + 膝关节1DOF + 踝关节2DOF）。

2.2 正运动学

对于n连杆腿部，末端（足部）位置可通过正向运动学计算：

$${\mathbf{p}}_{foot}=\mathbf{f}(\mathbf{q})$$

其中 $\mathbf{q}=[q_1,q_2,...,q_n{]}^T$ 为关节角度向量。

使用DH参数法或指数积公式可建立完整的正运动学模型。

2.3 雅可比矩阵

足部速度 ${\dot{\mathbf{p}}}_{foot}$ 与关节角速度 $\dot{\mathbf{q}}$ 的关系：

$${\dot{\mathbf{p}}}_{foot}=\mathbf{J}(\mathbf{q})\dot{\mathbf{q}}$$

其中 $\mathbf{J}(\mathbf{q})\in {\mathbb{R}}^{6\times n}$ 为腿部雅可比矩阵。

3. 步态周期分析

3.1 步态周期定义

一个完整的步态周期定义为从一侧足跟着地到同侧足跟再次着地的时间：

$$T_{gait}=T_{stance}+T_{swing}$$

其中：

• $T_{stance}$：支撑相（stance phase），足部与地面接触
• $T_{swing}$：摆动相（swing phase），足部在空中摆动

3.2 双支撑相与单支撑相

• 双支撑相（Double Support）：两脚同时着地，占步态周期的约20%
• 单支撑相（Single Support）：仅一脚着地，另一脚摆动，占约80%

3.3 步态参数

• 步长（Step Length）：$L_{step}$
• 步宽（Step Width）：$W_{step}$
• 步高（Step Height）：$H_{step}$
• 步态周期：$T_{gait}$
• 步行速度：$v=L_{step}/T_{gait}$

4. 稳定性判据

4.1 零力矩点（ZMP）

ZMP是地面反作用力产生的力矩在水平方向分量为零的点。ZMP稳定性判据：

稳定条件：ZMP必须位于支撑多边形（support polygon）内。

支撑多边形是由支撑脚与地面接触点构成的凸包。对于单脚支撑，支撑多边形为足底接触区域；对于双脚支撑，为两脚接触点的凸包。

4.2 ZMP计算

对于质心位置 ${\mathbf{p}}_{com}=[x_{com},y_{com},z_{com}{]}^T$，ZMP坐标为：

$$x_{zmp}=x_{com}-\frac{{\ddot{x}}_{com}{z}_{com}}{g}$$

$$y_{zmp}=y_{com}-\frac{{\ddot{y}}_{com}{z}_{com}}{g}$$

其中 $g$ 为重力加速度。

4.3 捕获点（Capture Point）

捕获点是使机器人在一步内恢复静态平衡的点：

$${\mathbf{r}}_{cp}={\mathbf{r}}_{com}+\frac{{\dot{\mathbf{r}}}_{com}}{\omega }$$

其中 $\omega =\sqrt{g/z_{com}}$ 为自然频率。

5. 简化动力学模型

5.1 线性倒立摆模型（LIPM）

将机器人简化为质点（质心）连接无质量杆到地面接触点：

$$\ddot{x}=\frac{g}{z_c}(x-x_{zmp})$$

其中：

• $x$：质心水平位置
• $z_c$：质心恒定高度
• $x_{zmp}$：ZMP位置

5.2 LIPM的解析解

LIPM方程的解析解：

$$x(t)=(x_0-x_{zmp})\cosh (\omega t)+\frac{{\dot{x}}_0}{\omega }\sinh (\omega t)+x_{zmp}$$

$$\dot{x}(t)=(x_0-x_{zmp})\omega \sinh (\omega t)+{\dot{x}}_0\cosh (\omega t)$$

其中 $\omega =\sqrt{g/z_c}$。

5.3 轨道能量

LIPM的轨道能量（Orbital Energy）：

$$E=\frac{1}{2}{\dot{x}}^2-\frac{1}{2}{\omega }^2(x-x_{zmp}{)}^2$$

轨道能量守恒，可用于分析步态稳定性。

6. 步态生成方法

6.1 基于ZMP的步态规划

ZMP预览控制：

目标：设计质心轨迹使ZMP跟踪参考轨迹。

优化问题：

$$\underset{\ddot{x}}{\min }\sum _{i=1}^N(x_{zmp,i}-x_{zmp,i}^{ref}{)}^2+\alpha {\ddot{x}}_i^2$$

约束：ZMP动力学方程

6.2 混合零动态（HZD）

基于零动态的步态生成方法：

1. 定义输出函数（虚拟约束）：
   $$\mathbf{y}=\mathbf{h}(\mathbf{q})={\mathbf{q}}_{actuated}-{\mathbf{h}}_d(\theta )$$

2. 设计期望轨迹 ${\mathbf{h}}_d(\theta )$ 使输出收敛到零

3. 通过李导数计算控制律

6.3 被动行走

利用重力势能转化为动能实现自然步态：

• 下坡行走：利用重力补偿能量损失
• 主动控制：在平地上通过关节驱动补偿能量

7. 控制策略

7.1 分层控制架构

高层规划 → 步态生成 → 关节控制

• 高层规划：路径规划、步态选择
• 中层控制：ZMP跟踪、质心控制
• 底层控制：关节力矩/位置控制

7.2 基于ZMP的控制

ZMP跟踪控制：

$$\tau ={\mathbf{J}}^T{\mathbf{K}}_p({\mathbf{p}}_{zmp}^{ref}-{\mathbf{p}}_{zmp})+{\mathbf{K}}_d\dot{\mathbf{e}}$$

7.3 模型预测控制（MPC）

预测未来N步的质心运动，优化控制输入：

$$\underset{\mathbf{u}}{\min }\sum _{k=0}^{N-1}\Vert {\mathbf{x}}_{k+1}-{\mathbf{x}}^{ref}{\Vert }_Q^2+\Vert {\mathbf{u}}_k{\Vert }_R^2$$

约束：

• 动力学约束
• ZMP约束（在支撑多边形内）
• 关节限位

8. 地面接触与冲击

8.1 地面接触模型

弹簧-阻尼模型：

$$F_n=k_c\delta +d_c\dot{\delta }$$

其中：

• $\delta$：地面穿透深度
• $k_c$：接触刚度
• $d_c$：阻尼系数

8.2 冲击动力学

足部落地的瞬时冲击：

$$\mathbf{M}(\mathbf{q})({\dot{\mathbf{q}}}^{+}-{\dot{\mathbf{q}}}^{-})={\mathbf{J}}^T{\mathbf{F}}_{imp}$$

约束（非反弹）：

$$\mathbf{J}{\dot{\mathbf{q}}}^{+}=0$$

8.3 碰撞后的速度更新

使用拉格朗日乘子法求解碰撞后速度：

$$\left[\begin{array}{cc}\mathbf{M}& {\mathbf{J}}^T\\ \mathbf{J}& 0\end{array}\right]\left[\begin{array}{c}{\dot{\mathbf{q}}}^{+}\\ \lambda \end{array}\right]=\left[\begin{array}{c}\mathbf{M}{\dot{\mathbf{q}}}^{-}\\ 0\end{array}\right]$$

9. Python仿真实现

9.1 双足机器人模型

import numpy as np
from scipy.integrate import solve_ivp
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle, Polygon
from matplotlib.animation import FuncAnimation

class BipedRobot:
    """简化双足机器人模型（平面5连杆）"""
    
    def __init__(self):
        # 物理参数
        self.m_torso = 20.0      # 躯干质量 (kg)
        self.m_thigh = 5.0       # 大腿质量 (kg)
        self.m_shank = 3.0       # 小腿质量 (kg)
        
        self.L_torso = 0.5       # 躯干长度 (m)
        self.L_thigh = 0.4       # 大腿长度 (m)
        self.L_shank = 0.4       # 小腿长度 (m)
        
        self.g = 9.81            # 重力加速度
        
        # 状态: [x_hip, y_hip, theta_torso, theta_l_thigh, theta_l_shank, 
        #        theta_r_thigh, theta_r_shank, 各角速度]
        self.state = np.zeros(12)
        
    def forward_kinematics(self, state=None):
        """计算正运动学"""
        if state is None:
            state = self.state
            
        x_h, y_h, theta_t, theta_lt, theta_ls, theta_rt, theta_rs = state[:7]
        
        # 躯干顶端
        x_t = x_h + self.L_torso * np.sin(theta_t)
        y_t = y_h + self.L_torso * np.cos(theta_t)
        
        # 左腿
        x_l_knee = x_h + self.L_thigh * np.sin(theta_lt)
        y_l_knee = y_h - self.L_thigh * np.cos(theta_lt)
        x_l_foot = x_l_knee + self.L_shank * np.sin(theta_ls)
        y_l_foot = y_l_knee - self.L_shank * np.cos(theta_ls)
        
        # 右腿
        x_r_knee = x_h + self.L_thigh * np.sin(theta_rt)
        y_r_knee = y_h - self.L_thigh * np.cos(theta_rt)
        x_r_foot = x_r_knee + self.L_shank * np.sin(theta_rs)
        y_r_foot = y_r_knee - self.L_shank * np.cos(theta_rs)
        
        return {
            'hip': np.array([x_h, y_h]),
            'torso_top': np.array([x_t, y_t]),
            'left_knee': np.array([x_l_knee, y_l_knee]),
            'left_foot': np.array([x_l_foot, y_l_foot]),
            'right_knee': np.array([x_r_knee, y_r_knee]),
            'right_foot': np.array([x_r_foot, y_r_foot])
        }
    
    def compute_com(self, state=None):
        """计算质心位置"""
        if state is None:
            state = self.state
            
        fk = self.forward_kinematics(state)
        
        # 各连杆质心（简化假设）
        com_torso = 0.5 * (fk['hip'] + fk['torso_top'])
        com_l_thigh = 0.5 * (fk['hip'] + fk['left_knee'])
        com_l_shank = 0.5 * (fk['left_knee'] + fk['left_foot'])
        com_r_thigh = 0.5 * (fk['hip'] + fk['right_knee'])
        com_r_shank = 0.5 * (fk['right_knee'] + fk['right_foot'])
        
        # 整体质心
        total_mass = self.m_torso + 2*self.m_thigh + 2*self.m_shank
        
        com = (self.m_torso * com_torso + 
               self.m_thigh * com_l_thigh + self.m_shank * com_l_shank +
               self.m_thigh * com_r_thigh + self.m_shank * com_r_shank) / total_mass
               
        return com
    
    def compute_zmp(self, state=None):
        """计算ZMP位置（简化模型）"""
        if state is None:
            state = self.state
            
        com = self.compute_com(state)
        dcom = np.zeros(2)  # 简化：假设已知质心加速度
        
        z_c = com[1]  # 质心高度
        
        # ZMP计算
        x_zmp = com[0] - dcom[0] * z_c / self.g
        
        return x_zmp

9.2 线性倒立摆仿真

class LinearInvertedPendulum:
    """线性倒立摆模型"""
    
    def __init__(self, zc=0.8, g=9.81):
        self.zc = zc  # 质心高度
        self.g = g    # 重力加速度
        self.omega = np.sqrt(g / zc)  # 自然频率
        
    def dynamics(self, t, state, x_zmp):
        """LIPM动力学"""
        x, dx = state
        ddx = self.omega**2 * (x - x_zmp)
        return [dx, ddx]
    
    def analytical_solution(self, x0, dx0, x_zmp, t):
        """解析解"""
        cosh_wt = np.cosh(self.omega * t)
        sinh_wt = np.sinh(self.omega * t)
        
        x_t = (x0 - x_zmp) * cosh_wt + dx0 / self.omega * sinh_wt + x_zmp
        dx_t = (x0 - x_zmp) * self.omega * sinh_wt + dx0 * cosh_wt
        
        return x_t, dx_t
    
    def compute_orbital_energy(self, x, dx, x_zmp):
        """计算轨道能量"""
        return 0.5 * dx**2 - 0.5 * self.omega**2 * (x - x_zmp)**2

9.3 步态生成器

class GaitGenerator:
    """步态生成器"""
    
    def __init__(self, T_gait=1.0, step_length=0.3, step_height=0.05):
        self.T_gait = T_gait
        self.step_length = step_length
        self.step_height = step_height
        
        self.T_ds = 0.2 * T_gait  # 双支撑相
        self.T_ss = 0.8 * T_gait  # 单支撑相
        
    def generate_foot_trajectory(self, t, support_leg='left'):
        """生成足部轨迹"""
        t_cycle = t % self.T_gait
        
        if support_leg == 'left':
            # 左脚支撑，右脚摆动
            if t_cycle < self.T_ds:
                # 双支撑相
                x_foot = 0
                y_foot = 0
            elif t_cycle < self.T_gait:
                # 单支撑相（右脚摆动）
                t_swing = t_cycle - self.T_ds
                T_swing = self.T_ss
                
                # 使用多项式插值
                s = t_swing / T_swing
                x_foot = self.step_length * (3*s**2 - 2*s**3)
                y_foot = self.step_height * np.sin(np.pi * s)
            else:
                x_foot = 0
                y_foot = 0
        else:
            # 右脚支撑，左脚摆动
            if t_cycle < self.T_ds:
                x_foot = self.step_length
                y_foot = 0
            elif t_cycle < self.T_gait:
                t_swing = t_cycle - self.T_ds
                T_swing = self.T_ss
                s = t_swing / T_swing
                x_foot = self.step_length * (1 - 3*s**2 + 2*s**3)
                y_foot = self.step_height * np.sin(np.pi * s)
            else:
                x_foot = self.step_length
                y_foot = 0
                
        return x_foot, y_foot
    
    def generate_zmp_trajectory(self, t):
        """生成ZMP参考轨迹"""
        t_cycle = t % self.T_gait
        
        if t_cycle < self.T_ds:
            # 双支撑相：ZMP从一只脚移到另一只脚
            alpha = t_cycle / self.T_ds
            x_zmp = self.step_length * alpha
        else:
            # 单支撑相：ZMP在支撑脚中心
            x_zmp = self.step_length
            
        return x_zmp

10. 仿真结果与分析

10.1 步态周期仿真

仿真结果展示了双足机器人的典型步态周期：

• 双支撑相：体重从一只脚转移到另一只脚
• 单支撑相：支撑腿保持直立，摆动腿向前摆动
• 足部落地点：精确控制实现稳定步行

10.2 ZMP轨迹分析

• ZMP始终保持在支撑多边形内
• 双支撑相ZMP平滑过渡
• 单支撑相ZMP稳定在支撑脚中心附近

10.3 稳定性评估

• 使用ZMP判据验证步态稳定性
• 轨道能量分析显示能量守恒特性
• 捕获点位置指示机器人平衡状态

11. 总结

本教程介绍了双足机器人步态动力学的核心概念：

1. 运动学建模：建立了双足机器人的正运动学和雅可比矩阵
2. 步态周期：分析了步态周期的各个阶段及其时间比例
3. 稳定性判据：详细介绍了ZMP稳定性判据及其计算方法
4. 简化模型：使用LIPM简化复杂动力学，便于分析和控制
5. 步态生成：介绍了基于ZMP和HZD的步态生成方法
6. 控制策略：讨论了分层控制和MPC等先进控制方法

双足机器人的步态动力学是一个活跃的研究领域，涉及生物力学、控制理论和机器学习等多个学科。未来的发展方向包括：

• 基于学习的步态生成
• 复杂地形适应性步行
• 动态跑步和跳跃
• 人机协作的安全性

参考文献

1. Vukobratović, M., & Borovac, B. (2004). Zero-moment point—thirty five years of its life. International Journal of Humanoid Robotics.
2. Kajita, S., et al. (2003). Biped walking pattern generation by using preview control of zero-moment point. ICRA.
3. Westervelt, E. R., et al. (2007). Feedback control of dynamic bipedal robot locomotion. CRC Press.
4. Grizzle, J. W., et al. (2014). Models, feedback control, and open problems of 3D bipedal robotic walking. Automatica.
5. Wieber, P. B. (2006). Trajectory free linear model predictive control for stable walking in the presence of strong perturbations. Humanoids.

"""
主题044：双足机器人的步态动力学 - 仿真代码
"""

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle, FancyBboxPatch, Arc
from matplotlib.animation import FuncAnimation
import matplotlib.patches as mpatches

# 设置中文字体
plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False


class LinearInvertedPendulum:
    """线性倒立摆模型"""
    
    def __init__(self, zc=0.8, g=9.81):
        self.zc = zc  # 质心高度
        self.g = g    # 重力加速度
        self.omega = np.sqrt(g / zc)  # 自然频率
        
    def dynamics(self, state, x_zmp):
        """LIPM动力学"""
        x, dx = state
        ddx = self.omega**2 * (x - x_zmp)
        return np.array([dx, ddx])
    
    def analytical_solution(self, x0, dx0, x_zmp, t):
        """解析解"""
        cosh_wt = np.cosh(self.omega * t)
        sinh_wt = np.sinh(self.omega * t)
        
        x_t = (x0 - x_zmp) * cosh_wt + dx0 / self.omega * sinh_wt + x_zmp
        dx_t = (x0 - x_zmp) * self.omega * sinh_wt + dx0 * cosh_wt
        
        return x_t, dx_t
    
    def compute_orbital_energy(self, x, dx, x_zmp):
        """计算轨道能量"""
        return 0.5 * dx**2 - 0.5 * self.omega**2 * (x - x_zmp)**2


class GaitGenerator:
    """步态生成器"""
    
    def __init__(self, T_gait=1.0, step_length=0.3, step_height=0.05, step_width=0.15):
        self.T_gait = T_gait
        self.step_length = step_length
        self.step_height = step_height
        self.step_width = step_width
        
        self.T_ds = 0.2 * T_gait  # 双支撑相
        self.T_ss = 0.8 * T_gait  # 单支撑相
        
    def generate_foot_trajectory(self, t, support_leg='left'):
        """生成足部轨迹"""
        t_cycle = t % self.T_gait
        
        if support_leg == 'left':
            # 左脚支撑，右脚摆动
            y_offset = -self.step_width / 2
            if t_cycle < self.T_ds:
                # 双支撑相
                x_foot = 0
                y_foot = y_offset
                z_foot = 0
            elif t_cycle < self.T_gait:
                # 单支撑相（右脚摆动）
                t_swing = t_cycle - self.T_ds
                T_swing = self.T_ss
                
                # 使用多项式插值
                s = t_swing / T_swing
                x_foot = self.step_length * (3*s**2 - 2*s**3)
                y_foot = y_offset + self.step_width * (3*s**2 - 2*s**3)
                z_foot = self.step_height * np.sin(np.pi * s)
            else:
                x_foot = 0
                y_foot = y_offset
                z_foot = 0
        else:
            # 右脚支撑，左脚摆动
            y_offset = self.step_width / 2
            if t_cycle < self.T_ds:
                # 双支撑相
                x_foot = self.step_length
                y_foot = y_offset
                z_foot = 0
            elif t_cycle < self.T_gait:
                t_swing = t_cycle - self.T_ds
                T_swing = self.T_ss
                s = t_swing / T_swing
                x_foot = self.step_length * (1 - 3*s**2 + 2*s**3)
                y_foot = y_offset - self.step_width * (3*s**2 - 2*s**3)
                z_foot = self.step_height * np.sin(np.pi * s)
            else:
                x_foot = self.step_length
                y_foot = y_offset
                z_foot = 0
                
        return x_foot, y_foot, z_foot
    
    def generate_zmp_trajectory(self, t):
        """生成ZMP参考轨迹"""
        t_cycle = t % self.T_gait
        
        if t_cycle < self.T_ds:
            # 双支撑相：ZMP从一只脚移到另一只脚
            alpha = t_cycle / self.T_ds
            x_zmp = self.step_length * alpha
            y_zmp = 0
        else:
            # 单支撑相：ZMP在支撑脚中心
            x_zmp = self.step_length
            y_zmp = 0
            
        return x_zmp, y_zmp
    
    def generate_com_trajectory(self, t, lipm):
        """生成质心轨迹（基于ZMP预览控制简化版）"""
        # 简化的质心轨迹：正弦波叠加
        x_com = 0.15 + 0.15 * np.sin(2 * np.pi * t / self.T_gait - np.pi/4)
        y_com = 0.03 * np.sin(2 * np.pi * t / self.T_gait)
        
        # 计算对应的ZMP
        ddx_com = -(2 * np.pi / self.T_gait)**2 * 0.15 * np.sin(2 * np.pi * t / self.T_gait - np.pi/4)
        x_zmp = x_com - ddx_com * lipm.zc / lipm.g
        
        return x_com, y_com, x_zmp


def plot_lipm_phase_portrait():
    """绘制LIPM相图"""
    lipm = LinearInvertedPendulum(zc=0.8)
    
    fig, axes = plt.subplots(1, 2, figsize=(14, 6))
    
    # 左图：不同初始条件的相轨迹
    ax1 = axes[0]
    x_zmp = 0  # ZMP位置
    
    # 绘制相轨迹
    for x0 in np.linspace(-0.3, 0.3, 7):
        for dx0 in np.linspace(-0.5, 0.5, 7):
            t = np.linspace(0, 3, 300)
            x_traj = []
            dx_traj = []
            
            x, dx = x0, dx0
            for dt in np.diff(t, prepend=0):
                x_traj.append(x)
                dx_traj.append(dx)
                state = np.array([x, dx])
                dstate = lipm.dynamics(state, x_zmp)
                x += dstate[0] * dt
                dx += dstate[1] * dt
            
            ax1.plot(x_traj, dx_traj, 'b-', alpha=0.3, linewidth=0.5)
    
    # 绘制鞍点
    ax1.plot(x_zmp, 0, 'r*', markersize=15, label='ZMP (鞍点)')
    
    # 绘制稳定和不稳定流形
    x_range = np.linspace(-0.4, 0.4, 100)
    ax1.plot(x_range, lipm.omega * (x_range - x_zmp), 'r--', linewidth=2, label='不稳定流形')
    ax1.plot(x_range, -lipm.omega * (x_range - x_zmp), 'g--', linewidth=2, label='稳定流形')
    
    ax1.set_xlabel('位置 x (m)', fontsize=12)
    ax1.set_ylabel('速度 dx/dt (m/s)', fontsize=12)
    ax1.set_title('线性倒立摆相图', fontsize=14)
    ax1.legend(loc='upper right')
    ax1.grid(True, alpha=0.3)
    ax1.set_xlim(-0.4, 0.4)
    ax1.set_ylim(-0.6, 0.6)
    
    # 右图：轨道能量等高线
    ax2 = axes[1]
    x_grid = np.linspace(-0.4, 0.4, 100)
    dx_grid = np.linspace(-0.6, 0.6, 100)
    X, DX = np.meshgrid(x_grid, dx_grid)
    
    E = lipm.compute_orbital_energy(X, DX, x_zmp)
    
    contour = ax2.contour(X, DX, E, levels=20, cmap='viridis')
    plt.colorbar(contour, ax=ax2, label='轨道能量 E')
    ax2.plot(x_zmp, 0, 'r*', markersize=15, label='ZMP')
    ax2.set_xlabel('位置 x (m)', fontsize=12)
    ax2.set_ylabel('速度 dx/dt (m/s)', fontsize=12)
    ax2.set_title('轨道能量等高线', fontsize=14)
    ax2.legend(loc='upper right')
    ax2.grid(True, alpha=0.3)
    
    plt.tight_layout()
    plt.savefig('D:\\文档\\多体动力学\\主题044\\figure1_lipm_phase.png', dpi=150, bbox_inches='tight')
    plt.close()
    print("图1已保存: LIPM相图")


def plot_gait_cycle():
    """绘制步态周期分析"""
    gait = GaitGenerator(T_gait=1.0, step_length=0.3, step_height=0.05)
    
    fig, axes = plt.subplots(2, 2, figsize=(14, 10))
    
    t = np.linspace(0, 3, 300)  # 3个步态周期
    
    # 左上图：足部轨迹
    ax1 = axes[0, 0]
    
    # 左脚轨迹
    left_foot_x = []
    left_foot_y = []
    left_foot_z = []
    
    # 右脚轨迹
    right_foot_x = []
    right_foot_y = []
    right_foot_z = []
    
    step_count = 0
    for ti in t:
        t_cycle = ti % gait.T_gait
        
        # 左脚（奇数周期摆动，偶数周期支撑）
        if step_count % 2 == 0:
            x_l, y_l, z_l = gait.generate_foot_trajectory(ti, 'left')
        else:
            x_l, y_l, z_l = gait.generate_foot_trajectory(ti, 'right')
            x_l += step_count * gait.step_length
        
        # 右脚（偶数周期摆动，奇数周期支撑）
        if step_count % 2 == 0:
            x_r, y_r, z_r = gait.generate_foot_trajectory(ti, 'right')
            x_r += step_count * gait.step_length
        else:
            x_r, y_r, z_r = gait.generate_foot_trajectory(ti, 'left')
            x_r += step_count * gait.step_length
        
        left_foot_x.append(x_l + step_count * gait.step_length)
        left_foot_y.append(y_l)
        left_foot_z.append(z_l)
        
        right_foot_x.append(x_r)
        right_foot_y.append(y_r)
        right_foot_z.append(z_r)
        
        if t_cycle >= gait.T_gait - 0.01:
            step_count += 1
    
    ax1.plot(t, left_foot_z, 'b-', linewidth=2, label='左脚高度')
    ax1.plot(t, right_foot_z, 'r-', linewidth=2, label='右脚高度')
    
    # 标记双支撑相
    for i in range(3):
        ax1.axvspan(i * gait.T_gait, i * gait.T_gait + gait.T_ds, alpha=0.2, color='green', label='双支撑相' if i == 0 else '')
    
    ax1.set_xlabel('时间 (s)', fontsize=12)
    ax1.set_ylabel('足部高度 (m)', fontsize=12)
    ax1.set_title('步态周期：足部垂直轨迹', fontsize=14)
    ax1.legend(loc='upper right')
    ax1.grid(True, alpha=0.3)
    ax1.set_ylim(-0.01, 0.07)
    
    # 右上图：水平轨迹
    ax2 = axes[0, 1]
    ax2.plot(t, left_foot_x, 'b-', linewidth=2, label='左脚X位置')
    ax2.plot(t, right_foot_x, 'r-', linewidth=2, label='右脚X位置')
    
    for i in range(3):
        ax2.axvspan(i * gait.T_gait, i * gait.T_gait + gait.T_ds, alpha=0.2, color='green')
    
    ax2.set_xlabel('时间 (s)', fontsize=12)
    ax2.set_ylabel('水平位置 (m)', fontsize=12)
    ax2.set_title('步态周期：足部水平轨迹', fontsize=14)
    ax2.legend(loc='upper left')
    ax2.grid(True, alpha=0.3)
    
    # 左下图：3D足部轨迹
    ax3 = axes[1, 0]
    
    # 绘制一个周期的3D轨迹
    t_one = np.linspace(0, gait.T_gait, 100)
    left_x, left_y, left_z = [], [], []
    right_x, right_y, right_z = [], [], []
    
    for ti in t_one:
        x_l, y_l, z_l = gait.generate_foot_trajectory(ti, 'left')
        x_r, y_r, z_r = gait.generate_foot_trajectory(ti, 'right')
        left_x.append(x_l)
        left_y.append(y_l)
        left_z.append(z_l)
        right_x.append(x_r)
        right_y.append(y_r)
        right_z.append(z_r)
    
    ax3.plot(left_x, left_z, 'b-', linewidth=2, label='左脚')
    ax3.plot(right_x, right_z, 'r-', linewidth=2, label='右脚')
    ax3.scatter([left_x[0]], [left_z[0]], c='blue', s=100, marker='o', label='起始')
    ax3.scatter([right_x[0]], [right_z[0]], c='red', s=100, marker='o')
    
    ax3.set_xlabel('水平位置 (m)', fontsize=12)
    ax3.set_ylabel('高度 (m)', fontsize=12)
    ax3.set_title('单周期足部轨迹（侧视图）', fontsize=14)
    ax3.legend(loc='upper right')
    ax3.grid(True, alpha=0.3)
    ax3.set_aspect('equal')
    
    # 右下图：步态时序图
    ax4 = axes[1, 1]
    
    # 创建步态时序
    phases = ['双支撑', '右单支撑', '双支撑', '左单支撑']
    colors = ['green', 'red', 'green', 'blue']
    
    for i, (phase, color) in enumerate(zip(phases, colors)):
        if i % 2 == 0:
            ax4.barh(0, gait.T_ds, left=i * gait.T_gait / 2, color=color, alpha=0.5, height=0.3)
        else:
            ax4.barh(0, gait.T_ss, left=(i-1) * gait.T_gait / 2 + gait.T_ds, color=color, alpha=0.5, height=0.3)
    
    ax4.set_xlabel('时间 (s)', fontsize=12)
    ax4.set_title('步态时序图', fontsize=14)
    ax4.set_yticks([])
    ax4.set_xlim(0, 2 * gait.T_gait)
    
    # 添加图例
    legend_elements = [
        mpatches.Patch(color='green', alpha=0.5, label='双支撑相'),
        mpatches.Patch(color='red', alpha=0.5, label='右脚支撑'),
        mpatches.Patch(color='blue', alpha=0.5, label='左脚支撑')
    ]
    ax4.legend(handles=legend_elements, loc='upper right')
    ax4.grid(True, alpha=0.3, axis='x')
    
    plt.tight_layout()
    plt.savefig('D:\\文档\\多体动力学\\主题044\\figure2_gait_cycle.png', dpi=150, bbox_inches='tight')
    plt.close()
    print("图2已保存: 步态周期分析")


def plot_zmp_stability():
    """绘制ZMP稳定性分析"""
    lipm = LinearInvertedPendulum(zc=0.8)
    gait = GaitGenerator(T_gait=1.0, step_length=0.3, step_height=0.05)
    
    fig, axes = plt.subplots(2, 2, figsize=(14, 10))
    
    t = np.linspace(0, 3, 300)
    
    # 生成质心和ZMP轨迹
    com_x = []
    com_y = []
    zmp_x = []
    zmp_y = []
    
    for ti in t:
        x_c, y_c, x_z = gait.generate_com_trajectory(ti, lipm)
        com_x.append(x_c)
        com_y.append(y_c)
        zmp_x.append(x_z)
        zmp_y.append(0)
    
    # 左上图：X方向轨迹
    ax1 = axes[0, 0]
    ax1.plot(t, com_x, 'b-', linewidth=2, label='质心 X')
    ax1.plot(t, zmp_x, 'r--', linewidth=2, label='ZMP X')
    
    # 绘制支撑多边形边界
    foot_length = 0.15
    for i in range(3):
        ax1.axhspan(i * gait.step_length - foot_length/2, i * gait.step_length + foot_length/2, 
                    xmin=i/3, xmax=(i+0.2)/3, alpha=0.2, color='green')
    
    ax1.set_xlabel('时间 (s)', fontsize=12)
    ax1.set_ylabel('位置 (m)', fontsize=12)
    ax1.set_title('质心与ZMP轨迹（X方向）', fontsize=14)
    ax1.legend(loc='upper right')
    ax1.grid(True, alpha=0.3)
    
    # 右上图：Y方向轨迹
    ax2 = axes[0, 1]
    ax2.plot(t, com_y, 'b-', linewidth=2, label='质心 Y')
    ax2.plot(t, zmp_y, 'r--', linewidth=2, label='ZMP Y')
    ax2.set_xlabel('时间 (s)', fontsize=12)
    ax2.set_ylabel('位置 (m)', fontsize=12)
    ax2.set_title('质心与ZMP轨迹（Y方向）', fontsize=14)
    ax2.legend(loc='upper right')
    ax2.grid(True, alpha=0.3)
    
    # 左下图：ZMP在支撑多边形内的位置
    ax3 = axes[1, 0]
    
    # 绘制支撑多边形（简化为矩形）
    foot_width = 0.08
    foot_length = 0.15
    
    # 绘制几个支撑多边形
    for i in range(3):
        rect = Rectangle((i * gait.step_length - foot_length/2, -foot_width/2), 
                         foot_length, foot_width, 
                         fill=True, facecolor='lightgreen', alpha=0.3, edgecolor='green', linewidth=2)
        ax3.add_patch(rect)
    
    # 绘制ZMP轨迹
    ax3.plot(zmp_x, zmp_y, 'r-', linewidth=2, label='ZMP轨迹')
    ax3.scatter(zmp_x[::30], zmp_y[::30], c='red', s=30, alpha=0.5)
    
    ax3.set_xlabel('X位置 (m)', fontsize=12)
    ax3.set_ylabel('Y位置 (m)', fontsize=12)
    ax3.set_title('ZMP在支撑多边形内的轨迹', fontsize=14)
    ax3.legend(loc='upper right')
    ax3.grid(True, alpha=0.3)
    ax3.set_aspect('equal')
    ax3.set_xlim(-0.2, 0.8)
    ax3.set_ylim(-0.1, 0.1)
    
    # 右下图：ZMP偏移量（稳定性指标）
    ax4 = axes[1, 1]
    
    # 计算ZMP到支撑多边形边界的距离
    zmp_margin = []
    for x_z in zmp_x:
        # 简化的边界检查
        foot_center = round(x_z / gait.step_length) * gait.step_length
        margin = foot_length/2 - abs(x_z - foot_center)
        zmp_margin.append(margin)
    
    ax4.plot(t, zmp_margin, 'b-', linewidth=2, label='ZMP边界裕度')
    ax4.axhline(y=0, color='r', linestyle='--', linewidth=2, label='稳定边界')
    ax4.fill_between(t, 0, zmp_margin, where=np.array(zmp_margin) > 0, 
                     alpha=0.3, color='green', label='稳定区域')
    
    ax4.set_xlabel('时间 (s)', fontsize=12)
    ax4.set_ylabel('边界裕度 (m)', fontsize=12)
    ax4.set_title('ZMP稳定性裕度', fontsize=14)
    ax4.legend(loc='upper right')
    ax4.grid(True, alpha=0.3)
    
    plt.tight_layout()
    plt.savefig('D:\\文档\\多体动力学\\主题044\\figure3_zmp_stability.png', dpi=150, bbox_inches='tight')
    plt.close()
    print("图3已保存: ZMP稳定性分析")


def plot_biped_walking():
    """绘制双足机器人步行仿真"""
    gait = GaitGenerator(T_gait=1.0, step_length=0.3, step_height=0.05)
    lipm = LinearInvertedPendulum(zc=0.9)
    
    fig, axes = plt.subplots(2, 2, figsize=(14, 12))
    
    # 物理参数
    L_torso = 0.5
    L_thigh = 0.4
    L_shank = 0.4
    
    # 左上图：步行序列快照
    ax1 = axes[0, 0]
    
    def draw_robot(ax, t, x_offset=0):
        """绘制机器人简图"""
        # 获取质心位置
        x_com, y_com, _ = gait.generate_com_trajectory(t, lipm)
        x_com += x_offset
        
        # 髋关节位置（简化：在质心下方）
        x_hip = x_com
        y_hip = y_com - 0.1
        
        # 获取足部位置
        t_cycle = t % gait.T_gait
        
        if t_cycle < gait.T_gait / 2:
            # 左脚支撑
            x_l_foot = x_offset + int(t / gait.T_gait) * gait.step_length
            y_l_foot = 0
            x_r_foot, y_r_foot, z_r_foot = gait.generate_foot_trajectory(t, 'right')
            x_r_foot += x_offset + int(t / gait.T_gait) * gait.step_length
            y_r_foot = z_r_foot
        else:
            # 右脚支撑
            x_r_foot = x_offset + (int(t / gait.T_gait) + 0.5) * gait.step_length
            y_r_foot = 0
            x_l_foot, y_l_foot, z_l_foot = gait.generate_foot_trajectory(t, 'left')
            x_l_foot += x_offset + int(t / gait.T_gait) * gait.step_length
            y_l_foot = z_l_foot
        
        # 绘制躯干
        torso_top = (x_hip, y_hip + L_torso)
        ax.plot([x_hip, torso_top[0]], [y_hip, torso_top[1]], 'k-', linewidth=4)
        
        # 绘制头部（圆形）
        head = plt.Circle((torso_top[0], torso_top[1] + 0.08), 0.06, fill=True, color='lightblue', ec='black')
        ax.add_patch(head)
        
        # 绘制左腿
        # 膝关节位置（简化计算）
        knee_l = (x_hip + 0.5 * (x_l_foot - x_hip), y_hip - 0.5 * L_thigh)
        ax.plot([x_hip, knee_l[0]], [y_hip, knee_l[1]], 'b-', linewidth=3)
        ax.plot([knee_l[0], x_l_foot], [knee_l[1], y_l_foot], 'b-', linewidth=3)
        
        # 绘制右腿
        knee_r = (x_hip + 0.5 * (x_r_foot - x_hip), y_hip - 0.5 * L_thigh)
        ax.plot([x_hip, knee_r[0]], [y_hip, knee_r[1]], 'r-', linewidth=3)
        ax.plot([knee_r[0], x_r_foot], [knee_r[1], y_r_foot], 'r-', linewidth=3)
        
        # 绘制足部
        foot_width = 0.06
        foot_length = 0.12
        
        # 左脚
        rect_l = Rectangle((x_l_foot - foot_length/2, y_l_foot), foot_length, foot_width, 
                          fill=True, facecolor='blue', alpha=0.7)
        ax.add_patch(rect_l)
        
        # 右脚
        rect_r = Rectangle((x_r_foot - foot_length/2, y_r_foot), foot_length, foot_width, 
                          fill=True, facecolor='red', alpha=0.7)
        ax.add_patch(rect_r)
        
        # 绘制髋关节
        ax.plot(x_hip, y_hip, 'ko', markersize=10)
        
        # 绘制膝关节
        ax.plot(knee_l[0], knee_l[1], 'bo', markersize=6)
        ax.plot(knee_r[0], knee_r[1], 'ro', markersize=6)
    
    # 绘制多个时间点的机器人
    times = [0, 0.25, 0.5, 0.75]
    x_offsets = [0, 0.4, 0.8, 1.2]
    
    for t, x_off in zip(times, x_offsets):
        draw_robot(ax1, t, x_off)
    
    ax1.set_xlim(-0.3, 2.0)
    ax1.set_ylim(-0.1, 1.2)
    ax1.set_aspect('equal')
    ax1.set_xlabel('水平位置 (m)', fontsize=12)
    ax1.set_ylabel('高度 (m)', fontsize=12)
    ax1.set_title('双足机器人步行序列', fontsize=14)
    ax1.grid(True, alpha=0.3)
    
    # 添加图例
    legend_elements = [
        plt.Line2D([0], [0], color='blue', linewidth=3, label='左腿'),
        plt.Line2D([0], [0], color='red', linewidth=3, label='右腿'),
        plt.Line2D([0], [0], color='black', linewidth=4, label='躯干')
    ]
    ax1.legend(handles=legend_elements, loc='upper right')
    
    # 右上图：关节角度轨迹
    ax2 = axes[0, 1]
    
    t = np.linspace(0, 2, 200)
    
    # 简化的关节角度计算
    theta_hip_l = []
    theta_knee_l = []
    theta_hip_r = []
    theta_knee_r = []
    
    for ti in t:
        t_cycle = ti % gait.T_gait
        
        # 简化的关节角度（基于正弦函数）
        if t_cycle < gait.T_gait / 2:
            # 左脚支撑
            theta_hip_l.append(0)
            theta_knee_l.append(0)
            theta_hip_r.append(0.3 * np.sin(2 * np.pi * t_cycle / gait.T_gait))
            theta_knee_r.append(0.5 * np.sin(2 * np.pi * t_cycle / gait.T_gait))
        else:
            # 右脚支撑
            theta_hip_l.append(0.3 * np.sin(2 * np.pi * t_cycle / gait.T_gait))
            theta_knee_l.append(0.5 * np.sin(2 * np.pi * t_cycle / gait.T_gait))
            theta_hip_r.append(0)
            theta_knee_r.append(0)
    
    ax2.plot(t, np.degrees(theta_hip_l), 'b-', linewidth=2, label='左髋关节')
    ax2.plot(t, np.degrees(theta_knee_l), 'b--', linewidth=2, label='左膝关节')
    ax2.plot(t, np.degrees(theta_hip_r), 'r-', linewidth=2, label='右髋关节')
    ax2.plot(t, np.degrees(theta_knee_r), 'r--', linewidth=2, label='右膝关节')
    
    ax2.set_xlabel('时间 (s)', fontsize=12)
    ax2.set_ylabel('关节角度 (度)', fontsize=12)
    ax2.set_title('关节角度轨迹', fontsize=14)
    ax2.legend(loc='upper right')
    ax2.grid(True, alpha=0.3)
    
    # 左下图：质心高度和速度
    ax3 = axes[1, 0]
    
    com_x = []
    com_y = []
    com_vx = []
    
    for ti in t:
        x_c, y_c, _ = gait.generate_com_trajectory(ti, lipm)
        com_x.append(x_c)
        com_y.append(y_c)
    
    # 计算速度
    com_vx = np.gradient(com_x, t)
    
    ax3_twin = ax3.twinx()
    
    line1 = ax3.plot(t, com_y, 'b-', linewidth=2, label='质心高度')
    line2 = ax3_twin.plot(t, com_vx, 'r--', linewidth=2, label='质心水平速度')
    
    ax3.set_xlabel('时间 (s)', fontsize=12)
    ax3.set_ylabel('质心高度 (m)', fontsize=12, color='blue')
    ax3_twin.set_ylabel('水平速度 (m/s)', fontsize=12, color='red')
    ax3.set_title('质心运动', fontsize=14)
    ax3.grid(True, alpha=0.3)
    
    lines = line1 + line2
    labels = [l.get_label() for l in lines]
    ax3.legend(lines, labels, loc='upper right')
    
    # 右下图：捕获点分析
    ax4 = axes[1, 1]
    
    # 计算捕获点
    capture_x = []
    capture_y = []
    
    for i, ti in enumerate(t):
        x_c = com_x[i]
        y_c = com_y[i]
        vx_c = com_vx[i]
        
        omega = np.sqrt(lipm.g / y_c)
        cp_x = x_c + vx_c / omega
        capture_x.append(cp_x)
    
    ax4.plot(t, com_x, 'b-', linewidth=2, label='质心位置')
    ax4.plot(t, capture_x, 'r--', linewidth=2, label='捕获点')
    
    # 绘制期望落脚点
    step_positions = [0, 0.3, 0.6]
    for pos in step_positions:
        if pos <= max(com_x):
            ax4.axvline(x=pos/0.15, color='green', linestyle=':', alpha=0.5)
            ax4.scatter([pos/0.15], [pos], c='green', s=100, marker='^', alpha=0.5)
    
    ax4.set_xlabel('时间 (s)', fontsize=12)
    ax4.set_ylabel('位置 (m)', fontsize=12)
    ax4.set_title('捕获点分析', fontsize=14)
    ax4.legend(loc='upper left')
    ax4.grid(True, alpha=0.3)
    
    plt.tight_layout()
    plt.savefig('D:\\文档\\多体动力学\\主题044\\figure4_biped_walking.png', dpi=150, bbox_inches='tight')
    plt.close()
    print("图4已保存: 双足机器人步行仿真")


def plot_stability_comparison():
    """绘制不同步态参数对稳定性的影响"""
    fig, axes = plt.subplots(2, 2, figsize=(14, 10))
    
    # 左上图：不同步长对ZMP的影响
    ax1 = axes[0, 0]
    
    step_lengths = [0.2, 0.3, 0.4]
    colors = ['blue', 'green', 'red']
    
    for step_len, color in zip(step_lengths, colors):
        gait = GaitGenerator(T_gait=1.0, step_length=step_len, step_height=0.05)
        lipm = LinearInvertedPendulum(zc=0.8)
        
        t = np.linspace(0, 2, 200)
        zmp_x = []
        
        for ti in t:
            _, _, x_z = gait.generate_com_trajectory(ti, lipm)
            zmp_x.append(x_z)
        
        ax1.plot(t, zmp_x, color=color, linewidth=2, label=f'步长={step_len}m')
    
    ax1.set_xlabel('时间 (s)', fontsize=12)
    ax1.set_ylabel('ZMP位置 (m)', fontsize=12)
    ax1.set_title('不同步长对ZMP轨迹的影响', fontsize=14)
    ax1.legend(loc='upper left')
    ax1.grid(True, alpha=0.3)
    
    # 右上图：不同步态周期对速度的影响
    ax2 = axes[0, 1]
    
    gait_periods = [0.8, 1.0, 1.2]
    
    for T_gait, color in zip(gait_periods, colors):
        gait = GaitGenerator(T_gait=T_gait, step_length=0.3, step_height=0.05)
        lipm = LinearInvertedPendulum(zc=0.8)
        
        t = np.linspace(0, 3, 300)
        com_x = []
        
        for ti in t:
            x_c, _, _ = gait.generate_com_trajectory(ti, lipm)
            com_x.append(x_c)
        
        com_v = np.gradient(com_x, t)
        
        ax2.plot(t, com_v, color=color, linewidth=2, label=f'周期={T_gait}s')
    
    ax2.set_xlabel('时间 (s)', fontsize=12)
    ax2.set_ylabel('质心速度 (m/s)', fontsize=12)
    ax2.set_title('不同步态周期对速度的影响', fontsize=14)
    ax2.legend(loc='upper right')
    ax2.grid(True, alpha=0.3)
    
    # 左下图：不同质心高度的影响
    ax3 = axes[1, 0]
    
    zc_values = [0.6, 0.8, 1.0]
    
    for zc, color in zip(zc_values, colors):
        lipm = LinearInvertedPendulum(zc=zc)
        
        # 初始条件
        x0, dx0 = 0.1, 0.2
        x_zmp = 0
        
        t = np.linspace(0, 2, 200)
        x_traj = []
        
        x, dx = x0, dx0
        for dt in np.diff(t, prepend=0):
            x_traj.append(x)
            state = np.array([x, dx])
            dstate = lipm.dynamics(state, x_zmp)
            x += dstate[0] * dt
            dx += dstate[1] * dt
        
        ax3.plot(t, x_traj, color=color, linewidth=2, label=f'zc={zc}m')
    
    ax3.set_xlabel('时间 (s)', fontsize=12)
    ax3.set_ylabel('质心位置 (m)', fontsize=12)
    ax3.set_title('不同质心高度对稳定性的影响', fontsize=14)
    ax3.legend(loc='upper right')
    ax3.grid(True, alpha=0.3)
    
    # 右下图：步行速度与稳定性裕度
    ax4 = axes[1, 1]
    
    speeds = []
    margins = []
    
    for step_len in np.linspace(0.1, 0.5, 20):
        for T_gait in np.linspace(0.6, 1.5, 20):
            speed = step_len / T_gait
            
            # 简化的稳定性裕度计算
            # 步长越大、周期越短，稳定性裕度越小
            margin = 0.1 - 0.1 * (step_len / 0.5) - 0.05 * (0.6 / T_gait)
            margin = max(margin, 0.01)
            
            speeds.append(speed)
            margins.append(margin)
    
    scatter = ax4.scatter(speeds, margins, c=margins, cmap='RdYlGn', s=50, alpha=0.6)
    plt.colorbar(scatter, ax=ax4, label='稳定性裕度')
    
    ax4.set_xlabel('步行速度 (m/s)', fontsize=12)
    ax4.set_ylabel('稳定性裕度 (m)', fontsize=12)
    ax4.set_title('步行速度与稳定性裕度的关系', fontsize=14)
    ax4.grid(True, alpha=0.3)
    
    plt.tight_layout()
    plt.savefig('D:\\文档\\多体动力学\\主题044\\figure5_stability_comparison.png', dpi=150, bbox_inches='tight')
    plt.close()
    print("图5已保存: 稳定性对比分析")


def main():
    """主函数：运行所有仿真"""
    print("=" * 60)
    print("主题044：双足机器人的步态动力学 - 仿真开始")
    print("=" * 60)
    
    # 创建输出目录
    import os
    output_dir = 'D:\\文档\\多体动力学\\主题044'
    os.makedirs(output_dir, exist_ok=True)
    
    # 运行各个仿真
    print("\n[1/5] 生成LIPM相图...")
    plot_lipm_phase_portrait()
    
    print("\n[2/5] 生成步态周期分析...")
    plot_gait_cycle()
    
    print("\n[3/5] 生成ZMP稳定性分析...")
    plot_zmp_stability()
    
    print("\n[4/5] 生成双足机器人步行仿真...")
    plot_biped_walking()
    
    print("\n[5/5] 生成稳定性对比分析...")
    plot_stability_comparison()
    
    print("\n" + "=" * 60)
    print("所有仿真完成！图片保存在:", output_dir)
    print("=" * 60)


if __name__ == '__main__':
    main()