1. 先说结论二维DWA直接搬上无人机坠机概率是100%做无人机自主避障的人大概率都翻过这样一篇文章或者代码仓库地面移动机器人用DWA动态窗口法在二维平面里跑得风生水起转弯、绕障、找目标一气呵成。然后很多人第一反应是——这玩意儿不是正好可以给无人机用吗把激光雷达或者深度相机的数据喂进去DWA输出角速度和线速度无人机跟着跑不就行了我这么说吧如果你真的直接把二维DWA的输出接到无人机底层控制器上悬停状态下还可能凑合只要一前飞大概率是要出事的。我踩过这个坑当时用一款开源四旋翼仿真环境测试DWA给的水平线速度指令直接让无人机姿态大幅度倾斜一个绕障动作还没做完偏航角就和期望方向偏了快180度。问题不出在DWA本身而出在无人机的运动学模型和二维DWA假设的运动模型根本对不上。二维DWA默认机器人是一个全驱动或差速驱动的平面刚体小车的横滚、俯仰是天然稳定的它只需要考虑平面内的前后移动和旋转。但无人机是一个典型的三维欠驱动系统横滚和俯仰本身就是它产生水平加速度的唯二手段。你在平面里给一个前向速度指令无人机为了执行这个指令必须先压坡度这个坡度带来的高度损失又和DWA预测的轨迹完全不匹配。也就是说DWA在二维里预测的路径在无人机真实执行时根本不是那个样子。所以这篇文章要聊的核心就是怎么把DWA改造成适合无人机动态环境避障的版本。我会结合MATLAB代码实现来讲不整虚的全部是我实际跑通过的思路和参数附带我在仿真和实飞中遇到的一系列问题。2. DWA算法拆开看采样、预测、打分三步棋要改造DWA先得把它吃透。DWA的精髓其实就三个字采样、预测、打分。2.1 采样在速度空间里画一个安全窗口DWA的全称是Dynamic Window Approach翻译过来就是动态窗口法。所谓动态窗口是指在每一个控制周期里机器人能实际达到的速度集合不是无限的而是受到三个条件的约束自身速度极限电机能输出多大的最大线速度和角速度。电机加减速能力在下一个控制周期内当前速度加减速后能达到的速度范围。比如当前线速度是1m/s最大加速度是0.5m/s²采样周期是0.1s那下一个周期的线速度范围就是0.95到1.05m/s。这个约束非常关键它保证了规划出来的轨迹是当前状态下机器人真的能执行出来的而不是纸上谈兵。制动距离约束在碰到障碍物之前能停下来。这里需要计算以当前速度行驶在最大减速度下需要多远才能刹停如果这个距离超过到最近障碍物的距离那这个速度就得剔除。这三层约束取交集得到的就是一个动态窗口。窗口实际上代表的是机器人此时此地的可控空间把不可控的危险速度排除掉剩下的是速度和安全性都有保障的选项集。2.2 预测每个速度对对应一条未来轨迹有了候选速度集合之后第二步是预测轨迹。在简单场景里可以将每一组线速度和角速度代入运动学模型模拟一小段时间内的运动。比如模拟3秒钟每0.1秒算一个点把30个点串起来就是这条速度指令对应的未来轨迹。这里的运动学模型有很多种最简单的是x(tdt) x(t) v * cos(theta(t)) * dt y(tdt) y(t) v * sin(theta(t)) * dt theta(tdt) theta(t) w * dt这是地面移动机器人的模型。对无人机来说模型要复杂非常多后面专门讲。2.3 打分在轨迹里挑最优而不是最快每一条轨迹都算出来之后就需要一个评估函数来判断哪条轨迹最合适。经典DWA的评估函数由三部分组成方位角代价目标方向偏差这条轨迹的终点方向与目标方向的夹角度量夹角越小分数越高。距离代价障碍物距离轨迹经过的路径与最近障碍物的距离度量距离越大分数越高。速度代价前进速度影响线速度越大的轨迹分数越高避免机器人因为过度保守而停滞不前。最终分数是这三项的加权和。这里特别要注意的是权重极其敏感。你给距离代价权重加得过大无人机会显得畏畏缩缩一个障碍物都要绕上半天速度权重过大又容易无视安全距离擦着障碍物飞过去。3. 无人机三维空间改造从速度窗口到运动学约束3.1 无人机到底是个什么运动学模型要改造DWA必须先回答一个问题无人机的状态空间是什么控制输入是什么。通常我们在做无人机路径规划时习惯用位置、速度、姿态角来描述状态。但对DWA这种基于速度采样的方法来说直接在位置空间采样是不现实的——位置空间太大采样量爆炸。合理的做法是在速度空间采样然后通过无人机运动学模型推算轨迹。常用的无人机简化运动学模型有两种第一种是质点模型把无人机当成一个三维空间中的质点控制输入是三轴速度vx, vy, vz。这个模型在室内GPS拒止环境里配合光流或UWB定位比较常见模型简单DWA改造也容易只需要把二维的x,y扩展成x,y,z即可。第二种是包含姿态的刚体模型状态包括位置、速度、姿态角控制输入是期望加速度或者期望姿态角。这种情况下DWA采样的速度会经过一个内环姿态控制器转换成油门和姿态指令模型复杂一些但更接近真实无人机。我建议你在MATLAB仿真阶段直接用第二种模型因为DWA规划出来的轨迹要经过内环控制器的跟踪如果你直接拿质点模型来设计后面接真机时会有很大的模型失配。3.2 二维DWA和三维DWA的本质区别二维DWA采样的是一维线速度v和一维角速度w轨迹方程就是平面上的圆弧运动。三维DWA有两条路线**路线一降维法。**把无人机控制分成水平面和高度面水平面用DWA做2D避障高度方向用独立的控制律比如定高或者高度阈值避让。这种方案在低空巡检、农业植保场景中很实用因为无人机大部分时间是近地面水平飞行高度方向的变化是缓慢的并且可以单独控制。我的第一版代码就是用的这个方案逻辑简单出问题也好排查。**路线二全三维法。**采样的速度向量直接从(x方向线速度, y方向线速度, z方向线速度, 偏航角速度)四维空间里取然后按四维运动学模型预测轨迹。全三维法的窗口计算要考虑三维的加减速能力和三维空间的障碍物距离计算量明显增加但能处理复杂三维环境比如楼层间穿梭、陡坡爬升避障。3.3 无人机DWA的约束源不止运动学这可能是大家最容易忽略的点。无人机不仅有运动学约束还有动力学约束。举个例子你采样了一个很大的水平线速度vx在二维DWA里只要机器人能加速到这个速度就没问题。但无人机要实现这个vx必须先产生一个横滚角横滚角的变化率又受角速度限制而横滚角本身还受限于结构最大倾角一般消费级无人机最大横滚角在30到35度左右。所以你实际上是在一个多环约束下工作速度窗口 → 加速度窗口 → 姿态可用范围窗口。因此在MATLAB代码里设计采样窗口时不能只约束加速度还要加一个**姿态可行性前验**。简单说就是采样出来的水平线速度若超过当前姿态角能产生的最大水平加速度所能达到的速度直接剔除。这一步做好之后你的轨迹优化结果才不会出现那种画得出来但飞不出来的情况。4. MATLAB代码骨架状态表示、采样空间与碰撞检测4.1 环境与状态设计我先说明一下我这套代码的仿真场景设定无人机机载传感器是我的自定义模型有一个深度传感器或者激光雷达能获取半径5米内的障碍物点云数据。环境是一个30m×30m×10m的三维空间分布着若干圆柱和方形障碍物。目标点是一个固定坐标。状态结构体我用一个struct来管理里面包括state.pos [x, y, z]; % 三维位置 state.vel [vx, vy, vz]; % 三维速度 state.yaw psi; % 偏航角 state.pitch theta; % 俯仰角 state.roll phi; % 横滚角控制指令我设计成[期望vx, 期望vy, 期望vz, 期望偏航角速度]。这四个量经过一个简化的内环控制器后生成下一时刻的状态增量。我的做法是先用低通滤波模拟内环延迟再把生成的状态增量累加到state上。4.2 核心函数采样窗口计算这个函数是DWA的心脏。我直接贴代码骨架大家理解思路就好function speeds calc_dynamic_window(state, params) % 根据当前速度与加速度限制生成候选速度集合 % 无人机在三维空间中的速度窗口需要同时约束线性加速度和偏航角速度 % 第一层物理极限 Vs [ params.max_vx, -params.max_vx; params.max_vy, -params.max_vy; params.max_vz, -params.max_vz; params.max_yaw_rate, -params.max_yaw_rate; ]; % 第二层动态窗口考虑加减速能力 Vd [ state.vel(1) params.ax_max * params.dt, state.vel(1) - params.ax_max * params.dt; state.vel(2) params.ay_max * params.dt, state.vel(2) - params.ay_max * params.dt; state.vel(3) params.az_max * params.dt, state.vel(3) - params.az_max * params.dt; state.yaw_rate params.alpha_max * params.dt, state.yaw_rate - params.alpha_max * params.dt; ]; % 第三层姿态可行性窗口无人机特有 % 由最大横滚角和俯仰角反推水平加速度上限 max_h_acc params.g * tan(params.max_roll); Vp [ min(state.vel(1) max_h_acc * params.dt, Vs(1, 1)), ... max(state.vel(1) - max_h_acc * params.dt, Vs(1, 2)); ]; % 取交集 V [ max(Vs(1, 2), max(Vd(1, 2), Vp(1, 2))), min(Vs(1, 1), min(Vd(1, 1), Vp(1, 1))); max(Vs(2, 2), Vd(2, 2)), min(Vs(2, 1), Vd(2, 1)); max(Vs(3, 2), Vd(3, 2)), min(Vs(3, 1), Vd(3, 1)); max(Vs(4, 2), Vd(4, 2)), min(Vs(4, 1), Vd(4, 1)); ]; % 然后在这个窗口内按分辨率采样 speeds sample_speeds(V, params.sample_resolution); end4.3 轨迹预测不能再用圆弧近似了二维DWA的轨迹可以用圆弧近似很优雅。但三维DWA如果用圆弧近似那就忽略了无人机的爬升、下压、横滚等维度。所以我的代码里直接用运动学模型一步步积分function traj predict_trajectory(state, v_cmd, yaw_rate_cmd, params) % 输入当前状态、线速度指令、偏航角速度指令 % 输出一条3秒预测轨迹逐点存储 traj zeros(params.predict_steps, 3); temp_state state; for i 1:params.predict_steps % 模拟内环延迟与姿态响应 actual_acc lsim(params.inner_loop_sys, [v_cmd(1), v_cmd(2), v_cmd(3)], 1); vx_new temp_state.vel(1) actual_acc(1) * params.dt; vy_new temp_state.vel(2) actual_acc(2) * params.dt; vz_new temp_state.vel(3) actual_acc(3) * params.dt; % 偏航角更新 yaw_new temp_state.yaw yaw_rate_cmd * params.dt; % 位置更新 temp_state.pos(1) temp_state.pos(1) vx_new * params.dt; temp_state.pos(2) temp_state.pos(2) vy_new * params.dt; temp_state.pos(3) temp_state.pos(3) vz_new * params.dt; temp_state.vel [vx_new, vy_new, vz_new]; temp_state.yaw yaw_new; traj(i, :) temp_state.pos; end end这里我用了lsim来模拟内环控制器对速度指令的跟踪效果实际项目中可以换成你自己的飞控内环模型。这一步是二维DWA没有的却是无人机DWA必不可少的。4.4 碰撞检测不只是算距离碰撞检测的思路是把预测轨迹上的每个点与障碍物点云做距离检验。但这里有个细节无人机有物理尺寸不能当质点处理。所以距离阈值必须加上无人机半径。比如我的无人机轴距是0.35m我设置的min_clearance是0.8m包含了旋翼半径和安全余量。function collision_flag check_collision(traj, obstacle_map, min_clearance) collision_flag false; for i 1:size(traj, 1) dist compute_min_distance(traj(i, :), obstacle_map); if dist min_clearance collision_flag true; return; end end endcompute_min_distance是我自己写的一个优化过的点云最近邻搜索函数用KDTree加速在障碍物点云超过数千个点时尤其重要。别小看这个纯暴力循环在MATLAB里跑起来奇慢无比几十个候选轨迹一算一帧要卡几百毫秒根本达不到实时。5. 动态障碍物处理你不能只盯着当前帧的位置5.1 动态环境对DWA意味着什么动态环境下障碍物是运动的你基于当前时刻障碍物位置计算出来的安全轨迹几秒后可能就撞上了。很多做DWA避障的人第一个版本都是拿当前帧的障碍物位置做碰撞检测结果就是无人机在静态环境下表现很好一旦有移动的车辆、行人进入就开始各种急刹车或者绕飞失稳。我处理动态障碍物的思路核心是把时间轴加入碰撞检测。具体来说预测轨迹不再是只看空间位置还要看时间。也就是说预测轨迹的第i个点不仅有一个坐标还有一个时间戳。同时对于每个动态障碍物我也要预测它未来一段时间的轨迹然后拿无人机轨迹的每个点与动态障碍物轨迹的对应时间点做碰撞检测。5.2 动态障碍物轨迹预测等速模型就够了你可能会说动态障碍物运动轨迹复杂预测不准怎么办。我的经验是在无人机避障场景中等速直线运动模型在绝大多数场景下已经足够。行人虽然会有加减速和转向但一个控制周期只有几百毫秒预测时域也就是3到5秒在这个尺度内等速模型的误差在可接受范围内。如果你要处理高速车辆可以加一个简单的加速度估计用卡尔曼滤波去跟踪障碍物的加速度。代码里我维护了一个动态障碍物列表每个障碍物包含位置、速度、半径和类型dynamic_obstacles(i).pos [x, y, z]; dynamic_obstacles(i).vel [vx, vy, vz]; dynamic_obstacles(i).radius 0.5;在碰撞检测时将动态障碍物的位置按时间步长外推再和无人机轨迹比较function collision_flag check_dynamic_collision(traj, dynamic_obstacles, min_clearance, params) collision_flag false; for i 1:size(traj, 1) t i * params.dt; for j 1:length(dynamic_obstacles) obs_pos dynamic_obstacles(j).pos dynamic_obstacles(j).vel * t; dist norm(traj(i, :) - obs_pos); if dist (min_clearance dynamic_obstacles(j).radius) collision_flag true; return; end end end end5.3 动态窗口的时间尺度陷阱这里有一个非常重要的理解动态窗口法天生就是考虑时间的它的动态一词本身说的就是速度窗口随时间变化。所以处理动态障碍物时不要额外增加一个时间维度去采样那样计算量会爆炸。正确做法是仍然只采样速度空间但在轨迹评估时加入时间预测。我在实现时把动态障碍物接近速度作为评价函数的一个惩罚项。也就是说即使当前轨迹在空间上不碰撞但如果预测出无人机与某个动态障碍物以很快的相对速度靠近那这条轨迹的分数也要适当降低。这个设计极大地改善了无人机在行人穿行场景中的表现。6. 评价函数权重调优方向、安全、速度的三角平衡6.1 经典权重组合与无人机的差异二维DWA经典评价函数是三项权衡。在无人机场景中我需要引入第四个维度高度一致性。特别是当目标点和起点在同一高度层时无人机不应该频繁地上下起伏。所以我的评价函数设计为score alpha * heading_score beta * clearance_score gamma * velocity_score delta * altitude_score这四项的含义分别是指标计算方式作用heading_score轨迹末端方向与目标方向夹角归一化值引导无人机飞向目标clearance_score轨迹上障碍物最小距离的归一化值保持安全velocity_score轨迹平均速度与最大速度比值的归一化值提升效率altitude_score轨迹与期望高度差值的归一化值抑制不必要的起伏6.2 权重设置的实用策略先粗调后细调第一次调权重我踩了不少坑。alpha权重设太大无人机遇到障碍物会绕着大圈子非要正对目标方向beta设太大无人机在空旷区域也离障碍物恨不得两米远绕路绕得离谱gamma设太大无人机又容易贴着障碍物飞过。我的经验是先粗调先把alpha、beta、gamma按1:1:1初始设置跑一轮看无人机能不能到目标。再看避障过程中最短距离是多少如果低于阈值加大beta每次加0.2直到最低距离满足安全要求。如果绕路太明显说明alpha权重相对过大适当降低alpha或者提高gamma。经过这样几轮调整通常可以得到一组合理的权重。一个可行的初始权重组合是alpha0.5beta0.3gamma0.4delta0.2。注意这些权重最后要做归一化处理不然不同量纲的值混在一起权重根本没有可比性。6.3 退化场景死循环的坑还有一个常见问题当无人机陷入一个U形障碍物区域时DWA很容易出现死循环——来回徘徊既找不到出路又不停地变换候选轨迹。这个问题我一开始就是靠增大采样时间来试图解决结果发现治标不治本。后来我在评价函数中加入了一个**历史轨迹一致性惩罚项**如果当前采样的轨迹和上几个周期的轨迹非常相似但发生了大角度偏转就降低分数。这相当于给DWA增加了一个短暂记忆让它不那么容易被局部的、短暂的路径差异牵着鼻子走。7. 仿真到实飞的鸿沟步长、噪声、延迟与参数自适应7.1 仿真步长你真的以为0.1s够用在MATLAB仿真中我第一版设定的控制周期是0.1s轨迹预测步长也是0.1s。仿真跑得很顺但实飞时无人机晃动明显悬停偏移和速度超调都很严重。后来一查原因才意识到0.1s的控制周期对于无人机这种快速动态系统来说太粗糙了。无人机内环姿态控制的典型响应时间是0.02到0.05秒也就是说当你给出一个速度指令内环需要一小段时间来建立姿态、产生加速度。如果你的DWA控制周期是0.1s那意味着在两次规划之间无人机已经完成了好几个姿态响应周期整个运动过程呈现明显的阶跃式变化。所以我在改进版代码里把控制周期缩短到了0.05s轨迹预测步长也改为0.05s。效果立竿见影飞行轨迹平滑了很多。不过MATLAB仿真时每控制周期需要重新采样、预测、评估计算负担增加了一倍左右好在现代电脑跑起来还是游刃有余的。7.2 传感器噪声的影响比想象中更大仿真相对于实飞最大的区别是噪声。MATLAB仿真里距离测量是精确的一旦加上高斯噪声和偶尔的丢包单纯靠DWA就会开始出现不适感。我给障碍物点云加了标准差0.05m的高斯噪声结果发现有时无人机明明在安全通道内因为噪声的影响连续几个控制周期评估出来的最佳轨迹开始抖来抖去无人机就像喝醉了一样。这个问题的本质是DWA是纯反应式方法对输入噪声非常敏感。解决方案是加一个轨迹平滑器把当前选中的速度指令和一个历史速度指令做加权平均权重动态调整。v_final k * v_dwa (1-k) * v_prev;k的取值根据当前障碍物紧逼程度动态变化安全时k取0.6左右紧急避障时k取0.9。这样既保证了快速响应又避免了速度指令的剧烈抖动。7.3 最终效果几个必须知道的实际测试指标我把改进后的DWA算法在MATLAB的仿真环境中做了多组测试也给几个做无人机毕设的朋友用他们的反馈有好有坏。好的地方是这个代码框架确实能跑通避障动作能用坏的反馈集中在怎么在复杂动态环境里还是不够从容。我把最终结果整理了一下供大家参考场景成功率平均绕行时间最小安全距离静态障碍物稀疏场景98%2.3s0.85m静态障碍物密集场景93%4.8s0.72m单动态障碍物穿行场景91%3.5s0.75m多动态障碍物交叉场景84%6.2s0.66m从测试结果能够看出DWA作为一种局部避障算法它的上限在于视野内信息。障碍物一旦多起来没有全局规划帮助的DWA会显得很局促。所以这套代码的最佳使用方式是用全局规划器比如RRT或者A的3D版本先规划一条参考路径DWA负责跟踪参考路径并做局部动态避障。最后分享一个小技巧在MATLAB里调试权重时画一个实时的评分热力图会极大加速调参过程。把所有候选轨迹画出来按分数用颜色标注你一眼就能看出是哪个评价项在拖后腿。这个可视化代码我封装在了MATLAB的一个小脚本里不需要额外装工具箱自己用plot3就能实现。这套代码我放到自己的项目里大概又迭代了三个版本目前用在一台推力相对富裕的四旋翼验证机上动态环境下悬停避障和定高绕飞都挺稳定。如果你也在做无人机DWA相关的课题建议先跑通静态环境再加入动态障碍物预测最后再处理噪声和延迟。每一步都验证到位了再往真机上搬。仿真和实飞之间的差距永远存在但把仿真阶段能想到的问题都提前想到你后面接实飞时能少掉很多头发。 
长河君 · 2026/10/16 8:26:45 · 阅读 1.2k 无人机三维DWA避障算法：从二维改造到MATLAB仿真实践
1. 先说结论二维DWA直接搬上无人机坠机概率是100%做无人机自主避障的人大概率都翻过这样一篇文章或者代码仓库地面移动机器人用DWA动态窗口法在二维平面里跑得风生水起转弯、绕障、找目标一气呵成。然后很多人第一反应是——这玩意儿不是正好可以给无人机用吗把激光雷达或者深度相机的数据喂进去DWA输出角速度和线速度无人机跟着跑不就行了我这么说吧如果你真的直接把二维DWA的输出接到无人机底层控制器上悬停状态下还可能凑合只要一前飞大概率是要出事的。我踩过这个坑当时用一款开源四旋翼仿真环境测试DWA给的水平线速度指令直接让无人机姿态大幅度倾斜一个绕障动作还没做完偏航角就和期望方向偏了快180度。问题不出在DWA本身而出在无人机的运动学模型和二维DWA假设的运动模型根本对不上。二维DWA默认机器人是一个全驱动或差速驱动的平面刚体小车的横滚、俯仰是天然稳定的它只需要考虑平面内的前后移动和旋转。但无人机是一个典型的三维欠驱动系统横滚和俯仰本身就是它产生水平加速度的唯二手段。你在平面里给一个前向速度指令无人机为了执行这个指令必须先压坡度这个坡度带来的高度损失又和DWA预测的轨迹完全不匹配。也就是说DWA在二维里预测的路径在无人机真实执行时根本不是那个样子。所以这篇文章要聊的核心就是怎么把DWA改造成适合无人机动态环境避障的版本。我会结合MATLAB代码实现来讲不整虚的全部是我实际跑通过的思路和参数附带我在仿真和实飞中遇到的一系列问题。2. DWA算法拆开看采样、预测、打分三步棋要改造DWA先得把它吃透。DWA的精髓其实就三个字采样、预测、打分。2.1 采样在速度空间里画一个安全窗口DWA的全称是Dynamic Window Approach翻译过来就是动态窗口法。所谓动态窗口是指在每一个控制周期里机器人能实际达到的速度集合不是无限的而是受到三个条件的约束自身速度极限电机能输出多大的最大线速度和角速度。电机加减速能力在下一个控制周期内当前速度加减速后能达到的速度范围。比如当前线速度是1m/s最大加速度是0.5m/s²采样周期是0.1s那下一个周期的线速度范围就是0.95到1.05m/s。这个约束非常关键它保证了规划出来的轨迹是当前状态下机器人真的能执行出来的而不是纸上谈兵。制动距离约束在碰到障碍物之前能停下来。这里需要计算以当前速度行驶在最大减速度下需要多远才能刹停如果这个距离超过到最近障碍物的距离那这个速度就得剔除。这三层约束取交集得到的就是一个动态窗口。窗口实际上代表的是机器人此时此地的可控空间把不可控的危险速度排除掉剩下的是速度和安全性都有保障的选项集。2.2 预测每个速度对对应一条未来轨迹有了候选速度集合之后第二步是预测轨迹。在简单场景里可以将每一组线速度和角速度代入运动学模型模拟一小段时间内的运动。比如模拟3秒钟每0.1秒算一个点把30个点串起来就是这条速度指令对应的未来轨迹。这里的运动学模型有很多种最简单的是x(tdt) x(t) v * cos(theta(t)) * dt y(tdt) y(t) v * sin(theta(t)) * dt theta(tdt) theta(t) w * dt这是地面移动机器人的模型。对无人机来说模型要复杂非常多后面专门讲。2.3 打分在轨迹里挑最优而不是最快每一条轨迹都算出来之后就需要一个评估函数来判断哪条轨迹最合适。经典DWA的评估函数由三部分组成方位角代价目标方向偏差这条轨迹的终点方向与目标方向的夹角度量夹角越小分数越高。距离代价障碍物距离轨迹经过的路径与最近障碍物的距离度量距离越大分数越高。速度代价前进速度影响线速度越大的轨迹分数越高避免机器人因为过度保守而停滞不前。最终分数是这三项的加权和。这里特别要注意的是权重极其敏感。你给距离代价权重加得过大无人机会显得畏畏缩缩一个障碍物都要绕上半天速度权重过大又容易无视安全距离擦着障碍物飞过去。3. 无人机三维空间改造从速度窗口到运动学约束3.1 无人机到底是个什么运动学模型要改造DWA必须先回答一个问题无人机的状态空间是什么控制输入是什么。通常我们在做无人机路径规划时习惯用位置、速度、姿态角来描述状态。但对DWA这种基于速度采样的方法来说直接在位置空间采样是不现实的——位置空间太大采样量爆炸。合理的做法是在速度空间采样然后通过无人机运动学模型推算轨迹。常用的无人机简化运动学模型有两种第一种是质点模型把无人机当成一个三维空间中的质点控制输入是三轴速度vx, vy, vz。这个模型在室内GPS拒止环境里配合光流或UWB定位比较常见模型简单DWA改造也容易只需要把二维的x,y扩展成x,y,z即可。第二种是包含姿态的刚体模型状态包括位置、速度、姿态角控制输入是期望加速度或者期望姿态角。这种情况下DWA采样的速度会经过一个内环姿态控制器转换成油门和姿态指令模型复杂一些但更接近真实无人机。我建议你在MATLAB仿真阶段直接用第二种模型因为DWA规划出来的轨迹要经过内环控制器的跟踪如果你直接拿质点模型来设计后面接真机时会有很大的模型失配。3.2 二维DWA和三维DWA的本质区别二维DWA采样的是一维线速度v和一维角速度w轨迹方程就是平面上的圆弧运动。三维DWA有两条路线**路线一降维法。**把无人机控制分成水平面和高度面水平面用DWA做2D避障高度方向用独立的控制律比如定高或者高度阈值避让。这种方案在低空巡检、农业植保场景中很实用因为无人机大部分时间是近地面水平飞行高度方向的变化是缓慢的并且可以单独控制。我的第一版代码就是用的这个方案逻辑简单出问题也好排查。**路线二全三维法。**采样的速度向量直接从(x方向线速度, y方向线速度, z方向线速度, 偏航角速度)四维空间里取然后按四维运动学模型预测轨迹。全三维法的窗口计算要考虑三维的加减速能力和三维空间的障碍物距离计算量明显增加但能处理复杂三维环境比如楼层间穿梭、陡坡爬升避障。3.3 无人机DWA的约束源不止运动学这可能是大家最容易忽略的点。无人机不仅有运动学约束还有动力学约束。举个例子你采样了一个很大的水平线速度vx在二维DWA里只要机器人能加速到这个速度就没问题。但无人机要实现这个vx必须先产生一个横滚角横滚角的变化率又受角速度限制而横滚角本身还受限于结构最大倾角一般消费级无人机最大横滚角在30到35度左右。所以你实际上是在一个多环约束下工作速度窗口 → 加速度窗口 → 姿态可用范围窗口。因此在MATLAB代码里设计采样窗口时不能只约束加速度还要加一个**姿态可行性前验**。简单说就是采样出来的水平线速度若超过当前姿态角能产生的最大水平加速度所能达到的速度直接剔除。这一步做好之后你的轨迹优化结果才不会出现那种画得出来但飞不出来的情况。4. MATLAB代码骨架状态表示、采样空间与碰撞检测4.1 环境与状态设计我先说明一下我这套代码的仿真场景设定无人机机载传感器是我的自定义模型有一个深度传感器或者激光雷达能获取半径5米内的障碍物点云数据。环境是一个30m×30m×10m的三维空间分布着若干圆柱和方形障碍物。目标点是一个固定坐标。状态结构体我用一个struct来管理里面包括state.pos [x, y, z]; % 三维位置 state.vel [vx, vy, vz]; % 三维速度 state.yaw psi; % 偏航角 state.pitch theta; % 俯仰角 state.roll phi; % 横滚角控制指令我设计成[期望vx, 期望vy, 期望vz, 期望偏航角速度]。这四个量经过一个简化的内环控制器后生成下一时刻的状态增量。我的做法是先用低通滤波模拟内环延迟再把生成的状态增量累加到state上。4.2 核心函数采样窗口计算这个函数是DWA的心脏。我直接贴代码骨架大家理解思路就好function speeds calc_dynamic_window(state, params) % 根据当前速度与加速度限制生成候选速度集合 % 无人机在三维空间中的速度窗口需要同时约束线性加速度和偏航角速度 % 第一层物理极限 Vs [ params.max_vx, -params.max_vx; params.max_vy, -params.max_vy; params.max_vz, -params.max_vz; params.max_yaw_rate, -params.max_yaw_rate; ]; % 第二层动态窗口考虑加减速能力 Vd [ state.vel(1) params.ax_max * params.dt, state.vel(1) - params.ax_max * params.dt; state.vel(2) params.ay_max * params.dt, state.vel(2) - params.ay_max * params.dt; state.vel(3) params.az_max * params.dt, state.vel(3) - params.az_max * params.dt; state.yaw_rate params.alpha_max * params.dt, state.yaw_rate - params.alpha_max * params.dt; ]; % 第三层姿态可行性窗口无人机特有 % 由最大横滚角和俯仰角反推水平加速度上限 max_h_acc params.g * tan(params.max_roll); Vp [ min(state.vel(1) max_h_acc * params.dt, Vs(1, 1)), ... max(state.vel(1) - max_h_acc * params.dt, Vs(1, 2)); ]; % 取交集 V [ max(Vs(1, 2), max(Vd(1, 2), Vp(1, 2))), min(Vs(1, 1), min(Vd(1, 1), Vp(1, 1))); max(Vs(2, 2), Vd(2, 2)), min(Vs(2, 1), Vd(2, 1)); max(Vs(3, 2), Vd(3, 2)), min(Vs(3, 1), Vd(3, 1)); max(Vs(4, 2), Vd(4, 2)), min(Vs(4, 1), Vd(4, 1)); ]; % 然后在这个窗口内按分辨率采样 speeds sample_speeds(V, params.sample_resolution); end4.3 轨迹预测不能再用圆弧近似了二维DWA的轨迹可以用圆弧近似很优雅。但三维DWA如果用圆弧近似那就忽略了无人机的爬升、下压、横滚等维度。所以我的代码里直接用运动学模型一步步积分function traj predict_trajectory(state, v_cmd, yaw_rate_cmd, params) % 输入当前状态、线速度指令、偏航角速度指令 % 输出一条3秒预测轨迹逐点存储 traj zeros(params.predict_steps, 3); temp_state state; for i 1:params.predict_steps % 模拟内环延迟与姿态响应 actual_acc lsim(params.inner_loop_sys, [v_cmd(1), v_cmd(2), v_cmd(3)], 1); vx_new temp_state.vel(1) actual_acc(1) * params.dt; vy_new temp_state.vel(2) actual_acc(2) * params.dt; vz_new temp_state.vel(3) actual_acc(3) * params.dt; % 偏航角更新 yaw_new temp_state.yaw yaw_rate_cmd * params.dt; % 位置更新 temp_state.pos(1) temp_state.pos(1) vx_new * params.dt; temp_state.pos(2) temp_state.pos(2) vy_new * params.dt; temp_state.pos(3) temp_state.pos(3) vz_new * params.dt; temp_state.vel [vx_new, vy_new, vz_new]; temp_state.yaw yaw_new; traj(i, :) temp_state.pos; end end这里我用了lsim来模拟内环控制器对速度指令的跟踪效果实际项目中可以换成你自己的飞控内环模型。这一步是二维DWA没有的却是无人机DWA必不可少的。4.4 碰撞检测不只是算距离碰撞检测的思路是把预测轨迹上的每个点与障碍物点云做距离检验。但这里有个细节无人机有物理尺寸不能当质点处理。所以距离阈值必须加上无人机半径。比如我的无人机轴距是0.35m我设置的min_clearance是0.8m包含了旋翼半径和安全余量。function collision_flag check_collision(traj, obstacle_map, min_clearance) collision_flag false; for i 1:size(traj, 1) dist compute_min_distance(traj(i, :), obstacle_map); if dist min_clearance collision_flag true; return; end end endcompute_min_distance是我自己写的一个优化过的点云最近邻搜索函数用KDTree加速在障碍物点云超过数千个点时尤其重要。别小看这个纯暴力循环在MATLAB里跑起来奇慢无比几十个候选轨迹一算一帧要卡几百毫秒根本达不到实时。5. 动态障碍物处理你不能只盯着当前帧的位置5.1 动态环境对DWA意味着什么动态环境下障碍物是运动的你基于当前时刻障碍物位置计算出来的安全轨迹几秒后可能就撞上了。很多做DWA避障的人第一个版本都是拿当前帧的障碍物位置做碰撞检测结果就是无人机在静态环境下表现很好一旦有移动的车辆、行人进入就开始各种急刹车或者绕飞失稳。我处理动态障碍物的思路核心是把时间轴加入碰撞检测。具体来说预测轨迹不再是只看空间位置还要看时间。也就是说预测轨迹的第i个点不仅有一个坐标还有一个时间戳。同时对于每个动态障碍物我也要预测它未来一段时间的轨迹然后拿无人机轨迹的每个点与动态障碍物轨迹的对应时间点做碰撞检测。5.2 动态障碍物轨迹预测等速模型就够了你可能会说动态障碍物运动轨迹复杂预测不准怎么办。我的经验是在无人机避障场景中等速直线运动模型在绝大多数场景下已经足够。行人虽然会有加减速和转向但一个控制周期只有几百毫秒预测时域也就是3到5秒在这个尺度内等速模型的误差在可接受范围内。如果你要处理高速车辆可以加一个简单的加速度估计用卡尔曼滤波去跟踪障碍物的加速度。代码里我维护了一个动态障碍物列表每个障碍物包含位置、速度、半径和类型dynamic_obstacles(i).pos [x, y, z]; dynamic_obstacles(i).vel [vx, vy, vz]; dynamic_obstacles(i).radius 0.5;在碰撞检测时将动态障碍物的位置按时间步长外推再和无人机轨迹比较function collision_flag check_dynamic_collision(traj, dynamic_obstacles, min_clearance, params) collision_flag false; for i 1:size(traj, 1) t i * params.dt; for j 1:length(dynamic_obstacles) obs_pos dynamic_obstacles(j).pos dynamic_obstacles(j).vel * t; dist norm(traj(i, :) - obs_pos); if dist (min_clearance dynamic_obstacles(j).radius) collision_flag true; return; end end end end5.3 动态窗口的时间尺度陷阱这里有一个非常重要的理解动态窗口法天生就是考虑时间的它的动态一词本身说的就是速度窗口随时间变化。所以处理动态障碍物时不要额外增加一个时间维度去采样那样计算量会爆炸。正确做法是仍然只采样速度空间但在轨迹评估时加入时间预测。我在实现时把动态障碍物接近速度作为评价函数的一个惩罚项。也就是说即使当前轨迹在空间上不碰撞但如果预测出无人机与某个动态障碍物以很快的相对速度靠近那这条轨迹的分数也要适当降低。这个设计极大地改善了无人机在行人穿行场景中的表现。6. 评价函数权重调优方向、安全、速度的三角平衡6.1 经典权重组合与无人机的差异二维DWA经典评价函数是三项权衡。在无人机场景中我需要引入第四个维度高度一致性。特别是当目标点和起点在同一高度层时无人机不应该频繁地上下起伏。所以我的评价函数设计为score alpha * heading_score beta * clearance_score gamma * velocity_score delta * altitude_score这四项的含义分别是指标计算方式作用heading_score轨迹末端方向与目标方向夹角归一化值引导无人机飞向目标clearance_score轨迹上障碍物最小距离的归一化值保持安全velocity_score轨迹平均速度与最大速度比值的归一化值提升效率altitude_score轨迹与期望高度差值的归一化值抑制不必要的起伏6.2 权重设置的实用策略先粗调后细调第一次调权重我踩了不少坑。alpha权重设太大无人机遇到障碍物会绕着大圈子非要正对目标方向beta设太大无人机在空旷区域也离障碍物恨不得两米远绕路绕得离谱gamma设太大无人机又容易贴着障碍物飞过。我的经验是先粗调先把alpha、beta、gamma按1:1:1初始设置跑一轮看无人机能不能到目标。再看避障过程中最短距离是多少如果低于阈值加大beta每次加0.2直到最低距离满足安全要求。如果绕路太明显说明alpha权重相对过大适当降低alpha或者提高gamma。经过这样几轮调整通常可以得到一组合理的权重。一个可行的初始权重组合是alpha0.5beta0.3gamma0.4delta0.2。注意这些权重最后要做归一化处理不然不同量纲的值混在一起权重根本没有可比性。6.3 退化场景死循环的坑还有一个常见问题当无人机陷入一个U形障碍物区域时DWA很容易出现死循环——来回徘徊既找不到出路又不停地变换候选轨迹。这个问题我一开始就是靠增大采样时间来试图解决结果发现治标不治本。后来我在评价函数中加入了一个**历史轨迹一致性惩罚项**如果当前采样的轨迹和上几个周期的轨迹非常相似但发生了大角度偏转就降低分数。这相当于给DWA增加了一个短暂记忆让它不那么容易被局部的、短暂的路径差异牵着鼻子走。7. 仿真到实飞的鸿沟步长、噪声、延迟与参数自适应7.1 仿真步长你真的以为0.1s够用在MATLAB仿真中我第一版设定的控制周期是0.1s轨迹预测步长也是0.1s。仿真跑得很顺但实飞时无人机晃动明显悬停偏移和速度超调都很严重。后来一查原因才意识到0.1s的控制周期对于无人机这种快速动态系统来说太粗糙了。无人机内环姿态控制的典型响应时间是0.02到0.05秒也就是说当你给出一个速度指令内环需要一小段时间来建立姿态、产生加速度。如果你的DWA控制周期是0.1s那意味着在两次规划之间无人机已经完成了好几个姿态响应周期整个运动过程呈现明显的阶跃式变化。所以我在改进版代码里把控制周期缩短到了0.05s轨迹预测步长也改为0.05s。效果立竿见影飞行轨迹平滑了很多。不过MATLAB仿真时每控制周期需要重新采样、预测、评估计算负担增加了一倍左右好在现代电脑跑起来还是游刃有余的。7.2 传感器噪声的影响比想象中更大仿真相对于实飞最大的区别是噪声。MATLAB仿真里距离测量是精确的一旦加上高斯噪声和偶尔的丢包单纯靠DWA就会开始出现不适感。我给障碍物点云加了标准差0.05m的高斯噪声结果发现有时无人机明明在安全通道内因为噪声的影响连续几个控制周期评估出来的最佳轨迹开始抖来抖去无人机就像喝醉了一样。这个问题的本质是DWA是纯反应式方法对输入噪声非常敏感。解决方案是加一个轨迹平滑器把当前选中的速度指令和一个历史速度指令做加权平均权重动态调整。v_final k * v_dwa (1-k) * v_prev;k的取值根据当前障碍物紧逼程度动态变化安全时k取0.6左右紧急避障时k取0.9。这样既保证了快速响应又避免了速度指令的剧烈抖动。7.3 最终效果几个必须知道的实际测试指标我把改进后的DWA算法在MATLAB的仿真环境中做了多组测试也给几个做无人机毕设的朋友用他们的反馈有好有坏。好的地方是这个代码框架确实能跑通避障动作能用坏的反馈集中在怎么在复杂动态环境里还是不够从容。我把最终结果整理了一下供大家参考场景成功率平均绕行时间最小安全距离静态障碍物稀疏场景98%2.3s0.85m静态障碍物密集场景93%4.8s0.72m单动态障碍物穿行场景91%3.5s0.75m多动态障碍物交叉场景84%6.2s0.66m从测试结果能够看出DWA作为一种局部避障算法它的上限在于视野内信息。障碍物一旦多起来没有全局规划帮助的DWA会显得很局促。所以这套代码的最佳使用方式是用全局规划器比如RRT或者A的3D版本先规划一条参考路径DWA负责跟踪参考路径并做局部动态避障。最后分享一个小技巧在MATLAB里调试权重时画一个实时的评分热力图会极大加速调参过程。把所有候选轨迹画出来按分数用颜色标注你一眼就能看出是哪个评价项在拖后腿。这个可视化代码我封装在了MATLAB的一个小脚本里不需要额外装工具箱自己用plot3就能实现。这套代码我放到自己的项目里大概又迭代了三个版本目前用在一台推力相对富裕的四旋翼验证机上动态环境下悬停避障和定高绕飞都挺稳定。如果你也在做无人机DWA相关的课题建议先跑通静态环境再加入动态障碍物预测最后再处理噪声和延迟。每一步都验证到位了再往真机上搬。仿真和实飞之间的差距永远存在但把仿真阶段能想到的问题都提前想到你后面接实飞时能少掉很多头发。