🤖 ZMP稳定判据

import math class ZMPAnalyzer: def __init__(self, mass=6.2, g=9.81, z_com=0.2): self.mass = mass self.g = g self.z_com = z_com def compute_zmp(self, com_pos, com_vel, com_acc, angular_momentum_rate=(0,0,0)): # ZMP = CoM - (z_hat x H_dot) / (m*g) # Simplified: ZMP_x = x - (z_ddot*H_dot_y - 0) / (m*g) etc. x, y, z = com_pos ax, ay, az = com_acc Hx, Hy, Hz = angular_momentum_rate zmp_x = x - (self.mass * az * (z - 0) - Hy) / (self.mass * self.g + self.mass * az) zmp_y = y - (-self.mass * az * 0 + Hx) / (self.mass * self.g + self.mass * az) # Simplified LIPM version zmp_x_lipm = x - (z / self.g) * ax zmp_y_lipm = y - (z / self.g) * ay return (zmp_x, zmp_y), (zmp_x_lipm, zmp_y_lipm) def check_stability(self, zmp, support_polygon): hull = self.convex_hull(support_polygon) if len(hull) < 3: return False, 0 inside = self.point_in_polygon(zmp, hull) margin = self.distance_to_polygon(zmp, hull) return inside, margin @staticmethod def convex_hull(points): pts = sorted(set(points)) if len(pts) <= 2: return pts lower = [] for p in pts: while len(lower) >= 2 and (lower[-1][0]-lower[-2][0])*(p[1]-lower[-2][1]) - (lower[-1][1]-lower[-2][1])*(p[0]-lower[-2][0]) <= 0: lower.pop() lower.append(p) upper = [] for p in reversed(pts): while len(upper) >= 2 and (upper[-1][0]-upper[-2][0])*(p[1]-upper[-2][1]) - (upper[-1][1]-upper[-2][1])*(p[0]-upper[-2][0]) <= 0: upper.pop() upper.append(p) return lower[:-1] + upper[:-1] @staticmethod def point_in_polygon(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 @staticmethod def distance_to_polygon(point, polygon): n = len(polygon) min_dist = float('inf') for i in range(n): p1 = polygon[i] p2 = polygon[(i+1) % n] dx, dy = p2[0]-p1[0], p2[1]-p1[1] seg_sq = dx*dx + dy*dy if seg_sq < 1e-10: d = math.hypot(point[0]-p1[0], point[1]-p1[1]) else: t = max(0, min(1, ((point[0]-p1[0])*dx + (point[1]-p1[1])*dy)/seg_sq)) d = math.hypot(point[0]-(p1[0]+t*dx), point[1]-(p1[1]+t*dy)) min_dist = min(min_dist, d) return min_dist zmp = ZMPAnalyzer() print("=" * 55) print(" ZMP Stability Criterion Simulation") print("=" * 55) # Support polygon (4-leg stance, body 0.4x0.2) support = [(0.2, 0.1), (0.2, -0.1), (-0.2, 0.1), (-0.2, -0.1)] # Test various CoM accelerations print("\n [ZMP under different accelerations]") print(f" Support polygon area = {zmp.distance_to_polygon((0,0), support)*1e4:.0f} cm^2 (approx)") test_cases = [ ((0, 0, 0.2), (0, 0, 0), "standing still"), ((0, 0, 0.2), (1, 0, 0), "forward accel 1m/s2"), ((0, 0, 0.2), (2, 0, 0), "forward accel 2m/s2"), ((0, 0, 0.2), (0, 1, 0), "lateral accel 1m/s2"), ((0, 0, 0.2), (3, 1, 0), "combined accel"), ((0.05, 0, 0.2), (1, 0, 0), "offset+accel"), ] for com_pos, com_acc, desc in test_cases: zmp_full, zmp_lipm = zmp.compute_zmp(com_pos, (0,0,0), com_acc) stable, margin = zmp.check_stability(zmp_lipm, support) print(f" {desc:30s}: ZMP=({zmp_lipm[0]:.4f},{zmp_lipm[1]:.4f}) " f"margin={margin*1000:.1f}mm [{'STABLE' if stable else 'UNSTABLE'}]") # Trot gait ZMP analysis print("\n [Trot Gait ZMP Analysis]") # During trot: 2 legs in contact, diagonal pair trot_support_diag1 = [(0.2, 0.1), (-0.2, -0.1)] # LF+RB trot_support_diag2 = [(0.2, -0.1), (-0.2, 0.1)] # RF+LB for com_acc, desc in [((0,0,0), "no accel"), ((0.5,0,0), "fwd 0.5m/s2"), ((1.0,0,0), "fwd 1.0m/s2")]: zmp_full, zmp_lipm = zmp.compute_zmp((0,0,0.2), (0,0,0), com_acc) print(f" {desc:20s}: ZMP=({zmp_lipm[0]:.4f},{zmp_lipm[1]:.4f})") print(f" Distance to diag1 line: {zmp.distance_to_polygon(zmp_lipm, trot_support_diag1)*1000:.1f}mm") print(f" Distance to diag2 line: {zmp.distance_to_polygon(zmp_lipm, trot_support_diag2)*1000:.1f}mm") # Max acceleration before instability print("\n [Max Acceleration Before Instability]") for direction in ['forward', 'lateral']: for step in range(1, 50): a = step * 0.1 if direction == 'forward': acc = (a, 0, 0) else: acc = (0, a, 0) zmp_full, zmp_lipm = zmp.compute_zmp((0,0,0.2), (0,0,0), acc) stable, _ = zmp.check_stability(zmp_lipm, support) if not stable: print(f" {direction}: max stable accel = {(a-0.1):.1f} m/s2") break print() print(" OK - ZMP analysis simulation complete")