关键词：路径规划、A*、Hybrid A*、RRT*、Minimum Snap、B样条、ESDF、DWA、人工势场

做过飞控或者自主导航的同学大概都有这种感受：网上关于 A*、RRT 的文章一抓一大把，但真正把无人机从"栅格地图上的一条折线"飞到"平滑、可执行、满足动力学约束的轨迹"这一整条链路讲清楚的却不多。本文按照工业界主流的 前端搜索 + 后端优化 两段式框架，把无人机路径规划的原理、算法、代码和工程踩坑串一遍。

全文较长，建议先收藏再看。目录：

1. 路径规划到底在解什么问题
2. 环境建模：地图怎么表示
3. 前端：搜索类算法（Dijkstra / A* / JPS / Hybrid A* / D* Lite）
4. 前端：采样类算法（RRT / RRT* / Informed RRT* / BIT*）
5. 后端：轨迹优化（Minimum Snap / B样条 / 梯度优化）
6. 局部避障：APF / DWA / VO
7. 完整 C++ 实现：3D A* + 路径简化
8. 工程落地的十个坑
9. 算法选型建议

---

一、路径规划到底在解什么问题

先把概念分清楚，这是很多文章混为一谈的地方：

层级	名称	输出	典型频率
任务层	任务规划	航点序列、作业顺序	秒级 / 一次性
全局层	路径规划（Path Planning）	几何路径，只有位置没有时间	0.1~1 Hz
局部层	轨迹规划（Trajectory Planning）	带时间参数的 $p(t)$，可微	5~20 Hz
控制层	位置/姿态控制	电机转速	200~1000 Hz

路径（Path）是几何量，轨迹（Trajectory）是时间的函数。 A* 给你的是一串栅格中心点连成的折线，直接丢给控制器的结果是无人机在每个拐角急停、超调、姿态剧烈抖动——因为折线的一阶导数不连续，加速度在拐点处是无穷大。所以后端优化不是"锦上添花"，而是必需环节。

形式化地讲，我们要求解的是一个带约束的最优控制问题：

$$\underset{p(t)}{\min }{\int }_0^T{\Vert p^{(k)}(t)\Vert }^{2}dt+\rho T$$

$$\text{s.t.}p(0)=p_s,p(T)=p_g,p(t)\in \mathcal{F},\Vert \dot{p}\Vert \le v_{max},\Vert \ddot{p}\Vert \le a_{max}$$

其中 $\mathcal{F}$ 是自由空间（free space），$k$ 一般取 4（snap，即位置的四阶导数）。为什么是 4？后面讲微分平坦时会解释。

这个问题是非凸的（障碍物约束导致自由空间非凸），直接求解会陷入局部最优甚至无解。工程上的通用套路就是：

前端用离散搜索找到一条"拓扑正确"的初值路径 → 后端在这条路径的邻域内做连续优化。

---

二、环境建模：地图怎么表示

规划算法的性能上限，往往由地图结构决定。

2.1 占据栅格地图（Occupancy Grid）

把空间离散成边长 $r$ 的立方体，每个格子存占据概率。二维用 vector<vector<uint8_t>>，三维直接一维数组加索引换算：

inline int toAddress(int x, int y, int z) const {
    return x * grid_size_(1) * grid_size_(2) + y * grid_size_(2) + z;
}

优点是查询 $O(1)$、实现简单；缺点是内存随分辨率立方增长。10m×10m×5m 空间在 0.1m 分辨率下就是 50 万个格子，0.05m 分辨率则是 400 万。

膨胀（inflation）很关键：把无人机当质点处理的前提是障碍物已经按机体外接圆半径 + 安全余量膨胀过。膨胀半径给小了会撞，给大了会把狭窄通道堵死导致无解。

2.2 八叉树 / OctoMap

用八叉树对空旷区域做合并，同样精度下内存能省一个数量级，octomap 库是事实标准。代价是查询要遍历树，比数组慢几倍。适合大场景建图，实时规划时通常再转成局部栅格。

2.3 ESDF（欧氏符号距离场）

这是现代基于优化的规划器（Fast-Planner、EGO-Planner）的核心数据结构。每个栅格存的不是"占没占"，而是到最近障碍物的欧氏距离 $d(x)$。

