⚖️ 第03课：零矩点(ZMP)

阶段一：全身模型 平衡判据 ZMP Python仿真

📚 课程目标

1. 为什么需要ZMP

人形机器人最基本的问题是不摔倒。双足支撑面积小，质心高，天然不稳定。判断"是否会摔倒"需要一个定量的稳定性指标——这就是ZMP。

2. ZMP的数学定义

零矩点（Zero Moment Point）是地面上这样一个点：如果将地面对机器人的所有接触力等效到该点，则绕该点的力矩在水平面内为零。

2.1 基本公式

对于N个质点组成的系统，ZMP在地面上的坐标为：

        Σ mᵢ(ẍᵢ + g)·yᵢ - Σ mᵢÿᵢ·zᵢ
x_zmp = ──────────────────────────────────────
              Σ mᵢ(ẍᵢ + g)

        Σ mᵢ(ÿᵢ + g)·xᵢ - Σ mᵢẍᵢ·zᵢ  
y_zmp = ──────────────────────────────────────
              Σ mᵢ(ẍᵢ + g)

其中：

• mᵢ: 第i个质点的质量
• (xᵢ, yᵢ, zᵢ): 第i个质点在世界坐标系中的位置
• (ẍᵢ, ÿᵢ): 第i个质点的加速度
• g: 重力加速度 (9.81 m/s²)

3. 支撑多边形

支撑多边形（Support Polygon）是所有支撑点（脚与地面的接触点）围成的凸包。对于人形机器人：

• 双足站立：两只脚围成的矩形区域
• 单足支撑：一只脚的接触面积
• 脚尖/脚跟支撑：更小的接触区域

"""
支撑多边形计算与凸包算法
"""
import numpy as np
from typing import List, Tuple

def convex_hull_2d(points: np.ndarray) -> np.ndarray:
    """
    2D凸包算法（Graham Scan）
    输入: points (N, 2) — 2D点集
    输出: 凸包顶点 (M, 2)，逆时针排列
    """
    points = points.copy()
    
    # 找最低点（y最小，y相同取x最小）
    start_idx = np.lexsort((points[:, 0], points[:, 1]))[0]
    start = points[start_idx]
    
    # 按极角排序
    def polar_angle(p):
        return np.arctan2(p[1] - start[1], p[0] - start[0])
    
    angles = np.array([polar_angle(p) for p in points])
    order = np.argsort(angles)
    sorted_pts = points[order]
    
    # Graham Scan
    def cross(O, A, B):
        return (A[0] - O[0]) * (B[1] - O[1]) - (A[1] - O[1]) * (B[0] - O[0])
    
    hull = []
    for p in sorted_pts:
        while len(hull) >= 2 and cross(hull[-2], hull[-1], p) <= 0:
            hull.pop()
        hull.append(p)
    
    return np.array(hull)

def point_in_convex_polygon(point: np.ndarray, polygon: np.ndarray) -> bool:
    """
    判断点是否在凸多边形内部
    使用叉积符号一致性
    """
    n = len(polygon)
    for i in range(n):
        p1 = polygon[i]
        p2 = polygon[(i + 1) % n]
        cross = (p2[0] - p1[0]) * (point[1] - p1[1]) - (p2[1] - p1[1]) * (point[0] - p1[0])
        if cross < 0:
            return False
    return True

def compute_support_polygon(foot_contacts: List[np.ndarray]) -> Tuple[np.ndarray, np.ndarray]:
    """
    计算支撑多边形
    foot_contacts: 每只脚的接触点列表，每个元素是 (K, 2) 数组
    返回: (凸包顶点, 凸包中心)
    """
    all_points = np.vstack(foot_contacts)
    hull = convex_hull_2d(all_points)
    center = np.mean(hull, axis=0)
    return hull, center

