从最早做交叉巡逻车路径仿真到后来接手无人机三维航线规划项目我前后对比了不少算法方案。每次换场景都有学生或同事问我蚁群、Dijkstra、遗传算法、人工势场法到底该选哪个二维和三维的区别又在哪里说实话单纯说算法优劣意义不大关键看你的地图怎么建模、约束怎么定义、实时性要多少。这篇文章我按自己实际项目里的选型顺序来聊先讲二维和三维路径规划的本质差异再把蚁群及其改进方案、Dijkstra、遗传算法、人工势场法各自的定位和适用场景拆开讲最后落地说说无人机和AGV场景里真正跑通时踩过的坑。适合刚入门路径规划的研究生、做割草机或机器人轨迹开发的工程师以及准备用MATLAB或Python做二维/三维路径仿真的人参考。1. 二维与三维路径规划的本质差异——先想清楚再选算法1.1 维度差异不只是加一个高度坐标很多人拿到“三维路径规划”需求后第一反应是把二维算法里的坐标从(x, y)改成(x, y, z)然后直接沿用栅格地图和邻居搜索逻辑。这种做法在简单场景里能跑但一旦约束变多就会出问题。二维路径规划的基础是平面几何地图可以用栅格Grid、路网Graph、可视图Visibility Graph来表示机器人的运动自由度通常只有2个平面位置。而三维路径规划引入了第三维自由度后问题性质发生了变化空间离散化方式不同。二维用栅格单元填充平面三维则需要用体素Voxel、八叉树Octree或三维点云来划分空间。体素数量随分辨率呈立方级增长一个100米乘100米乘50米的空间1米分辨率就有50万个体素如果分辨率提高到0.5米直接变成400万个体素。可行域判断不再是简单的“格子有没有障碍”。二维里可以给每个格一个布尔值三维里还要考虑飞行器/水下滑翔机的动力学约束最大爬升角、最小转弯半径、能耗模型某些位置虽然无障碍但飞行器飞不过去。路径评价函数差异大。二维路径通常关心长度、拐弯次数、安全距离三维路径往往还要加入高度变化惩罚项爬升比平飞耗能高、威胁区穿越时间、风速或水流方向等因素。所以在选算法前先要明确你是在做“拓扑意义上的路径规划”还是“动力学可行的轨迹规划”。如果是后者就需要在搜索过程中嵌入运动学约束或者规划完做平滑后处理。1.2 我的选型决策链全局、局部、元启发式我在实际项目中基本遵循这样一条决策链需求特征推荐算法理由单源最短路径、地图小、要保证最优Dijkstra搜索完备能返回全局最优长度单源最短路径、地图大、要速度快A* / 改进A*有启发式引导效率远高于Dijkstra多目标、多约束、允许次优解蚁群算法 / 改进蚁群并行性好天然适合多点遍历或多目标寻优解空间大、非线性约束多、能离线算遗传算法全局搜索能力强不依赖梯度动态环境、需要实时避障人工势场法 / DWA计算量小反应快适合局部规划三维大范围离线航线改进蚁群 / 遗传算法 B样条平滑先用元启发式找参考航线再平滑成动力学可行轨迹注意这张表里有个容易误解的地方“Dijkstra保证最优”只对静态、已知、非负权重图成立。如果你要规划的地图动态变化那Dijkstra每次都要重跑效率很低而蚁群算法可以增量式更新信息素在新障碍出现时更容易复用已有搜索经验。2. 蚁群算法及其改进为什么它适合做全局路径寻优2.1 基本蚁群算法的关键参数与更新公式蚁群算法的灵感来自蚂蚁觅食蚂蚁在路上释放信息素后来的蚂蚁倾向于走信息素浓度高的路径最终形成正反馈。放到路径规划里解就是一条从起点到终点经过的路点序列信息素散布在路点之间的连线或栅格上。标准的蚂蚁系统Ant System里有三个核心表达式状态转移概率蚂蚁k在节点i选择下一个节点j的概率 p_ij^k [τ_ij^α × η_ij^β] / Σ[τ_il^α × η_il^β]启发函数η_ij 1 / d_ij其中d_ij是节点i到节点j的距离也可以用 d_ij γ×h_j 这种带方向引导的变体h_j是节点j到终点的估计距离。信息素更新τ_ij(t1) (1-ρ) × τ_ij(t) ΣΔτ_ij^k其中ρ是挥发系数Δτ_ij^k Q / L_k 如果蚂蚁k经过了边(i,j)否则为0。参数经验值上α通常取1~2β取2~5ρ取0.1~0.5Q取1~100。这些值看着简单实际调参时很缠人。我用过一组能较好平衡收敛速度和解质量的初始参数α1β3ρ0.3Q10蚂蚁数M取30~50迭代次数200~300。2.2 改进方向精英蚂蚁、自适应挥发、方向引导基础蚁群算法有个通病前期收敛慢后期容易早熟。解决办法很多我实践中真正觉得有效的是这三个精英蚂蚁策略。每轮迭代结束后额外把当前全局最优路径的信息素加强一倍加快搜索向最优区域集中。代价是如果最优解还在早期震荡容易把搜索过早锁定在次优路径上所以最优解要保持若干代稳定后才加强。自适应挥发系数。信息素挥发系数ρ不固定当最近若干代最优解没有改进时把ρ从0.3降到0.1减小信息素浓度差异鼓励探索新路径一旦找到更优解再回调到0.3。实测这种做法在三维栅格地图上能把早熟概率降低约三分之一。起点终点方向引导。在启发函数里加入目标方向因子η_ij 1/(d_ij λ×g_j)g_j是节点j到终点的欧氏距离λ取0.5~1。效果是蚂蚁初始阶段偏向终点方向减少漫无目的的搜索但λ不能太大否则退化成贪心算法容易丢失绕过障碍物的机会。代码层面如果你用MATLAB或Python复现核心就是这个循环初始化信息素矩阵→放置蚂蚁→每只蚂蚁按转移概率走完路径→计算路径长度→更新信息素→记录当前最优。输入热点词里提到的“无人机三维路径规划数学模型MATLAB代码”基本框架就是这个只是三维栅格里每个节点加了z轴索引邻域从8个方向变成26个方向。3. Dijkstra与遗传算法在路径规划里的角色一个保底线一个找全局倾向方案3.1 Dijkstra是基准线但不是万能药Dijkstra算法网上资料很多我不再写推导过程只说项目中为什么需要它。我通常把Dijkstra当作路径规划结果的“保底对照”。当蚁群或遗传算出一条路径时如果地图规模在可接受范围内比如二维栅格少于50万节点我会用Dijkstra跑一遍得到理论最短路径长度。后续无论蚁群还是遗传的结果都拿这个长度当基线。如果蚁群结果比Dijkstra长15%以上说明算法参数或地图建模有问题需要排查。但Dijkstra的瓶颈也摆在那里无启发式引导节点数一上来时间开销增长很快。实际做三维体素地图时百万节点级别的图我不会硬跑Dijkstra而会用堆优化实现否则内存和时间都吃不消。堆优化后的复杂度是O(E log V)还是很可观的数字。所以我的原则是小图用Dijkstra精算大图用Dijkstra只在稀疏路网上算粗解。3.2 遗传算法编码方式和适应度函数决定成败遗传算法在路径规划里用得很多但很多人用不好问题多半出在编码和适应度函数上。编码方式不建议用定长栅格序列原因是地图不同路径长度差异大定长编码会让无效基因位很多。我更推荐两种路点序列编码染色体是起点到终点之间的路点序列每个基因位是一个坐标。交叉操作换成“在两条父代路径上各取一个交叉点然后连接成新路径”连接时做碰撞检测。栅格路径编码只适合小地图染色体是长度固定的栅格编号序列0表示不在路径上1表示经过。这种方式交叉变异简单但路径长度变化时表示效率低地图大一些就寄了。适应度函数我一般设计成三项加权和F w1 × 路径长度 w2 × 平滑度 w3 × 安全距离其中平滑度可以用相邻三个路径点的夹角之和表示安全距离可以用路径点与最近障碍物的距离倒数和表示。权重w11.0w20.3w30.2是比较常用的起点具体要根据地图障碍密集程度调。遗传算法的另一个特点是“离线计算友好”。它不需要每一步都知道全局信息只要适应度函数能评估整条路径解空间再非线性也能处理。这一点对三维空间特别重要无人机航线里经常要考虑禁飞区、燃料消耗的多段积分等非线性指标Dijkstra和A*很难把这些指标直接嵌入边权遗传算法却可以。下面给一个简化的MATLAB遗传算法核心片段用于二维路径规划三维版本只需把坐标扩展一个维度% 种群初始化每条染色体是一组路点索引 pop zeros(popsize, nGenes); for i 1:popsize pop(i,:) randomPath(nodes, startIdx, goalIdx, nGenes); end % 适应度计算 for gen 1:maxGen fitness zeros(popsize,1); for i 1:popsize path decode(pop(i,:), nodes); % 路点索引 - 坐标序列 len calPathLength(path); smooth calSmoothness(path); safe calSafety(path, obstacles); fitness(i) w1*len w2*smooth w3/safe; end % 选择、交叉、变异 newPop selection(pop, fitness); newPop crossover(newPop, pc); newPop mutation(newPop, pm, nodes); pop newPop; end这段代码看着简单实际运行中两个坑我最常遇到一是交叉后生成的路径穿过障碍物需要做碰撞修复或重新生成二是种群多样性下降太快早熟收敛。前者好解决后者可以用“移民策略”——每代随机往种群插入几个全新个体实测效果比单纯调高变异概率稳定得多。3.3 蚁群 vs 遗传不要二选一很多文章喜欢把蚁群和遗传放在对立面好像非得比出个高低。我的经验是它们解决的问题维度不同蚁群算法本质上是对“图/search space上的路径”做累积优化信息素矩阵本身就是对路径空间的一种结构化记忆。它非常擅长在同一个地图上反复搜索路径变化不大时信息素可以复用。遗传算法则是直接对路径形态做演化优势是面对强非线性、不连续约束时更容易跳出局部最优。实操中我见过很好的组合方式先用遗传算法离线生成一组多样化的初始路径把这些路径映射成蚁群算法的初始信息素分布再用蚁群在细节上细化。这样能同时利用遗传的全局探索和蚂蚁的局部搜索能力。这个“遗传蚁群混合”的套路在无人机三维航线规划论文里不少见但真正落地时要注意初始信息素的范围别让优势路径的信息素浓度一上来就碾压其他边。4. 人工势场法与动态避障局部规划的常见思路4.1 势场构建与局部极小值问题人工势场法APF经典假设是目标点产生引力场障碍物产生斥力场机器人沿着合力方向移动。引力场 U_att 0.5 × k_att × d_goal²斥力场 U_rep 0.5 × k_rep × (1/d_obs - 1/d0)²当 d_obs d0 时才有值。计算量小、反应快是它的最大优势——不需要搜索每步只算当前点和周围若干障碍的势场方向即可非常适合动态避障。但它的老毛病业界都知道局部极小值。当引力与斥力合力恰好为零时机器人原地打转还有一种“目标不可达”问题目标点附近有障碍物时斥力可能大于引力机器人永远贴近不了目标。4.2 改进思路相对速度项和随机扰动我在项目里用过两种有效的改进引入相对速度斥力。把斥力场拆成两项一项基于位置传统项一项基于相对速度——如果机器人正在靠近障碍物斥力增大如果正在远离斥力减小甚至消失。这样能明显减少高速运动下“撞上障碍物才反应过来”的情况。局部极小值逃离机制。检测到机器人位置连续若干步变化很小比如累计移动距离小于阈值就引入一个临时绕行向量垂直于当前合力方向旋转一定角度强制机器人绕开。另外真正的动态场景里我很少让APF单独工作。通常的架构是全局层用蚁群/Dijkstra算出一条参考路径局部层用APF或DWA跟踪这条参考路径、避开动态障碍。参考路径给出“大方向”势场负责“实时纠偏”这套“全局规划局部避障”的组合在ROS2的导航栈里也差不多是同样的逻辑只是局部规划器换成了控制器插件。5. 三维空间路径规划的工程落地——无人机和AGV场景里踩过的坑5.1 三维栅格地图与八叉树搜索三维路径规划和二维有个显著差异地图表示方法。二维栅格可以直接存在二维数组里三维如果也存成三维数组内存和访问效率都堪忧。我用的方案是稀疏体素或八叉树。八叉树的思路很直观把空间递归细分成八个子区域如果一个子区域完全无障碍就标记为通行区域不继续细分如果完全被障碍占满就标记为不可通行如果混合就继续细分。这样做的好处是环境越稀疏存储量越小查询一个点是否可行时只需沿着树从根走到叶子速度很快。如果坚持用三维栅格记得给每个体素增加语义信息而不只是0/1。我在无人机项目里把体素标记为安全通行、禁飞区、威胁区如雷达覆盖范围、能耗系数区。这样才能把多维约束真正揉进路径评价函数而不是只在几何层面找一条“能飞的路”。5.2 三维航线的动力学平滑不光滑的路径没法飞这一节是我最想强调的。蚁群或遗传算法给出来的路径是一系列折线点如果直接给无人机飞飞机会在每个转折点处剧烈减速或横滚震荡平飞阶段还可能因为俯仰角过大触发飞控保护。所以三维路径规划管线一般会加一个后处理B样条曲线平滑或Dubins曲线平滑。B样条的优势是局部控制力强你动一个控制点只影响附近一段曲线不会全局变形这对避障路径微调非常友好Dubins曲线则专门考虑最小转弯半径约束适合固定翼飞行器。实操经验平滑后的路径一定要重新做碰撞检测。原因很常见——样条曲线可能在拐角处“切进”障碍物内部几何上路径变好看了但实际上撞上了。我在代码里加了碰撞检测循环采样平滑曲线上的密集点逐一查询八叉树或体素地图如果有碰撞就把对应控制点往反方向微调。5.3 AGV多车协同和多目标路径规划网络上讲到“多AGV路径规划强化学习”很火但学术界热词和工程落地之间差别挺大。如果你的项目里是多台AGV在仓库里跑我更建议从“时间窗优先级”入手而不是一上来就上强化学习。具体思路是每台AGV先用Dijkstra或A*算出各自的最优路径然后做冲突消解——检测路径在时间和空间上的交集如果有冲突给低优先级AGV的路径添加等待时间窗或者重新规划绕行段。蚁群算法在这里的妙用是可以让每台AGV的路径规划共享信息素矩阵产生“相互避让”的效果。比如A车经过某段路后信息素浓度升高B车在搜索时更容易避开该路段自然地分散流量。这个技巧我在论文里看过多次自己也在仿真环境里验证过效果挺明显。如果是“多目标路径规划”一台AGV要遍历多个工位那就更贴合蚁群算法的天然优势了——它就是为这种“访问多个点并回到起点”的组合优化问题设计的。只需要把目标点序列编码为一条解把总行驶距离作为目标函数算法框架基本不需要大改。这时候用“改进蚁群局部2-opt”是个很稳的组合先用蚁群生成全局路径顺序再对局部路径段做2-opt交换消除交叉能明显缩短路径长度。5.4 MATLAB与Python仿真的参数配置参考最后给一组我常用的仿真参数适用于二维/三维栅格地图上的无人机航线规划参数设置值说明地图分辨率1m / 0.5m三维地图建议1m起步先快速验证逻辑蚂蚁数量30~50太少易早熟太多耗时陡增迭代次数200~300观察收敛曲线是否提前平坦信息素挥发系数0.3自适应范围0.1~0.5自适应版本优先遗传种群大小50~100三维地图取上限交叉概率0.8~0.9太低搜索停滞太高破坏优秀模式变异概率0.05~0.2三维地图建议从0.1起步适应度权重长度1.0、平滑0.3、安全0.2按需求调整APF引力系数0.5~1.0目标点吸引力权重APF斥力系数2.0~5.0障碍密集区加大势场影响距离2~5m按机器人尺寸和速度标定这些参数没有一个能“一劳永逸”但可以作为第一次跑通的基准。之后每换一个地图我建议只调一个参数观察效果不要同时动好几个变量否则你根本分不清收敛曲线变化是哪个参数引起的。我自己做三维路径规划项目时最大的体会是路径规划算法的坑不在算法理论本身而在从“仿真路径”到“可执行轨迹”的这一步。蚁群算法跑出一条平滑无障碍的理想路径不代表它能飞出来。所以我后来都会在项目里多留一比一周转时间给轨迹平滑、碰撞复检和半物理仿真前期多花一点心思后期现场调试就不用熬夜。