它的价值在于：优化器需要梯度。碰撞代价函数可以写成

$$J_c=\sum _i\max (0,d_{thr}-d(p_i){)}^2$$

而 $\nabla d(x)$ 可以直接由 ESDF 差分得到，这就把"避障"变成了一个可微的、能塞进梯度下降的项。

ESDF 的高效构建用的是 Felzenszwalb 距离变换，三个维度依次做一维下包络扫描，复杂度 $O(n)$：

// 一维距离变换（下包络算法）核心
template <typename F_get_val, typename F_set_val>
void computeEDT(F_get_val f_get_val, F_set_val f_set_val, int start, int end) {
    int v[end - start + 1];
    double z[end - start + 2];
    int k = start;
    v[start] = start;
    z[start] = -std::numeric_limits<double>::max();
    z[start + 1] = std::numeric_limits<double>::max();

    for (int q = start + 1; q <= end; q++) {
        k++;
        double s;
        do {
            k--;
            s = ((f_get_val(q) + q * q) - (f_get_val(v[k]) + v[k] * v[k])) / (2 * q - 2 * v[k]);
        } while (s <= z[k]);
        k++;
        v[k] = q;
        z[k] = s;
        z[k + 1] = std::numeric_limits<double>::max();
    }

    k = start;
    for (int q = start; q <= end; q++) {
        while (z[k + 1] < q) k++;
        double val = (q - v[k]) * (q - v[k]) + f_get_val(v[k]);
        f_set_val(q, val);
    }
}

注意：EGO-Planner 的一大贡献就是不建全局 ESDF，而是在碰撞发生时局部生成排斥梯度，把建图开销砍掉了 70% 以上。如果你的机载算力紧张（比如 Jetson Nano 级别），这个思路值得抄。

2.4 拓扑图 / PRM

预先在自由空间随机撒点并连边，形成一张路网图。适合地图静态、需要反复查询多组起终点的场景（如仓储 AGV 调度）。无人机动态飞行场景用得少。

---

三、前端：搜索类算法

3.1 Dijkstra

从起点开始按代价一圈圈往外扩散，扩到终点为止。保证最优，但没有方向性，在三维空间里等于把整个球体都搜一遍，实时场景基本不用。

3.2 A*

在 Dijkstra 基础上加了启发项，代价函数：

$$f(n)=g(n)+h(n)$$

• $g(n)$：起点到 $n$ 的实际代价
• $h(n)$：$n$ 到终点的估计代价

$h$ 的选择直接决定性能，这是面试高频考点：

启发函数	表达式	适用
曼哈顿距离	$|dx|+|dy|+|dz|$	只能走 6 邻域（不可对角）
欧氏距离	$\sqrt{d{x}^2+d{y}^2+d{z}^2}$	任意方向，但偏保守、扩展节点多
对角距离(Diagonal)	见下方代码	26 邻域栅格的最优选择

3D 对角距离：设 $dx\ge dy\ge dz$（排序后），

$$h=\sqrt{3}dz+\sqrt{2}(dy-dz)+(dx-dy)$$

可采纳性（admissible）：$h(n)\le h^{\ast }(n)$ 时 A* 保证最优。如果乘上权重 $ϵ>1$ 变成 Weighted A*，搜索速度大幅提升，代价是路径长度不超过最优解的 $ϵ$ 倍——工程上 $ϵ$ 取 1.0~1.5 是很划算的交易。

另外加一个极小的 tie-breaker（如 $h\times (1+1/1000)$）可以避免同 $f$ 值节点大量堆积，实测能减少 30% 以上的扩展节点。

3.3 JPS（Jump Point Search）

针对均匀代价栅格的 A* 加速版。核心洞察：在空旷区域，很多路径是对称的（走"上→右"和"右→上"结果一样），A* 把这些等价路径全都展开了，纯属浪费。

JPS 通过"跳点"规则直接跳过这些对称节点，只在遇到强制邻居（forced neighbor，即障碍物导致路径必须转向的位置）时才产生新节点。在空旷大地图上比 A* 快 10 倍以上，且路径完全一致。

