🤖 多地形策略切换

import math, random class TerrainStrategySwitcher: def __init__(self): self.current_strategy = 'flat_trot' self.confidence = 1.0 self.transition_time = 0 def classify_terrain_features(self, imu_vibration, foot_slip, slope_angle, roughness): features = { 'flat': 1.0 - roughness - abs(slope_angle)/0.5, 'rough': roughness * 2, 'slope': abs(slope_angle) * 3, 'stairs': 0.5 if imu_vibration > 0.3 and roughness < 0.3 else 0, 'slippery': foot_slip * 3, } best = max(features, key=features.get) return best, features[best] def get_strategy(self, terrain_type): strategies = { 'flat': {'gait': 'trot', 'freq': 2.0, 'step_h': 0.04, 'step_l': 0.12, 'speed': 1.0}, 'rough': {'gait': 'walk', 'freq': 1.0, 'step_h': 0.06, 'step_l': 0.08, 'speed': 0.5}, 'slope': {'gait': 'walk', 'freq': 0.8, 'step_h': 0.05, 'step_l': 0.06, 'speed': 0.3}, 'stairs': {'gait': 'walk', 'freq': 0.6, 'step_h': 0.10, 'step_l': 0.05, 'speed': 0.2}, 'slippery': {'gait': 'walk', 'freq': 0.8, 'step_h': 0.03, 'step_l': 0.06, 'speed': 0.4}, } return strategies.get(terrain_type, strategies['flat']) def should_switch(self, new_type, new_confidence): if new_type == self.current_strategy: return False if new_confidence > 0.6 and self.transition_time <= 0: return True return False def simulate_mission(self, terrain_sequence): results = [] for t, terrain_type, features in terrain_sequence: classified, conf = self.classify_terrain_features(**features) strategy = self.get_strategy(classified) if self.should_switch(classified, conf): old = self.current_strategy self.current_strategy = classified self.transition_time = 0.5 results.append((t, 'SWITCH', old, classified, strategy)) else: results.append((t, 'KEEP', self.current_strategy, classified, strategy)) self.transition_time = max(0, self.transition_time - 0.1) self.confidence = conf return results tss = TerrainStrategySwitcher() print("=" * 55) print(" Terrain Strategy Switching Simulation") print("=" * 55) # Terrain classification print("\n [Terrain Classification Test]") test_inputs = [ {'imu_vibration': 0.1, 'foot_slip': 0.05, 'slope_angle': 0, 'roughness': 0.05}, {'imu_vibration': 0.2, 'foot_slip': 0.1, 'slope_angle': 0.2, 'roughness': 0.3}, {'imu_vibration': 0.4, 'foot_slip': 0.05, 'slope_angle': 0, 'roughness': 0.2}, {'imu_vibration': 0.3, 'foot_slip': 0.5, 'slope_angle': 0.05, 'roughness': 0.1}, {'imu_vibration': 0.5, 'foot_slip': 0.1, 'slope_angle': 0.4, 'roughness': 0.1}, ] for features in test_inputs: classified, conf = tss.classify_terrain_features(**features) strategy = tss.get_strategy(classified) print(f" {features} -> {classified} (conf={conf:.2f}) -> {strategy['gait']} @ {strategy['speed']:.1f}m/s") # Mission simulation print(f"\n [Mission Simulation]") terrain_seq = [ (0.0, 'flat', {'imu_vibration': 0.1, 'foot_slip': 0.05, 'slope_angle': 0, 'roughness': 0.05}), (1.0, 'flat', {'imu_vibration': 0.1, 'foot_slip': 0.05, 'slope_angle': 0, 'roughness': 0.05}), (2.0, 'rough', {'imu_vibration': 0.3, 'foot_slip': 0.1, 'slope_angle': 0, 'roughness': 0.4}), (3.0, 'rough', {'imu_vibration': 0.3, 'foot_slip': 0.1, 'slope_angle': 0, 'roughness': 0.4}), (4.0, 'slope', {'imu_vibration': 0.2, 'foot_slip': 0.1, 'slope_angle': 0.3, 'roughness': 0.1}), (5.0, 'flat', {'imu_vibration': 0.1, 'foot_slip': 0.05, 'slope_angle': 0, 'roughness': 0.05}), ] results = tss.simulate_mission(terrain_seq) for t, action, current, detected, strategy in results: print(f" t={t:.1f}s {action}: {current} -> {detected} [{strategy['gait']} @ {strategy['speed']:.1f}m/s]") print() print(" OK - Strategy switching simulation complete")