无人机三维路径规划：蚁群、A*、RRT*算法原理与Matlab对比实战

做无人机三维路径规划仿真，绕不开一个问题——同一张地图上，蚁群算法、A*、RRT* 到底该怎么选、各有什么脾气。很多刚接触这个方向的人，上来就把三种算法各写一遍，跑出一张路径图就收工，可真被问一句“为什么这个场景用A不用RRT”，又说不清楚。这篇文章我想用Matlab做一次完整的三维路径规划对比实验，把三种算法的原理、实现关键点、同环境下的实测现象和选型思路一次讲透。

标题里的三个算法，基本代表了三类技术路线：蚁群算法是群体智能搜索，A是启发式图搜索，RRT是随机采样增量搜索。它们各有适用面，也各有短板。如果你正在做课程设计、毕业论文，或者在给实际无人机方案做路径规划预研，这篇文章可以直接拿来当参考。

1. 三维路径规划的问题建模与方案思路

1.1 为什么一定要做三维，二维规划差在哪

很多人一开始会问，二维规划直接求最短路径不就行了吗？但实际飞行的环境里，无人机面对的是立体空间：山峰有起伏，建筑有高度，树林会遮挡，还可能有必须保持的安全飞行高度。二维地图只能表达“平面上哪里能去”，表达不了“在什么高度去”。一旦遇到地形起伏、需要爬升跨越障碍的场景，二维规划出来的路径很可能贴着坡面，甚至直接从山体中间穿过去。

我在做三维栅格规划时的思路很直接：把飞行空间离散成一个个立方体网格，每个格子要么是自由空间，要么是障碍物。这样一来，路径搜索就变成了在三维网格里寻找一串连通格子的过程。相比二维，三维至少多出两个关键约束，一个是高度代价，另一个是爬升/下降角约束。无人机从低处往高处飞，连续几个格子爬升太快就可能超出飞行性能包线，这不像地面小车，可以原地转向或者急爬急降。

所以三维路径规划的核心，不是把二维算法简单加一个“Z轴”，而是要在搜索过程中同步处理安全、长度、平滑度和动力学限制。这也是三种算法对比时最值得关注的地方。

1.2 地图建模与评价函数设计

三维环境在Matlab里通常用一个三维01矩阵表示，map = zeros(X, Y, Z)，值为1表示障碍，值为0表示可通行。网格尺寸要结合无人机尺寸和任务场景来定。网格太大，路径精度差，可能漏掉细窄通道；网格太小，状态空间爆炸，算法速度会明显下降。我常用的折中是让单个网格边长略大于无人机最大外形尺寸，这样能保证“点路径”在工程上可以近似看作安全路径。

我生成测试地图时最常用两种方式：一种是随机撒障碍物点，再用膨胀操作把障碍区域连续化；另一种是用高斯函数叠加制造山峰地形，更接近真实巡检场景。后者在论文仿真里更常见，因为地形连续且能测试高度约束。

三种算法想公平对比，必须共用同一个评价函数。我习惯把代价定义为：

J = w1 * L + w2 * S + w3 * T

其中L是路径总长度，S是安全裕度惩罚（离障碍物越近惩罚越大），T是路径转角惩罚（防止相邻线段夹角过小导致急转弯）。权重w1、w2、w3按场景调节，比如室内狭小空间加大w2，开阔荒地加大w1。三种算法在同一个J下找路径，比完才有意义，这个原则很多人一开始没注意。

% 生成简单山峰测试地形 X = 1:20; Y = 1:20; [XX, YY] = meshgrid(X, Y); Z = 3 * exp(-((XX-8).^2 + (YY-12).^2)/20) + ... 2 * exp(-((XX-15).^2 + (YY-6).^2)/15); map = zeros(20, 20, 10); for i = 1:20 for j = 1:20 h = ceil(Z(i, j)); map(i, j, 1:min(h, 10)) = 1; end end

这段代码生成的是20乘20网格、10层高度的三维地形，高度向上分别累积障碍物，模拟一座有高低起伏的山脉。起点和终点选在地形两侧的低空格，路径必须在山峰之间找一条既能绕开障碍、又能控制爬升率的航线。

2. 三种算法的原理剖析与Matlab实现要点

2.1 蚁群算法：信息素怎么铺出一条三维航线

蚁群算法的灵感来自蚂蚁觅食。蚂蚁在行走时会向地面释放信息素，路径越短，单位长度上信息素就越浓；后续蚂蚁倾向于走信息素浓的路线，形成正反馈，最终整个群体收敛到一条较短路线上。放到三维栅格路径规划里，蚂蚁就是一个个在网格上爬行的“智能体”，信息素矩阵和地图同尺寸，每次迭代后更新。