代价：只适用于均匀代价栅格（不能有"草地贵、公路便宜"这种权重），且实现复杂度明显高于 A*。三维 JPS 的邻居剪枝规则要分 26 种情况讨论。

3.4 Hybrid A*（混合 A*）

前面的算法都把无人机当成"可以原地任意转向的质点"。对于四旋翼在低速下这个假设还行，对固定翼、无人车、以及带最大速度/加速度约束的高速四旋翼就不成立了。

Hybrid A* 的做法是：状态从栅格索引变成连续状态，节点扩展从"走到相邻格子"变成"用一小段可行运动基元（motion primitive）前进 $\tau$ 时间"。

以四旋翼为例，取状态 $s=[p,v]$，控制量为常值加速度 $u=a$，则运动基元是：

$$p(\tau )=p_0+v_0\tau +\frac{1}{2}u{\tau }^2,v(\tau )=v_0+u\tau$$

对 $u$ 在 $[-a_{max},a_{max}{]}^3$ 上离散采样（比如每维取 -1/0/1 三档，共 27 种），每次扩展生成 27 条短弧线。这些弧线天然满足动力学，搜出来的路径直接就是可飞的。

边代价包含控制量和时间：$e=(\Vert u{\Vert }^2+\rho )\tau$。

启发函数则可以用无障碍情况下的最优控制解（庞特里亚金极小值原理求解的两点边值问题），这比欧氏距离紧得多，能大幅减少扩展。Fast-Planner 的 Kinodynamic A* 就是这个路子。

代价：状态维度从 3 升到 6，需要对 $[p,v]$ 联合离散化做去重（同一格子里速度差别大的状态不能当成同一个节点）。

3.5 D* Lite / 增量式重规划

地图动态变化时，从头跑 A* 很浪费。D* Lite 从终点向起点反向搜索并维护 rhs 值，当局部地图更新时只重算受影响的分支，重规划耗时能降低一个数量级。

不过说实话，在无人机领域 D* Lite 的实际使用率并不高。原因是四旋翼的局部地图更新非常频繁（10~30Hz），而局部规划范围通常只有 5~10 米，直接重跑一遍 A* 也就 1~5 ms，维护增量结构的复杂度收益不明显。D Lite 更适合大地图、低频更新的地面机器人。*

---

四、前端：采样类算法

4.1 RRT（快速扩展随机树）

初始化：树 T 只含起点
循环 N 次：
    x_rand ← 在空间中随机采样（以概率 p 直接采样终点，goal bias）
    x_near ← T 中距 x_rand 最近的节点
    x_new  ← 从 x_near 朝 x_rand 前进步长 step
    若 x_near→x_new 无碰撞：将 x_new 加入 T
    若 x_new 距终点 < 阈值：连接并返回路径

优点：概率完备、天然适应高维空间（机械臂 7 自由度以上基本只能用采样法）、不需要显式建图，只需要一个碰撞检测函数。

缺点：路径质量极差，弯弯绕绕像喝醉了；结果不确定，两次运行结果不同——这对需要复现和标定的工程系统是个麻烦。

4.2 RRT*

在 RRT 基础上加了两个操作，使其渐近最优：

1. ChooseParent：新节点不直接连最近点，而是在半径 $r$ 邻域内选一个使 $cost$ 最小的父节点
2. Rewire：检查邻域内其它节点，如果经由新节点到达它们代价更低，就改接父节点

邻域半径按 $r=\gamma (\log n/n{)}^{1/d}$ 衰减（$d$ 是维度），这是保证渐近最优的理论要求。

代价是收敛慢——"渐近最优"意味着要采样到天荒地老。所以有了后续一堆改进：

• RRT-Connect：从起点和终点同时长两棵树，相向生长，速度快数倍，但不保证最优
• Informed RRT*：找到初始解后，把采样域限制在以起终点为焦点的超椭球内（长轴 = 当前最优路径长度），收敛速度提升显著
• BIT*（Batch Informed Trees）：把采样和搜索结合，一批一批地采样并用启发式搜索处理，兼具 A* 的效率和采样法的高维适应性

4.3 智能优化算法（蚁群、粒子群、遗传）

CSDN 上这类文章特别多，但我必须泼一盆冷水：这些算法在真实无人机实时规划中几乎不用。

