1. 城市低空航迹规划基础环境建模

任务说明

本案例构建一个可供航迹规划算法直接调用的城市低空环境模型，包含三类核心要素：建筑物（三维盒体）、禁飞区（水平多边形+高度范围）、地形起伏（规则网格高程）。输出为统一的环境查询接口，支持后续航迹规划案例的空间碰撞检测与代价计算。

环境模型数据结构

采用面向对象方式定义环境要素，所有坐标使用局部笛卡尔坐标系（单位：米），高程基准为平均海平面。建筑物用轴对齐包围盒表示；禁飞区用水平多边形顶点序列加高度区间表示；地形用规则网格存储高程值。

import numpy as np
from typing import List, Tuple

class Building:
    """建筑物：轴对齐包围盒，坐标范围 [xmin,xmax]×[ymin,ymax]×[zmin,zmax]"""
    def __init__(self, xmin, xmax, ymin, ymax, zmin, zmax):
        self.xmin, self.xmax = xmin, xmax
        self.ymin, self.ymax = ymin, ymax
        self.zmin, self.zmax = zmin, zmax

    def contains_point(self, x, y, z):
        """判断点是否在建筑物内部（含边界）"""
        return (self.xmin <= x <= self.xmax and
                self.ymin <= y <= self.ymax and
                self.zmin <= z <= self.zmax)

class NoFlyZone:
    """禁飞区：水平多边形（顶点按逆时针排列）+ 高度范围 [zmin, zmax]"""
    def __init__(self, polygon_xy: List[Tuple[float, float]], zmin, zmax):
        self.polygon = polygon_xy
        self.zmin, self.zmax = zmin, zmax

    def contains_point(self, x, y, z):
        """射线法判断水平点是否在多边形内，且高度在禁飞范围内"""
        if not (self.zmin <= z <= self.zmax):
            return False
        inside = False
        n = len(self.polygon)
        j = n - 1
        for i in range(n):
            xi, yi = self.polygon[i]
            xj, yj = self.polygon[j]
            if ((yi > y) != (yj > y)) and (x < (xj - xi) * (y - yi) / (yj - yi) + xi):
                inside = not inside
            j = i
        return inside

class TerrainGrid:
    """地形网格：规则网格存储高程，原点在西南角，网格间距为 cell_size"""
    def __init__(self, origin_x, origin_y, cell_size, elevation_matrix: np.ndarray):
        self.ox, self.oy = origin_x, origin_y
        self.cell_size = cell_size
        self.elev = np.asarray(elevation_matrix, dtype=float)
        self.rows, self.cols = self.elev.shape

    def elevation_at(self, x, y):
        """双线性插值获取任意坐标处的地形高程"""
        # 网格坐标
        gx = (x - self.ox) / self.cell_size
        gy = (y - self.oy) / self.cell_size
        # 边界钳制
        gx = min(max(gx, 0), self.cols - 1.001)
        gy = min(max(gy, 0), self.rows - 1.001)
        x0, y0 = int(np.floor(gx)), int(np.floor(gy))
        x1, y1 = min(x0 + 1, self.cols - 1), min(y0 + 1, self.rows - 1)
        # 双线性权重
        wx, wy = gx - x0, gy - y0
        h00 = self.elev[y0, x0]
        h10 = self.elev[y0, x1]
        h01 = self.elev[y1, x0]
        h11 = self.elev[y1, x1]
        return (h00 * (1 - wx) * (1 - wy) +
                h10 * wx * (1 - wy) +
                h01 * (1 - wx) * wy +
                h11 * wx * wy)