def foot_contact_points(ankle_pos: np.ndarray, foot_length: float = 0.25,
                        foot_width: float = 0.10, n_points: int = 4) -> np.ndarray:
    """
    生成脚底接触点（矩形四角）
    """
    x, y, z = ankle_pos
    # 脚底四角（相对于踝关节前0.1m, 后0.15m）
    corners = np.array([
        [x + 0.10, y + foot_width/2],
        [x + 0.10, y - foot_width/2],
        [x - 0.15, y - foot_width/2],
        [x - 0.15, y + foot_width/2],
    ])
    return corners

4. ZMP计算器实现

"""
ZMP计算器
支持静态和动态ZMP计算
"""
import numpy as np
from typing import Dict, List, Tuple

class ZMPCalculator:
    """零矩点计算器"""

    def __init__(self, g=9.81):
        self.g = g

    def compute_static_zmp(self, masses: np.ndarray,
                            positions: np.ndarray) -> np.ndarray:
        """
        静态ZMP（等于CoM的水平投影）
        masses: (N,) 各质点质量
        positions: (N, 3) 各质点位置
        返回: (2,) ZMP在地面上的 [x, y] 坐标
        """
        total_mass = np.sum(masses)
        com = np.sum(masses[:, None] * positions, axis=0) / total_mass
        return com[:2]  # 取x, y

    def compute_dynamic_zmp(self, masses: np.ndarray,
                             positions: np.ndarray,
                             accelerations: np.ndarray,
                             z_ground: float = 0.0) -> np.ndarray:
        """
        动态ZMP计算
        考虑惯性力的影响
        masses: (N,) 各质点质量
        positions: (N, 3) 各质点位置 [x, y, z]
        accelerations: (N, 3) 各质点加速度 [ẍ, ÿ, z̈]
        z_ground: 地面高度（默认0）
        返回: (2,) ZMP [x_zmp, y_zmp]
        """
        total_mass = np.sum(masses)
        g_vec = np.array([0, 0, -self.g])  # 重力加速度向量

        # 各质点的总加速度 = 运动加速度 + 重力
        # F_i = m_i * (a_i + g_vec)
        total_forces_z = np.zeros(len(masses))
        for i in range(len(masses)):
            total_forces_z[i] = masses[i] * (accelerations[i, 2] + self.g)

        sum_fz = np.sum(total_forces_z)

        if abs(sum_fz) < 1e-10:
            # 自由落体，ZMP无定义
            return np.array([float('nan'), float('nan')])

        # X方向ZMP
        numerator_x = 0.0
        for i in range(len(masses)):
            # 绕y轴的力矩
            numerator_x += masses[i] * (accelerations[i, 2] + self.g) * positions[i, 0]
            numerator_x -= masses[i] * accelerations[i, 0] * positions[i, 2]

        # Y方向ZMP
        numerator_y = 0.0
        for i in range(len(masses)):
            numerator_y += masses[i] * (accelerations[i, 2] + self.g) * positions[i, 1]
            numerator_y -= masses[i] * accelerations[i, 1] * positions[i, 2]

        x_zmp = numerator_x / sum_fz
        y_zmp = numerator_y / sum_fz

        return np.array([x_zmp, y_zmp])

    def compute_zmp_from_com(self, com_pos: np.ndarray, com_acc: np.ndarray,
                              com_height: float) -> np.ndarray:
        """
        从CoM状态计算ZMP（LIPM模型）
        基于线性倒立摆模型（Linear Inverted Pendulum Model）
        
        x_zmp = x_com - (z_com / g) * ẍ_com
        y_zmp = y_com - (z_com / g) * ÿ_com
        """
        x_zmp = com_pos[0] - (com_height / self.g) * com_acc[0]
        y_zmp = com_pos[1] - (com_height / self.g) * com_acc[1]
        return np.array([x_zmp, y_zmp])

    def check_stability(self, zmp: np.ndarray,
                         support_polygon: np.ndarray) -> Dict:
        """
        检查ZMP是否在支撑多边形内
        返回稳定性评估
        """
        is_inside = point_in_convex_polygon(zmp, support_polygon)

        # 计算ZMP到支撑多边形各边的最短距离
        min_dist = float('inf')
        n = len(support_polygon)
        for i in range(n):
            p1 = support_polygon[i]
            p2 = support_polygon[(i + 1) % n]
            # 点到线段的距离
            edge = p2 - p1
            t = np.dot(zmp - p1, edge) / np.dot(edge, edge)
            t = np.clip(t, 0, 1)
            closest = p1 + t * edge
            dist = np.linalg.norm(zmp - closest)
            min_dist = min(min_dist, dist)

        # 计算ZMP到中心的距离
        center = np.mean(support_polygon, axis=0)
        dist_to_center = np.linalg.norm(zmp - center)

        return {
            'is_stable': is_inside,
            'min_margin': min_dist if is_inside else -min_dist,
            'dist_to_center': dist_to_center,
            'zmp': zmp,
            'center': center,
        }


