import math
class AdaptiveGaitController:
def __init__(self, mass=6.2):
self.mass = mass
self.g = 9.81
def select_gait_params(self, terrain_roughness, speed_desired, slope_angle=0):
if terrain_roughness > 0.5:
gait = 'walk'
duty = 0.80
freq = 0.8
step_h = 0.08
elif terrain_roughness > 0.2:
gait = 'walk'
duty = 0.70
freq = 1.2
step_h = 0.05
elif speed_desired > 1.5:
gait = 'trot'
duty = 0.50
freq = 2.5
step_h = 0.06
else:
gait = 'trot'
duty = 0.55
freq = 2.0
step_h = 0.04
step_length = speed_desired / freq
if slope_angle > 0.2:
step_h *= 1.3
step_length *= 0.8
freq *= 0.9
return {'gait': gait, 'duty': duty, 'freq': freq,
'step_h': step_h, 'step_l': step_length}
def compute_energy_cost(self, params, distance=1.0):
speed = params['step_l'] * params['freq']
time = distance / speed
n_steps = int(time * params['freq'] * 4)
F = self.mass * self.g
cost_per_step = F * params['step_h'] * 2 # lift + lower
total_energy = n_steps * cost_per_step
cost_of_transport = total_energy / (self.mass * self.g * distance)
return total_energy, cost_of_transport
def simulate_adaptive_walk(self, segments):
total_dist = 0
total_time = 0
total_energy = 0
for roughness, speed_target, slope, dist in segments:
params = self.select_gait_params(roughness, speed_target, slope)
actual_speed = params['step_l'] * params['freq']
time = dist / actual_speed
energy, cot = self.compute_energy_cost(params, dist)
total_dist += dist
total_time += time
total_energy += energy
return total_dist, total_time, total_energy
agc = AdaptiveGaitController()
print("=" * 55)
print(" Adaptive Gait Control Simulation")
print("=" * 55)
# Gait selection
print("\n [Gait Selection]")
for rough, speed, slope in [(0.1, 1.0, 0), (0.3, 0.5, 0), (0.7, 0.3, 0),
(0.1, 2.0, 0), (0.2, 1.0, 0.3), (0.5, 0.5, 0.2)]:
params = agc.select_gait_params(rough, speed, slope)
print(f" rough={rough:.1f} speed={speed:.1f} slope={slope:.1f}: "
f"{params['gait']} freq={params['freq']:.1f}Hz step_l={params['step_l']*1000:.0f}mm")
# Energy comparison
print(f"\n [Energy Cost Comparison]")
for gait_name, params in [('Walk-slow', {'gait':'walk','duty':0.8,'freq':0.8,'step_h':0.08,'step_l':0.06}),
('Walk-fast', {'gait':'walk','duty':0.7,'freq':1.2,'step_h':0.05,'step_l':0.10}),
('Trot-slow', {'gait':'trot','duty':0.55,'freq':2.0,'step_h':0.04,'step_l':0.10}),
('Trot-fast', {'gait':'trot','duty':0.5,'freq':2.5,'step_h':0.06,'step_l':0.15})]:
energy, cot = agc.compute_energy_cost(params)
speed = params['step_l'] * params['freq']
print(f" {gait_name:12s}: speed={speed:.2f}m/s, energy={energy:.1f}J, CoT={cot:.4f}")
# Adaptive walk simulation
print(f"\n [Adaptive Walk - Mixed Terrain]")
segments = [
(0.1, 1.5, 0, 2.0), # flat, fast
(0.4, 0.8, 0.1, 1.0), # rough, medium
(0.7, 0.4, 0.2, 0.5), # very rough, slow
(0.1, 2.0, 0, 3.0), # flat, fast
]
dist, time, energy = agc.simulate_adaptive_walk(segments)
print(f" Total: {dist:.1f}m in {time:.1f}s, energy={energy:.1f}J, avg_speed={dist/time:.2f}m/s")
print()
print(" OK - Adaptive gait simulation complete")
仿真结果：
=======================================================
Adaptive Gait Control Simulation
=======================================================
[Gait Selection]
rough=0.1 speed=1.0 slope=0.0: trot freq=2.0Hz step_l=500mm
rough=0.3 speed=0.5 slope=0.0: walk freq=1.2Hz step_l=417mm
rough=0.7 speed=0.3 slope=0.0: walk freq=0.8Hz step_l=375mm
rough=0.1 speed=2.0 slope=0.0: trot freq=2.5Hz step_l=800mm
rough=0.2 speed=1.0 slope=0.3: trot freq=1.8Hz step_l=400mm
rough=0.5 speed=0.5 slope=0.2: walk freq=1.2Hz step_l=417mm
[Energy Cost Comparison]
Walk-slow : speed=0.05m/s, energy=642.3J, CoT=10.5600
Walk-fast : speed=0.12m/s, energy=243.3J, CoT=4.0000
Trot-slow : speed=0.20m/s, energy=194.6J, CoT=3.2000
Trot-fast : speed=0.38m/s, energy=189.8J, CoT=3.1200
[Adaptive Walk - Mixed Terrain]
Total: 6.5m in 5.3s, energy=233.6J, avg_speed=1.22m/s
OK - Adaptive gait simulation complete