搞无人机路径规划绕不开“全局找路 局部避障”这个老话题。我之前试过只用粒子群优化PSO做全局规划静态场景下效果确实不错但障碍物一旦动起来重规划的成本高到没法接受无人机经常还没绕开新障碍就撞上去了后来又单独上动态窗口法DWA做局部躲避动态障碍是能躲开了但无人机在复杂环境里容易原地绕圈跑到一半就钻进死胡同出不来。这俩毛病刚好互补所以我就把PSO和DWA串成了一个混合规划器先用粒子群在全局尺度上找一条平滑的参考航迹再让动态窗口法在飞行过程中实时规避突发障碍。整套仿真在Matlab里跑下来三维动静结合环境下的避障效果比我想象中稳。这篇文章就从需求拆解、算法原理、环境建模、核心代码、仿真分析和调参经验几个角度完整记录一遍适合正在做无人机路径规划、机器人导航或者相关课题的同学参考。1. 需求拆解为什么要把PSO和DWA绑在一起1.1 单一全局规划器的痛点很多刚接触路径规划的人会有一个错觉全局规划器只要跑得够快理论上就能在障碍物出现时立刻重新规划一条新路径那还要局部规划器干什么现实完全不是这样。全局规划通常建立在“地图已知且基本不变”的假设上它需要把整条可行路径搜索出来这个过程无论用什么算法都需要一定计算时间。在三维空间里搜索空间比二维大了不止一个量级一次重规划动辄几十上百毫秒。无人机在飞行时几十毫秒够它飞出好几米了动态障碍物移动速度再快一点重规划根本来不及。我第一次尝试只用PSO做三维避障在静态障碍物场景里收敛得很好路径也平滑。但只要把场景里加上一个匀速运动的球体障碍整个规划就崩了每迭代一轮适应度函数里都要把新障碍数据重新算进去粒子群整体震荡路径抖动非常厉害而且时不时就有一条路径穿过障碍物附近被罚分罚到离谱。这说明把动态信息强行塞进全局优化框架里既不优雅也不可靠。1.2 单一局部规划器的困境DWA本身的逻辑很直白在当前时刻根据无人机自身的速度限制枚举所有可能的速度组合模拟未来一小段时间内的运动轨迹再用评价函数选一条得分最高的轨迹执行。这个思路天生适合动态环境因为它的“视界”就是眼前这一亩三分地反应速度快能及时躲开突然出现的障碍。但问题恰恰出在“眼前”两个字上。DWA的每一次决策只依赖局部信息缺少对终点方向的长期约束。我单独用DWA跑一个U形障碍场景时无人机绕到一半因为前方所有可选轨迹的航向得分都差不多它就选择了一个相对省事但方向偏了的速度结果在障碍物侧面来回摆动跑成了“画圈模式”。这是典型的局部极小值问题光靠多添几个权重系数解决不了必须有一个全局信息在背后给它指方向。1.3 混合架构全局找路局部保命PSO负责“指方向”DWA负责“躲眼前”这个组合的思路几乎是水到渠成。PSO的输出结果是整条参考路径上的一系列航迹点DWA在执行飞行时不需要每一次都计算到终点的全局距离只需要跟踪参考路径上的局部目标点即可。这个局部目标点会随着无人机前进不断更新整个系统兼顾了全局最优性和局部响应速度。在Matlab里实现这套架构最关键的是定义清楚两个模块之间的数据接口PSO输出的是一个N×3的路径点矩阵DWA收到的则是其中离当前位置最近且前向一定距离的那个参考点。我在实际实现中把“跟踪距离”设为1.5米也就是无人机每次以1.5米之外的参考点作为局部目标这样既能避免目标点太近造成频繁切换也能防止太远导致局部避障失去了意义。这个值不是拍脑袋定的和无人机速度、DWA采样周期都有关系后面调参部分会详细说。2. 算法原理PSO与DWA到底在干什么2.1 PSO的粒子表示与适应度函数设计粒子群优化的核心在于“粒子怎么编码”和“怎么评价”。在三维路径规划场景里我采用了一种比较常用的编码方式预设路径上一共有M个中间路径点每个点用三维坐标表示于是一个粒子就是一个长度为3M的向量。M取多少取决于路径复杂度和地图尺寸我测试下来M取8到12比较合适太少路径不够灵活太多搜索维度过高收敛速度会被拖垮。评价一个粒子即一条候选路径好坏我用了三个代价项加权求和路径长度代价计算相邻路径点的欧氏距离总和保证路径尽量短安全代价分成两部分若路径点直接落在障碍物内部则给一个极大惩罚若距离障碍物表面小于安全阈值则按威胁程度线性惩罚平滑度代价则计算相邻三段路径之间的转向角度角度越大罚分越高避免无人机频繁掉头。权重设置上我最初用了路径长度1.0、安全代价2.0、平滑度0.5的组合。后来发现安全代价权重其实可以再拉高因为安全永远是第一位的。我最终调成1.0、3.0、0.4收敛出来的路径在保证安全的前提下依然比较短。粒子群更新公式就是标准的速度-位置更新速度 惯性权重 × 当前速度 个体学习因子 × 随机数 ×个体最优 − 当前位置 群体学习因子 × 随机数 ×群体最优 − 当前位置。这里有一个容易被忽略的细节惯性权重w应该随迭代线性递减从0.9衰减到0.4。因为前期需要较大的搜索范围防止粒子过早扎堆后期则希望粒子在最优解附近精细搜索w小了收敛更稳定。2.2 DWA的采样空间与轨迹评估DWA在三维空间里的采样维度要比二维多不少。二维DWA只需要采样线速度v和角速度ω三维无人机模型如果简化成质点不显式考虑姿态角则要采样三个方向的分速度vx、vy、vz外加偏航角速度yaw_rate。不过实际仿真中为了避免采样组合爆炸我把垂直速度vz的采样范围和水平速度的采样范围区别对待。比如水平速度范围设为0到3米/秒垂直速度范围设为-1到1米/秒这样既能满足三维飞行需要又不会因为采样空间太稀疏而错失最优轨迹。轨迹预测部分我沿用了经典DWA的“圆轨迹假设”在极短时间间隔内无人机可以近似作匀速直线运动所以每一小段的位移就是速度乘以采样时间把多段累加起来就得到预测轨迹。预测时间窗口我设为2秒步长0.1秒也就是每条候选轨迹由20个点组成。窗口太短来不及提前避障窗口太长运动学模型的误差会被放大评价结果失真。评价函数包括三项航向偏差项候选轨迹末端方向与局部目标方向的夹角夹角越小得分越高速度项鼓励无人机保持较快速度但这里要做个平衡不能一味追求速度导致避障不及时安全余量项计算预测轨迹与所有障碍物的最小距离距离越大越安全。2.3 三维场景下的特殊处理三维路径规划相比二维最大的区别不是多了一个坐标轴而是“障碍物感知”和“运动学约束”的建模方式都变了。二维栅格地图里每个格子只有占用和空闲两种状态三维地图需要额外考虑高度信息。我用的方法是用多层二维栅格叠加来近似三维空间每个栅格点存储一个高度值无人机的高度必须大于该点的高程值才算安全。动态障碍物的建模也做了简化。动态障碍物用一个中心坐标加半径的球体表示它的运动状态是位置加匀速速度向量。碰撞检测时计算无人机预测轨迹点到球心的距离小于球半径就判定为碰撞。如果障碍物是移动的还需要在时间轴上做预测也就是计算未来某个时刻障碍物可能到达的位置再和无人机同一时刻的预测位置做距离判断。这在Matlab里用矩阵运算实现起来很方便一个meshgrid加逻辑判断就能批量算完一整个预测轨迹的碰撞状态。3. Matlab环境建模与仿真框架搭建3.1 三维栅格地图与动态障碍物建模我搭建的仿真环境是一个80×80米的水平区域高度上限30米地图分辨率设置为1米也就是一个80×80×30的逻辑三维格点。静态障碍物我自己设计了几个不规则形状的“山体”和几个柱状障碍用Matlab的surf函数配合高度矩阵来可视化。实际判断时则把地图转成一个逻辑数组某点(x, y, z)是否被占用通过查表就能快速判断。动态障碍物我设计了两种运动模式一种是沿直线匀速运动的球体模拟另一个低空飞行器另一种是在固定区域来回往返的球体模拟类似巡逻路径的移动物体。这两种模式基本能覆盖常见的动态避障测试需求。地图的数据结构我用了一个结构体struct包含分辨率、尺寸、静态障碍高度矩阵、动态障碍位置与速度等字段。这个结构体会同时传给PSO和DWA模块保证两个模块看到的是同一份环境信息避免因为数据不一致产生莫名其妙的规划结果。3.2 无人机的运动学模型仿真中我把无人机当成一个三维空间的质点来处理状态量是位置坐标(x, y, z)和三个方向的速度(vx, vy, vz)。没有引入无人机本身的偏航角、俯仰角动力学原因是这个研究的重点在路径规划算法不在飞行控制。质点模型已经能够体现路径规划的核心逻辑而且能和DWA的速度采样直接对应做起来最干净。无人机的性能约束包括水平最大速度3米/秒垂直最大速度1米/秒最大加速度1米/秒²。这些约束直接写进DWA的采样边界里保证规划出来轨迹都是无人机物理上能执行的。3.3 碰撞检测与评价指标碰撞检测我封装成了两个函数一个是静态碰撞检测输入一个三维坐标点判断该点是否在静态障碍物内部或者离障碍物太近另一个是动态碰撞检测输入一条预测轨迹和当前所有动态障碍物的未来状态判断整条轨迹上是否存在碰撞点。评价算法效果的指标我比较看重三条路径总长、飞行总时间、最小安全距离。路径总长反映全局规划质量飞行总时间反映整体效率最小安全距离则直观体现避障是否到位。这组指标在仿真结束后自动统计我会在控制台一次性输出方便做对比评估。4. 核心代码实现与关键参数解析4.1 PSO全局路径规划代码PSO模块的入口函数如下它接收三维地图、起点、终点以及全局路径点数输出一条全局参考路径function [path, best_fitness_curve] pso_global_plan(map3d, start_pos, goal_pos, num_waypoints) % 参数设置 num_particles 50; max_iter 200; dim num_waypoints * 3; % 粒子位置初始化在起点到终点的连线附近加随机扰动 line_coords linspace(0, 1, num_waypoints 2); base_line start_pos line_coords * (goal_pos - start_pos); base_line base_line(2:end-1, :); % 去掉首尾固定点 particles repmat(base_line(:), num_particles, 1) randn(num_particles, dim) * 2; velocities zeros(num_particles, dim); pbest particles; gbest particles(1, :); gbest_fitness inf; w_start 0.9; w_end 0.4; for iter 1:max_iter w w_start - (w_start - w_end) * iter / max_iter; for i 1:num_particles fit evaluate_path_3d(particles(i, :), map3d, start_pos, goal_pos, num_waypoints); pbest_fit evaluate_path_3d(pbest(i, :), map3d, start_pos, goal_pos, num_waypoints); if fit pbest_fit pbest(i, :) particles(i, :); end if fit gbest_fitness gbest_fitness fit; gbest particles(i, :); end end r1 rand(num_particles, dim); r2 rand(num_particles, dim); velocities w * velocities 2.0 * r1 .* (pbest - particles) 2.0 * r2 .* (repmat(gbest, num_particles, 1) - particles); particles particles velocities; best_fitness_curve(iter) gbest_fitness; %#okAGROW end path [start_pos; reshape(gbest, num_waypoints, 3); goal_pos]; end这里有个容易踩的坑粒子初始化如果直接用随机数铺满整个搜索空间路径很容易乱成一团进化效率极低。我在初始化时让粒子围绕起点到终点的直线分布相当于在一条可行参考线附近做局部搜索收敛速度快非常多而且初始路径质量就有基本保障。4.2 适应度函数的实现细节适应度函数决定了粒子群的进化方向这个函数写得越合理规划结果越干净function fit evaluate_path_3d(x, map3d, start_pos, goal_pos, num_waypoints) points reshape(x, num_waypoints, 3); path [start_pos; points; goal_pos]; % 路径长度代价 segment_len sqrt(sum(diff(path).^2, 2)); length_cost sum(segment_len); % 安全代价 safe_cost 0; min_safe_dis 1.5; penalty 1000; for k 1:size(path, 1) p path(k, :); if check_static_collision(p, map3d) safe_cost safe_cost penalty; else dist_obs min_distance_to_static_obs(p, map3d); if dist_obs min_safe_dis safe_cost safe_cost (min_safe_dis - dist_obs) * 10; end end end % 平滑度代价 smooth_cost 0; for k 2:size(path, 1) - 1 v1 path(k, :) - path(k-1, :); v2 path(k1, :) - path(k, :); cos_angle dot(v1, v2) / (norm(v1) * norm(v2) eps); cos_angle max(min(cos_angle, 1), -1); turn_angle acos(cos_angle); smooth_cost smooth_cost turn_angle^2; end fit 1.0 * length_cost 3.0 * safe_cost 0.4 * smooth_cost; end这段代码里有两处值得留意一是安全权重给到了3.0这个权重确实是越高越保守但不能太高否则粒子会为了绕远路把路径长度拉得很长整体效率反而下降二是平滑度代价用了夹角平方相比直接用夹角平方项对大角度转向的惩罚更重能够有效压制路径“锯齿”现象。4.3 DWA局部避障代码DWA模块是实时避障的核心代码如下function [v_best, traj_best] dwa_local_plan(state, local_target, obstacles_dyn, map3d, params) % 速度采样边界由无人机物理约束决定 vx_range 0:0.2:params.vx_max; vy_range -params.vy_max:0.2:params.vy_max; vz_range -params.vz_max:0.2:params.vz_max; yaw_range -params.yaw_max:params.dyaw:params.yaw_max; v_best [0 0 0 0]; score_best -inf; traj_best []; for vx vx_range for vy vy_range for vz vz_range for yaw_rate yaw_range vel [vx, vy, vz, yaw_rate]; traj predict_trajectory(state, vel, params.dt, params.horizon_steps); if check_dynamic_collision(traj, obstacles_dyn) continue; end % 三项评价指标 heading_score calc_heading_score(traj, local_target); vel_score norm([vx, vy, vz]) / params.v_norm_fac; clear_score calc_clearance_score(traj, obstacles_dyn, map3d); score params.w_head * heading_score params.w_vel * vel_score params.w_clear * clear_score; if score score_best score_best score; v_best vel; traj_best traj; end end end end end end四层for循环嵌套在Matlab里确实跑得慢一个采样周期的计算量大概是几百条轨迹。为了加速我用了两个技巧一是提前排除掉明显不合理的速度组合比如无人机已经接近某个障碍物还保持高速飞的组合二是把所有候选轨迹存成矩阵用Matlab的矩阵运算批量计算碰撞检测而不是逐条调用函数。实测优化之后单个DWA周期从80毫秒降到15毫秒左右达到了仿真实时性的基本要求。4.4 主循环全局规划与局部避障的衔接主流程的逻辑很简单先调PSO生成全局参考路径然后进入一个仿真循环每一帧用DWA计算当前最优速度更新无人机位置并判断是否需要切换局部目标点。核心代码如下% 第一阶段全局规划 global_path pso_global_plan(map3d, start_pos, goal_pos, num_waypoints); % 第二阶段局部跟踪与避障 state [start_pos, 0, 0, 0, 0]; trajectory_hist state(1:3); target_idx 1; for t 1:params.sim_steps % 更新局部目标点选取前方1.5米处最近的参考点 while target_idx size(global_path, 1) dist_to_target norm(global_path(target_idx, :) - state(1:3)); if dist_to_target params.lookahead_dist target_idx target_idx 1; else break; end end local_target global_path(target_idx, :); % 调用DWA计算最优速度 [v_best, ~] dwa_local_plan(state, local_target, obstacles_dyn, map3d, params); % 更新状态 state(1:3) state(1:3) v_best(1:3) * params.dt; state(4:6) v_best; trajectory_hist(end1, :) state(1:3); %#okAGROW % 目标点达到判断 if norm(state(1:3) - goal_pos) 1.5 break; end end这里要特别提醒一点局部目标点的切换不能太频繁。我第一次写的时候每帧都重新找最近参考点结果在参考路径曲率较大的位置目标点会在两个相邻点之间来回跳导致DWA输出的速度方向抖动。加了一个“只有距离小于前瞻阈值才切换”的条件之后这个问题就消失了。而前瞻阈值的选取也很有讲究无人机速度快、DWA控制频率低时阈值要适当调大。5. 仿真实验与结果分析5.1 静态场景全局路径的收敛效果第一个实验场景是静态障碍物起点在(5, 5, 5)终点在(75, 75, 15)中间分布着两个山体状障碍物和三个柱状障碍物。PSO运行200代之后适应度曲线从初始的450左右下降到180左右前半段下降很快后100代基本平稳。从规划出的路径看无人机从起点出发后先爬升到安全高度绕过第一个山体再下降到相对较低的高度从柱状障碍之间穿过最后爬升至终点高度。路径整体很平滑没有锯齿状的抖动。路径总长约130米相比起点到终点直线距离约99米多出来的31米基本都花在了翻越障碍物上这个代价是合理且必要的。我也对比了M取不同值的效果。M太小比如4路径过于僵化遇到复杂地形根本绕不过去M太大比如15粒子维度变成45收敛所需迭代次数明显增加而且经常出现某些路径点冗余堆叠在一起的情况。综合来看M取10在当前地图尺寸下表现最均衡。5.2 动态场景实时避障与重规划第二个实验在静态场景基础上加入两个动态障碍物一个是沿直线从左侧穿入规划区域速度1.5米/秒半径2米另一个在终点附近往返巡逻速度0.8米/秒半径1.5米。无人机在飞行过程中第一次动态障碍出现在第35秒左右DWA在预测轨迹阶段提前检测到碰撞风险果断选择了降速和侧向偏移的组合绕开障碍之后迅速回到参考路径附近。整个避障过程最大横向偏移约3米没有出现大幅度的绕路。第二次动态障碍在终点附近因为巡逻障碍的运动方向与无人机前进方向垂直DWA选择先悬停等待一个短窗口再从障碍后方绕过去这个行为从结果上看非常“聪明”其实是航向得分和安全得分共同作用的结果。这里有个细节值得分享如果DWA的预测时间窗口设得太短比如只有0.5秒第一次动态避障就会失败因为无人机在0.5秒内根本来不及做出足够的横向位移等到发现碰撞时已经晚了。我把窗口拉长到2秒之后避障动作有了充分的提前量成功率大幅提升。5.3 参数对性能的影响我对几个关键参数做了敏感性测试结论如下惯性权重衰减区间对整个优化效果影响非常大。固定w0.9不衰减粒子群一直保持大范围搜索收敛到150代以后还会出现路径跳变固定w0.4不衰减前期探索不足粒子容易困在局部最优路径里。线性衰减从0.9到0.4在这组测试里效果最好。DWA评价函数的三个权重w_head、w_vel、w_clear之间是此消彼长的关系。w_clear权重过高会导致无人机不敢前进在障碍物附近频繁减速甚至停滞w_vel权重过高则会让无人机“莽”着往前冲避障不及时。我最终用的比例是w_head0.5、w_vel0.2、w_clear0.3这个组合在“及时反应”和“不轻易停摆”之间找到了平衡点。我还测试了前瞻距离从1米到3米的变化。前瞻距离太短无人机容易贴着障碍物边缘走危险系数高太长则会让DWA过早对远处的参考点做出反应出现不必要的绕路。1.5米在3米/秒的速度下约等于0.5秒的前视时间视觉上较为从容也和DWA预测窗口匹配。6. 常见问题与调参避坑6.1 问题速查表下面这个表格是我在调试过程中存下的典型问题、可能原因和对应解法现在基本成了我的速查工具现象可能原因解决方案PSO收敛到很差的局部最优粒子数太少或初始分布太散增加粒子数到80以上初始化时沿起点-终点直线加扰动粒子群后期路径还在大幅跳变惯性权重没有衰减或衰减过早使用线性衰减0.9到0.4并确认迭代次数足够DWA在障碍物附近经常停滞安全权重过高或预测窗口过短调低w_clear、拉长预测窗口到2秒以上无人机避障路径严重偏离参考线前瞻距离太长或航向权重不足缩短前瞻距离或提高w_head占比仿真运行速度太慢四层循环采样太多无效轨迹用矩阵加速碰撞检测先做物理可行性筛选明明没碰障碍物却报碰撞安全阈值设得过大把min_safe_dis从1.5调到0.5~1.0之间6.2 我的调参顺序建议很多新手拿到这套代码第一反应就是把所有参数蒙一遍看哪个组合效果最好。我的建议是不要这样做而是按下面这个顺序来先定PSO的路径点数量M。这个参数直接决定搜索空间维度必须根据地图尺寸和障碍物复杂度来定地图越复杂M越大但不宜超过15。然后是PSO的粒子数和迭代次数优先观察适应度曲线是否收敛如果曲线尾部还在明显下降说明迭代次数不够如果曲线前20代就平了说明粒子数可以减半来省时间。再调DWA的速度采样范围和预测窗口。预测窗口先在1.5秒到2秒之间选定然后根据无人机速度做微调。采样范围要保证能覆盖无人机物理上可行的速度组合太窄会让算法没有选择空间太宽则浪费时间在无效轨迹上。最后调DWA三个权重。我的经验是先用默认比例跑一遍观察无人机在遇到障碍时的行为如果总在很远的地方就拐弯说明w_clear偏高如果临近障碍才猛打方向说明w_head偏高。根据行为反馈微调一次只调一个参数。6.3 一个隐藏很深的坑PSO与DWA的坐标系对齐这个坑严格来说不是算法问题而是工程项目里最容易犯的低级错误。我在调试时发现了一个诡异现象PSO规划出来的全局路径在可视化里完全正常但DWA跟踪时总是偏离目标方向。排查到最后发现PSO输出的路径点坐标是“网格坐标”也就是栅格序号而DWA里使用的状态量是“世界坐标”实际米制坐标。两者相差了一个分辨率系数的倍数导致局部目标点被放到了错误的位置。这个问题不是Matlab特有的任何把“地图索引”和“现实坐标”混用的代码都会有这个坑。我后来在PSO输出位置强制做了一次坐标转换并加了断言来保证坐标范围合法才彻底解决。写代码时建议用注释把每个变量的单位标注清楚这种经验都是用加班换来的。7. 最后再分享一点实操层面的体会整套系统跑下来我最深的感触是混合规划器的难点不在单个算法本身而在两个算法之间数不清的边界条件。PSO的参数确定了全局搜索的精细度DWA的参数决定了局部反应的灵敏度这两个模块单独拿出来都很成熟但把它们优雅地拼到一起需要反复实验、积累手感。有一次我为了验证算法的鲁棒性故意把动态障碍物速度从1米/秒调到2.5米/秒让无人机几乎没有任何冗余时间去躲避。结果DWA在最后一刻选择了剧烈的爬升动作虽然轨迹险到极限但还是成功避开了。那次仿真让我意识到三维路径规划相比二维最大的天然优势就是多了一个可以利用的逃逸维度——别把垂直方向的速度限制得太死它在某些极端时刻就是救命的关键。如果你是刚接触这个方向我建议先别急着上完整的三维动态场景。先把PSO和DWA分别跑熟理解每个参数在单模块里的作用再合并到一套框架里调参。另外Matlab仿真里跑出来的最优参数换到其他地图和环境下通常需要重新标定不存在一套参数打天下的说法。把参数曲线和结果日志保存下来对比能帮你更快地找到不同场景下参数的变化规律。