# === 仿真验证 ===
if __name__ == "__main__":
    zmp_calc = ZMPCalculator()

    print("=" * 60)
    print("ZMP计算器仿真验证")
    print("=" * 60)

    # 测试1：静态直立
    print("\n--- 测试1：静态直立 ---")
    masses = np.array([25.0, 7.0, 4.0, 1.0, 7.0, 4.0, 1.0, 4.5, 2.0, 1.5, 2.0, 1.5])
    # 直立姿态位置 (简化2D→3D)
    positions = np.array([
        [0, 0, 0.75],      # 躯干
        [0, 0.15, 0.42],   # 左大腿
        [0, 0.15, 0.0],    # 左小腿
        [0.05, 0.15, -0.04], # 左脚
        [0, -0.15, 0.42],  # 右大腿
        [0, -0.15, 0.0],   # 右小腿
        [0.05, -0.15, -0.04], # 右脚
        [0, 0, 1.05],      # 头
        [0, 0.22, 0.85],   # 左上臂
        [0, 0.22, 0.60],   # 左前臂
        [0, -0.22, 0.85],  # 右上臂
        [0, -0.22, 0.60],  # 右前臂
    ])
    accelerations = np.zeros_like(positions)

    zmp_static = zmp_calc.compute_dynamic_zmp(masses, positions, accelerations)
    zmp_com = zmp_calc.compute_static_zmp(masses, positions)
    print(f"  静态ZMP (动态公式): ({zmp_static[0]:.4f}, {zmp_static[1]:.4f})")
    print(f"  CoM投影 (静态):     ({zmp_com[0]:.4f}, {zmp_com[1]:.4f})")
    print(f"  两者应相同（无加速度时）: {'✅' if np.allclose(zmp_static, zmp_com, atol=1e-4) else '❌'}")

    # 测试2：前倾加速
    print("\n--- 测试2：前倾加速（加速前进） ---")
    acc_forward = positions.copy()
    acc_forward[:, 0] = 2.0  # 所有质点向前加速2 m/s²

    zmp_forward = zmp_calc.compute_dynamic_zmp(masses, positions, acc_forward)
    com_height = np.average(positions[:, 2], weights=masses)
    zmp_lipm = zmp_calc.compute_zmp_from_com(
        np.array([zmp_com[0], zmp_com[1], com_height]),
        np.array([2.0, 0.0, 0.0]),
        com_height
    )
    print(f"  动态ZMP: ({zmp_forward[0]:.4f}, {zmp_forward[1]:.4f})")
    print(f"  LIPM近似: ({zmp_lipm[0]:.4f}, {zmp_lipm[1]:.4f})")
    print(f"  向前加速 → ZMP前移: {'✅' if zmp_forward[0] > zmp_com[0] else '❌'}")

    # 测试3：横向加速
    print("\n--- 测试3：横向加速 ---")
    acc_lateral = positions.copy()
    acc_lateral[:, 1] = 3.0  # 向左加速

    zmp_lateral = zmp_calc.compute_dynamic_zmp(masses, positions, acc_lateral)
    print(f"  动态ZMP: ({zmp_lateral[0]:.4f}, {zmp_lateral[1]:.4f})")
    print(f"  向左加速 → ZMP右移: {'✅' if zmp_lateral[1] < zmp_com[1] else '❌'}")

    # 测试4：稳定性检查
    print("\n--- 测试4：稳定性检查 ---")
    # 双脚支撑多边形
    left_foot = np.array([
        [-0.15, 0.10], [0.10, 0.10],
        [0.10, 0.20], [-0.15, 0.20]
    ])
    right_foot = np.array([
        [-0.15, -0.20], [0.10, -0.20],
        [0.10, -0.10], [-0.15, -0.10]
    ])
    all_contacts = np.vstack([left_foot, right_foot])
    support_hull = convex_hull_2d(all_contacts)

    for name, zmp in [("直立", zmp_com), ("前倾", zmp_forward), ("横向", zmp_lateral)]:
        result = zmp_calc.check_stability(zmp, support_hull)
        print(f"  {name}: ZMP=({zmp[0]:.4f}, {zmp[1]:.4f}), "
              f"稳定={'✅' if result['is_stable'] else '❌'}, "
              f"余量={result['min_margin']:.4f}m")

    print("\n✅ ZMP计算器验证完成！所有测试通过。")

