🤖 毕业项目：全地形四足机器人

import math, random class AllTerrainQuadruped: def __init__(self, mass=6.2, body_L=0.4, body_W=0.2, L_thigh=0.15, L_calf=0.15): self.mass = mass self.g = 9.81 self.body_L = body_L self.body_W = body_W self.L_thigh = L_thigh self.L_calf = L_calf self.max_leg_reach = L_thigh + L_calf def assess_terrain(self, heightmap): if not heightmap: return {'type': 'flat', 'roughness': 0, 'slope': 0, 'max_step': 0} h_vals = [h for _, h in heightmap] h_mean = sum(h_vals)/len(h_vals) h_var = sum((h-h_mean)**2 for h in h_vals) / len(h_vals) if len(heightmap) >= 2: slope = (heightmap[-1][1] - heightmap[0][1]) / max(0.01, heightmap[-1][0] - heightmap[0][0]) else: slope = 0 max_step = max(abs(h_vals[i]-h_vals[i-1]) for i in range(1, len(h_vals))) if len(h_vals) > 1 else 0 roughness = math.sqrt(h_var) if roughness < 0.01 and abs(slope) < 0.1: ttype = 'flat' elif abs(slope) > 0.3: ttype = 'steep_slope' elif max_step > 0.05: ttype = 'stairs' elif roughness > 0.03: ttype = 'rough' else: ttype = 'mild_slope' return {'type': ttype, 'roughness': roughness, 'slope': slope, 'max_step': max_step} def plan_strategy(self, terrain_info): ttype = terrain_info['type'] if ttype == 'flat': return {'gait': 'trot', 'freq': 2.0, 'step_h': 0.04, 'step_l': 0.12, 'speed': 1.0} elif ttype == 'mild_slope': return {'gait': 'trot', 'freq': 1.5, 'step_h': 0.05, 'step_l': 0.10, 'speed': 0.6} elif ttype == 'steep_slope': return {'gait': 'walk', 'freq': 0.8, 'step_h': 0.06, 'step_l': 0.06, 'speed': 0.3} elif ttype == 'stairs': return {'gait': 'walk', 'freq': 0.6, 'step_h': 0.12, 'step_l': 0.05, 'speed': 0.2} else: # rough return {'gait': 'walk', 'freq': 1.0, 'step_h': 0.08, 'step_l': 0.08, 'speed': 0.4} def simulate_mission(self, terrain_segments): total_dist = 0 total_time = 0 total_energy = 0 results = [] for seg_name, heightmap, distance in terrain_segments: info = self.assess_terrain(heightmap) strategy = self.plan_strategy(info) actual_speed = strategy['speed'] time = distance / actual_speed energy = self.mass * self.g * strategy['step_h'] * strategy['freq'] * 4 * time total_dist += distance total_time += time total_energy += energy results.append((seg_name, info['type'], strategy, actual_speed, time, energy)) return results, total_dist, total_time, total_energy random.seed(42) atq = AllTerrainQuadruped() print("=" * 55) print(" All-Terrain Quadruped Final Project") print("=" * 55) # Terrain assessment print(f"\n [Terrain Assessment]") test_maps = { 'Flat road': [(x*0.05, 0) for x in range(20)], 'Gentle hill': [(x*0.05, 0.05*x) for x in range(20)], 'Rocky path': [(x*0.05, random.gauss(0, 0.03)) for x in range(20)], 'Stairs': [(x*0.05, 0.10*int(x/3)) for x in range(20)], 'Steep climb': [(x*0.05, 0.15*x) for x in range(20)], } for name, hmap in test_maps.items(): info = atq.assess_terrain(hmap) print(f" {name:15s}: type={info['type']:12s} rough={info['roughness']:.3f} " f"slope={info['slope']:.3f} max_step={info['max_step']:.3f}") # Mission simulation print(f"\n [Full Mission Simulation]") terrain_segs = [ ('Flat road', test_maps['Flat road'], 5.0), ('Rocky path', test_maps['Rocky path'], 2.0), ('Stairs', test_maps['Stairs'], 1.0), ('Gentle hill', test_maps['Gentle hill'], 3.0), ('Flat road', test_maps['Flat road'], 5.0), ] results, td, tt, te = atq.simulate_mission(terrain_segs) for name, ttype, strat, speed, time, energy in results: print(f" {name:12s} ({ttype:12s}): {strat['gait']:4s} @ {speed:.1f}m/s, " f"{time:.1f}s, {energy:.0f}J") print(f"\n TOTAL: {td:.1f}m in {tt:.1f}s, {te:.0f}J energy, avg_speed={td/tt:.2f}m/s") print() print(" OK - All-terrain quadruped simulation complete") print() print(" ============================================") print(" CONGRATULATIONS! Course completed!") print(" ============================================") print() print(" You have mastered:") print(" - Leg kinematics and gait generation") print(" - Dynamics, balance, and ZMP stability") print(" - Terrain perception and adaptation") print(" - MPC, RL, and intelligent control") print(" - Full 3D simulation and special maneuvers") print() print(" Next steps:") print(" - Implement on real hardware") print(" - Explore advanced RL (PPO, SAC)") print(" - Multi-robot coordination") print(" - Dynamic obstacle avoidance")