🤖 视觉引导行走

import math class VisionGuidedWalker: def __init__(self, map_res=0.02, fov=1.0): self.map_res = map_res self.fov = fov def simulate_depth_camera(self, terrain_func, n_points=50): points = [] for i in range(n_points): x = -self.fov/2 + self.fov * i / (n_points-1) z = terrain_func(x) points.append((x, z)) return points def detect_obstacles(self, points, threshold=0.02): obstacles = [] for i in range(1, len(points)): dx = points[i][0] - points[i-1][0] dz = points[i][1] - points[i-1][1] if abs(dx) > 0: slope = abs(dz / dx) if slope > 1.0 or abs(dz) > threshold: obstacles.append((points[i][0], abs(dz), slope)) return obstacles def plan_footsteps(self, points, obstacles, step_length=0.1, n_steps=8): footsteps = [] current_x = 0 for step in range(n_steps): target_x = current_x + step_length # Find safe landing zone best_x = target_x best_z = 0 min_risk = float('inf') for px, pz in points: dist = abs(px - target_x) if dist < step_length: risk = 0 for ox, oh, os in obstacles: risk += max(0, 1 - abs(px - ox) / 0.05) * oh total = dist * 10 + risk if total < min_risk: min_risk = total best_x = px best_z = pz footsteps.append((best_x, best_z)) current_x = best_x return footsteps vgw = VisionGuidedWalker() print("=" * 55) print(" Vision-Guided Walking Simulation") print("=" * 55) terrain1 = lambda x: 0.02 * math.sin(5*x) + 0.01 * math.cos(11*x) terrain2 = lambda x: 0.05 if 0.2 < x < 0.3 else (0.03 if 0.5 < x < 0.55 else 0) for name, tfunc in [("wavy", terrain1), ("obstacles", terrain2)]: print(f"\n [Terrain: {name}]") pts = vgw.simulate_depth_camera(tfunc) obs = vgw.detect_obstacles(pts) print(f" Detected {len(obs)} obstacles") for ox, oh, os in obs[:5]: print(f" x={ox:.3f}m height={oh:.3f}m slope={os:.2f}") steps = vgw.plan_footsteps(pts, obs) print(f" Planned {len(steps)} footsteps:") for i, (fx, fz) in enumerate(steps): print(f" Step {i}: ({fx:.3f}, {fz:.4f})") print() print(" OK - Vision-guided walking simulation complete")