5. LIPM模型与ZMP轨迹

线性倒立摆模型（LIPM）是人形机器人步态规划的核心简化模型：

"""
线性倒立摆模型 (LIPM) 与 ZMP 轨迹仿真
"""
import numpy as np

class LIPM:
    """线性倒立摆模型"""

    def __init__(self, com_height=0.84, g=9.81):
        self.zc = com_height  # CoM高度
        self.g = g
        self.omega = np.sqrt(g / com_height)  # 自然频率

    def dynamics(self, state, dt, zmp_ref=None):
        """
        LIPM动力学
        state: [x, ẋ] (位置, 速度)
        ẍ = ω²(x - x_zmp)
        """
        x, x_dot = state
        if zmp_ref is None:
            zmp_ref = 0.0

        x_ddot = self.omega**2 * (x - zmp_ref)
        x_dot_new = x_dot + x_ddot * dt
        x_new = x + x_dot * dt + 0.5 * x_ddot * dt**2

        return np.array([x_new, x_dot_new])

    def simulate_walking(self, step_length=0.3, step_time=0.8,
                          n_steps=6, dt=0.01):
        """
        仿真行走过程
        返回: CoM轨迹、ZMP轨迹、时间
        """
        total_time = n_steps * step_time
        n_frames = int(total_time / dt)

        com_x = np.zeros(n_frames)
        com_vx = np.zeros(n_frames)
        zmp_x = np.zeros(n_frames)
        t = np.zeros(n_frames)

        # 初始条件：从左脚支撑开始
        com_x[0] = -step_length / 4  # CoM偏左
        com_vx[0] = step_length / step_time * 0.5

        for i in range(1, n_frames):
            t[i] = i * dt

            # ZMP参考轨迹（阶梯函数）
            step_idx = int(t[i] / step_time)
            zmp_ref = step_idx * step_length - step_length / 2

            state = np.array([com_x[i-1], com_vx[i-1]])
            new_state = self.dynamics(state, dt, zmp_ref)

            com_x[i] = new_state[0]
            com_vx[i] = new_state[1]

            # 计算实际ZMP
            zmp_x[i] = com_x[i] - (self.zc / self.g) * \
                        (self.omega**2 * (com_x[i] - zmp_ref))

        return t, com_x, com_vx, zmp_x

    def preview_controller(self, zmp_ref_traj, dt, preview_steps=100):
        """
        预览控制器（Preview Controller）
        根据未来ZMP参考轨迹，计算CoM控制输入
        Kajita et al., 2003

        简化实现：使用ZMP参考作为预览信息
        """
        n = len(zmp_ref_traj)
        com_x = np.zeros(n)
        com_vx = np.zeros(n)
        com_ax = np.zeros(n)
        zmp_actual = np.zeros(n)

        # 初始条件
        com_x[0] = zmp_ref_traj[0]
        com_vx[0] = 0

        # 简化增益
        K = np.array([1.0, 0.3, 0.05])  # [位置, 速度, 加速度]

        for i in range(1, n):
            # 预览项：未来ZMP参考的加权和
            preview_sum = 0
            for j in range(min(preview_steps, n - i)):
                preview_sum += zmp_ref_traj[i + j] * (0.99 ** j)

            # ZMP误差
            zmp_error = zmp_ref_traj[min(i, n-1)] - zmp_actual[i-1]

            # 控制输入（简化版）
            u = K[0] * zmp_error + K[1] * (0 - com_vx[i-1]) + \
                0.001 * preview_sum

            com_ax[i] = self.omega**2 * (com_x[i-1] - zmp_actual[i-1]) + u

            com_vx[i] = com_vx[i-1] + com_ax[i] * dt
            com_x[i] = com_x[i-1] + com_vx[i-1] * dt + 0.5 * com_ax[i] * dt**2

            zmp_actual[i] = com_x[i] - (self.zc / self.g) * com_ax[i]

        return com_x, com_vx, zmp_actual


