搞无人机自主避障绕不开的一个算法就是DWA。以前我用它做过地面移动机器人后来转到四旋翼上发现这套基于动态窗口的局部路径规划思路在三维空间里一样能打而且配合MATLAB做仿真验证调参周期比真机试错快得多。这篇就把我当时用DWA在动态环境下做无人机避障路径优化的完整过程盘一遍包括算法原理、MATLAB代码结构、动态障碍物的处理思路以及那些文档里不会写的参数调优坑点。适合刚接触路径规划、想在MATLAB里跑通一套无人机避障demo的同学也适合已经跑过全局规划、想给系统加上局部避障能力的朋友参考。1. 项目整体设计与思路拆解1.1 为什么在动态环境下选DWA无人机避障方案其实不少人工势场法、RRT系列、A*、MPC各有各的适用场景。我之所以在这个项目里坚持用DWA核心原因是它在动态环境下的性价比非常高。人工势场法在静态环境里表现不错但遇到运动中的障碍物容易陷入局部极小点无人机可能在障碍物前反复振荡。RRT属于采样规划能容忍高维空间但生成的路径往往不够平滑需要后处理实时性也有点吃紧。MPC在理论上是比较优的解法但计算开销大放在嵌入式飞控上跑对硬件要求偏高。DWA的思路完全不同。它不做全局搜索只在当前时刻的指令速度和角速度空间里采样出一批候选速度然后用运动学模型向前模拟一小段轨迹用评价函数打分挑分数最高的作为当前执行指令。这种短视策略丢掉了一定的全局最优性但换来了极高的实时性和对动态环境的快速反应能力。特别是当环境中存在移动的障碍物时DWA每一步都在重新做决策天然适配这种快速变化的场景。在后来的项目里我们用的是全局规划局部规划的双层架构。全局层用RRT或A先算出一条参考路径局部层用DWA负责实时避障。DWA不需要知道整个地图只要传感器能给出当前窗口内的障碍物信息就能跑起来。这种解耦设计让系统维护和迭代都方便很多。1.2 无人机三维空间里的DWA要改什么地面机器人的DWA经典实现是在二维平面上做(x, y, θ)状态空间的采样。但四旋翼无人机是六自由度的直接套用二维DWA会丢掉高度变化和姿态调整的能力。我这里做了一个中间层处理不让DWA直接控制姿态而是把DWA的输出变成期望速度指令再交给底层姿态控制器去执行。具体来说无人机的DWA状态量包含x、y、z三个位置分量和一个偏航角ψ。速度空间从二维的(v, ω)扩展成(vx, vy, vz, ωψ)四维。采样时要注意vz的范围比xy平面速度小很多因为无人机在垂直方向上的机动能力本来就受限暴力采样只会生成一堆飞行器根本飞不出来的轨迹。还有一点很关键DWA在无人机上不能只看制导层要跟飞控内部的加速度限制、最大倾斜角限制做联动。也就是说DWA采样出来的速度指令必须经过一个可行性过滤确保无人机当前状态能执行出来。否则仿真里看起来漂亮的轨迹上真机直接掉高度或者爆炸。1.3 评价函数的权重设计思路DWA的核心竞争力很大程度体现在评价函数设计上方位角评价项heading奖励向目标点或全局参考路径对齐的方向障碍物距离评价项dist惩罚靠近障碍物的轨迹速度评价项velocity奖励保持前进速度避免无人机停滞这三个权重系数就是控制无人机性格的旋钮。α大飞机倾向于直奔目标哪怕障碍物比较近也敢穿β大飞机变得保守离障碍物老远就开始绕γ大飞机跑得快但可能牺牲平滑度。动态环境下权重不是固定不变的我做了一个根据环境拥挤程度动态调整权重的逻辑。当检测到周围的障碍物密集度超过一定阈值时自动增大β降低α让无人机优先保证安全间距。这个改进在仿真里表现非常明显固定权重的版本遇到密集动态障碍物时经常需要急刹车而自适应权重版本的飞行路径明显更顺畅。2. 动态窗口法的核心原理与MATLAB代码实现2.1 动态窗口是怎么切出来的DWA里的动态窗口指的是在速度空间中用三个约束切出来的一块可行区域。这也是整个算法名字的由来。第一个约束是电机/执行器物理极限约束。无人机任何时刻能达到的速度范围是有限的比如最大水平速度3m/s垂直速度1m/s最大角速度1rad/s。采样不能超出这些硬边界。第二个约束是动力学约束。在下一个控制周期到来之前无人机能加速到什么样的速度受限于最大加速度。假设当前水平速度是1.5m/s最大水平加速度0.5m/s²控制周期0.1s那下一时刻的水平速度最快也只能到1.6m/s。这个约束是动态窗口这两个字的直接来源它随着当前速度状态实时变化。第三个约束是安全约束。在候选速度对应的模拟轨迹上如果无人机会撞上障碍物就把这一组速度从备选集合里剔除。实际代码里不会等真的撞上去才判定而是预留一个安全刹车距离确保在任何速度下都能在碰撞之前把飞机停下来。这三个约束在MATLAB里汇总成一个采样集合格式很简单% 采样空间生成核心逻辑 for vx vx_min : vx_res : vx_max for vy vy_min : vy_res : vy_max for vz vz_min : vz_res : vz_max for w w_min : w_res : w_max % 1. 判断是否在动力学约束范围内 if abs(vx - current_vx) accel_lim(1) * dt continue; end % 2. 判断轨迹是否安全 traj simulateTrajectory(state, [vx, vy, vz, w], dt, sim_time); if checkCollision(traj, obstacles) continue; end % 3. 评价函数打分 score evaluate(traj, goal, obstacles); % 记录候选速度及分数 end end end end2.2 轨迹预测的细节轨迹预测是DWA评价的基础。对每一组候选速度模拟出一段时间内的运动轨迹然后在这条轨迹上评估方向、安全性和速度。无人机运动学模型可以用简化版本function traj predictUAVTrajectory(state, cmd, dt, simTime) % state: [x, y, z, psi] % cmd: [vx, vy, vz, w_psi] nSteps ceil(simTime / dt); traj zeros(nSteps 1, 3); traj(1, :) state(1:3); x state(1); y state(2); z state(3); psi state(4); for i 1:nSteps % 坐标系转换把机体坐标系速度转到世界坐标系 wx cmd(1) * cos(psi) - cmd(2) * sin(psi); wy cmd(1) * sin(psi) cmd(2) * cos(psi); wz cmd(3); x x wx * dt; y y wy * dt; z z wz * dt; psi psi cmd(4) * dt; traj(i 1, :) [x, y, z]; end end这里有个细节无人机和地面机器人的运动学很不一样。地面机器人近似是阿克曼或差速模型车辆只能朝当前朝向的方向前进。但无人机是全向的它可以保持机头朝向不变同时往侧向平移。所以在预测轨迹时速度转换那一步尤其重要不能像地面机器人那样直接叠加。我用的模型里cmd的速度是机体坐标系表示的。这意味着偏航角ψ决定了速度在世界坐标系里的投影方向。这个转换在仿真代码里必须严丝合缝一旦写错无人机就会出现明明朝东飞实际轨迹却偏北的诡异行为。我早期写这个模型时反过来转过一次结果调了大半天才发现是这样的低级错误。2.3 评价函数具体怎么打分评价函数的实现直接决定了避障行为的风格。我的打分函数分三部分方位角部分衡量轨迹末端点方向与目标/全局路径方向的偏差。代码里我做了归一化处理让偏离角度在0到π之间映射到1到0function headingScore calcHeadingScore(traj, refPath, idx, worldGoal) % 优先参考全局路径上的前瞻点没有全局路径就用终点 if idx length(refPath) target refPath(idx, 1:3); else target worldGoal; end endPoint traj(end, 1:3); vecToTarget target - endPoint; vecToTarget vecToTarget / norm(vecToTarget); dirOfTraj traj(end, 1:3) - traj(1, 1:3); dirOfTraj dirOfTraj / norm(dirOfTraj); dotProd dot(vecToTarget, dirOfTraj); angle acos(max(min(dotProd, 1), -1)); headingScore 1 - angle / pi; end注意这个函数里我用的是轨迹首末两点的方向向量而不是末端的速度向量。这样做的好处是降低噪声敏感性轨迹更加稳定。障碍物距离部分安全距离内的轨迹会得到接近0的分数距离越远分数越高但距离大到一定程度之后分数增长减缓。这种饱和设计是有意为之的。如果距离分一直线性增长无人机会倾向于绕极端远的路避开所有障碍物飞行效率低得离谱。我用的是反比例映射把危险区域和平坦区域做了区分。在危险区域里距离的微小增加会带来分数的大幅提升无人机就有很强的动机远离危险区在安全区域里距离增加的边际收益递减无人机也就会更看重速度分和方向分。速度部分按当前速度vx, vy, vz的合成模长归一化。这里同样有坑合成速度很大但方向噪声明显的轨迹速度分很高却可能在终点附近方向偏差大。这类冲突最后是靠加权平衡解决的从经验看速度权重通常取中间值过高的速度权重会让无人机在障碍物附近反复折返。最终分数是把三个分值乘以各自的权重系数后求和。注意各分量的取值范围应该设计成量级一致避免某一个项天然占据主导。我在代码里做了异常处理万一出现NaN直接把该候选速度分数置为负无穷防止选出一个无效速度。3. 动态环境建模仿真与关键参数调优3.1 动态障碍物怎么建模既然项目名强调了动态环境那光仿真静态障碍物就有点挂羊头卖狗肉了。动态障碍物的建模我总结出三种本质不同的类型处理方式差异挺大。第一类是匀速直线运动的障碍物。这种最简单每个仿真周期根据障碍物的速度更新位置就行。在DWA的轨迹预测时可以考虑障碍物的运动趋势如果无人机模拟轨迹的时间窗较短且障碍物运动速度也已知可以在碰撞检测时把障碍物的预测位置算出来。这种前瞻处理能明显减少与移动障碍物的碰撞。第二类是速度随机变化的障碍物。模拟这种障碍物时在每个控制周期给障碍物的速度加一个随机扰动。这种场景下DWA的每步重规划特性就能充分发挥因为无人机不会去预测障碍物太远期的位置而是实时响应最新状态。第三类是周期性运动的障碍物比如一个来回巡逻的障碍物。这类其实最考验参数调节如果DWA的轨迹模拟时间窗足够长无人机可以学会在障碍物运动周期的某个窗口期穿越。但如果时间窗太短无人机只能在障碍物来回摇摆中不停急刹。MATLAB里模拟动态障碍物的核心代码% 动态障碍物类定义 classdef DynamicObstacle properties position % 当前位置 [x, y, z] velocity % 当前速度 [vx, vy, vz] type % linear, random, periodic radius % 碰撞半径 end methods function update(obj, dt) switch obj.type case linear obj.position obj.position obj.velocity * dt; case random noise randn(1,3) * 0.2; obj.velocity obj.velocity noise; obj.velocity capVelocity(obj.velocity); obj.position obj.position obj.velocity * dt; case periodic % 摆动模型 phase phase dt * omega; obj.position center amplitude * sin(phase); end end end end有个容易被忽略的细节是障碍物的碰撞半径。真实传感器探测到的不是质点而是有一定体积的物体碰撞检测时得把障碍物半径也加进去。我比较推荐在障碍物半径基础上增加一个安全余量这个余量跟无人机尺寸和飞行速度有关。比如带桨叶的无人机飞行速度越快安全余量应该越大因为飞手/算法反应不及时的风险更大。3.2 采样分辨率对性能的影响DWA的性能很大程度受采样分辨率影响。分辨率太粗速度空间里存在很多空洞候选轨迹不够丰富经常错过最优解。分辨率太细每个周期要计算数百条轨迹的碰撞和得分实时性就下来了。我做过一组对比测试。采样分辨率粗的时候无人机在简单空旷环境下基本没问题但一遇到稍微复杂的动态障碍物集群就开始抽风经常出现卡在半路不知道怎么动的状态。为什么因为最优速度恰好落在采样网格的缝隙里无人机只能选到次优速度在靠近障碍物时没办法精准逃逸。后来我把采样分辨率细化了轨迹计算数量从每周期几百条上升到几千条MATLAB在普通电脑上一开始确实跑不动。优化方案是先用粗分辨率快速扫描全局确定几个得分较高的候选区域然后在高分区域附近做细分辨率复审。这种分层采样策略在保持轨迹质量的同时把计算量压缩到原来的三分之一左右。分层采样在MATLAB里实现并不复杂% 第一层粗采样 [vx_grid, vy_grid, vz_grid, w_grid] sampleVelocitySpace(0.5, 0.5, 0.3, 0.2); scores computeScores(vx_grid, vy_grid, vz_grid, w_grid); % 第二层在高分区域附近做细采样 [best_idx] find(scores max(scores) * 0.85); for each_best in best_idx refine_around(each_best, 0.1, 0.1, 0.05, 0.05); end实际用下来如果每个控制周期窗口是100ms粗采样200组细采样500组整体计算量在MATLAB里大概是几十毫秒实时性完全够用。3.3 权重参数的经验调法参数调优这块我是真的踩了不少坑说几个比较典型的。方位角权重α和障碍物距离权重β的关系基本决定了无人机避障的风格。α太大无人机就像一个愣头青看到目标就往前冲哪怕面前障碍物很近也要赌一把擦过去因为偏离目标方向的惩罚太严重。β太大无人机变得怯战明明可以从两个障碍物之间穿过去它非要绕地球半圈走一条绝对安全的路。我建议的动态环境初始参数大致是α0.4、β0.6、γ0.2。注意三个权重不需要归一化到和为1因为三个分数的量级通常已经被各自限制在0到1之间了。不过这只是起点关键还是在自己的仿真场景里看着飞行曲线调整。调参时有个好习惯把评价函数的三个分项输出到日志里画出曲线。看起来只是多了一步的数据记录实际调试效率能翻好几倍。比如之前遇到一个场景无人机在障碍物附近原地打转。单纯看轨迹草图猜是方向角的问题。后来把分项输出拉出来一看发现障碍物距离分一直保持在非常低的值原来是β权重在动态场景下被初始值压得太低无人机根本不在乎碰撞风险。把β从0.4拉到0.65问题立刻消失了。还遇到过一种情况无人机会在目标点附近不停绕圈怎么都停不下来。排查后发现是速度权重γ过高。无人机到达目标近旁时方位角分数和障碍物距离分数几乎恒定但高速状态能拿到更高的速度分无人机就被诱惑着保持飞行绕成了陀螺。后来加了目标距离模糊判断的减速逻辑当无人机距离目标点小于某个阈值时速度分权重降为0只比方位角和障碍物距离问题解决。3.4 避障决策与实际控制周期仿真里容易忽略的是控制频率的问题。DWA规划出来的速度指令是发给飞控的但飞控不会无限快地执行它有自己的控制频率。我用的仿真模型里DWA周期设置为0.2s飞控执行周期是0.02s。这两者之间的衔接需要做速度指令的平滑插值否则无人机速度会出现锯齿状突变。一种做法是保留上一个周期的DWA输出速度作为缓存新速度指令到来之前按最大加速度约束逐步过渡到目标速度。代码实现如下function cmd_out smoothVelocity(cmd_prev, cmd_des, dt_control, accel_lim_per_cycle) delta cmd_des - cmd_prev; max_delta accel_lim_per_cycle * dt_control; if norm(delta) max_delta cmd_out cmd_des; else % 限制加速度方向上的变化速度 cmd_out cmd_prev delta / norm(delta) * max_delta; end end在MATLAB里做这个平滑的好处是能等值检查无人机的加速度曲线是否满足约束。我后期检查一个动态避障场景时发现无人机某次急转弯过程中垂直加速度超标就是因为平滑函数里没把z轴加速度限制纳入结果无人机理论上已经穿越障碍物下方实际物理上做不到那么快下降。所以说DWA避障决策只是上半场下半场的控制执行同样重要。仿真能通过大概说明规划逻辑是好的真机飞得好才说明规划和控制完全匹配了。4. 动态环境下DWA避障的常见问题与排查经验4.1 无人机在动态环境中震荡发散怎么排查动态环境里最容易出现的问题是无人机在障碍物附近震荡轨迹既不收敛也不碰撞就像一个人在拥挤街道上挪来挪去不知道往哪走。排查这种震荡问题我的经验是先看动态窗口的边界是否收敛到了过小区域。如果无人机当前速度本身就很大且加速度限制严格那它能采样的速度空间范围会很小可选的候选速度就少决策自然不稳定。解决方向是限制无人机的巡航速度不要贴着动态窗口的边缘跑预留调整空间。另一种震荡来源于评价函数的时间窗设置过短。DWA预测轨迹一般只预测未来1~2秒如果附近障碍物运动速度较快无人机看不到更远期的状态变化就会出现短视决策。每一步看似局部最优但放到全局视角就是在震荡。这时的处理方式有两种一是加大模拟时间窗从2秒扩展到3秒让评价能感知更远的未来二是在评价函数中加入一致性惩罚项参考上一周期选择的轨迹方向同方向的轨迹打分加成不一致的轨迹扣分。这个改动直接看效果震荡次数短期内少了接近一半。实现一致性惩罚也简单在score里加一个项即可% 一致性惩罚 heading_prev directionVector(prev_traj); heading_curr directionVector(candidate_traj); consist_score 1 - acos(dot(heading_prev, heading_curr)) / pi; score score w_consist * consist_score;这里的w_consist不宜太大经验值在0.1左右太大的话会抑制正常的避障变向响应。4.2 动态障碍物突然闯进安全走廊怎么办动态环境最让人头疼的其实是无障碍物优先级突变的场景。比如无人机正沿着一条走廊飞行突然一个快速移动的障碍物横切进入走廊。这种情况下如果只靠DWA的当前采样速度空间去躲避往往来不及因为无人机当前速度朝向可能与避障所需的紧急转向差距过大超出了最大加速度限制。我的做法是在DWA之上加了一个紧急避障层。当检测到障碍物相对无人机的最小接近距离和相对速度乘积低于某个危险阈值时紧急避障层会直接输出一个高优先级转弯指令绕开DWA的正常决策流程优先保证不碰撞。这个紧急层的触发阈值设计需要谨慎。设得太灵敏障碍物离得还很远就开始急转弯飞行路径变得很突兀功耗也高设得太迟钝紧急避障层形同虚设。我是根据无人机最大减速性能倒推出来的在当前速度下以最大减速度刹车到停止需要的距离作为紧急制动的启动距离阈值。这个逻辑跟车辆保持安全车距的思路一脉相承。紧急避障的转向方向选择有个小技巧同时计算向左和向右两种避让路径选择离目标方向更近的那个。如果两侧距离都危险那就考虑到转向侧后重新规划路径这种逻辑在MATLAB里用几行数组就能判别。4.3 MATLAB仿真的性能优化经验MATLAB仿真的性能瓶颈主要在轨迹碰撞检测。如果障碍物数量多且模拟轨迹点数也多双层循环的计算量会急速膨胀。我实测过当障碍物数量到50个轨迹点100个时每次完整评价计算需要几百毫秒实时仿真基本卡死。优化手段有几种。第一个是把障碍物位置组织成KD树或网格索引碰撞检测时只需要查询无人机轨迹邻近范围内的障碍物距离远的直接跳过。MATLAB自带KD树查找函数querykdtree使用起来还算方便。第二个是减少轨迹模拟点数。在保证模拟时间窗不变的前提下把dt从0.1s增大到0.2s轨迹点数减少一半碰撞检测计算量跟着降一半。代价是轨迹精度下降但DWA本身选择速度不是定点冲刺粗轨迹也足够评价。第三个是矢量化编程。用MATLAB矩阵运算一次性算完整个轨迹与所有障碍物的距离比for循环快很多倍% 矢量化计算所有障碍物的距离 for i 1:size(obstacles, 1) dist sqrt(sum((traj - obstacles(i, :)) .^ 2, 2)); minDist(i) min(dist); end这段代码如果还嫌慢可以把obstacles矩阵和traj矩阵做一个维度的扩展用三维数组一次性算也是可行的不过代码可读性会差一些。4.4 常见问题速查表问题现象可能原因解决办法无人机在空旷环境飞行时频繁转向采样分辨率太粗最优速度未捕捉在现有最优速度邻域做细分重采样无人机在动态障碍物前急刹时间窗口太短看不到障碍物移动趋势增加sim_time参数或者给动态障碍物加运动预测无人机反复横跳不前进一致性惩罚项缺失或权重过低增加轨迹方向一致性项权重0.1左右无人机高速撞向障碍物安全距离阈值过小紧急避障未触发增大安全距离阈值按最大减速度反推输出速度在仿真里剧烈震荡平滑函数未限制加速度增加速度平滑插值限制每周期速度增量到达目标点附近无法悬停速度评价权重过高在目标邻域内将速度权重降为0多障碍物场景计算卡顿障碍物碰撞检测太耗费算力改用KD树或网格索引或减少轨迹模拟点数这张表里的大部分问题都是在MATLAB仿真阶段暴露出来的。说实话动态环境下跑DWA仿真的价值不只是看最终路径画得多漂亮而是在于能让问题快速可控地复现和定位。固定随机种子、记录状态日志、分阶段验证这三个好习惯能省下大量调参时间。避障这东西真机试错的成本太高了一次碰撞可能就几组桨叶没了。MATLAB仿真里跑通、把参数调稳再上真机做有限的验证是我这两年体会最深的流程。这个项目之后我把仿真环境也留作了一套回归测试每次改动算法就先把公开场景跑一遍确认没有引入新的问题再做下一步。在这种快速反馈循环里DWA的动态避障能力才能真正打磨得可以上真机用。最后分享一个小技巧无论算法和仿真做得多么精细在实际飞行的动态环境里传感器噪声和通信延迟永远比仿真模型更折磨人。如果在仿真里都能感受到某些参数的极端敏感性那真机上的风险会被进一步放大。所以给DWA的参数留一些冗余、避免每种参数都贴着性能边界来设是让这整套方案能稳定落地的关键。 
长河君 · 2026/10/24 2:57:52 · 阅读 1.2k 基于MATLAB的无人机DWA动态避障算法实践与调优
搞无人机自主避障绕不开的一个算法就是DWA。以前我用它做过地面移动机器人后来转到四旋翼上发现这套基于动态窗口的局部路径规划思路在三维空间里一样能打而且配合MATLAB做仿真验证调参周期比真机试错快得多。这篇就把我当时用DWA在动态环境下做无人机避障路径优化的完整过程盘一遍包括算法原理、MATLAB代码结构、动态障碍物的处理思路以及那些文档里不会写的参数调优坑点。适合刚接触路径规划、想在MATLAB里跑通一套无人机避障demo的同学也适合已经跑过全局规划、想给系统加上局部避障能力的朋友参考。1. 项目整体设计与思路拆解1.1 为什么在动态环境下选DWA无人机避障方案其实不少人工势场法、RRT系列、A*、MPC各有各的适用场景。我之所以在这个项目里坚持用DWA核心原因是它在动态环境下的性价比非常高。人工势场法在静态环境里表现不错但遇到运动中的障碍物容易陷入局部极小点无人机可能在障碍物前反复振荡。RRT属于采样规划能容忍高维空间但生成的路径往往不够平滑需要后处理实时性也有点吃紧。MPC在理论上是比较优的解法但计算开销大放在嵌入式飞控上跑对硬件要求偏高。DWA的思路完全不同。它不做全局搜索只在当前时刻的指令速度和角速度空间里采样出一批候选速度然后用运动学模型向前模拟一小段轨迹用评价函数打分挑分数最高的作为当前执行指令。这种短视策略丢掉了一定的全局最优性但换来了极高的实时性和对动态环境的快速反应能力。特别是当环境中存在移动的障碍物时DWA每一步都在重新做决策天然适配这种快速变化的场景。在后来的项目里我们用的是全局规划局部规划的双层架构。全局层用RRT或A先算出一条参考路径局部层用DWA负责实时避障。DWA不需要知道整个地图只要传感器能给出当前窗口内的障碍物信息就能跑起来。这种解耦设计让系统维护和迭代都方便很多。1.2 无人机三维空间里的DWA要改什么地面机器人的DWA经典实现是在二维平面上做(x, y, θ)状态空间的采样。但四旋翼无人机是六自由度的直接套用二维DWA会丢掉高度变化和姿态调整的能力。我这里做了一个中间层处理不让DWA直接控制姿态而是把DWA的输出变成期望速度指令再交给底层姿态控制器去执行。具体来说无人机的DWA状态量包含x、y、z三个位置分量和一个偏航角ψ。速度空间从二维的(v, ω)扩展成(vx, vy, vz, ωψ)四维。采样时要注意vz的范围比xy平面速度小很多因为无人机在垂直方向上的机动能力本来就受限暴力采样只会生成一堆飞行器根本飞不出来的轨迹。还有一点很关键DWA在无人机上不能只看制导层要跟飞控内部的加速度限制、最大倾斜角限制做联动。也就是说DWA采样出来的速度指令必须经过一个可行性过滤确保无人机当前状态能执行出来。否则仿真里看起来漂亮的轨迹上真机直接掉高度或者爆炸。1.3 评价函数的权重设计思路DWA的核心竞争力很大程度体现在评价函数设计上方位角评价项heading奖励向目标点或全局参考路径对齐的方向障碍物距离评价项dist惩罚靠近障碍物的轨迹速度评价项velocity奖励保持前进速度避免无人机停滞这三个权重系数就是控制无人机性格的旋钮。α大飞机倾向于直奔目标哪怕障碍物比较近也敢穿β大飞机变得保守离障碍物老远就开始绕γ大飞机跑得快但可能牺牲平滑度。动态环境下权重不是固定不变的我做了一个根据环境拥挤程度动态调整权重的逻辑。当检测到周围的障碍物密集度超过一定阈值时自动增大β降低α让无人机优先保证安全间距。这个改进在仿真里表现非常明显固定权重的版本遇到密集动态障碍物时经常需要急刹车而自适应权重版本的飞行路径明显更顺畅。2. 动态窗口法的核心原理与MATLAB代码实现2.1 动态窗口是怎么切出来的DWA里的动态窗口指的是在速度空间中用三个约束切出来的一块可行区域。这也是整个算法名字的由来。第一个约束是电机/执行器物理极限约束。无人机任何时刻能达到的速度范围是有限的比如最大水平速度3m/s垂直速度1m/s最大角速度1rad/s。采样不能超出这些硬边界。第二个约束是动力学约束。在下一个控制周期到来之前无人机能加速到什么样的速度受限于最大加速度。假设当前水平速度是1.5m/s最大水平加速度0.5m/s²控制周期0.1s那下一时刻的水平速度最快也只能到1.6m/s。这个约束是动态窗口这两个字的直接来源它随着当前速度状态实时变化。第三个约束是安全约束。在候选速度对应的模拟轨迹上如果无人机会撞上障碍物就把这一组速度从备选集合里剔除。实际代码里不会等真的撞上去才判定而是预留一个安全刹车距离确保在任何速度下都能在碰撞之前把飞机停下来。这三个约束在MATLAB里汇总成一个采样集合格式很简单% 采样空间生成核心逻辑 for vx vx_min : vx_res : vx_max for vy vy_min : vy_res : vy_max for vz vz_min : vz_res : vz_max for w w_min : w_res : w_max % 1. 判断是否在动力学约束范围内 if abs(vx - current_vx) accel_lim(1) * dt continue; end % 2. 判断轨迹是否安全 traj simulateTrajectory(state, [vx, vy, vz, w], dt, sim_time); if checkCollision(traj, obstacles) continue; end % 3. 评价函数打分 score evaluate(traj, goal, obstacles); % 记录候选速度及分数 end end end end2.2 轨迹预测的细节轨迹预测是DWA评价的基础。对每一组候选速度模拟出一段时间内的运动轨迹然后在这条轨迹上评估方向、安全性和速度。无人机运动学模型可以用简化版本function traj predictUAVTrajectory(state, cmd, dt, simTime) % state: [x, y, z, psi] % cmd: [vx, vy, vz, w_psi] nSteps ceil(simTime / dt); traj zeros(nSteps 1, 3); traj(1, :) state(1:3); x state(1); y state(2); z state(3); psi state(4); for i 1:nSteps % 坐标系转换把机体坐标系速度转到世界坐标系 wx cmd(1) * cos(psi) - cmd(2) * sin(psi); wy cmd(1) * sin(psi) cmd(2) * cos(psi); wz cmd(3); x x wx * dt; y y wy * dt; z z wz * dt; psi psi cmd(4) * dt; traj(i 1, :) [x, y, z]; end end这里有个细节无人机和地面机器人的运动学很不一样。地面机器人近似是阿克曼或差速模型车辆只能朝当前朝向的方向前进。但无人机是全向的它可以保持机头朝向不变同时往侧向平移。所以在预测轨迹时速度转换那一步尤其重要不能像地面机器人那样直接叠加。我用的模型里cmd的速度是机体坐标系表示的。这意味着偏航角ψ决定了速度在世界坐标系里的投影方向。这个转换在仿真代码里必须严丝合缝一旦写错无人机就会出现明明朝东飞实际轨迹却偏北的诡异行为。我早期写这个模型时反过来转过一次结果调了大半天才发现是这样的低级错误。2.3 评价函数具体怎么打分评价函数的实现直接决定了避障行为的风格。我的打分函数分三部分方位角部分衡量轨迹末端点方向与目标/全局路径方向的偏差。代码里我做了归一化处理让偏离角度在0到π之间映射到1到0function headingScore calcHeadingScore(traj, refPath, idx, worldGoal) % 优先参考全局路径上的前瞻点没有全局路径就用终点 if idx length(refPath) target refPath(idx, 1:3); else target worldGoal; end endPoint traj(end, 1:3); vecToTarget target - endPoint; vecToTarget vecToTarget / norm(vecToTarget); dirOfTraj traj(end, 1:3) - traj(1, 1:3); dirOfTraj dirOfTraj / norm(dirOfTraj); dotProd dot(vecToTarget, dirOfTraj); angle acos(max(min(dotProd, 1), -1)); headingScore 1 - angle / pi; end注意这个函数里我用的是轨迹首末两点的方向向量而不是末端的速度向量。这样做的好处是降低噪声敏感性轨迹更加稳定。障碍物距离部分安全距离内的轨迹会得到接近0的分数距离越远分数越高但距离大到一定程度之后分数增长减缓。这种饱和设计是有意为之的。如果距离分一直线性增长无人机会倾向于绕极端远的路避开所有障碍物飞行效率低得离谱。我用的是反比例映射把危险区域和平坦区域做了区分。在危险区域里距离的微小增加会带来分数的大幅提升无人机就有很强的动机远离危险区在安全区域里距离增加的边际收益递减无人机也就会更看重速度分和方向分。速度部分按当前速度vx, vy, vz的合成模长归一化。这里同样有坑合成速度很大但方向噪声明显的轨迹速度分很高却可能在终点附近方向偏差大。这类冲突最后是靠加权平衡解决的从经验看速度权重通常取中间值过高的速度权重会让无人机在障碍物附近反复折返。最终分数是把三个分值乘以各自的权重系数后求和。注意各分量的取值范围应该设计成量级一致避免某一个项天然占据主导。我在代码里做了异常处理万一出现NaN直接把该候选速度分数置为负无穷防止选出一个无效速度。3. 动态环境建模仿真与关键参数调优3.1 动态障碍物怎么建模既然项目名强调了动态环境那光仿真静态障碍物就有点挂羊头卖狗肉了。动态障碍物的建模我总结出三种本质不同的类型处理方式差异挺大。第一类是匀速直线运动的障碍物。这种最简单每个仿真周期根据障碍物的速度更新位置就行。在DWA的轨迹预测时可以考虑障碍物的运动趋势如果无人机模拟轨迹的时间窗较短且障碍物运动速度也已知可以在碰撞检测时把障碍物的预测位置算出来。这种前瞻处理能明显减少与移动障碍物的碰撞。第二类是速度随机变化的障碍物。模拟这种障碍物时在每个控制周期给障碍物的速度加一个随机扰动。这种场景下DWA的每步重规划特性就能充分发挥因为无人机不会去预测障碍物太远期的位置而是实时响应最新状态。第三类是周期性运动的障碍物比如一个来回巡逻的障碍物。这类其实最考验参数调节如果DWA的轨迹模拟时间窗足够长无人机可以学会在障碍物运动周期的某个窗口期穿越。但如果时间窗太短无人机只能在障碍物来回摇摆中不停急刹。MATLAB里模拟动态障碍物的核心代码% 动态障碍物类定义 classdef DynamicObstacle properties position % 当前位置 [x, y, z] velocity % 当前速度 [vx, vy, vz] type % linear, random, periodic radius % 碰撞半径 end methods function update(obj, dt) switch obj.type case linear obj.position obj.position obj.velocity * dt; case random noise randn(1,3) * 0.2; obj.velocity obj.velocity noise; obj.velocity capVelocity(obj.velocity); obj.position obj.position obj.velocity * dt; case periodic % 摆动模型 phase phase dt * omega; obj.position center amplitude * sin(phase); end end end end有个容易被忽略的细节是障碍物的碰撞半径。真实传感器探测到的不是质点而是有一定体积的物体碰撞检测时得把障碍物半径也加进去。我比较推荐在障碍物半径基础上增加一个安全余量这个余量跟无人机尺寸和飞行速度有关。比如带桨叶的无人机飞行速度越快安全余量应该越大因为飞手/算法反应不及时的风险更大。3.2 采样分辨率对性能的影响DWA的性能很大程度受采样分辨率影响。分辨率太粗速度空间里存在很多空洞候选轨迹不够丰富经常错过最优解。分辨率太细每个周期要计算数百条轨迹的碰撞和得分实时性就下来了。我做过一组对比测试。采样分辨率粗的时候无人机在简单空旷环境下基本没问题但一遇到稍微复杂的动态障碍物集群就开始抽风经常出现卡在半路不知道怎么动的状态。为什么因为最优速度恰好落在采样网格的缝隙里无人机只能选到次优速度在靠近障碍物时没办法精准逃逸。后来我把采样分辨率细化了轨迹计算数量从每周期几百条上升到几千条MATLAB在普通电脑上一开始确实跑不动。优化方案是先用粗分辨率快速扫描全局确定几个得分较高的候选区域然后在高分区域附近做细分辨率复审。这种分层采样策略在保持轨迹质量的同时把计算量压缩到原来的三分之一左右。分层采样在MATLAB里实现并不复杂% 第一层粗采样 [vx_grid, vy_grid, vz_grid, w_grid] sampleVelocitySpace(0.5, 0.5, 0.3, 0.2); scores computeScores(vx_grid, vy_grid, vz_grid, w_grid); % 第二层在高分区域附近做细采样 [best_idx] find(scores max(scores) * 0.85); for each_best in best_idx refine_around(each_best, 0.1, 0.1, 0.05, 0.05); end实际用下来如果每个控制周期窗口是100ms粗采样200组细采样500组整体计算量在MATLAB里大概是几十毫秒实时性完全够用。3.3 权重参数的经验调法参数调优这块我是真的踩了不少坑说几个比较典型的。方位角权重α和障碍物距离权重β的关系基本决定了无人机避障的风格。α太大无人机就像一个愣头青看到目标就往前冲哪怕面前障碍物很近也要赌一把擦过去因为偏离目标方向的惩罚太严重。β太大无人机变得怯战明明可以从两个障碍物之间穿过去它非要绕地球半圈走一条绝对安全的路。我建议的动态环境初始参数大致是α0.4、β0.6、γ0.2。注意三个权重不需要归一化到和为1因为三个分数的量级通常已经被各自限制在0到1之间了。不过这只是起点关键还是在自己的仿真场景里看着飞行曲线调整。调参时有个好习惯把评价函数的三个分项输出到日志里画出曲线。看起来只是多了一步的数据记录实际调试效率能翻好几倍。比如之前遇到一个场景无人机在障碍物附近原地打转。单纯看轨迹草图猜是方向角的问题。后来把分项输出拉出来一看发现障碍物距离分一直保持在非常低的值原来是β权重在动态场景下被初始值压得太低无人机根本不在乎碰撞风险。把β从0.4拉到0.65问题立刻消失了。还遇到过一种情况无人机会在目标点附近不停绕圈怎么都停不下来。排查后发现是速度权重γ过高。无人机到达目标近旁时方位角分数和障碍物距离分数几乎恒定但高速状态能拿到更高的速度分无人机就被诱惑着保持飞行绕成了陀螺。后来加了目标距离模糊判断的减速逻辑当无人机距离目标点小于某个阈值时速度分权重降为0只比方位角和障碍物距离问题解决。3.4 避障决策与实际控制周期仿真里容易忽略的是控制频率的问题。DWA规划出来的速度指令是发给飞控的但飞控不会无限快地执行它有自己的控制频率。我用的仿真模型里DWA周期设置为0.2s飞控执行周期是0.02s。这两者之间的衔接需要做速度指令的平滑插值否则无人机速度会出现锯齿状突变。一种做法是保留上一个周期的DWA输出速度作为缓存新速度指令到来之前按最大加速度约束逐步过渡到目标速度。代码实现如下function cmd_out smoothVelocity(cmd_prev, cmd_des, dt_control, accel_lim_per_cycle) delta cmd_des - cmd_prev; max_delta accel_lim_per_cycle * dt_control; if norm(delta) max_delta cmd_out cmd_des; else % 限制加速度方向上的变化速度 cmd_out cmd_prev delta / norm(delta) * max_delta; end end在MATLAB里做这个平滑的好处是能等值检查无人机的加速度曲线是否满足约束。我后期检查一个动态避障场景时发现无人机某次急转弯过程中垂直加速度超标就是因为平滑函数里没把z轴加速度限制纳入结果无人机理论上已经穿越障碍物下方实际物理上做不到那么快下降。所以说DWA避障决策只是上半场下半场的控制执行同样重要。仿真能通过大概说明规划逻辑是好的真机飞得好才说明规划和控制完全匹配了。4. 动态环境下DWA避障的常见问题与排查经验4.1 无人机在动态环境中震荡发散怎么排查动态环境里最容易出现的问题是无人机在障碍物附近震荡轨迹既不收敛也不碰撞就像一个人在拥挤街道上挪来挪去不知道往哪走。排查这种震荡问题我的经验是先看动态窗口的边界是否收敛到了过小区域。如果无人机当前速度本身就很大且加速度限制严格那它能采样的速度空间范围会很小可选的候选速度就少决策自然不稳定。解决方向是限制无人机的巡航速度不要贴着动态窗口的边缘跑预留调整空间。另一种震荡来源于评价函数的时间窗设置过短。DWA预测轨迹一般只预测未来1~2秒如果附近障碍物运动速度较快无人机看不到更远期的状态变化就会出现短视决策。每一步看似局部最优但放到全局视角就是在震荡。这时的处理方式有两种一是加大模拟时间窗从2秒扩展到3秒让评价能感知更远的未来二是在评价函数中加入一致性惩罚项参考上一周期选择的轨迹方向同方向的轨迹打分加成不一致的轨迹扣分。这个改动直接看效果震荡次数短期内少了接近一半。实现一致性惩罚也简单在score里加一个项即可% 一致性惩罚 heading_prev directionVector(prev_traj); heading_curr directionVector(candidate_traj); consist_score 1 - acos(dot(heading_prev, heading_curr)) / pi; score score w_consist * consist_score;这里的w_consist不宜太大经验值在0.1左右太大的话会抑制正常的避障变向响应。4.2 动态障碍物突然闯进安全走廊怎么办动态环境最让人头疼的其实是无障碍物优先级突变的场景。比如无人机正沿着一条走廊飞行突然一个快速移动的障碍物横切进入走廊。这种情况下如果只靠DWA的当前采样速度空间去躲避往往来不及因为无人机当前速度朝向可能与避障所需的紧急转向差距过大超出了最大加速度限制。我的做法是在DWA之上加了一个紧急避障层。当检测到障碍物相对无人机的最小接近距离和相对速度乘积低于某个危险阈值时紧急避障层会直接输出一个高优先级转弯指令绕开DWA的正常决策流程优先保证不碰撞。这个紧急层的触发阈值设计需要谨慎。设得太灵敏障碍物离得还很远就开始急转弯飞行路径变得很突兀功耗也高设得太迟钝紧急避障层形同虚设。我是根据无人机最大减速性能倒推出来的在当前速度下以最大减速度刹车到停止需要的距离作为紧急制动的启动距离阈值。这个逻辑跟车辆保持安全车距的思路一脉相承。紧急避障的转向方向选择有个小技巧同时计算向左和向右两种避让路径选择离目标方向更近的那个。如果两侧距离都危险那就考虑到转向侧后重新规划路径这种逻辑在MATLAB里用几行数组就能判别。4.3 MATLAB仿真的性能优化经验MATLAB仿真的性能瓶颈主要在轨迹碰撞检测。如果障碍物数量多且模拟轨迹点数也多双层循环的计算量会急速膨胀。我实测过当障碍物数量到50个轨迹点100个时每次完整评价计算需要几百毫秒实时仿真基本卡死。优化手段有几种。第一个是把障碍物位置组织成KD树或网格索引碰撞检测时只需要查询无人机轨迹邻近范围内的障碍物距离远的直接跳过。MATLAB自带KD树查找函数querykdtree使用起来还算方便。第二个是减少轨迹模拟点数。在保证模拟时间窗不变的前提下把dt从0.1s增大到0.2s轨迹点数减少一半碰撞检测计算量跟着降一半。代价是轨迹精度下降但DWA本身选择速度不是定点冲刺粗轨迹也足够评价。第三个是矢量化编程。用MATLAB矩阵运算一次性算完整个轨迹与所有障碍物的距离比for循环快很多倍% 矢量化计算所有障碍物的距离 for i 1:size(obstacles, 1) dist sqrt(sum((traj - obstacles(i, :)) .^ 2, 2)); minDist(i) min(dist); end这段代码如果还嫌慢可以把obstacles矩阵和traj矩阵做一个维度的扩展用三维数组一次性算也是可行的不过代码可读性会差一些。4.4 常见问题速查表问题现象可能原因解决办法无人机在空旷环境飞行时频繁转向采样分辨率太粗最优速度未捕捉在现有最优速度邻域做细分重采样无人机在动态障碍物前急刹时间窗口太短看不到障碍物移动趋势增加sim_time参数或者给动态障碍物加运动预测无人机反复横跳不前进一致性惩罚项缺失或权重过低增加轨迹方向一致性项权重0.1左右无人机高速撞向障碍物安全距离阈值过小紧急避障未触发增大安全距离阈值按最大减速度反推输出速度在仿真里剧烈震荡平滑函数未限制加速度增加速度平滑插值限制每周期速度增量到达目标点附近无法悬停速度评价权重过高在目标邻域内将速度权重降为0多障碍物场景计算卡顿障碍物碰撞检测太耗费算力改用KD树或网格索引或减少轨迹模拟点数这张表里的大部分问题都是在MATLAB仿真阶段暴露出来的。说实话动态环境下跑DWA仿真的价值不只是看最终路径画得多漂亮而是在于能让问题快速可控地复现和定位。固定随机种子、记录状态日志、分阶段验证这三个好习惯能省下大量调参时间。避障这东西真机试错的成本太高了一次碰撞可能就几组桨叶没了。MATLAB仿真里跑通、把参数调稳再上真机做有限的验证是我这两年体会最深的流程。这个项目之后我把仿真环境也留作了一套回归测试每次改动算法就先把公开场景跑一遍确认没有引入新的问题再做下一步。在这种快速反馈循环里DWA的动态避障能力才能真正打磨得可以上真机用。最后分享一个小技巧无论算法和仿真做得多么精细在实际飞行的动态环境里传感器噪声和通信延迟永远比仿真模型更折磨人。如果在仿真里都能感受到某些参数的极端敏感性那真机上的风险会被进一步放大。所以给DWA的参数留一些冗余、避免每种参数都贴着性能边界来设是让这整套方案能稳定落地的关键。