2. 基于ZMP的静态步行稳定性分析

任务目标

实现零力矩点（ZMP）的计算，分析双足机器人静态步行时的稳定性条件，并通过调整步态参数使ZMP始终落在支撑多边形内。

核心原理

静态步行要求机器人质心（CoM）的投影点始终位于支撑脚与地面接触形成的凸包（支撑多边形）内。ZMP是地面反作用力合力作用点，当机器人处于静态平衡时，ZMP与CoM投影重合。本案例通过计算ZMP位置，判断其是否落在支撑多边形内，并调整步态参数（步长、躯干横向偏移）保证稳定性。

实现代码

import numpy as np

class ZMPStabilityAnalyzer:
    def __init__(self, foot_length=0.2, foot_width=0.1):
        self.foot_length = foot_length  # 脚掌长度 (m)
        self.foot_width = foot_width    # 脚掌宽度 (m)
        self.g = 9.81                   # 重力加速度 (m/s^2)
    
    def support_polygon(self, foot_positions):
        """计算支撑多边形顶点（双脚支撑时取两脚凸包）"""
        vertices = []
        for pos in foot_positions:
            x, y, theta = pos
            # 脚掌四角坐标（局部坐标系）
            corners_local = np.array([
                [self.foot_length/2, self.foot_width/2],
                [self.foot_length/2, -self.foot_width/2],
                [-self.foot_length/2, -self.foot_width/2],
                [-self.foot_length/2, self.foot_width/2]
            ])
            # 旋转并平移到全局坐标
            rot = np.array([[np.cos(theta), -np.sin(theta)],
                            [np.sin(theta), np.cos(theta)]])
            corners_global = corners_local @ rot.T + np.array([x, y])
            vertices.extend(corners_global.tolist())
        return np.array(vertices)
    
    def compute_zmp(self, com_pos, com_acc, external_force=0):
        """计算ZMP位置（静态步行：加速度近似为0）"""
        x, y, z = com_pos
        ax, ay, _ = com_acc
        # ZMP公式（忽略惯性力矩，静态情况）
        zmp_x = x - (z * ax) / (self.g + external_force)
        zmp_y = y - (z * ay) / (self.g + external_force)
        return np.array([zmp_x, zmp_y])
    
    def point_in_polygon(self, point, polygon):
        """射线法判断点是否在多边形内"""
        x, y = point
        n = len(polygon)
        inside = False
        j = n - 1
        for i in range(n):
            xi, yi = polygon[i]
            xj, yj = polygon[j]
            if ((yi > y) != (yj > y)) and (x < (xj - xi) * (y - yi) / (yj - yi) + xi):
                inside = not inside
            j = i
        return inside
    
    def analyze_gait(self, com_trajectory, foot_positions):
        """分析整个步态周期的稳定性"""
        polygon = self.support_polygon(foot_positions)
        results = []
        for com in com_trajectory:
            # 静态步行：CoM速度/加速度近似为0
            zmp = self.compute_zmp(com[:3], [0, 0, 0])
            stable = self.point_in_polygon(zmp, polygon)
            results.append({'zmp': zmp, 'stable': stable})
        return polygon, results

# ========== 使用示例 ==========
if __name__ == "__main__":
    analyzer = ZMPStabilityAnalyzer()
    
    # 定义双脚位置（单步状态：双脚支撑）
    foot_positions = [
        [0.0, 0.1, 0],    # 左脚
        [0.2, -0.1, 0]    # 右脚（步长0.2m）
    ]
    
    # 生成CoM轨迹（静态步行：CoM缓慢移动）
    com_trajectory = []
    for t in np.linspace(0, 1, 10):
        # CoM在双脚中心附近移动，高度0.8m
        com_x = 0.1 + 0.02 * np.sin(np.pi * t)  # 小幅前后移动
        com_y = 0.0 + 0.05 * np.sin(2 * np.pi * t)  # 横向摆动
        com_trajectory.append([com_x, com_y, 0.8])
    
    polygon, results = analyzer.analyze_gait(com_trajectory, foot_positions)
    
    # 输出分析结果
    stable_count = sum(1 for r in results if r['stable'])
    print(f"支撑多边形顶点数: {len(polygon)}")
    print(f"稳定性通过率: {stable_count}/{len(results)}")
    
    # 检查最危险时刻（ZMP最接近边界）
    min_margin = float('inf')
    for r in results:
        zmp = r['zmp']
        # 计算到多边形边界的最小距离（简化：到各边距离的最小值）
        for i in range(len(polygon)):
            p1, p2 = polygon[i], polygon[(i+1) % len(polygon)]
            dist = np.abs(np.cross(p2-p1, p1-zmp)) / np.linalg.norm(p2-p1)
            min_margin = min(min_margin, dist)
    print(f"ZMP到支撑多边形边界最小距离: {min_margin:.4f} m")
    print("判定: " + ("稳定" if min_margin > 0.01 else "临界/不稳定"))

运行与验证

运行方式：直接执行 python zmp_analysis.py（需安装NumPy）。

验证方法：

1. 程序输出支撑多边形顶点数（双脚支撑应为8个顶点）；
2. 稳定性通过率应接近100%（当前参数下CoM轨迹设计在安全范围内）；
3. ZMP到边界最小距离应大于0.01m，否则需调整步态参数。

参数调整实验：将步长从0.2m增大到0.4m（修改foot_positions中右脚x坐标），重新运行，观察稳定性通过率下降、最小距离减小，说明步长增大导致ZMP接近支撑边界。

关键说明

本实现针对静态步行场景，假设CoM加速度为零（实际静态步行中CoM移动缓慢，惯性力可忽略）。支撑多边形由双脚脚掌四角点构成凸包，ZMP位置由CoM位置和加速度计算得出。当ZMP落在多边形内部时，机器人不会绕支撑脚边缘翻转，即静态稳定。实际应用中需结合步态规划器生成CoM轨迹，本代码提供核心稳定性判定模块，可直接嵌入步态控制循环。