原因很直接：迭代次数动辄成百上千，单次规划耗时几百毫秒到几秒，且没有任何最优性或完备性保证，参数还多得离谱。它们的价值主要在离线场景——比如多机任务分配、覆盖航线的顺序优化（本质是 TSP 变种），这类"组合优化"问题确实是启发式算法的主场。

做毕设可以写，上机载别用。

---

五、后端：轨迹优化

前端给了一条折线，现在要把它变成光滑可飞的时间函数。

5.1 微分平坦（Differential Flatness）

这是理解四旋翼轨迹规划的理论基石。Mellinger 在 2011 年证明：四旋翼系统是微分平坦的，平坦输出为

$$\sigma =[x,y,z,\psi {]}^T$$

也就是说，所有状态量（姿态、角速度）和控制量（四个电机推力）都可以由位置轨迹及其有限阶导数、以及偏航角显式表示出来。

具体链条是：

• 位置的二阶导 → 期望加速度 → 结合重力得到期望推力方向 → 期望姿态（roll/pitch）
• 位置的三阶导（jerk）→ 姿态角速度
• 位置的四阶导（snap）→ 姿态角加速度 → 电机转速差

结论：只要规划出一条四阶连续可导的位置曲线，飞控就一定能跟踪它。 这就是为什么优化目标要选 minimize snap——最小化 snap 等价于最小化电机转速的剧烈变化，直接对应能耗和可跟踪性。

（对固定翼、无人车这类非完整约束系统，minimum jerk 或曲率连续的 Dubins/Reeds-Shepp 曲线更常用。）

5.2 Minimum Snap 轨迹生成

把路径按航点分成 $M$ 段，每段用 $N$ 阶多项式表示（通常 $N=7$，因为 snap 优化需要 8 个自由度）：

$$p_j(t)=\sum _{i=0}^N{c}_{j,i}{t}^i$$

目标函数：

$$J=\sum _{j=1}^M{\int }_{T_{j-1}}^{T_j}{\left(\frac{d^4{p}_j(t)}{d{t}^4}\right)}^{2}dt={\mathbf{c}}^T\mathbf{Qc}$$

$\mathbf{Q}$ 是分块对角的 Hessian，每块可以解析写出：

$$Q_j(i,l)=\frac{i!}{(i-4)!}\cdot \frac{l!}{(l-4)!}\cdot \frac{T_j^{i+l-7}-T_{j-1}^{i+l-7}}{i+l-7},i,l\ge 4$$

约束是等式约束（起终点的位置/速度/加速度，中间点位置，段间连续性）：

$$\mathbf{Ac}=\mathbf{d}$$

于是问题变成一个标准 QP，用 OSQP、qpOASES 或者直接闭式求解都行。

闭式解技巧：Richter 提出把决策变量从多项式系数 $\mathbf{c}$ 映射到端点导数 $\mathbf{d}$（即 $\mathbf{c}={\mathbf{A}}^{-1}\mathbf{d}$），再把 $\mathbf{d}$ 分成已知量 ${\mathbf{d}}_F$ 和自由量 ${\mathbf{d}}_P$，通过置换矩阵 $\mathbf{C}$ 消元：

$$J={\left[\begin{array}{c}{\mathbf{d}}_F\\ {\mathbf{d}}_P\end{array}\right]}^T\mathbf{C}{\mathbf{A}}^{-T}\mathbf{Q}{\mathbf{A}}^{-1}{\mathbf{C}}^T\left[\begin{array}{c}{\mathbf{d}}_F\\ {\mathbf{d}}_P\end{array}\right]={\left[\begin{array}{c}{\mathbf{d}}_F\\ {\mathbf{d}}_P\end{array}\right]}^T\left[\begin{array}{cc}{R}_{FF}& R_{FP}\\ R_{PF}& R_{PP}\end{array}\right]\left[\begin{array}{c}{\mathbf{d}}_F\\ {\mathbf{d}}_P\end{array}\right]$$

令梯度为零得：

$${\mathbf{d}}_P^{\ast }=-R_{PP}^{-1}{R}_{FP}^T{\mathbf{d}}_F$$

