3. 考虑建筑物高度的三维航迹规划
任务说明
本案例针对城市低空环境，将建筑物高度信息融入三维栅格地图，实现考虑建筑物高度的三维航迹规划。输入为城市区域内的建筑物分布（含位置与高度），输出为从起点到终点、能够避开所有建筑物的三维航迹点序列。核心约束为：航迹在水平方向上不得穿越建筑物占地范围，在垂直方向上不得低于建筑物顶部高度（即航迹点高度需大于所覆盖栅格处的建筑物高度）。
三维栅格地图构建
将城市区域离散化为三维栅格，每个栅格用(x, y, z)索引表示，其中x、y为水平坐标，z为高度层索引。栅格状态分为两类：自由栅格（可通行）与障碍栅格（不可通行）。建筑物占据的水平栅格范围内，所有高度低于或等于建筑物高度的栅格均标记为障碍；高于建筑物顶部的栅格保持自由。航迹规划在三维栅格空间中进行，A*算法搜索从起点栅格到终点栅格的最优路径。
代码实现
import heapq
import math
class Building:
"""建筑物定义：中心坐标、底面半宽、高度"""
def __init__(self, cx, cy, half_width, height):
self.cx = cx
self.cy = cy
self.half_width = half_width
self.height = height
def contains(self, x, y):
"""判断水平坐标(x,y)是否在建筑物占地范围内"""
return (abs(x - self.cx) <= self.half_width and
abs(y - self.cy) <= self.half_width)
class GridMap3D:
"""三维栅格地图"""
def __init__(self, x_min, x_max, y_min, y_max, z_max, resolution):
self.x_min = x_min
self.x_max = x_max
self.y_min = y_min
self.y_max = y_max
self.z_max = z_max # 最大高度层索引（含）
self.res = resolution # 栅格边长（米）
self.x_size = int((x_max - x_min) / resolution) + 1
self.y_size = int((y_max - y_min) / resolution) + 1
self.z_size = z_max + 1
# 障碍标记：True表示该栅格不可通行
self.obstacle = [[[False for _ in range(self.z_size)]
for _ in range(self.y_size)]
for _ in range(self.x_size)]
def world_to_grid(self, wx, wy, wz):
"""世界坐标转栅格索引"""
gx = int((wx - self.x_min) / self.res)
gy = int((wy - self.y_min) / self.res)
gz = int(wz / self.res)
return gx, gy, gz
def grid_to_world(self, gx, gy, gz):
"""栅格索引转世界坐标（取栅格中心）"""
wx = self.x_min + (gx + 0.5) * self.res
wy = self.y_min + (gy + 0.5) * self.res
wz = (gz + 0.5) * self.res
return wx, wy, wz
def add_buildings(self, buildings):
"""将建筑物写入地图：占地范围内低于或等于楼顶的栅格设为障碍"""
for b in buildings:
# 建筑物覆盖的栅格范围
gx0 = int((b.cx - b.half_width - self.x_min) / self.res)
gx1 = int((b.cx + b.half_width - self.x_min) / self.res)
gy0 = int((b.cy - b.half_width - self.y_min) / self.res)
gy1 = int((b.cy + b.half_width - self.y_min) / self.res)
gz_top = int(b.height / self.res)
for gx in range(max(0, gx0), min(self.x_size, gx1 + 1)):
for gy in range(max(0, gy0), min(self.y_size, gy1 + 1)):
for gz in range(0, min(self.z_size, gz_top + 1)):
self.obstacle[gx][gy][gz] = True
def is_free(self, gx, gy, gz):
"""判断栅格是否自由（在地图范围内且非障碍）"""
if (0 <= gx < self.x_size and 0 <= gy < self.y_size
and 0 <= gz < self.z_size):
return not self.obstacle[gx][gy][gz]
return False
def get_neighbors(self, gx, gy, gz):
"""获取26邻域中自由且在地图内的邻居栅格"""
neighbors = []
for dx in (-1, 0, 1):
for dy in (-1, 0, 1):
for dz in (-1, 0, 1):
if dx == 0 and dy == 0 and dz == 0:
continue
nx, ny, nz = gx + dx, gy + dy, gz + dz
if self.is_free(nx, ny, nz):
neighbors.append((nx, ny, nz))
return neighbors
def heuristic(a, b):
"""三维欧氏距离启发函数"""
return math.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2 + (a[2]-b[2])**2)
def astar_3d(grid, start_w, goal_w):
"""三维A*航迹规划，输入输出均为世界坐标"""
start = grid.world_to_grid(*start_w)
goal = grid.world_to_grid(*goal_w)
# 检查起终点合法性
if not grid.is_free(*start):
raise ValueError("起点位于障碍物内或地图外")
if not grid.is_free(*goal):
raise ValueError("终点位于障碍物内或地图外")
open_set = []
heapq.heappush(open_set, (0.0, start))
came_from = {}
g_score = {start: 0.0}
f_score = {start: heuristic(start, goal)}
while open_set:
_, current = heapq.heappop(open_set)
if current == goal:
# 回溯路径
path = []
while current in came_from:
path.append(current)
current = came_from[current]
path.append(start)
path.reverse()
# 转为世界坐标
return [grid.grid_to_world(*p) for p in path]
for neighbor in grid.get_neighbors(*current):
# 移动代价：直线移动为1，对角移动为sqrt(2)或sqrt(3)
dx = neighbor[0] - current[0]
dy = neighbor[1] - current[1]
dz = neighbor[2] - current[2]
move_cost = math.sqrt(dx*dx + dy*dy + dz*dz)
tentative_g = g_score[current] + move_cost
if tentative_g < g_score.get(neighbor, float('inf')):
came_from[neighbor] = current
g_score[neighbor] = tentative_g
f = tentative_g + heuristic(neighbor, goal)
f_score[neighbor] = f
heapq.heappush(open_set, (f, neighbor))
return None # 无可行路径
# ========== 示例：构建地图并规划 ==========
if __name__ == "__main__":
# 地图范围：100m x 100m，最大高度50m，栅格分辨率1m
grid = GridMap3D(0, 100, 0, 100, 50, 1.0)
# 定义三栋建筑物
buildings = [
Building(30, 30, 5, 20), # 中心(30,30)，半宽5m，高20m
Building(60, 45, 8, 35), # 中心(60,45)，半宽8m，高35m
Building(40, 70, 6, 15), # 中心(40,70)，半宽6m，高15m
]
grid.add_buildings(buildings)
# 起点与终点（世界坐标）
start_w = (5.0, 5.0, 5.0) # 低空起点
goal_w = (90.0, 90.0, 30.0) # 目标点
path = astar_3d(grid, start_w, goal_w)
if path:
print(f"规划成功，航迹点数量：{len(path)}")
for i, wp in enumerate(path):
print(f"点{i}: ({wp[0]:.1f}, {wp[1]:.1f}, {wp[2]:.1f})")
else:
print("未找到可行航迹")关键实现说明
- 障碍判定逻辑：
add_buildings方法将建筑物水平占地范围内、高度不超过楼顶的栅格全部标记为障碍。因此航迹在水平方向不会穿越建筑，在垂直方向不会低于楼顶高度。 - 三维搜索空间：A*算法在三维栅格中搜索，每个节点可向26个邻域方向移动，移动代价按实际欧氏距离计算，保证路径在三维空间中平滑且最短。
- 起终点合法性检查：若起点或终点落在障碍栅格内，直接抛出异常，避免无效搜索。
运行与验证
运行方式：将代码保存为plan_3d.py，在Python 3环境中执行python plan_3d.py。代码仅依赖Python标准库，无需额外安装包。
验证方法：观察输出航迹点序列，逐点检查以下条件：
- 每个航迹点对应的栅格均为自由栅格（即不在任何建筑物内部或下方）；
- 航迹点从起点(5,5,5)连续变化至终点(90,90,30)，相邻点间距不超过栅格对角距离（约1.73m）；
- 若输出“规划成功”，说明存在可行路径；若输出“未找到可行航迹”，说明当前起终点间无满足避障条件的路径。
判定标准：程序正常输出航迹点序列且无异常抛出，且人工检查各点均满足上述避障条件，即可判定案例实现正确。