蚂蚁从起点出发，在当前位置的可行邻域里按概率选择下一步。经典转移概率公式是：

P_ij = (tau_ij^alpha * eta_ij^beta) / sum(tau_ik^alpha * eta_ik^beta)

其中tau是信息素浓度，eta是启发信息，通常取距离期望值的倒数。alpha控制信息素的权重，beta控制启发信息的权重。启发信息里我会加上高度代价，避免蚂蚁选到爬升过大的格子。这种“距离代价+高度代价”的启发式设计，是三维场景跟二维最大的区别。

信息素更新分两步：先按挥发系数rho做全局衰减，再给本次迭代中蚂蚁走过的路径增加信息素。路径越短，单只蚂蚁贡献的信息素越多，公式写起来很简单：

tau_new = (1 - rho) * tau_old + delta_tau

Matlab里三维信息素矩阵的更新并不复杂，但要注意只对自由空间更新，障碍物格的tau始终是0。我初始参数大概给 alpha=1、beta=2、rho=0.3、Q=1，蚂蚁数m=30，迭代200代。alpha太大会让蚂蚁过早集中在一条局部最优路径上，rho太小会让信息素迟迟不消退，收敛明显变慢。

蚁群算法的优点是有全局搜索能力，路径一般比较平滑，能自然绕开连续障碍；缺点是计算量大、参数敏感，地图规模稍大，时间就从秒级跳到分钟级。

2.2 A*：带方向感的启发式图搜索

A* 的思路和蚁群完全不同。它从起点开始，不断扩展当前代价最小、前景最好的节点，维护两个表：开放表存放待考察节点，关闭表存放已经确定展开过的节点。每个节点的评估值F=G+H，G是从起点到当前节点的真实代价，H是从当前节点到终点的启发式估计。

三维空间里，当前格子的邻居不再只是上下左右四个方向，而是至少26个邻域（六个面、十二条棱、八个角点）。邻域越多，A* 搜索得越细致，但代价也越大。实际做无人机规划时，我会限制爬升梯度，比如只允许当前格子向高度差不超过1层的邻域移动，这样路径天然更符合飞行性能。启发函数H通常用欧氏距离，计算量小，而且在网格空间里能保证找到最优解。

Matlab实现时可以用三维逻辑数组当关闭表，用结构体数组保存节点的父节点指针。地图像素总数在几万格以内，Matlab跑起来都还好；一旦超过几十万格，开放表会膨胀得很快，内存占用就很可观。

A* 的核心优势是确定性：同样的地图和起点终点，每次跑出的路径完全一致，而且在地图已知时搜索效率高。劣势是路径通常紧贴障碍物边缘，出现大量折线，看着“能用”但“不好飞”。路径后处理往往比算法本身更耗时。

2.3 RRT*：被名字吓住的随机采样算法

RRT* 的思路跟前面两个完全不是一个路子。它不做全图遍历，而是不断在地图里随机撒点，把点连到已有的搜索树上，逐渐向外扩张。普通RRT找到一条可行路径就停，RRT则多了最关键的一步——重新布线。新节点加入后，它会检查附近半径范围内的现有节点，如果从新节点绕过去比原路径成本更低，就把这些节点的父节点改成新节点。这个操作让RRT具有渐进最优性：迭代次数越多，路径越接近最优。

Matlab里实现RRT*，核心是几个函数：随机采样、寻找最近邻、按步长生成新节点、碰撞检测、重连邻域搜索。小规模场景可以直接暴力遍历所有节点找最近邻；地图规模大，建议用KD树，否则每次找最近邻都要全量扫描，迭代一多就非常慢。

RRT* 的随机性是个双刃剑：它不需要显式构建完整地图，能快速探索高维复杂空间，非常适合动态环境和复杂地形；但每次运行结果不一样，路径毛毛糙糙、锯齿感明显，要让路径收敛到比较优的状态，往往需要上千次迭代。实际工程里几乎都要接一层路径平滑处理，否则飞控很难受。

2.4 三种算法放在一张表里看

为了后面讲故事方便，先给一张直观对照表：

这张表是我在这些年做路径规划实验后的经验总结，具体数值会随地图规模、参数变化，但趋势基本一致。后面第三部分的实测数据也是在验证这张表。

3. 同一三维场景下三种算法的实操对比

3.1 实验设定与公平性约定

对比实验最怕变量不一致。我固定了以下几项：地图尺寸20x20x10，障碍物比例约18%，用 rng(42) 固定随机种子确保每次生成的地图完全一致。起点固定在(2, 2, 2)，终点固定在(18, 18, 8)。三个算法共用同一份三维地图和同一个评价函数，每组实验独立跑10次，结果取平均值。