一次矩阵求逆搞定，比迭代 QP 快得多。注意数值稳定性：$\mathbf{A}$ 在段时间跨度大时条件数极差，务必做时间归一化（每段时间映射到 $[0,1]$），否则 7 阶多项式在 $t=10$ 时 $t^7=10^7$，矩阵直接爆掉。这个坑我见过太多人踩。

5.3 时间分配

Minimum Snap 有个前提：每段的时间 $T_j$ 是已知的。但时间怎么定？

• 梯形速度法：按段长和 $v_{max},a_{max}$ 算梯形速度剖面所需时间，简单实用，工程首选
• 按距离比例分配：$T_j\propto \Vert p_j-p_{j-1}\Vert$ 或其开方，最粗暴
• 梯度下降优化时间：把 $T$ 也作为决策变量，目标加上 $\rho \sum T_j$，用数值梯度迭代。效果好但慢

时间给短了 → 轨迹超速超加速度，飞控跟不上；时间给长了 → 飞得像老太太散步。实际做法是先粗算，然后检查轨迹的最大速度/加速度，超限就把该段时间乘以一个系数（如 1.2）重新求解，迭代几次即可收敛。

5.4 B 样条 + 梯度优化（现代主流）

多项式方案的问题：碰撞约束不好加。你解出来的光滑曲线可能会"抄近道"穿过障碍物，传统做法是在碰撞段中间插入新航点再重解，反复迭代，很不优雅。

均匀 B 样条的两个性质完美解决了这个问题：

1. 凸包性：$k$ 阶 B 样条的每一段完全落在其 $k+1$ 个控制点构成的凸包内 → 只要控制点凸包不碰障碍，曲线就安全
2. 导数仍是 B 样条：速度、加速度的控制点可由位置控制点差分直接得到 → 动力学约束可以直接施加在控制点上，变成线性约束

$$V_i=\frac{p_{i+1}-p_i}{\Delta t},A_i=\frac{V_{i+1}-V_i}{\Delta t}$$

于是优化目标写成：

$$J={\lambda }_s{J}_{smooth}+{\lambda }_c{J}_{collision}+{\lambda }_d{J}_{dynamic}$$

• $J_{smooth}$：控制点的三阶差分平方和（弹性带能量）
• $J_{collision}$：由 ESDF 给出，$\sum \max (0,d_{thr}-d(Q_i){)}^3$
• $J_{dynamic}$：超出 $v_{max}/a_{max}$ 的惩罚

直接扔给 NLopt / L-BFGS 求解。Fast-Planner、EGO-Planner 走的都是这条路，实时性可以做到几毫秒一次，支持 5m/s 以上的自主飞行。

近两年 MINCO（Fast-Lab 的 GCOPTER）进一步把时空联合优化做到了极致——空间参数和时间参数解耦但同时优化，且保持 $O(M)$ 复杂度，目前是 SOTA。

---

六、局部避障：反应式方法

6.1 人工势场法（APF）

目标点产生引力，障碍物产生斥力，合力方向就是运动方向：