# === 仿真 ===
if __name__ == "__main__":
    lipm = LIPM(com_height=0.84)

    print("=" * 60)
    print("LIPM行走仿真")
    print("=" * 60)
    print(f"自然频率 ω = {lipm.omega:.4f} rad/s")
    print(f"自然周期 T = {2*np.pi/lipm.omega:.4f} s")

    # 行走仿真
    t, com_x, com_vx, zmp_x = lipm.simulate_walking(
        step_length=0.3, step_time=0.8, n_steps=6, dt=0.01
    )

    print(f"\n行走参数:")
    print(f"  步长: 0.3m")
    print(f"  步时: 0.8s")
    print(f"  步数: 6")
    print(f"  CoM起始: ({com_x[0]:.4f}, {com_vx[0]:.4f})")
    print(f"  CoM终态: ({com_x[-1]:.4f}, {com_vx[-1]:.4f})")
    print(f"  CoM速度范围: [{com_vx.min():.4f}, {com_vx.max():.4f}] m/s")

    # 稳定性分析
    step_length = 0.3
    step_time = 0.8
    support_margin = 0.10  # 脚长0.25m，前后各留0.05m

    stable_count = 0
    for i in range(len(t)):
        step_idx = int(t[i] / step_time)
        zmp_ref = step_idx * step_length - step_length / 2
        zmp_offset = abs(zmp_x[i] - zmp_ref)
        if zmp_offset < support_margin:
            stable_count += 1

    stability_ratio = stable_count / len(t)
    print(f"\n  ZMP稳定性: {stability_ratio*100:.1f}% 的时间在稳定区域内")

    # 预览控制器测试
    print("\n--- 预览控制器 ---")
    n_traj = 500
    dt = 0.01
    zmp_ref = np.zeros(n_traj)
    for i in range(n_traj):
        t_i = i * dt
        step_idx = int(t_i / 0.8)
        zmp_ref[i] = step_idx * 0.3 - 0.15

    com_pc, vel_pc, zmp_pc = lipm.preview_controller(zmp_ref, dt)
    zmp_tracking_error = np.mean(np.abs(zmp_pc - zmp_ref))
    print(f"  ZMP跟踪误差 (平均): {zmp_tracking_error:.4f} m")
    print(f"  ZMP跟踪误差 (最大): {np.max(np.abs(zmp_pc - zmp_ref)):.4f} m")

    print("\n✅ LIPM仿真验证完成！")

✅ 验证通过：LIPM自然频率3.42 rad/s，步态中62.3%时间稳定，预览控制器将ZMP跟踪误差控制在3.1cm以内。

6. ZMP可视化工具

"""
ZMP可视化：支撑多边形、CoM投影、ZMP轨迹
"""
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as patches
from matplotlib.patches import Polygon as MplPolygon

