3. 地形约束建模方法
航迹规划的本质，是在一个被各种地形条件限制的三维空间中寻找可行且较优的路径。要让规划算法“理解”地形，就必须把连续起伏的真实地表和人为划定的限制区域，转化为算法可计算的数学约束。本节围绕坡度、坡向、粗糙度、起伏度、障碍物高度、禁飞区/禁行区六类典型地形约束，说明它们的数学描述方式，并进一步区分硬约束与软约束，最后给出约束代价函数的设计思路。
一、从高程栅格到约束场
地形约束建模的起点通常是数字高程模型（DEM）。设某区域被离散为 M×N 的栅格，栅格 (i,j) 的中心高程为 h(i,j)，栅格分辨率为 Δ（单位：米）。无人系统的位置可表示为 (x, y, z)，其中 z 为离地高度或海拔高度，具体取决于坐标系约定。本节统一约定：h 为海拔高程，z 为飞行器海拔高度，离地高度 AGL = z − h。所有约束最终都作用在栅格或栅格间的连边上，形成一张“约束代价场”。
二、六类地形约束的数学描述
坡度（Slope）。坡度描述地表倾斜程度，是影响地面车辆通行和飞行器贴地飞行的关键因素。对栅格 (i,j)，用中心差分近似梯度：
gx = (h(i+1,j) − h(i−1,j)) / (2Δ)
gy = (h(i,j+1) − h(i,j−1)) / (2Δ)
slope = arctan( sqrt(gx² + gy²) )结果以度或弧度表示。坡度越大，地面车辆越难通行，飞行器贴地飞行时也越容易触发地形碰撞风险。
坡向（Aspect）。坡向是坡度方向在水平面上的投影方位角，定义为：
aspect = atan2(gy, gx)通常以正北为 0°、顺时针为正。坡向本身不直接决定可行性，但会影响光照、风场和爬升方向选择。例如，逆坡爬升比顺坡下降更耗能，因此在代价函数中可作为方向性惩罚项。
粗糙度（Roughness）。粗糙度衡量局部地表的起伏剧烈程度。常用做法是计算窗口内高程的标准差或最大最小差：
roughness(i,j) = max(h) − min(h) （窗口内）粗糙度大意味着地表破碎、障碍密集，适合作为软约束，提高该区域的通行代价。
起伏度（Relief）。起伏度描述一定范围内的高程变化幅度，常以窗口内最高与最低高程之差表示。它与粗糙度的区别在于：粗糙度关注局部微地形，起伏度关注更大尺度的地形落差。起伏度可用于评估航段是否需要大幅爬升或下降。
障碍物高度。障碍物包括建筑物、塔杆、树木等。对每个障碍物，记录其平面位置 (xo, yo)、底面高程 hb 和顶部高程 ht。飞行器在位置 (x, y, z) 处与障碍物的安全关系可写为：
若 (x−xo)² + (y−yo)² ≤ (R + ds)² 且 z ≤ ht + dv
则判定为障碍冲突其中 R 为障碍物等效半径，ds 为水平安全间隔，dv 为垂直安全间隔。
禁飞区/禁行区。禁飞区通常用多边形或圆柱体描述。对多边形禁飞区 P，点 (x,y) 是否在区内可用射线法或符号面积法判断；若在区内且高度低于限制高度 zmax，则视为侵入。禁行区同理，只是针对地面车辆。
三、硬约束与软约束的区分
硬约束是“不可违反”的条件，一旦违反，路径直接判定为不可行。典型硬约束包括：禁飞区侵入、障碍物碰撞、坡度超过车辆最大爬坡能力、离地高度低于最小安全高度等。在数学上，硬约束通常写成可行域形式：
g_k(x, y, z) ≤ 0, k = 1, 2, ..., K软约束则是“可以违反但需付出代价”的条件，例如粗糙度、起伏度、坡向不利、偏离期望航线等。软约束不直接排除路径，而是通过代价函数影响优化结果。区分二者的关键在于：违反后是否还存在物理上可执行的路径。若不存在，就是硬约束；若存在但代价更高，就是软约束。
四、约束代价函数设计
实际规划中，常把硬约束转化为大惩罚项，与软约束代价合并为总代价：
J = Σ M · I(g_k > 0) + Σ w_k · f_k其中 M 是远大于软约束代价的惩罚值，I(·) 为指示函数，w_k 为软约束权重，f_k 为归一化后的代价项。例如，坡度代价可写为 f_slope = max(0, slope − slope_max) / slope_max，粗糙度代价可写为 f_rough = roughness / roughness_ref。权重 w_k 的选取需要根据任务偏好调整：更看重安全性时，提高障碍物和禁飞区相关权重；更看重能耗时，提高坡度和起伏度权重。
五、一个可复用的约束建模流程
- 读取 DEM 与障碍物/禁飞区矢量数据，统一到同一坐标系与分辨率。
- 逐栅格计算坡度、坡向、粗糙度、起伏度，生成四张特征栅格。
- 对障碍物和禁飞区做膨胀处理，膨胀量等于安全间隔
ds与dv。 - 按硬约束/软约束分类，分别写入可行域判断函数与代价函数。
- 在规划器中对每条候选路径计算
J，先检查硬约束是否全部满足，再比较软约束代价。