Python中无人机3D轨迹与禁飞区几何相交检测的高效实现问询

核心需求梳理 #

• 基于遗传编程生成含100个顶点、99条线段的无人机3D飞行轨迹，需完成数千轮迭代，每轮执行约200次碰撞检测评估
• 障碍物需支持凹面3D网格、曲面类型，障碍物预处理仅需执行一次，评估环节需优先保证速度
• 已排除的方案：凸包检测（仅支持凸形障碍物）、Shapely+Z轴范围的2.5D方案（3D场景适配性差）、大型3D库线面相交（担心额外开销）

低开销纯数学实现方案 #

1. 凹面体线段相交检测（适配网格/多面体） #

预处理步骤（仅执行一次）

• 将3D网格/多面体的每个面转换为平面方程：ax + by + cz + d = 0，同时存储每个面的顶点集合，用于后续面内点判断
• 统一每个面的法向量方向（如全部指向障碍物外部）
• 为每个障碍物计算轴对齐包围盒（AABB），存储最小/最大坐标值

实时检测逻辑（单线段）

线段表示为起点P0(x0,y0,z0)到终点P1(x1,y1,z1)，参数化方程：P(t) = P0 + t*(P1-P0), t∈[0,1]

• 第一步：快速排除不相交障碍物
  计算线段的轴对齐包围盒，与障碍物AABB做重叠判断，无重叠则直接跳过该障碍物
• 第二步：线段与平面相交判断
  将线段参数化方程代入平面方程，求解t值：
  t = -(a*x0 + b*y0 + c*z0 + d)/(a*(x1-x0)+b*(y1-y0)+c*(z1-z0))
  • 若分母绝对值小于极小值（如1e-8）：线段与平面平行，跳过该面
  • 若t不在[0,1]区间：交点在线段外部，跳过该面
• 第三步：交点是否在面的多边形内部
  采用3D转2D的射线法：
  1. 选择面法向量分量最小的坐标轴作为投影轴（避免投影后多边形重叠）
  2. 将交点和所有面顶点投影到对应2D平面
  3. 用2D射线法判断投影点是否在投影多边形内部（逻辑与Shapely的点-in-polygon一致）

2. 曲面障碍物检测（平衡精度与速度） #

• 若曲面为参数化类型（如NURBS），预处理时离散为高精度三角网格，复用上述网格检测逻辑
• 若需高精度检测，可直接使用线段与参数曲面的相交公式，但计算量较大，建议仅针对关键曲面使用

非Python加速方案 #

• 将核心碰撞检测逻辑用C/C++实现，编译为动态链接库，通过Python的ctypes或cffi调用，速度比纯Python快5-10倍
• 追求极致性能时，可将评估模块用CUDA实现，批量处理多线段碰撞检测

纯Python核心逻辑代码片段 #

import numpy as np

def plane_line_intersection(plane_eq, p0, p1):
    # plane_eq: (a, b, c, d) 对应 ax+by+cz+d=0
    a, b, c, d = plane_eq
    dir_vec = p1 - p0
    denom = a*dir_vec[0] + b*dir_vec[1] + c*dir_vec[2]
    if abs(denom) < 1e-8:
        return None  # 线段与平面平行，无交点
    t = -(a*p0[0] + b*p0[1] + c*p0[2] + d) / denom
    if t < 0 or t > 1:
        return None  # 交点在线段外
    return p0 + t*dir_vec

def point_in_polygon_2d(point, polygon):
    # 2D射线法判断点是否在多边形内
    x, y = point
    inside = False
    n = len(polygon)
    for i in range(n):
        p1 = polygon[i]
        p2 = polygon[(i+1)%n]
        if ((p1[1] > y) != (p2[1] > y)):
            x_intersect = (y - p1[1])*(p2[0]-p1[0])/(p2[1]-p1[1]) + p1[0]
            if x < x_intersect:
                inside = not inside
    return inside

def segment_obstacle_intersection(segment, obstacle):
    p0, p1 = segment
    # 先判断AABB是否重叠
    aabb_min, aabb_max = obstacle['aabb']
    seg_min = np.minimum(p0, p1)
    seg_max = np.maximum(p0, p1)
    if np.any(seg_max < aabb_min) or np.any(seg_min > aabb_max):
        return False
    # 遍历障碍物的每个面
    for face in obstacle['faces']:
        intersect = plane_line_intersection(face['plane'], p0, p1)
        if intersect is None:
            continue
        # 投影到2D平面
        proj_axis = face['proj_axis']
        proj_point = np.delete(intersect, proj_axis)
        proj_polygon = [np.delete(v, proj_axis) for v in face['vertices']]
        if point_in_polygon_2d(proj_point, proj_polygon):
            return True
    return False

内容的提问来源于stack exchange，提问作者Andre Geo