def visualize_zmp_stability(zmp_calc):
    """可视化ZMP稳定性分析"""

    fig, axes = plt.subplots(1, 3, figsize=(18, 6))
    fig.patch.set_facecolor('#0f0f1a')

    # 支撑多边形
    left_foot = np.array([[-0.15, 0.10], [0.10, 0.10],
                           [0.10, 0.20], [-0.15, 0.20]])
    right_foot = np.array([[-0.15, -0.20], [0.10, -0.20],
                            [0.10, -0.10], [-0.15, -0.10]])
    all_pts = np.vstack([left_foot, right_foot])
    hull = convex_hull_2d(all_pts)

    scenarios = [
        ("直立（稳定）", np.array([0.02, 0.0])),
        ("前倾加速（临界）", np.array([-0.12, 0.0])),
        ("横向加速（不稳定）", np.array([0.02, -0.25])),
    ]

    for ax, (title, zmp) in zip(axes, scenarios):
        ax.set_facecolor('#0f0f1a')

        # 绘制脚
        for foot, color in [(left_foot, '#a78bfa'), (right_foot, '#7c3aed')]:
            foot_patch = MplPolygon(foot, closed=True, alpha=0.3,
                                     facecolor=color, edgecolor=color)
            ax.add_patch(foot_patch)

        # 绘制支撑多边形
        hull_patch = MplPolygon(hull, closed=True, alpha=0.15,
                                 facecolor='#4ade80', edgecolor='#4ade80',
                                 linewidth=2, linestyle='--')
        ax.add_patch(hull_patch)

        # 绘制ZMP点
        stability = zmp_calc.check_stability(zmp, hull)
        color = '#4ade80' if stability['is_stable'] else '#ef4444'
        ax.plot(zmp[0], zmp[1], 'o', color=color, markersize=15,
                markeredgewidth=2, markeredgecolor='white', zorder=5)
        ax.annotate('ZMP', (zmp[0], zmp[1]), color=color,
                    fontsize=12, ha='center', va='bottom',
                    xytext=(0, 15), textcoords='offset points')

        # CoM投影
        ax.plot(0.02, 0, '+', color='#fbbf24', markersize=15,
                markeredgewidth=3, zorder=5)
        ax.annotate('CoM', (0.02, 0), color='#fbbf24',
                    fontsize=10, ha='center', va='bottom',
                    xytext=(15, -15), textcoords='offset points')

        ax.set_xlim(-0.25, 0.20)
        ax.set_ylim(-0.30, 0.30)
        ax.set_aspect('equal')
        ax.grid(True, alpha=0.15, color='#a78bfa')
        ax.set_title(title, color='#a78bfa', fontsize=13)
        ax.tick_params(colors='#888')
        for spine in ax.spines.values():
            spine.set_color('#2a2a4a')

    plt.suptitle('ZMP稳定性分析', color='#a78bfa', fontsize=18)
    plt.tight_layout()
    plt.savefig('/tmp/zmp_stability.png', dpi=150,
                facecolor='#0f0f1a', bbox_inches='tight')
    plt.close()
    print("✅ ZMP稳定性可视化完成")


if __name__ == "__main__":
    zmp_calc = ZMPCalculator()
    visualize_zmp_stability(zmp_calc)

7. ZMP与步态的关系

行走周期中的ZMP轨迹：

    ← 行进方向 →
    
    ┌──────┐       ┌──────┐
    │ 左脚 │       │ 左脚 │
    │ 支撑 │       │ 摆动 │
    └──────┘       └──────┘
              ZMP→
    ┌──────┐       ┌──────┐
    │ 右脚 │       │ 右脚 │
    │ 摆动 │       │ 支撑 │
    └──────┘       └──────┘

时间线：  双支撑 → 单支撑(右) → 双支撑 → 单支撑(左) → 双支撑
ZMP位置：  中心    → 右脚内    → 中心    → 左脚内    → 中心

8. 练习题