1. 从竞赛题目到工程实践一次航迹规划模型的深度复盘去年我带着团队的小伙伴们把“华为杯”研究生数学建模竞赛2019年的F题也就是那个关于智能飞行器航迹规划模型的题目从头到尾、从模型到代码实实在在地做了一遍。这不仅仅是为了复现一篇优秀论文更是想搞清楚一个在竞赛中看起来逻辑严密的数学模型当它真正要落地成一行行代码去驱动一个虚拟的“飞行器”时会遇到哪些纸上谈兵时根本想不到的坑。今天我就把这次从“理论最优”到“工程可行”的完整过程以及其中积累的实战经验毫无保留地分享出来。无论你是正在备战数模竞赛的学生还是对路径规划算法感兴趣的工程师相信这些踩过的坑和总结的技巧都能让你少走很多弯路。这道题的核心是要求我们为飞行器在复杂三维地形中规划出一条从起点到终点的最优航迹。这个“最优”包含了多重约束要尽可能短经济性要保证飞行安全高度不撞山要满足飞行器的物理性能最大爬升/俯冲角、最小转弯半径还要尽可能平滑以减少能耗。题目给出了数字高程地图DEM数据本质上就是一张巨大的、包含每个点海拔高度的网格图。我们的任务就是在这张三维网格里找出一条满足所有约束的“最优路径”。这听起来很像经典的A*算法在三维空间的扩展但实际上约束的复杂性和三维空间的连续性让问题变得极具挑战性。接下来我将分步拆解我们是如何构建模型、选择算法、并用Python将其实现最终让规划出的航迹既能“算得出来”也能“飞得过去”。2. 问题本质与建模核心将连续空间离散化拿到题目第一件事不是急着找算法而是彻底理解问题的边界和本质。飞行器的航迹规划是一个连续空间优化问题理论上航迹可以是无限条曲线。但计算机无法处理无限我们必须将其离散化。这是整个建模的基石。2.1 环境建模三维网格世界的构建题目提供的DEM数据是一个矩阵每个元素代表一个地理栅格点的海拔高度。我们首先用Python的numpy将其加载进来。import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # 假设dem_data是一个二维numpy数组 dem_data np.loadtxt(terrain_data.txt) rows, cols dem_data.shape但这只是地形高度。为了规划航迹我们需要构建一个三维网格空间。一个直观的想法是将地理平面经纬度或XY坐标按固定分辨率如30米划分网格每个网格点的高度由DEM插值得到。那么一个网格点就可以用一个三维坐标(x, y, z)表示其中z是海拔高度加上一个安全裕度。注意这里有一个关键细节——安全飞行高度。题目要求飞行器不能低于某个安全高度飞行。这个安全高度不是常数它应该是(x, y)处的地形高度h_terrain(x, y)加上一个最小离地间隙例如150米。所以空间中任意一点的可飞行高度下限是H_min(x, y) h_terrain(x, y) 150。我们的航迹上所有点的高度z必须大于等于H_min。于是我们的搜索空间就从连续的三维空间变成了一个离散的三维网格图。每个节点是一个(x, y)位置而该节点的“状态”还包括了当前高度z。但z也是连续的怎么办一种方法是把高度也离散化但这会急剧增加搜索空间。更常用的方法是在搜索过程中将高度作为一个需要被约束和优化的连续变量来处理。2.2 航迹表示与约束的数学描述一条航迹可以看作是一系列有序的三维航路点P0, P1, ..., Pn的集合其中P0是起点Pn是终点。我们的目标是找到这样一条点列使得某个目标函数最优如总长度最短同时满足所有约束。核心约束的数学化高度安全约束对于航迹上任意点Pi(xi, yi, zi)有zi H_min(xi, yi)。这是硬约束必须满足。最大爬升/俯冲角约束飞行器物理限制。假设相邻航路点Pi和Pj之间的水平距离为L_h高度差为Δz则航迹段与水平面的夹角θ满足tanθ Δz / L_h。约束条件为|θ| θ_max例如15°。这保证了航迹不会过于陡峭。最小转弯半径约束这是最复杂的约束之一。在三维空间中转弯不仅涉及水平方向的偏航还可能伴随滚转。一个简化的方法是我们主要考虑在水平面上的投影轨迹的曲率。对于连续三个航路点Pi-1, Pi, Pi1可以计算它们在水平面上投影点所形成圆弧的半径R要求R R_min。这保证了飞行器能平滑转向。航迹平滑度/总长度约束目标函数通常是最小化总航程Σ distance(Pi, Pi1)。同时为了航迹平滑也可以加入惩罚项如对相邻航段方向变化的惩罚。经过这样的梳理问题就清晰了在一个带障碍地形的三维离散空间中寻找一条连接起点和终点的路径该路径需要满足一系列关于几何形状的约束并使得总成本最小。这是一个典型的约束路径规划问题。3. 算法选型为什么A*及其变种是更优解面对这样的问题初学者可能会想到动态规划、遗传算法、粒子群优化等。但在我们实际尝试和对比后基于图搜索的A*算法及其变种展现出了在效率和解的质量之间最好的平衡。下面详细说说为什么。3.1 经典A*算法在三维空间的直接应用与局限A*算法的核心是评估函数f(n) g(n) h(n)其中g(n)是从起点到节点n的实际代价h(n)是从节点n到终点的预估代价启发函数。在三维网格中我们可以定义每个网格中心为一个节点。从一个节点可以移动到其26邻域上、下、左、右、前、后及所有对角线方向的节点。移动代价g通常就是欧几里得距离。启发函数h可以使用到终点的欧几里得距离这在三维空间是可采纳的永远不会高估实际代价能保证A*找到最优解。直接应用的巨大问题维度灾难即使一个100x100的地理网格在高度上再离散成10层就是10万个节点。26邻域搜索会让状态空间爆炸。约束处理困难A*的标准版本很难优雅地融入转弯半径、爬升角等复杂约束。你需要在扩展节点时检查当前路径片段是否满足所有约束这非常繁琐且容易导致大量无效搜索。节点定义过于简单一个网格位置(x, y, z)不足以描述飞行器的状态。例如从不同方向到达(x, y, z)点其“状态”是不同的因为它影响了后续转弯的可能性。这引出了状态空间搜索的概念。3.2 升级策略状态空间A*与运动基元为了解决上述问题我们采用了状态空间A* 结合运动基元的方法。这是将A*应用于机器人路径规划特别是无人机路径规划的常见且有效的方法。核心思想状态扩展不再把节点定义为单纯的位置(x, y, z)而是定义为状态例如(x, y, z, ψ, θ)其中ψ是偏航角水平方向θ是俯仰角。这样状态包含了位置和姿态能更好地描述飞行器的瞬时运动情况。运动基元从一个状态S出发飞行器能执行的动作不是任意方向的移动而是一系列预定义的、符合其动力学模型的运动基元。例如“保持当前俯仰角和偏航角向前飞行距离d”、“向左偏航Δψ度同时爬升Δθ度飞行距离d”等。每个运动基元都是一小段可行的轨迹片段其本身已经满足了最大角速度、加速度等约束。在状态空间中搜索A*算法在状态空间图上运行。从一个状态节点通过应用所有可能的运动基元生成一系列新的后继状态节点。g(n)是累积的轨迹长度或时间、能耗h(n)是从当前状态(x,y,z)到终点位置的欧几里得距离忽略姿态。这种方法的好处约束内化转弯半径、爬升角等约束已经在设计运动基元时被满足了。搜索算法只需要在基元库中选取无需在线进行复杂的几何校验。解的质量高生成的路径天然就是动力学可行的、平滑的。搜索空间更精确虽然状态维度变高了5维 vs 3维但每个状态都更有意义无效扩展大大减少。我们的实现选择考虑到竞赛时间和代码复杂性我们做了一定简化。我们没有实现完整的5维状态空间而是采用了3.5维的近似节点是(x, y, z)但在扩展时我们记录了到达该节点的“父节点”和“祖父节点”。通过这三个点我们可以近似计算出当前航段的航向和俯仰角并以此来判断下一个候选节点是否满足转弯和爬升约束。这是一种在搜索效率和约束完整性之间的折中在实践中效果不错。4. 工程实现关键Python代码的模块化设计与核心函数剖析理论模型和算法思路清晰后工程实现就是下一个战场。我们把代码分成几个核心模块确保结构清晰易于调试和扩展。4.1 地形处理与安全走廊生成这是所有规划的基础。我们不仅要读入DEM还要生成一个安全高度图和一个障碍物地图。class TerrainManager: def __init__(self, dem_data, safety_margin150): self.dem dem_data self.safety_margin safety_margin self.safe_height_map self.dem safety_margin # 障碍物地图True表示不可通行如高度超过某个阈值或特殊区域 self.obstacle_map self._generate_obstacle_map() def get_min_safe_height(self, x, y): 通过双线性插值获取任意(x,y)点的最小安全高度 # 实现插值逻辑 pass def is_collision(self, point_3d): 检查一个三维点是否与地形或障碍物碰撞 x, y, z point_3d return z self.get_min_safe_height(x, y) def _generate_obstacle_map(self): # 例如可以标记出安全高度超过某个绝对上限的区域为障碍 # 或者根据题目要求标记出禁飞区 obstacle np.zeros_like(self.dem, dtypebool) # ... 具体的障碍生成逻辑 return obstacle实操心得一插值的精度与效率。在get_min_safe_height函数中如果对每个查询点都进行精确的双线性插值在A*搜索中会成为性能瓶颈每秒数百万次查询。我们的优化策略是预处理一个高分辨率的查找表。将原始DEM网格进行上采样例如用scipy.interpolate.griddata生成一个更精细的网格(x_fine, y_fine, H_min_fine)。在搜索时直接通过最近邻或简单的双线性插值在这个精细网格上查询速度会快几个数量级。这是用空间内存换时间的典型例子。4.2 航迹规划器核心状态空间A*的实现这是代码的心脏部分。我们实现了一个StateSpaceAStarPlanner类。import heapq from dataclasses import dataclass, field from typing import Tuple, List, Optional dataclass(orderTrue) class SearchNode: A*搜索算法中的节点 f_score: float # f g h g_score: float field(compareFalse) # 从起点到本节点的实际代价 state: Tuple[float, float, float] field(compareFalse) # (x, y, z) # 为了计算约束需要知道历史状态 parent_state: Optional[Tuple] field(compareFalse, defaultNone) parent_yaw: Optional[float] field(compareFalse, defaultNone) # 父节点的偏航角 parent_pitch: Optional[float] field(compareFalse, defaultNone) # 父节点的俯仰角 class StateSpaceAStarPlanner: def __init__(self, terrain_manager, max_pitch_deg15, min_turn_radius500): self.terrain terrain_manager self.max_pitch np.deg2rad(max_pitch_deg) self.min_turn_radius min_turn_radius # 定义运动基元 (delta_x, delta_y, delta_z, cost) # 这里简化为一组固定步长和方向的移动 self.motion_primitives self._generate_motion_primitives(step_size100) def plan(self, start_3d, goal_3d): open_set [] start_node SearchNode( f_scoreself._heuristic(start_3d, goal_3d), g_score0, statestart_3d, parent_stateNone ) heapq.heappush(open_set, start_node) came_from {} # 记录最优路径 g_score_map {start_3d: 0} # 到达每个状态的最佳g值 while open_set: current_node heapq.heappop(open_set) if self._is_close(current_node.state, goal_3d, tolerance50): return self._reconstruct_path(came_from, current_node.state) # 扩展当前节点 for motion in self.motion_primitives: new_state self._apply_motion(current_node.state, motion) # **关键检查1地形碰撞** if self.terrain.is_collision(new_state): continue # **关键检查2爬升/俯冲角约束** if not self._check_pitch_constraint(current_node.state, new_state): continue # **关键检查3转弯半径约束需要父节点信息** if current_node.parent_state is not None: if not self._check_turn_constraint(current_node.parent_state, current_node.state, new_state): continue tentative_g_score current_node.g_score motion.cost # 如果这个状态还没访问过或者找到了更优路径 if new_state not in g_score_map or tentative_g_score g_score_map[new_state]: g_score_map[new_state] tentative_g_score h self._heuristic(new_state, goal_3d) f tentative_g_score h new_node SearchNode( f_scoref, g_scoretentative_g_score, statenew_state, parent_statecurrent_node.state, parent_yawself._calc_yaw(current_node.parent_state, current_node.state) if current_node.parent_state else None, parent_pitchself._calc_pitch(current_node.parent_state, current_node.state) if current_node.parent_state else None ) heapq.heappush(open_set, new_node) came_from[new_state] current_node.state return None # 规划失败 def _check_pitch_constraint(self, from_state, to_state): dx to_state[0] - from_state[0] dy to_state[1] - from_state[1] dz to_state[2] - from_state[2] horizontal_dist np.sqrt(dx*dx dy*dy) if horizontal_dist 1e-6: # 避免除零 return True pitch np.arctan2(dz, horizontal_dist) return abs(pitch) self.max_pitch def _check_turn_constraint(self, prev_state, curr_state, next_state): # 计算水平投影点 p1 np.array([prev_state[0], prev_state[1]]) p2 np.array([curr_state[0], curr_state[1]]) p3 np.array([next_state[0], next_state[1]]) # 计算向量 v1 p2 - p1 v2 p3 - p2 if np.linalg.norm(v1) 1e-6 or np.linalg.norm(v2) 1e-6: return True # 计算转弯半径近似通过三点定圆或向量夹角 # 简化计算转向角通过最小转弯半径换算成最小转向角约束 angle self._angle_between(v1, v2) # 根据最小转弯半径和步长计算允许的最大转向角 # max_angle step_length / min_turn_radius (弧度制近似) max_angle self._motion_step_length / self.min_turn_radius return angle max_angle # ... 其他辅助函数_heuristic, _apply_motion, _reconstruct_path等实操心得二启发函数的设计与搜索效率。_heuristic函数即h(n)极大地影响A的搜索效率。对于三维空间欧几里得距离是标准的可采纳启发函数。但是如果地形起伏非常大从当前点到终点的直线距离可能远小于实际需要绕山而行的距离导致h(n)低估实际代价的程度很大A会倾向于扩展更多节点接近广度优先搜索。一个改进策略是使用加权A*f(n) g(n) ε * h(n)其中ε 1。这会让算法更“贪婪”地朝向目标显著加快搜索速度但代价是可能找不到最优解而是找到一个次优解。在竞赛或实际工程中ε取1.5到2.0常常能在速度和最优性之间取得很好的平衡。我们在代码中可以通过一个参数轻松切换。实操心得三状态判重的技巧。在状态空间搜索中g_score_map用于记录到达某个state的最佳代价。但我们的state是(x, y, z)是一个连续值。直接使用浮点数元组作为字典键进行相等比较是不可靠的因为计算误差。我们的做法是将连续空间离散化为一个固定分辨率的网格然后将state量化为网格索引(ix, iy, iz)用这个索引作为判重的键。这本质上是将连续状态空间再次离散化到一个更粗糙的“状态栅格”中。这个栅格的分辨率需要仔细选择太粗会丢失精度导致找不到可行路径太细则会让判重失效搜索空间膨胀。通常这个分辨率可以设定为运动基元步长的1/2到1/3。5. 从路径到航迹后处理与平滑优化A*搜索出来的路径是一系列离散的航路点。这条“原生路径”往往存在两个问题1) 由于离散网格和运动基元的限制路径是折线状的不够平滑2) 可能紧贴着安全高度飞行虽然安全但不够“优雅”抗扰动能力差。因此后处理至关重要。5.1 关键点提取与B样条平滑我们不需要对所有路径点进行平滑那样计算量大且容易破坏约束。一个标准的流程是Douglas-Peucker算法抽稀保留路径的主要转折点去除冗余的共线点。这能显著减少后续处理的点数。B样条曲线拟合使用三次B样条曲线对抽稀后的关键点进行拟合。B样条具有局部支撑性和凸包性质非常适合路径平滑。from scipy.interpolate import splprep, splev import numpy as np def smooth_path_with_bspline(path_points, s0.5): 使用B样条平滑路径。 path_points: (n, 3)的路径点数组 s: 平滑因子。s0要求曲线通过所有点s越大平滑度越高。 # 确保点数足够 if len(path_points) 4: return path_points # 分离坐标 x path_points[:, 0] y path_points[:, 1] z path_points[:, 2] # 拟合B样条 # 注意参数u是累积弦长能更好地处理点分布不均的情况 tck, u splprep([x, y, z], ss) # 在原始参数u上生成更密的点得到平滑路径 u_new np.linspace(u.min(), u.max(), len(path_points)*2) x_new, y_new, z_new splev(u_new, tck) smoothed_path np.column_stack((x_new, y_new, z_new)) return smoothed_path平滑带来的新问题平滑后的曲线点可能不再严格满足安全高度约束因为B样条是数学拟合拟合点可能比原始点更低。因此平滑后必须重新进行碰撞检测。我们的策略是在平滑后的路径上密集采样检查每个采样点的高度是否大于当地安全高度。如果发生碰撞有两种处理方式局部调整将碰撞点附近的一段B样条控制点向上移动重新拟合。迭代平滑增加平滑因子s让曲线更“松驰”但可能偏离原始路径更多。安全走廊膨胀在最初生成安全高度时就额外增加一个“平滑裕度”为后续的平滑操作留出空间。这是最有效的一劳永逸的方法。5.2 动力学可行性复查与速度剖面生成对于智能飞行器尤其是无人机规划出的几何路径还需要考虑动力学可行性即飞行器能否以自身的能力最大加速度、最大角速度准确地跟踪这条路径。一个简化的复查方法是计算路径的曲率和挠率。对于参数化后的平滑路径r(s) (x(s), y(s), z(s))其曲率κ(s)反映了路径转弯的剧烈程度挠率τ(s)反映了路径扭曲的程度。要求整条路径上κ(s) κ_max由最小转弯半径决定τ(s) τ_max。如果复查不通过可能需要进一步平滑或者接受一条更长的、但更平缓的路径。最后根据路径和飞行器的性能最大速度、最大加速度可以生成一条速度剖面。例如在转弯半径小的地方降低速度在直线段加速。这属于轨迹规划的范畴比单纯的路径规划更进一步。在竞赛层面如果能提到这一点并给出简单设计如根据曲率线性缩放速度会是很大的加分项。6. 可视化与调试让问题无处遁形在开发过程中强大的可视化工具能帮你节省大量调试时间。我们主要使用matplotlib的3D绘图功能。def visualize_planning(terrain_manager, raw_path, smoothed_path, start, goal): fig plt.figure(figsize(15, 10)) ax fig.add_subplot(111, projection3d) # 1. 绘制地形 X, Y np.meshgrid(np.arange(terrain_manager.dem.shape[1]), np.arange(terrain_manager.dem.shape[0])) surf ax.plot_surface(X, Y, terrain_manager.dem, cmapterrain, alpha0.5, linewidth0, antialiasedTrue) # 2. 绘制安全高度面半透明 surf_safe ax.plot_surface(X, Y, terrain_manager.safe_height_map, cmapwinter, alpha0.3) # 3. 绘制原始路径红色虚线 if raw_path is not None: raw_path np.array(raw_path) ax.plot(raw_path[:,0], raw_path[:,1], raw_path[:,2], r--, linewidth2, labelRaw A* Path) # 4. 绘制平滑后路径绿色实线 if smoothed_path is not None: smoothed_path np.array(smoothed_path) ax.plot(smoothed_path[:,0], smoothed_path[:,1], smoothed_path[:,2], g-, linewidth3, labelSmoothed Path (B-spline)) # 5. 绘制起点和终点 ax.scatter(start[0], start[1], start[2], cblue, s100, markero, labelStart) ax.scatter(goal[0], goal[1], goal[2], cred, s100, marker^, labelGoal) ax.set_xlabel(X (m)) ax.set_ylabel(Y (m)) ax.set_zlabel(Altitude (m)) ax.legend() plt.title(3D Flight Path Planning Result) plt.show()实操心得四调试时把中间状态都画出来。不要只画最终路径。在A*搜索过程中可以把open_set和closed_set中的节点用浅色点云画出来这能直观地看到算法的搜索过程是盲目搜索还是直奔目标。把被碰撞检测拒绝的候选点用红色x标出能看到地形约束是如何起作用的。把违反转弯约束的点用黄色标出。这种可视化能帮你快速定位算法逻辑错误或参数设置不合理的地方。例如如果你发现搜索点云完全偏离了目标方向很可能是启发函数h(n)出了问题如果路径在某个区域反复震荡可能是运动基元步长设置不当或约束检查太严。7. 性能优化与高级技巧探讨当地图变大、约束变复杂时基础的A*搜索可能会非常慢。以下是我们尝试过且有效的优化手段双向搜索同时从起点和终点开始搜索直到两边的搜索区域相遇。这能显著减少搜索空间尤其是在起点和终点距离很远时。Jump Point Search (JPS) 的启发JPS是用于网格地图的A*优化算法能跳过大量不必要的中间节点。虽然标准的JPS适用于二维均匀网格但其思想——识别“跳跃点”——可以借鉴。在三维中我们可以定义在主要方向x, y, z上的跳跃规则当路径没有障碍且满足约束时直接跳到下一个关键转折点。任何时间搜索A*必须搜索到终点才能返回路径。Anytime A*算法允许你在任何时间点中断搜索并返回当前找到的最佳路径可能不是最优的。随着时间增加路径会不断被优化。这对于有实时性要求的场景非常有用。使用更高效的数据结构Python内置的heapq对于中小规模问题足够。但对于超大规模问题可以考虑使用更快的优先队列库如heapdict。此外g_score_map和came_from使用字典键是量化后的状态索引这比用元组快。并行化探索A*算法本身是顺序的但扩展节点时的碰撞检测、约束检查等操作可以并行化。利用Python的multiprocessing或concurrent.futures模块将待扩展的一批节点分发给多个进程进行可行性校验能有效利用多核CPU。最后我想强调的是数学建模竞赛中的“实现”和工业级的“实现”要求是不同的。竞赛中你需要在有限时间内给出一个概念清晰、逻辑正确、结果合理的解决方案和代码。这意味着你可以做一些合理的简化比如我们的3.5维状态空间可以使用一些启发式策略如加权A*最重要的是整个流程的完整性和可复现性。你的代码应该结构清晰注释完整关键参数易于调整并且附上一份说明文档解释每个模块的作用和算法的局限性。这份完整的、可运行的代码连同你对模型深入的思考和这些工程实现上的细节处理才是让你从众多参赛者中脱颖而出的关键。