4. 倒立摆模型简化

任务目标

将双足机器人简化为线性倒立摆模型（Linear Inverted Pendulum Model, LIPM），建立质心（CoM）水平运动与零力矩点（ZMP）之间的线性关系，为后续步态规划提供简化动力学基础。

模型简化假设

• 机器人总质量集中于质心，质心高度保持恒定（z = z_c）；
• 支撑腿视为无质量伸缩杆，仅传递地面反力；
• 忽略踝关节力矩，ZMP位于支撑脚范围内；
• 水平方向运动与垂直方向解耦，仅研究水平面内动力学。

核心方程

在LIPM中，质心水平加速度与ZMP位置满足线性关系：

ddot{x} = (g / z_c) * (x - p_x)

其中 x 为质心水平位置，p_x 为ZMP位置，g 为重力加速度，z_c 为恒定质心高度。该方程为线性时不变二阶系统，可解析求解，是步态规划中轨迹生成的基础。

关键完整代码（Python + NumPy）

import numpy as np

class LIPM:
    """线性倒立摆模型：恒定质心高度，水平运动与ZMP线性关系"""
    def __init__(self, z_c=0.8, g=9.81):
        self.z_c = z_c      # 质心高度 [m]
        self.g = g          # 重力加速度 [m/s^2]
        self.w = np.sqrt(g / z_c)   # 倒立摆自然频率 [rad/s]

    def compute_zmp_from_com(self, x, ddot_x):
        """由质心位置和加速度反算ZMP位置"""
        return x - (self.z_c / self.g) * ddot_x

    def compute_com_trajectory(self, x0, v0, p_x, t, dt=0.001):
        """
        给定恒定ZMP位置p_x，解析求解质心轨迹
        返回: (t_array, x_array, v_array)
        """
        # 双曲函数解析解
        cosh_wt = np.cosh(self.w * t)
        sinh_wt = np.sinh(self.w * t)

        # 质心位置: x(t) = (x0 - p_x)*cosh(wt) + (v0/w)*sinh(wt) + p_x
        x_t = (x0 - p_x) * cosh_wt + (v0 / self.w) * sinh_wt + p_x

        # 质心速度: v(t) = (x0 - p_x)*w*sinh(wt) + v0*cosh(wt)
        v_t = (x0 - p_x) * self.w * sinh_wt + v0 * cosh_wt

        # 质心加速度: a(t) = w^2 * (x_t - p_x)
        a_t = self.w**2 * (x_t - p_x)

        # 由质心状态反算ZMP（应恒等于p_x，用于验证）
        zmp_calc = self.compute_zmp_from_com(x_t, a_t)

        t_array = np.full_like(x_t, t) if np.isscalar(t) else t
        return t_array, x_t, v_t, zmp_calc

    def simulate(self, x0, v0, p_x, duration=1.0, dt=0.001):
        """仿真接口：返回时间序列"""
        t = np.arange(0, duration + dt, dt)
        _, x_t, v_t, zmp_calc = self.compute_com_trajectory(x0, v0, p_x, t)
        return t, x_t, v_t, zmp_calc

# ========== 最小验证闭环 ==========
if __name__ == "__main__":
    # 参数设置
    model = LIPM(z_c=0.8, g=9.81)
    x0 = 0.0        # 初始质心位置 [m]
    v0 = 0.5        # 初始质心速度 [m/s]
    p_x = 0.1       # 恒定ZMP位置 [m]
    duration = 2.0  # 仿真时长 [s]

    # 运行仿真
    t, x, v, zmp = model.simulate(x0, v0, p_x, duration)

    # 验证1: ZMP反算误差（应接近0）
    zmp_error = np.max(np.abs(zmp - p_x))
    print(f"ZMP反算最大误差: {zmp_error:.2e} m")

    # 验证2: 动力学方程残差 (ddot{x} - w^2*(x-p_x) ≈ 0)
    # 由解析解计算加速度
    a_analytic = model.w**2 * (x - p_x)
    # 数值微分验证
    a_numeric = np.gradient(v, t)
    dyn_error = np.max(np.abs(a_numeric - a_analytic))
    print(f"动力学方程最大残差: {dyn_error:.2e} m/s^2")

    # 验证3: 输出关键时间点状态
    idx = np.array([0, len(t)//4, len(t)//2, 3*len(t)//4, -1])
    print("\n时间点状态采样:")
    print(f"{'t(s)':>8} {'x(m)':>10} {'v(m/s)':>10} {'ZMP(m)':>10}")
    for i in idx:
        print(f"{t[i]:8.3f} {x[i]:10.4f} {v[i]:10.4f} {zmp[i]:10.4f}")

    # 验证4: 断言检查
    assert zmp_error < 1e-8, "ZMP反算误差过大"
    assert dyn_error < 1e-6, "动力学残差过大"
    print("\n所有验证通过：模型实现正确")

运行方式

python lipm_model.py

需要安装 numpy：pip install numpy

验证方法与判定标准

• ZMP反算一致性：由质心轨迹反算的ZMP应与设定值一致，最大误差 < 1e-8 m，证明正逆动力学关系自洽；
• 动力学方程残差：数值微分得到的加速度与解析加速度最大残差 < 1e-6 m/s²，证明轨迹满足LIPM微分方程；
• 状态输出：终端打印的采样点显示质心位置随时间呈双曲增长趋势（因 v0 > 0 且 p_x < x 时不稳定），符合LIPM开环特性；
• 程序末尾断言通过且无异常退出，即判定实现正确。

模型用途说明

该LIPM实现可直接用于：给定ZMP轨迹反推质心运动（步态规划正问题），或给定期望质心轨迹计算所需ZMP（稳定性判据）。实际双足行走中，ZMP必须保持在支撑多边形内，本模型为这一约束提供了线性化分析工具。若要扩展至三维，只需在 y 方向独立应用相同方程即可。