固定随机种子这一点特别重要。RRT* 本身是随机采样算法，蚁群算法初期也有随机性，如果不固定种子或者不重复统计，看到的结果可能只是某一次运气好，不是算法真实水平。所有论文级对比都应当至少跑10次以上，别只跑一次就下结论。

实验环境就是普通台式机，Matlab版本较新的某发行版。说实话Matlab不是性能最强的平台，但做算法原型验证、画图、跟论文复现都非常方便，所以作为对比实验平台绰绰有余。

3.2 蚁群算法实现与实测现象

蚁群算法在Matlab里实现的核心是状态转移和信息素更新。下面这段伪代码是转移概率部分的关键逻辑：

% 当前节点可行邻域中，按转移概率选择下一步 idx = find(neighborFlag); % 合法且未访问的邻域索引 denom = sum(tau(idx).^alpha .* eta(idx).^beta); if denom == 0 next = randsample(idx, 1); else p = (tau(idx).^alpha .* eta(idx).^beta) ./ denom; next = randsample(idx, 1, true, p); end

每只蚂蚁从起点爬到终点后，根据整条路径的代价计算delta_tau，再统一更新信息素矩阵。一次迭代放30只蚂蚁，记录本次最优路径；200代之后输出最优结果。

实测下来，蚁群算法前50代基本在“乱逛”，路径长度波动很大，经常拐到远离终点的死角里。大约从80代开始，信息素优势逐渐显现，绕远路的蚂蚁越来越少，路径长度开始稳定下降；到160代以后基本收敛，波动幅度很小。最终得到的路径在三个算法里看着最“自然”，转折角相对温和，整体没有贴着障碍物边缘走。

代价是计算时间最长。20x20x10这样的小地图，单次跑完200代大概需要十秒到几十秒不等，取决于蚁群数量和邻域规模。把地图放大到50x50x20，运行时间会迅速涨到几分钟，这就是我常说蚁群算法更适合离线规划的原因——你想让它在线实时重规划，硬件压力很大。

3.3 A* 实现与实测现象

A* 的实现相对机械：维护开放表、关闭表，不断从开放表里取F值最小的节点展开。Matlab里可以用结构体数组保存节点状态，也可以用三个三维数组分别保存G、H、F。

启发式部分很直接：

H = sqrt((nx-goal(1))^2 + (ny-goal(2))^2 + (nz-goal(3))^2); F = G_current + H;

我限制邻居只在高度差不超过1的格子里选，26邻域里实际可用的约17个左右，这一步是为了满足无人机爬升角约束，同时也是A*搜索速度的优化手段。跑下来的结果很惊人：路径在一瞬间就出来了，几乎是毫秒级。路径总长度是三者中最短的，因为它本质上在做离散空间里的精确最短搜索。

但它的路径形态也是三者中最“僵硬”的：大量直角转折，贴墙走、贴障碍走，相邻线段夹角经常很小。放在Paper里直接展示，容易被审稿人质疑“这路径没法飞”。我用后处理把路径按最大转角约束做抽稀和插值，才让曲线能看一些。所以这里有个重要经验：A* 更适合做“底层粗规划”，它输出的路径通常不是最终航线，而是需要再接一个平滑层。

3.4 RRT* 实现与实测现象

RRT* 的核心代码看起来不复杂，一个小循环就能搭出来框架，但细节很磨人。生成新节点后，重连邻域搜索的半径设多少，直接决定算法质量和速度。

% RRT* 新节点生成 Xnew = Xnear + step * (Xrand - Xnear) / norm(Xrand - Xnear); % 重连：检查邻域内所有节点 withinRadius = find(dist(Xnear, allNodes) < rewireRadius); for i = 1:length(withinRadius) if collisionFree(Xnew, nodes(i)) && ... cost(newNode) + edgeCost < cost(nodes(i)) parent(nodes(i)) = newNode; end end

我用的采样策略里加了目标偏置：每次有一定概率直接把终点当作采样点，让树更积极地朝目标方向长；步长取1.5倍网格边长，重连半径取2倍步长。最初运行只迭代300次，很快找到一条可行路径，但路径弯弯绕绕很厉害，锯齿严重。把迭代次数提到1500次后，重连机制开始起作用，路径长度明显下降，整体走向也更合理。

RRT* 的速度介于A和蚁群之间。同一个场景下，1500次迭代大概耗时接近秒级，比A慢，但远远快于蚁群的离线优化。内存占用最小，因为整棵树只保存节点坐标、父节点指针和累计代价，不像A那样要维护全图开放表。不过随机性带来的重复性很差，同样的参数连跑10次，路径长度波动可能有20%到30%，所以用RRT做对比实验时必须统计多次取均值。

3.5 指标统计与路径后处理

