import math, random
class TerrainPerception:
def __init__(self, grid_size=0.02, map_size=2.0):
self.grid_size = grid_size
self.map_size = map_size
self.n = int(map_size / grid_size)
self.heightmap = [[0.0]*self.n for _ in range(self.n)]
def generate_terrain(self, terrain_type='flat'):
for i in range(self.n):
for j in range(self.n):
x = i * self.grid_size - self.map_size/2
y = j * self.grid_size - self.map_size/2
if terrain_type == 'flat':
self.heightmap[i][j] = 0
elif terrain_type == 'slope':
self.heightmap[i][j] = 0.2 * x
elif terrain_type == 'stairs':
step = 0.03
self.heightmap[i][j] = step * int(x / 0.15) if x > 0 else 0
elif terrain_type == 'rough':
self.heightmap[i][j] = 0.01 * math.sin(5*x) * math.cos(7*y) + random.gauss(0, 0.005)
def get_height(self, x, y):
i = int((x + self.map_size/2) / self.grid_size)
j = int((y + self.map_size/2) / self.grid_size)
if 0 <= i < self.n and 0 <= j < self.n:
return self.heightmap[i][j]
return 0
def compute_gradient(self, x, y):
h = self.grid_size
hx1 = self.get_height(x+h, y)
hx0 = self.get_height(x-h, y)
hy1 = self.get_height(x, y+h)
hy0 = self.get_height(x, y-h)
dzdx = (hx1 - hx0) / (2*h)
dzdy = (hy1 - hy0) / (2*h)
return dzdx, dzdy
def classify_terrain(self, x, y, window=0.1):
heights = []
for dx in range(-5, 6):
for dy in range(-5, 6):
hx = self.get_height(x + dx*0.02, y + dy*0.02)
heights.append(hx)
if not heights:
return 'unknown', 0
h_mean = sum(heights)/len(heights)
h_var = sum((h-h_mean)**2 for h in heights) / len(heights)
max_grad = 0
for i in range(-3, 4):
dzdx, dzdy = self.compute_gradient(x + i*0.02, y + i*0.02)
max_grad = max(max_grad, math.sqrt(dzdx**2 + dzdy**2))
if h_var < 1e-4 and max_grad < 0.05:
return 'flat', h_var
elif max_grad < 0.3:
return 'rough', h_var
elif max_grad < 0.7:
return 'slope', max_grad
else:
return 'cliff', max_grad
def find_foothold(self, x, y, search_radius=0.15):
best_pos = (x, y)
best_score = -1
for dx_i in range(-7, 8):
for dy_i in range(-7, 8):
fx = x + dx_i * 0.02
fy = y + dy_i * 0.02
dzdx, dzdy = self.compute_gradient(fx, fy)
grad = math.sqrt(dzdx**2 + dzdy**2)
h = self.get_height(fx, fy)
# Score: flat + close + known height
flat_score = max(0, 1 - grad/0.3)
dist = math.sqrt((fx-x)**2 + (fy-y)**2)
dist_score = max(0, 1 - dist/search_radius)
score = 0.6 * flat_score + 0.4 * dist_score
if score > best_score:
best_score = score
best_pos = (fx, fy, h)
return best_pos, best_score
tp = TerrainPerception()
print("=" * 55)
print(" Terrain Perception Simulation")
print("=" * 55)
for ttype in ['flat', 'slope', 'stairs', 'rough']:
tp.generate_terrain(ttype)
print(f"\n [{ttype.upper()} terrain]")
test_pts = [(0, 0), (0.3, 0), (0.6, 0.3), (-0.3, 0.3)]
for x, y in test_pts:
h = tp.get_height(x, y)
dzdx, dzdy = tp.compute_gradient(x, y)
grad = math.sqrt(dzdx**2 + dzdy**2)
cls, metric = tp.classify_terrain(x, y)
print(f" ({x:.1f},{y:.1f}): h={h:.4f}m grad={grad:.3f} class={cls}")
# Foothold search
print(f"\n [Foothold Search on rough terrain]")
tp.generate_terrain('rough')
targets = [(0.2, 0.1), (-0.1, 0.3), (0.4, -0.2)]
for x, y in targets:
fh, score = tp.find_foothold(x, y)
print(f" Request ({x:.2f},{y:.2f}) -> foothold ({fh[0]:.3f},{fh[1]:.3f},{fh[2]:.4f}) score={score:.3f}")
print()
print(" OK - Terrain perception simulation complete")
仿真结果：
=======================================================
Terrain Perception Simulation
=======================================================
[FLAT terrain]
(0.0,0.0): h=0.0000m grad=0.000 class=flat
(0.3,0.0): h=0.0000m grad=0.000 class=flat
(0.6,0.3): h=0.0000m grad=0.000 class=flat
(-0.3,0.3): h=0.0000m grad=0.000 class=flat
[SLOPE terrain]
(0.0,0.0): h=0.0000m grad=0.200 class=rough
(0.3,0.0): h=0.0600m grad=0.200 class=slope
(0.6,0.3): h=0.1200m grad=0.200 class=rough
(-0.3,0.3): h=-0.0600m grad=0.300 class=slope
[STAIRS terrain]
(0.0,0.0): h=0.0000m grad=0.000 class=flat
(0.3,0.0): h=0.0600m grad=0.750 class=cliff
(0.6,0.3): h=0.1200m grad=0.750 class=cliff
(-0.3,0.3): h=0.0000m grad=0.000 class=flat
[ROUGH terrain]
(0.0,0.0): h=-0.0065m grad=0.084 class=slope
(0.3,0.0): h=0.0048m grad=0.118 class=slope
(0.6,0.3): h=0.0008m grad=0.156 class=rough
(-0.3,0.3): h=0.0102m grad=0.220 class=slope
[Foothold Search on rough terrain]
Request (0.20,0.10) -> foothold (0.200,0.100,0.0012) score=0.908
Request (-0.10,0.30) -> foothold (-0.060,0.280,-0.0064) score=0.812
Request (0.40,-0.20) -> foothold (0.320,-0.180,0.0116) score=0.733
OK - Terrain perception simulation complete