$$U_{att}=\frac{1}{2}{k}_a\Vert p-p_g{\Vert }^2,U_{rep}=\{\begin{array}{ll}\frac{1}{2}{k}_r{\left(\frac{1}{d}-\frac{1}{d_0}\right)}^2& d\le d_0\\ 0& d>d_0\end{array}$$

$$F=-\nabla U_{att}-\nabla U_{rep}$$

优点是计算量极小（几十行代码），适合算力极度受限的场合。

致命缺陷：局部极小值。当障碍物恰好在无人机和目标点连线上时，引力和斥力抵消，无人机原地"卡死"或来回震荡。狭窄通道（两侧斥力叠加大于引力）也会导致进不去。

改良方案：加入随机扰动逃逸、引入旋转力场（让斥力带一个切向分量绕开障碍）、或者干脆把它降级为"最后一道安全防线"而不是主规划器。

6.2 DWA（动态窗口法）

在速度空间 $(v,\omega )$ 中采样。"动态窗口"指的是受当前速度和加减速能力限制、下一控制周期内可达的速度集合：

$$V_d=\{(v,\omega )|v\in [v_c-a\Delta t,v_c+a\Delta t]\}$$

对窗口内每组 $(v,\omega )$ 前向仿真一小段时间，用评价函数打分：

$$G=\alpha \cdot heading+\beta \cdot dist+\gamma \cdot velocity$$

分别对应朝向目标程度、离障碍物距离、速度大小。选最高分的速度执行。

DWA 天生考虑动力学约束，在差速轮机器人上是标配（ROS 的 dwa_local_planner）。四旋翼因为是全向的，速度空间是三维的，采样量大，用得相对少一些——但在需要严格限速的场景（比如室内贴墙飞行）仍然有价值。

6.3 VO / RVO / ORCA

处理动态障碍物和多机避让的标准方案。速度障碍（Velocity Obstacle）指的是"会在未来某时刻导致碰撞的相对速度集合"，避障就是选一个不在这个锥形集合里的速度。

RVO/ORCA 解决了两机对冲时互相让、让过头再让回来的震荡问题——各自只承担一半的避让责任。多无人机编队、集群表演必备。

---

七、完整 C++ 实现：3D A* + 路径简化

下面这份代码是可以直接编译运行的，不依赖任何第三方库（Eigen 也去掉了），方便直接拷进项目里改。

#include <cmath>
#include <algorithm>
#include <limits>
#include <queue>
#include <unordered_map>
#include <vector>

// ---------------- 基础类型 ----------------
struct Vec3i {
    int x = 0, y = 0, z = 0;
    bool operator==(const Vec3i& o) const {
        return x == o.x && y == o.y && z == o.z;
    }
};

struct Vec3iHash {
    std::size_t operator()(const Vec3i& v) const noexcept {
        // 三个大素数异或，实测在栅格场景下碰撞率可接受
        return (static_cast<std::size_t>(v.x) * 73856093) ^
               (static_cast<std::size_t>(v.y) * 19349663) ^
               (static_cast<std::size_t>(v.z) * 83492791);
    }
};

// ---------------- 地图接口 ----------------
// 实际项目里替换成你自己的 OccupancyGrid / OctoMap 封装
class GridMap {
public:
    GridMap(int sx, int sy, int sz, double res)
        : sx_(sx), sy_(sy), sz_(sz), res_(res), data_(sx * sy * sz, 0) {}

    bool inBound(const Vec3i& p) const {
        return p.x >= 0 && p.x < sx_ && p.y >= 0 && p.y < sy_ && p.z >= 0 && p.z < sz_;
    }
    // 注意：这里查询的应该是"膨胀后"的地图
    bool isOccupied(const Vec3i& p) const {
        return !inBound(p) || data_[idx(p)] != 0;
    }
    void setOccupied(const Vec3i& p) {
        if (inBound(p)) data_[idx(p)] = 1;
    }
    double resolution() const { return res_; }

private:
    int idx(const Vec3i& p) const { return (p.x * sy_ + p.y) * sz_ + p.z; }
    int sx_, sy_, sz_;
    double res_;
    std::vector<uint8_t> data_;
};

// ---------------- A* ----------------
class AStar3D {
public:
    explicit AStar3D(const GridMap& map, double heuristic_weight = 1.0)
        : map_(map), eps_(heuristic_weight) {}

    // 返回栅格路径；为空表示无解
    std::vector<Vec3i> search(const Vec3i& start, const Vec3i& goal) {
        std::vector<Vec3i> path;
        if (map_.isOccupied(start) || map_.isOccupied(goal)) return path;

        nodes_.clear();
        OpenList open;

        NodeInfo& s = nodes_[start];
        s.g = 0.0;
        open.push({eps_ * heuristic(start, goal), start});

        while (!open.empty()) {
            const Vec3i cur = open.top().idx;
            open.pop();

            NodeInfo& info = nodes_[cur];
            if (info.closed) continue;   // 惰性删除：跳过重复入堆的旧节点
            info.closed = true;

            if (cur == goal) return retrieve(start, goal);

            const double g_cur = info.g;   // 拷贝出来，避免后续插入后引用语义问题

            for (int dx = -1; dx <= 1; ++dx)
            for (int dy = -1; dy <= 1; ++dy)
            for (int dz = -1; dz <= 1; ++dz) {
                if (dx == 0 && dy == 0 && dz == 0) continue;

                Vec3i nb{cur.x + dx, cur.y + dy, cur.z + dz};
                if (map_.isOccupied(nb)) continue;

                NodeInfo& ninfo = nodes_[nb];
                if (ninfo.closed) continue;

                const double step = std::sqrt(double(dx * dx + dy * dy + dz * dz));
                const double ng = g_cur + step;
                if (ng < ninfo.g) {
                    ninfo.g = ng;
                    ninfo.parent = cur;
                    ninfo.has_parent = true;
                    open.push({ng + eps_ * heuristic(nb, goal), nb});
                }
            }
        }
        return path;   // 搜索空间耗尽，无解
    }

private:
    struct NodeInfo {
        double g = std::numeric_limits<double>::infinity();
        Vec3i parent;
        bool has_parent = false;
        bool closed = false;
    };
    struct QItem {
        double f;
        Vec3i idx;
        bool operator>(const QItem& o) const { return f > o.f; }
    };
    using OpenList = std::priority_queue<QItem, std::vector<QItem>, std::greater<QItem>>;

    // 3D 对角距离：26 邻域下的紧启发，且满足可采纳性
    static double heuristic(const Vec3i& a, const Vec3i& b) {
        double d[3] = {double(std::abs(a.x - b.x)),
                       double(std::abs(a.y - b.y)),
                       double(std::abs(a.z - b.z))};
        std::sort(d, d + 3);           // d[0] <= d[1] <= d[2]
        double h = std::sqrt(3.0) * d[0]
                 + std::sqrt(2.0) * (d[1] - d[0])
                 + (d[2] - d[1]);
        return h * 1.0001;             // tie-breaker，减少同 f 值节点堆积
    }

    std::vector<Vec3i> retrieve(const Vec3i& start, const Vec3i& goal) const {
        std::vector<Vec3i> path;
        Vec3i cur = goal;
        path.push_back(cur);
        while (!(cur == start)) {
            auto it = nodes_.find(cur);
            if (it == nodes_.end() || !it->second.has_parent) break;
            cur = it->second.parent;
            path.push_back(cur);
        }
        std::reverse(path.begin(), path.end());
        return path;
    }

    const GridMap& map_;
    double eps_;
    std::unordered_map<Vec3i, NodeInfo, Vec3iHash> nodes_;
};

7.1 路径简化：RDP + 视线检查

A* 输出的折线包含大量共线的冗余点，直接送给后端会让多项式段数暴涨。两步处理：

// Bresenham 式的三维视线检查：两点间连线是否无碰撞
bool lineOfSight(const GridMap& map, Vec3i a, Vec3i b) {
    int dx = b.x - a.x, dy = b.y - a.y, dz = b.z - a.z;
    int steps = std::max({std::abs(dx), std::abs(dy), std::abs(dz)});
    if (steps == 0) return true;
    for (int i = 1; i <= steps; ++i) {
        Vec3i p{a.x + int(std::round(double(dx) * i / steps)),
                a.y + int(std::round(double(dy) * i / steps)),
                a.z + int(std::round(double(dz) * i / steps))};
        if (map.isOccupied(p)) return false;
    }
    return true;
}

// 贪心剪枝：能直连就跳过中间点
std::vector<Vec3i> prunePath(const GridMap& map, const std::vector<Vec3i>& path) {
    std::vector<Vec3i> out;
    if (path.empty()) return out;
    size_t i = 0;
    out.push_back(path[0]);
    while (i < path.size() - 1) {
        size_t j = path.size() - 1;
        while (j > i + 1 && !lineOfSight(map, path[i], path[j])) --j;
        out.push_back(path[j]);
        i = j;
    }
    return out;
}

经过这一步，一条 200 个栅格点的路径通常能压缩到 5~15 个关键航点，正好作为 Minimum Snap 的输入。

采样步长要小于栅格分辨率，否则视线检查会"穿墙"——用 max(|dx|,|dy|,|dz|) 作为步数就能保证每步不超过一格。

---

八、工程落地的十个坑

按踩坑频率排序，都是血泪：

1. 膨胀半径没算对。必须是 机体外接圆半径 + 定位误差 + 控制跟踪误差 + 安全余量。只算机体尺寸的，迟早撞。

2. 前端路径直接给控制器。折线的曲率不连续，飞机在拐角必然超调。前端出来的是"参考"，不是"指令"。

3. 多项式数值爆炸。7 阶多项式不做时间归一化，段时长超过 5 秒时矩阵条件数就到 $10^{10}$ 量级了，解出来的系数全是垃圾。

4. 忽略感知-规划延迟。从雷达/相机出点云到轨迹下发，链路延迟常有 50~150ms。5m/s 飞行时这就是 0.25~0.75 米的位移。规划起点必须用"当前时刻 + 延迟"预测出来的状态，而不是最新观测到的位置，否则新轨迹和飞机实际状态对不上，会产生阶跃指令。

5. 重规划触发策略拍脑袋。常见的三个触发条件：定时（如 100ms）、检测到当前轨迹未来 $T_{check}$ 秒内会碰撞、里程碑式（飞完当前轨迹的 1/3）。三者取并集比较稳妥。

6. 坐标系混乱。ENU / NED / 机体系 / 相机系搞混是最常见的低级 bug。建议在代码里用类型系统区分（比如 PointENU 和 PointBody 是不同的 struct），编译期就把错误挡掉。

7. 无解时没有兜底。地图被噪声塞满、起点被误判为障碍、目标点在障碍内……这些都会导致规划失败。必须有降级策略：原地悬停 → 沿上一条轨迹减速停止 → 返航。绝不能"什么都不发"。

8. 起点被判为占据。因为定位误差或点云噪声，飞机自己所在的格子被标成了障碍，A* 直接返回空。处理办法是对起点周围做强制清空（假设飞机当前位置一定是安全的，因为它就在那儿）。

9. 优先队列没做惰性删除。std::priority_queue 不支持 decrease-key，只能重复入堆，取出时靠 closed 标志跳过。忘了这个判断会导致节点被重复扩展，路径出错。

10. 只在仿真里调参。仿真里的地图是干净的、延迟是零、定位是真值。实机上这三个假设全都不成立。参数一定要在真实平台上重标。

顺便一提，上面这套框架不只适用于无人机。水下机器人（ROV/AUV）的路径规划几乎可以照搬，主要差别在于：需要在代价函数里额外加入流场影响（顺流和逆流的能耗差异巨大），以及声学定位的更新率低（1~5Hz）导致状态估计延迟更大，第 4 条坑会被放大数倍。

---

九、算法选型建议

场景	前端	后端	说明
室内低速自主飞行	A* / JPS	Minimum Snap	最经典组合，代码量小
室外高速穿越（>3m/s）	Kinodynamic A*	B样条 + ESDF 梯度优化	Fast-Planner 方案
算力受限（如 Nano）	A* + 剪枝	EGO-Planner（免 ESDF）	省一大半算力
高维 / 复杂约束	Informed RRT* / BIT*	轨迹平滑	机械臂、带载机构
动态障碍 / 多机	上述任一	+ ORCA 层	加一层速度避让
离线航线规划、覆盖任务	遗传 / 蚁群	—	组合优化问题的主场
兜底安全层	—	APF	只做最后一道防线

给初学者的路线建议：先把 2D A* 手写一遍（不看任何参考），再扩到 3D，然后做路径剪枝，接着啃 Minimum Snap 的 QP 推导，最后上 B 样条。每一步都用 Matlab 或 Python 画图验证，别一上来就在 ROS 里搞。

推荐的开源实现（都在 GitHub 上）：ethz-asl/mav_trajectory_generation（Minimum Snap 工业级实现）、HKUST-Aerial-Robotics/Fast-Planner 和 ZJU-FAST-Lab/ego-planner（现代规划器全套）、ompl（采样算法大全）。

---

结语

无人机路径规划的技术栈这几年迭代得很快，但底层逻辑一直没变：离散搜索保证找得到，连续优化保证飞得好，反应式方法保证撞不上。 三层各司其职，理解了这个分工，再看任何新论文都不会迷路。

如果这篇文章帮你理清了思路，欢迎点赞收藏。代码部分可以直接拿去用，有问题在评论区交流。

---

本文为原创技术总结，转载请注明出处。