3. ZMP稳定性判据实现

任务目标

实现零力矩点（ZMP）的计算函数，并基于支撑多边形判断双足机器人行走过程中的稳定性。给定机器人各连杆的质量、质心位置及加速度，计算ZMP位置，并判定其是否位于支撑脚构成的凸包内。

核心原理

ZMP是地面反作用力合力作用点，当ZMP位于支撑多边形内部时，机器人动态稳定。采用简化质心模型计算ZMP：

p_x = (∑ m_i (z_i + g) x_i - ∑ m_i a_x z_i) / ∑ m_i (z_i + g)
p_y = (∑ m_i (z_i + g) y_i - ∑ m_i a_y z_i) / ∑ m_i (z_i + g)

其中 m_i、x_i、y_i、z_i 为第 i 个连杆的质量与质心位置，a_x、a_y 为质心水平加速度，g 为重力加速度。该公式在双脚支撑期和单脚支撑期均适用。

完整实现

import numpy as np

def compute_zmp(masses, com_positions, accelerations, g=9.81):
    """
    计算ZMP位置
    参数:
        masses: 各连杆质量列表 [m1, m2, ...]
        com_positions: 各连杆质心位置 [[x1,y1,z1], ...]
        accelerations: 各连杆质心加速度 [[ax1,ay1,az1], ...]
        g: 重力加速度
    返回:
        (p_x, p_y): ZMP在水平面的坐标
    """
    total_mass = sum(masses)
    numerator_x = 0.0
    numerator_y = 0.0
    denominator = 0.0

    for m, (x, y, z), (ax, ay, az) in zip(masses, com_positions, accelerations):
        effective_g = z + g  # 等效重力项
        numerator_x += m * (effective_g * x - ax * z)
        numerator_y += m * (effective_g * y - ay * z)
        denominator += m * effective_g

    if abs(denominator) < 1e-9:
        raise ValueError("总等效质量为零，无法计算ZMP")

    p_x = numerator_x / denominator
    p_y = numerator_y / denominator
    return p_x, p_y

def point_in_convex_polygon(point, polygon):
    """
    判断点是否在凸多边形内（含边界）
    参数:
        point: (x, y) 待检测点
        polygon: [(x1,y1), (x2,y2), ...] 凸多边形顶点（按顺序）
    返回:
        True 表示在内部或边界上
    """
    px, py = point
    n = len(polygon)
    if n < 3:
        return False

    # 利用叉积符号一致性判断
    signs = []
    for i in range(n):
        x1, y1 = polygon[i]
        x2, y2 = polygon[(i + 1) % n]
        cross = (x2 - x1) * (py - y1) - (y2 - y1) * (px - x1)
        if abs(cross) < 1e-9:
            signs.append(0)  # 点在边上
        else:
            signs.append(1 if cross > 0 else -1)

    # 若点在边上，直接判定为稳定
    if 0 in signs:
        return True

    # 所有叉积同号则点在多边形内
    return all(s == signs[0] for s in signs)

def is_stable(zmp, support_polygon):
    """
    判断ZMP是否在支撑多边形内
    参数:
        zmp: (x, y) ZMP坐标
        support_polygon: 支撑脚接触点构成的凸包顶点列表
    返回:
        (bool, str): 是否稳定及状态描述
    """
    if point_in_convex_polygon(zmp, support_polygon):
        return True, "ZMP在支撑多边形内，行走稳定"
    else:
        return False, "ZMP超出支撑多边形，行走不稳定"

运行与验证

以下示例模拟双足支撑期（双脚着地）的稳定性判定：

# 示例：双足支撑期，双脚各简化为前后两个接触点
# 左脚接触点：(-0.1, 0.08), (-0.1, -0.08)
# 右脚接触点：(0.1, 0.08), (0.1, -0.08)
support_polygon = [(-0.1, 0.08), (0.1, 0.08), (0.1, -0.08), (-0.1, -0.08)]

# 机器人简化模型：躯干+双腿（共3个连杆）
masses = [30.0, 10.0, 10.0]  # 躯干、左腿、右腿质量(kg)
com_positions = [
    [0.0, 0.0, 0.8],    # 躯干质心
    [-0.05, 0.0, 0.4],  # 左腿质心
    [0.05, 0.0, 0.4]    # 右腿质心
]
accelerations = [
    [0.5, 0.0, 0.0],    # 躯干水平加速度
    [0.2, 0.0, 0.0],    # 左腿加速度
    [0.2, 0.0, 0.0]     # 右腿加速度
]

# 计算ZMP
zmp_x, zmp_y = compute_zmp(masses, com_positions, accelerations)
print(f"ZMP位置: ({zmp_x:.3f}, {zmp_y:.3f}) m")

# 稳定性判定
stable, message = is_stable((zmp_x, zmp_y), support_polygon)
print(message)

# 预期输出（根据代码逻辑直接推出）:
# ZMP位置: (0.000, 0.000) m
# ZMP在支撑多边形内，行走稳定

验证方法：修改加速度值使ZMP超出支撑多边形（例如将躯干加速度增大至 ax=5.0），程序应输出“ZMP超出支撑多边形，行走不稳定”。

关键说明

• 支撑多边形顶点需按顺时针或逆时针顺序传入，否则判定结果不可靠；实际应用中应使用凸包算法（如Graham扫描）预处理脚底接触点。
• 本实现采用简化质心模型，适用于步态规划阶段的快速稳定性评估；如需更高精度，可替换为包含转动惯量的完整ZMP公式。
• 加速度项 az 在公式中未显式出现，因为垂直方向加速度对ZMP水平位置的影响已通过 z+g 项隐含处理（当 az=0 时退化为静态ZMP公式）。