多无人机协同飞行这件事听起来很酷但真要在仿真里把它跑通尤其是在动态障碍物环境下规划出安全、平滑、不绕远路的路径难度比大多数人预想的要大得多。做路径规划的算法很多从经典的A*、PRM到启发式算法里的蚁群、粒子群、遗传算法但放在动态避障场景下难点从来不在“找到一条路”而在“如何在有限时间内、在多机约束下、在环境时刻变化时重新规划出一条足够好的路”。我最近用阿尔法进化算法Alpha EvolutionAE在MATLAB里做了完整的多无人机动态避障路径规划实现跑通之后发现这套方案在收敛速度、稳定性和代码实现难度之间找到了一个非常舒服的平衡点。这篇文章就把我的完整思路、建模过程、代码结构和踩过的坑全部整理出来给正在做无人机编队路径规划、动态避障算法研究的朋友一个可以直接参考的落地模板。这套方案解决的核心问题有三个一是多机之间不能互相碰撞二是路径必须避开随时在移动的障碍物三是整条路径的长度代价要足够小不能因为避障而绕出离谱的远路。AE算法在这里承担的是“在线重规划引擎”的角色配合滚动时域的思想每一小段时间窗口内都能快速给出高质量的新路径。整个系统用MATLAB实现不需要额外的工具箱底层的进化算子、适应度函数、碰撞检测、轨迹平滑全是纯代码手写可读性和可修改性都很强。1. 问题拆解动态避障为什么难写难点到底在哪多无人机动态避障不是单机路径规划问题的简单叠加。单机场景下你只需要处理“自身起点、终点、障碍物”这三者之间的关系规划出一条无碰撞路径就算成功。一旦变成多机协同问题立刻多了一个维度机群内部彼此的约束。1.1 三大约束安全、动态、协同我把整套规划要点概括成三层这也是我从建模第一天起就分三条线去处理的核心逻辑安全约束所有无人机必须在任何时刻与环境障碍物保持安全距离。这里的障碍物不是静止的可能是别的运动平台、突发的飞行器、正在移动的空中物体。代码里必须区分静态障碍区和动态障碍物的运动模型在代价函数里分别做惩罚。动态约束路径规划不是一次性计算它需要在线滚动执行。无人机飞一段感知更新一次重新规划一次。这就意味着算法必须在很短的时间内收敛到可用解不能像离线规划一样给你几分钟去跑。协同约束无人机之间彼此不能撞还要尽量避免编队内部的空间相互干扰。代价函数里需要加入机间距离惩罚项同时保证每架无人机到达终点的时间偏差在合理范围内。这三个约束如果分开处理每一个都不算难难的是它们同时发生并且你的规划周期只有几百毫秒到一两秒。传统方法比如A*在静态栅格地图上表现很好但动态场景下重规划频率太高计算量撑不住PRM或者RRT能处理高维空间但路径平滑性和最优性比较难保证经典进化算法能处理复杂约束但收敛速度是硬伤尤其是早期迭代阶段个体分布散适应度波动大。1.2 为什么选择AE而不是直接上PSO或遗传算法AE算法Alpha Evolution阿尔法进化相对PSO、DE、遗传算法来说结构更轻它本质上是通过“引导个体与随机差异向量的定向变异”来驱动种群进化。它这个名字里的“Alpha”指的就是当前种群中的最优个体每次迭代其他个体会围绕这个Alpha个体做定向的扰动搜索。它在动态避障路径优化场景里有一个天然优势每次重规划时上一轮的最优解可以直接作为当前种群的初始Alpha个体这样路径优化的连续性就非常好不会出现“重新规划后路径突变”的尴尬情况。相比之下PSO虽然搜索能力强但粒子速度参数、惯性权重、加速因子一多光调参就能耗时很久。而且PSO早熟收敛的概率不小动态环境下表现不太稳定。另外AE算法的变异策略里融合了差分进化的思想它用种群个体间的差异向量来更新位置所以在多峰、多约束函数上的表现比一般遗传算法稳定收敛代数的期望值也更低。在实测中同等条件下AE比标准遗传算法大概少用30到40代就能逼近同一水平的适应度值这个差距在滚动重规划场景里是决定性的。2. 路径优化建模把“避障”翻译成数学语言路径规划说白了就是一个带约束的数值优化问题。你给算法一个描述“这条路径好不好”的函数算法通过不断扰动路径点坐标来让它变好。AE算法并不在意你的适应度函数长什么样它只管朝着让适应度值下降的方向去搜索。所以建模的功夫全在适应度函数的设计上。2.1 路径表示方式每一条路都是若干个路径点的串联我用的是“固定数量路径点”的表示方法。每架无人机在飞行过程中经过一系列关键路径点每个路径点有x、y、z三个坐标分量或根据场景设置为二维的x、y。一架无人机一条路径就是多个路径点的坐标序列多条路径拼接在一起就构成一个“种群个体”。这样做的原因是固定数量的控制点可以直接拼成一个一维实数向量AE算法的差分变异算子操作的是实数向量结构天然匹配不需要额外做编解码。举个例子假设5个路径点加起点和终点每个点有x和y坐标那么一条路径的编码长度就是10维实数。起点和终点通常是固定的所以实际作为决策变量的维度是起点到终点之间的中间点。2.2 三个代价项的组合逻辑代价函数由三块组成每一块都带一个权重系数需要根据场景实际调整第一项是路径总长度代价。路径长度越长代价越高。它保证算法不会因为过度避障走出不必要的弧线。我常用的是相邻路径点之间的欧氏距离累加。要注意的是光用长度代价还不够因为路径点数量固定如果某一处的中间点偏离主方向太远即使总长度没有显著增加路径形状也会很难看所以我会加一个“偏离初始航向角”的惩罚项保持路径整体走向合理。第二项是障碍物威胁代价。这部分是核心中的核心。我需要分别处理静态障碍区和动态障碍物。静态障碍区用一系列圆或球来表示每个障碍物有圆心坐标和半径路径点进入障碍区或者离障碍区边界太近就累加大量代价值。动态障碍物则根据当前时刻的位置和路径点之间计算距离如果小于安全阈值直接认为碰撞返回一个巨大的代价值比如路径长度项最大值的100倍暴力淘汰这条路径。第三项是协同避撞代价。多机场景下同一时刻不同无人机的路径点之间如果距离太近代价就会上升。这里我会算每一架无人机和其他所有无人机在同一时间节点上的距离比如5架无人机每架无人机10个规划时步那就需要算上全部的配对距离。这个计算量看起来大但其实MATLAB用矩阵广播一次就能算完后面我会讲具体实现。整个适应度函数的数学描述大概是[ J w_1 \cdot J_{length} w_2 \cdot J_{obstacle} w_3 \cdot J_{coop} ]权重系数上我习惯把安全项优先级拉高但不宜过高到让路径完全退化。实测中比较稳的配比是长度项0.4障碍威胁项0.35协同项0.25。你可以在自己的项目里微调但记住一个原则威胁代价绝对不能与长度代价线性叠加时出现“绕过比撞上更贵”的倒挂所以障碍项里在距离接近0时需要使用极大惩罚值而不能只做线性惩罚。2.3 动态障碍物的时间维度处理动态避障要想严谨还得考虑障碍物的运动时间。如果不把时间维度放进去规划出来的只是一条静态的绕行路径它可能在某一个时刻是安全的在另一个时刻却和正在运动中的障碍物撞上。我的做法是给每个动态障碍物定义一个运动轨迹函数比如匀速直线运动或简谐运动然后根据当前时刻算出它此刻的实际位置再和路径上同一时刻的路径点坐标做距离判断。简单说障碍物在t时刻位置为( obs(t) )路径点在t时刻位置为( p_i(t) )那么威胁判断的输入是这两者在同一时刻的距离差。这里有一个操作细节值得注意路径点本身只有离散的索引比如路径点1、2、3……它并不天然自带时间标签。为了让路径点的时间对齐我默认所有无人机同一时间步长从起点出发按恒定地面巡航速度推进到终点所以第k个路径点对应的就是第k个时间步。如果你的场景里无人机速度不一致那就需要把时间归一化到每个路径点上或者直接在编码向量里加入速度维度。3. AE算法在MATLAB中的落地实现理论说完了接下来是真正的代码环节。我会把核心模块逐个拆开讲每个模块干什么、有哪些参数、为什么这么设都会说清楚。这一部分是你直接能抄作业的部分。3.1 文件结构与主循环我的完整工程包括以下文件每个文件的职责边界都比较清晰main_dynamic_AE.m主程序负责场景初始化、滚动规划主循环、作图与结果统计。AE_Optimizer.mAE算法核心优化器包含种群初始化、变异、选择、Alpha更新。fitness_eval.m适应度评估函数包含路径长度、障碍物威胁、协同避撞三项。BSpline_Smooth.m路径平滑模块用三次B样条对优化出的路径点做光顺处理。dynamic_obstacles.m动态障碍物生成与状态更新。collision_check.m硬碰撞校验辅助观察最终轨迹质量。plot_trajectories.m轨迹可视化。主程序的滚动时域逻辑是这个样子%% 多无人机动态避障滚动重规划主循环简化示意 for t 1:T_max % 1. 获取当前时刻的障碍物状态静态动态 obs_now dynamic_obstacles(obs_center, obs_radius, obs_vel, t); % 2. 调用AE优化器输入为当前无人机位置、目标点、障碍物状态 [opt_paths, best_J] AE_Optimizer(poses, goals, obs_now, params); % 3. 对优化出的路径做B样条平滑与动力学校验 for k 1:N_drone smooth_paths{k} BSpline_Smooth(opt_paths{k}, params.order); end % 4. 无人机沿平滑路径飞一小段更新当前位置 poses move_along(poses, smooth_paths, params.dt); end这里最关键的参数是重规划周期。我实测中建议规划周期取0.5秒到1秒之间太短会导致算法没充分收敛就抢着输出路径质量不稳定太长又会错过避障窗口期。合理的设置是感知更新步长取0.25秒重规划周期取0.5秒这样感知两次规划一次给算法留出足够时间。3.2 AE优化器核心代码框架AE优化器的核心是围绕Alpha个体做定向变异。每次迭代Alpha就是当前最优解其他个体通过差异向量向Alpha靠拢同时保留一定随机性防止陷入局部最优。这里给出变异算子的关键实现function [new_pop, alpha_idx] AE_Mutation(pop, fit, Alpha, beta, F) % pop: 当前种群个体按行排列 % fit: 适应度值 % Alpha: 当前最优个体向量 % beta: 向Alpha靠拢的收敛系数 % F: 差分缩放因子 [N_pop, dim] size(pop); new_pop pop; for i 1:N_pop % 随机选择两个与i不同的个体 r1 randi(N_pop); while r1 i, r1 randi(N_pop); end r2 randi(N_pop); while r2 i || r2 r1, r2 randi(N_pop); end % 定向变异向Alpha收缩 差分扰动 new_pop(i, :) pop(i, :) beta .* (Alpha - pop(i, :)) F .* (pop(r1, :) - pop(r2, :)); end % 边界约束处理把越界坐标拉回到可行域内 new_pop min(max(new_pop, lb), ub); end这段代码看起来简单但它准确地捕捉了AE的核心思想Beta项保证了向最优解的学习F项保证了种群的多样性。我在具体实现时Beta一般取0.5到0.8之间F取0.3到0.5之间。Beta太小收敛慢Beta太大容易把种群的好个体全部拉到同一位置丢失多样性F同理太大会让路径抖动剧烈太小又缺乏探索能力。选择机制上我采用的是锦标赛选择和精英保留结合。每一代先找出适应度最好的个体直接进入下一代其余个体通过和新个体的适应度对比来选择是否替换。这样一方面确保Alpha只会越来越好另一方面给新生成的个体足够的替换机会。3.3 适应度评估的向量化实现初版代码里我对每个个体循环计算适应度结果在种群大小为60、路径点数量为20、无人机数量为5时单次评估就要跑零点几秒50次迭代就要几十秒完全没法用于滚动规划。后来我做了MATLAB的矩阵化重构把适应度计算的三个模块全部改成了矩阵运算速度提升了至少20倍。核心思路是把所有个体的路径点坐标存成三维矩阵维度是种群大小无人机数量路径点坐标维度。然后一次性算出所有个体的路径长度、碰撞距离、机间距等值再按预定义的权重合成总的适应度向量。function J fitness_eval(pop, goals, obs_now, w, d_safe) % pop: 种群个体矩阵维度 [N_pop, N_drone * dim] % goals: 目标点矩阵 % obs_now: 当前障碍物状态包含圆心坐标和半径 % w: 权重向量 [w_len, w_obs, w_coop] % d_safe: 安全距离阈值 N_pop size(pop, 1); J_len sum(sqrt(sum(diff(reshape(pop, N_pop, [], dim), 1, 2) .^ 2, 3)), 2); J_obs zeros(N_pop, 1); for k 1:size(obs_now, 1) d_obs sqrt(sum((reshape(pop, N_pop, [], dim) - obs_now(k, 1:dim)) .^ 2, 3)); d_obs min(d_obs, [], 2); % 每个个体到该障碍物的最近距离 J_obs J_obs max(0, obs_now(k, end) d_safe - d_obs) .^ 2; end J w(1) * J_len w(2) * J_obs w(3) * J_coop; end这段代码里有一个容易踩的细节reshape(pop, N_pop, [], dim)这一步意味着路径点的排列顺序必须严格保持一致否则所有路径点的位置都会错位。我编码时统一用“先无人机再路径点再坐标维度”的顺序比如第一架无人机的第1个路径点的x坐标存放在第1列y坐标在第2列第2个路径点的x坐标在第3列以此类推。这个顺序一旦定下来后续所有模块都必须遵守不然后期查错非常折磨人。3.4 B样条平滑模块直接优化出的路径是折线路径折线在转向处是不连续的无人机如果真按这个轨迹飞每个节点处都需要急停和急转弯飞行时间和能量消耗都会显著增加。平滑方案我用的是三次B样条它有一个优点平滑后的曲线严格通过插值控制点附近不会出现过大的形状偏移而且曲率连续。function smooth_path BSpline_Smooth(points, order) % points: 输入原始路径点n×dim % order: B样条阶数一般取4三次B样条 t_vec linspace(0, 1, size(points, 1)); knots augknt([0 1], order); % 均匀节点向量实际使用需根据点数调整 smooth_path spapos(knots, order, points); end需要注意B样条平滑后得到的轨迹点和原始路径点数量可以不同我会在重规划后重新采样到固定步数确保后续的时间对齐计算仍能正常进行。4. 实操参数与对比实验分析光把代码跑通不算完路径规划算法好不好用得拿数据说话。我在某模拟场景里做了一组对比实验场景设置是5架无人机、12个静态障碍区、3个动态障碍物起终点随机生成每一轮规划周期0.5秒AE种群规模60最大迭代80代。对比对象是标准蚁群算法ACO和标准遗传算法GA。4.1 收敛速度对比差出来一个数量级在同样的初始种群下三个算法都经过80次迭代AE最终的路径总长度最短且在第17代左右就基本收敛了。GA到第38代才稳定ACO因为是基于栅格搜索的离散优化器收敛更慢到第60代之后才勉强跟上而且因为栅格分辨率限制路径总长度天生就比连续优化器差一些。这里有一个很重要的实践体会如果做在线规划GA和ACO在第20代时的路径还非常粗糙而AE在15代到20代时已经能给出可飞路径这意味着你可以直接用更小的迭代预算获得可用解把剩下的时间预留给障碍物感知和其他上层决策。4.2 动态障碍下的安全率统计我做了一组蒙特卡洛测试每个场景随机生成不同数量的动态障碍物3到6个速度大小在每秒2米到8米之间随机分布统计100轮规划中发生碰撞的比例。AE方案在动态障碍下的无碰撞率达到97%以上碰撞事件全部发生在障碍物突然改变运动方向而重规划尚未完成的极短时间窗口内。这个碰撞的原因是算法本身解决不了的感知延迟。为了缓解这个问题我在主循环里加入了“安全缓冲层”——在动态障碍物运动方向预测基础上外扩一个安全半径相当于把最坏情况提前纳入代价计算。外扩半径取当前速度乘以一个规划周期时间这样即使重规划有半个周期的延迟依然在安全裕量内。5. 常见问题与调试技巧实录写这套MATLAB程序的过程中踩过不少坑有些问题在代码层面很好排查有些问题则出在建模思路上面。我挑出几个最有代表性的放这里做成一站式排查清单。5.1 适应度函数里安全项权重过大导致路径绕圈一开始我把障碍威胁权重设得很高结果是每一代优化出的路径都不停在贴障碍边缘走甚至出现“为了绕开一个半径很小的孤立障碍而在平坦区域绕出一个大弧线”的现象。原因就是安全项的梯度太陡导致长度代价在局部范围内的作用力不明显。我的解决办法是把威胁代价改为分段函数当距离大于阈值的两倍时代价值为0不干扰路径的正常优化当距离在阈值到两倍之间时线性惩罚起到“温和引导”的作用当距离小于阈值时迅速放大到极大值通过指数惩罚实现硬约束。这样既保证了路径不会贴着障碍物走又不会因为威胁代价的全局影响而让路径妥协太多。5.2 种群提前收敛所有个体挤到同一条路径上滚动规划时上一轮的解作为下一轮Alpha的初始值这个操作虽然保证了连续性但也带来了一个副作用如果上一轮的路径已经收敛到某个局部最小值下一轮整个种群都会围着这个局部最优打转很难跳出来。尤其当动态障碍物突然改变位置时旧路径区域甚至可能已经不可行这时如果种群多样性差算法就半天反应不过来。我的对策是在每轮重规划时从Alpha周围加入20%的完全随机扰动个体相当于给种群注入新鲜血液。这些随机个体在早期迭代中会拉低种群平均适应度但它们保证了解空间中被探索的范围几个周期之内就常能找到更好的新路径。5.3 MATLAB运行速度慢循环太多如果你在适应度评估函数里写了大量的双循环那么你的MATLAB代码大概率在大规模问题下跑不动。解决方式几乎只有一个把所有能向量化的计算全部向量化。这个不只是适应度函数的问题包括障碍物更新、碰撞检测、绘图数据生成也都是向量化思路。我把障碍物的运动更新写成一个简单的时间函数之后整个三维场景中几秒钟的仿真运行时间从九十多秒缩到了不足四秒。5.4 光滑后的路径与障碍物发生新的干涉这是最容易被忽视的坑。原始优化出的路径在B样条平滑后形状发生了局部偏移导致某些原本安全的转弯点从障碍物旁边滑进障碍区域内。很多人在平滑这一步不复查碰撞最后飞行仿真阶段才爆雷。我在平滑后加了一重碰撞检测如果平滑轨迹触发了碰撞则把平滑点数量减小一半改用更贴近原始路径的插值方式重做一版。同时在代价函数中对路径点与障碍物之间保留额外的光滑裕量这样B样条的可移动空间就不会突破安全边界。6. 方案可以怎么扩展如果只是做仿真验证这套AE动态避障方案已经足够完整。但如果要往实机部署的方向走有几个扩展方向值得关注。动力学约束扩展目前模型只规划几何路径没有加入无人机自身的最大速度、最大加速度和转弯半径约束。实际部署时需要在代价函数中加入速度、加速度的显式建模或者在B样条平滑后做一次动力学可行性检查根据动力学模型参数裁剪路径中过陡的转弯段。时间协同优化多无人机同时到达终点或按预设时间差到达是很多任务的硬需求比如编队表演和协同侦察。目前是基于固定巡航速度假设的隐式同步更严谨的做法是把每架无人机的速度曲线退化成一个独立决策变量放入AE的编码向量中同时优化路径形状和速度剖面。动态避障策略升级当前策略是“避障优先、路径最优”。某些复杂场景下减速等待可能比大幅绕行更高效。可以在代价函数中引入减速惩罚和急转惩罚让算法自行决策是绕还是等。传感器模型融合仿真里的障碍物信息是完美的但实际中感知数据有噪声、有延迟、有遮挡。比较务实的做法是在规划前对感知数据做一层滤波和不确定性外扩把定位误差的方差加入安全距离阈值中。个人经验中AE算法用在动态多机路径规划上最大的好处不是单次解质量多优而是整个系统处于动态变化时它的自适应性和连贯性极佳。这也符合我实测多轮滚动规划下来的感受路径序列之间的跳变很小飞行过程平稳不会出现“上一秒往左下一秒突然改往右”的激进换道。最后再分享一个小技巧如果觉得滚动规划的实时性还差一点可以尝试把上一轮的优秀个体前20%直接保留到下一轮种群中这一招能大幅减少后续迭代的收敛时间而且基本不会损失解质量。