class UrbanAirspace:
    """城市低空环境聚合模型：统一查询入口"""
    def __init__(self, buildings=None, noflyzones=None, terrain=None):
        self.buildings = buildings if buildings else []
        self.noflyzones = noflyzones if noflyzones else []
        self.terrain = terrain

    def is_obstacle(self, x, y, z):
        """判断点是否处于障碍状态（建筑物内部或禁飞区内）"""
        for b in self.buildings:
            if b.contains_point(x, y, z):
                return True
        for nf in self.noflyzones:
            if nf.contains_point(x, y, z):
                return True
        return False

    def ground_elevation(self, x, y):
        """获取地形高程，无地形时返回0"""
        if self.terrain is None:
            return 0.0
        return self.terrain.elevation_at(x, y)

    def is_collision_free(self, x, y, z, safety_margin=1.0):
        """航迹点安全检测：高于地面安全余量且不在障碍内"""
        if z < self.ground_elevation(x, y) + safety_margin:
            return False
        return not self.is_obstacle(x, y, z)

样例环境构建与验证

以下代码构建一个500m×500m范围的示例环境：包含两栋建筑物、一个多边形禁飞区以及一块带起伏的地形网格。随后对若干关键坐标点进行安全检测，验证模型查询功能。

# 构建示例环境
buildings = [
    Building(100, 150, 100, 160, 0, 80),    # 建筑A：80m高
    Building(300, 360, 250, 310, 0, 120),   # 建筑B：120m高
]

# 禁飞区：L形多边形，高度0~200m
nf_polygon = [(200, 200), (280, 200), (280, 260), (240, 260), (240, 230), (200, 230)]
nofly = NoFlyZone(nf_polygon, 0, 200)

# 地形：10m网格，5×5高程矩阵（单位m）
elev_data = np.array([
    [0, 2, 5, 8, 10],
    [1, 3, 6, 9, 12],
    [2, 5, 8, 11, 14],
    [3, 6, 10, 13, 16],
    [5, 8, 12, 15, 18],
])
terrain = TerrainGrid(origin_x=0, origin_y=0, cell_size=10, elevation_matrix=elev_data)

# 聚合模型
airspace = UrbanAirspace(buildings=buildings, noflyzones=[nofly], terrain=terrain)

# 验证点检测
test_points = [
    (120, 130, 50),    # 建筑A内部 → 障碍
    (330, 280, 100),   # 建筑B内部 → 障碍
    (250, 220, 100),   # 禁飞区内部 → 障碍
    (50, 50, 30),      # 空旷区域，高于地形 → 安全
    (50, 50, 5),       # 低于地形+安全余量 → 不安全
    (450, 450, 40),    # 远处空旷区域 → 安全
]

for pt in test_points:
    x, y, z = pt
    safe = airspace.is_collision_free(x, y, z, safety_margin=1.0)
    elev = airspace.ground_elevation(x, y)
    print(f"点({x},{y},{z}) 地面高程={elev:.1f}m → {'安全' if safe else '障碍/不安全'}")

预期输出

点(120,130,50) 地面高程=6.0m → 障碍/不安全
点(330,280,100) 地面高程=13.0m → 障碍/不安全
点(250,220,100) 地面高程=9.0m → 障碍/不安全
点(50,50,30) 地面高程=1.0m → 安全
点(50,50,5) 地面高程=1.0m → 障碍/不安全
点(450,450,40) 地面高程=18.0m → 安全

运行与验证方法

将上述代码保存为 urban_airspace.py，在安装有NumPy的Python 3.8+环境中执行 python urban_airspace.py。程序输出各测试点的安全判定结果。判定标准：前三个点因位于建筑物或禁飞区内必须判定为“障碍/不安全”；点(50,50,5)因低于地面高程1m+安全余量1m必须判定为“障碍/不安全”；其余两点应判定为“安全”。若输出与预期一致，则环境建模与查询接口验证通过。

环境要素可视化

上图展示了本案例构建的示例环境布局。图中红/橙色矩形为建筑物，蓝色L形区域为禁飞区，背景渐变表示地形高程变化（西南低、东北高）。黄色标记点为障碍测试点，绿色标记点为安全测试点。航迹规划算法可通过 UrbanAirspace.is_collision_free(x, y, z) 接口对任意候选航迹点进行安全判定，并以 ground_elevation() 获取地形约束，从而在后续案例中实现基于该环境模型的航迹搜索与风险规避。

📚 课程目录