三种算法跑完后，我用同一套后处理流程处理：先按最大转角约束抽稀转折点，再用三次样条插值做平滑。平滑后的路径必须重新做碰撞检测，这是个很容易踩的坑——插值虽然让曲线好看，但可能把路径拉进障碍物里，所以需要逐点确认。

最终统计表大概长这样：

路径长度上A最赢，蚁群因为要顾及信息素平滑性，路线会稍微绕一点，RRT在1500次迭代下离最优还有距离。但放在无人机实际飞行场景里，路径长度并不是唯一指标。A那种贴墙航线如果能飞但会让飞控频繁打杆，蚁群和RRT平滑之后反而更“好飞”，这就是为什么选型不能只看最短距离。

4. 结果解读、选型建议与避坑手册

4.1 四类典型场景下的选型建议

对比做完，别急着说谁好谁坏，先看场景。

如果你面对的是提前探明、固定不变的静态环境，而且地图网格规模不算大，A* 是最稳妥的起点。它确定、快速、代码简单，适合嵌入式部署和教学演示。缺点是多出来的贴墙锯齿，用平滑后处理补一下就行。

如果任务目标是离线规划一条高质量全局航线，不追求速度，比如电力巡检、测绘航线预设，蚁群算法更合适。它的全局搜索能力和路径舒展度是A*不具备的，但你要接受分钟级的计算时间，并仔细调alpha、rho这类参数。

如果环境复杂、可能动态变化，或者地图本身根本不完整，RRT* 是更现实的选择。它不需要预先构建全量地图，可以边飞边补树，适合未知环境探索和实时重规划场景。代价是每次结果不同、路径毛糙，你得设计好碰撞检测和重连半径，接一层平滑和安全性校验。

还有一种常见混合思路：先用蚁群或A做离线全局规划，把得到的路径作为RRT的采样偏置，让随机树在全局规划给出的通道附近生长。这种分层架构在工程上很实用，既保留了RRT*的灵活性，又弥补了它随机性过强的问题。

4.2 必看的常见坑与排查手册

第一号坑：蚁群算法一直不收敛。这通常不是代码逻辑错，而是参数不合适。alpha太大、rho太小、迭代次数不够都会让信息素迟迟集中在几条局部路径上。我的排错顺序是先检查信息素矩阵是否正常更新，再把rho从0.3往上提，alpha从1往下调，最后把迭代次数加一倍看趋势。

第二号坑：A地图稍大就内存爆。Matlab里开放表存的是结构体数组，节点一多，内存增长很夸张。解决办法有三个：换紧凑的数组表示、提高栅格分辨率粒度、或者改用双向A，让开放表从起点和终点同时扩张，规模能下降不少。

第三号坑：RRT* 的路径莫名穿过障碍物。多数情况是碰撞检测只检查了离散节点，没检查节点之间的连续线段。RRT*步长大于栅格边长时，一条边很可能跨越一个障碍格，必须做边级别的检测。我习惯在采样点之间按小步长插值采样，逐点验证，别贪省事。

第四号坑：对比实验跑得不公平。有人换了地图不换起点终点，有人A跑一次出结果，RRT就跑了200次迭代就拿来对比，还有人三种算法用了不同评价函数。这种对比谁胜出都不能说明问题。公平做法是同一地图、同一随机种子、同一评价函数、每组10次以上取均值，并把标准差也写出来。

第五号坑：后处理把路径平滑进障碍区。平滑后路径好看，但可能微微插进一个障碍格边缘。平滑前记录关键转折点，平滑后重新做碰撞检测，一旦撞了就退回平滑前的状态或者局部微调控制点。

另一个细节是，蚁群算法里蚂蚁在探索过程中容易被死胡同困住。三维栅格地图比二维更容易出现局部凹形障碍区，蚂蚁一旦钻进去，要绕很久才能出来。我的处理是在禁忌表里加最大步数限制，超过步数直接放弃本次搜索，不让单只蚂蚁无限原地打转。

写在最后的个人体会

按我的实践体验，三种算法里没有“万能最强”，只有“跟场景匹配”。如果你是想快速出图做演示，A* 上手最快；如果你想做论文级的全局优化对比实验，蚁群算法虽然慢，但能给你一个相对平滑的全局结果；如果你面对的是复杂未知环境，RRT* 的采样框架是最值得投入时间研究的。

最后分享一个小技巧：对比实验别只盯路径长度，把路径曲率、转折次数、安全距离中值一起统计出来，你会看到三个算法更立体的差异。我最近在做的一个模拟项目就是把蚁群算法输出的信息素浓度图，转成RRT的采样概率引导，结果比单独用RRT稳定不少。这个思路以后有机会再单独展开讲。