简介这份资源提供基于动态窗口法DWA的移动机器人动态避障MATLAB实现代码面向学习机器人路径规划、需要理解实时避障算法的高校学生与研究人员。DWA通过定义动态窗口在速度空间内搜索兼顾避障与目标趋近的最优轨迹资源涵盖环境栅格建模、速度分解与约束、动态窗口计算、路径评估与速度调整等核心模块可帮助读者掌握从理论到代码的完整落地思路。压缩包共5个文件约4.53MB包含2个m源码文件与3个gif演示动图源码对应机器人模型、地图处理与DWA主流程动图直观展示速度一致与不一致时的避障效果便于对照理解算法行为。目前已有4637人学习下载适合作为课程设计、科研入门或算法复现的参考素材读者可在此基础上修改参数、替换地图并扩展至更复杂的动态环境。1. 基于DWA的动态避障代码在MATLAB里到底怎么跑通移动机器人做局部路径规划DWADynamic Window Approach动态窗口法是绕不开的经典方案。它的核心思路很朴素在速度空间里采样一堆可行速度对前向模拟一小段轨迹用评价函数打分挑最优的那组速度下发。听起来简单但真正在MATLAB里把它跑起来、调稳、避得开动态障碍中间隔着一堆参数玄学。这篇笔记面向两类人一是刚接触局部规划、想在MATLAB里快速复现DWA的研究生或工程师二是已经跑过A*或RRT全局规划、想补上局部避障这一环的从业者。我会从速度空间采样讲起一路落到评价函数怎么写、参数怎么调、动态障碍怎么加、翻车了怎么排查。所有代码都是MATLAB原生实现不依赖任何工具箱复制到脚本里就能跑。2. DWA的速度空间与轨迹预测先把数学变成可执行的矩阵2.1 为什么DWA要在速度空间里采样而不是位置空间全局规划器A*、Dijkstra在栅格地图上搜的是一串离散位置点它不关心机器人能不能在两点之间平滑过渡。DWA反过来它在速度空间 $(v, \omega)$ 里做文章——线速度和角速度的组合决定了机器人未来一小段时间的运动轨迹。这个思路的好处是天然满足运动学约束你采样的速度必须落在机器人实际能执行的范围里。速度空间要经过三层约束裁剪才能得到最终用于采样的动态窗口第一层是机器人自身极限$v \in [v_{min}, v_{max}]$$\omega \in [\omega_{min}, \omega_{max}]$。第二层是电机加速度限制在一个仿真周期 $\Delta t$ 内速度变化不能超过 $a_{max} \cdot \Delta t$ 和 $\alpha_{max} \cdot \Delta t$。第三层是障碍物约束如果某个速度对应的轨迹会撞上障碍直接剔除。三层裁剪之后剩下的速度对集合就是动态窗口。采样时在窗口内均匀取 $n_v \times n_\omega$ 个点每个点前向模拟 $T$ 秒通常取2到3秒步长 $\Delta t$通常0.1秒。轨迹预测用的是差速轮模型$$ \begin{cases} x_{t1} x_t v \cos(\theta_t) \Delta t \ y_{t1} y_t v \sin(\theta_t) \Delta t \ \theta_{t1} \theta_t \omega \Delta t \end{cases} $$这个模型假设机器人是差速驱动的如果是阿克曼转向就得换模型。MATLAB里用矩阵运算可以一次性算出所有采样轨迹不用for循环逐条模拟速度能快一个量级。2.2 用MATLAB向量化生成候选轨迹下面这段代码是整个DWA的地基它接收当前位姿、速度窗口和采样参数返回所有候选轨迹的终点位姿和轨迹点矩阵。function [trajs, endpoints] generate_trajectories(pose, v_window, w_window, ... n_v, n_w, dt, predict_time) % pose: [x, y, theta] 当前位姿 % v_window: [v_min, v_max] 线速度窗口 % w_window: [w_min, w_max] 角速度窗口 % n_v, n_w: 采样数量 % dt: 仿真步长 % predict_time: 前向预测时长 v_samples linspace(v_window(1), v_window(2), n_v); w_samples linspace(w_window(1), w_window(2), n_w); [V, W] meshgrid(v_samples, w_samples); V V(:); W W(:); % 拉平成列向量每个元素是一组速度对 steps floor(predict_time / dt); n_traj length(V); % 预分配轨迹矩阵: n_traj x steps x 3 (x, y, theta) trajs zeros(n_traj, steps, 3); x repmat(pose(1), n_traj, 1); y repmat(pose(2), n_traj, 1); theta repmat(pose(3), n_traj, 1); for k 1:steps x x V .* cos(theta) * dt; y y V .* sin(theta) * dt; theta theta W * dt; trajs(:, k, :) cat(3, x, y, theta); end endpoints squeeze(trajs(:, end, :)); % n_traj x 3 end逻辑说明meshgrid把线速度和角速度的采样点做成笛卡尔积拉平后每一行就是一组速度对。循环里用向量化运算同时推进所有轨迹避免了逐条模拟的开销。trajs存的是每条轨迹每个时刻的位姿后面评价函数要用它来判断是否撞障碍。参数说明n_v和n_w一般取5到10太小会导致速度空间覆盖不够、机器人抖动太大则计算量暴涨。dt取0.1秒是经验值和大多数激光雷达或深度相机的更新频率匹配。predict_time取2.0到3.0秒太短看不到远处障碍太长则轨迹末端误差累积严重。提示如果你的机器人是阿克曼转向把运动学更新换成自行车模型其余评价逻辑不变。2.3 评价函数的四个分量怎么配比DWA的灵魂在评价函数。常见做法是四个分量加权求和方位角对齐、障碍距离、速度大小、路径跟随。每个分量归一化之后乘以权重总分最高的速度对被选中。方位角分量衡量机器人朝向和目标方向的偏差偏差越小分越高。障碍距离分量取轨迹上离最近障碍的最小距离距离越大越安全。速度分量直接奖励高速让机器人别磨蹭。路径跟随分量衡量轨迹终点离全局路径的横向偏差保证局部避障不跑偏。function score evaluate_trajectory(traj, goal, obstacles, global_path, weights) % traj: steps x 3 的单条轨迹 % goal: [x, y] 目标点 % obstacles: N x 2 障碍物圆心坐标 % global_path: M x 2 全局路径点 % weights: [w_heading, w_dist, w_vel, w_path] endpoint traj(end, :); % 方位角分量 goal_angle atan2(goal(2) - endpoint(2), goal(1) - endpoint(1)); heading_diff abs(angle_diff(goal_angle, endpoint(3))); heading_score pi - heading_diff; % 偏差越小分越高 % 障碍距离分量 min_dist inf; for i 1:size(traj, 1) d min(sqrt(sum((obstacles - traj(i, 1:2)).^2, 2))); if d min_dist min_dist d; end end if min_dist 0.2 dist_score -inf; % 撞了直接淘汰 else dist_score min_dist; end % 速度分量 vel_score traj(end, 1); % 用末端线速度近似 % 路径跟随分量 path_dist min(sqrt(sum((global_path - endpoint(1:2)).^2, 2))); path_score -path_dist; score weights(1)*heading_score weights(2)*dist_score ... weights(3)*vel_score weights(4)*path_score; end function d angle_diff(a, b) d mod(a - b pi, 2*pi) - pi; end逻辑说明angle_diff把角度差归一化到 $[-\pi, \pi]$避免绕圈问题。障碍距离分量里如果最小距离小于0.2米直接给负无穷相当于硬约束淘汰。路径跟随分量用负距离离全局路径越近分越高。参数说明四个权重没有万能值。我一般从[0.3, 0.4, 0.1, 0.2]起步障碍密集场景把w_dist提到0.5以上开阔场景把w_vel提到0.3让机器人跑快点。归一化方式不同权重差异很大建议先把每个分量除以自己的最大值再加权。3. 动态障碍物建模与滚动窗口让避障真正“动”起来3.1 动态障碍物的三种建模方式与选型静态障碍物在DWA里就是一堆固定圆心和半径的圆。动态障碍物麻烦得多因为它的位置随时间变化而DWA的轨迹预测是未来2到3秒必须预测障碍物在这段时间里在哪。常见做法有三种。第一种是匀速直线模型假设障碍物保持当前速度不变位置按 $p_{t1} p_t v_{obs} \Delta t$ 更新。实现最简单适合行人、其他机器人这类运动规律性强的场景。第二种是恒定加速度模型多一个加速度状态量适合车辆。第三种是概率预测用卡尔曼滤波或粒子滤波给出障碍物未来位置的概率分布DWA评价时用期望距离。实现复杂但更鲁棒。我一般先用匀速模型跑通因为大多数室内场景障碍物速度变化不快。如果发现机器人对突然变向的障碍物反应迟钝再升级到概率预测。function obs_pred predict_obstacles(obstacles, obs_vel, dt, steps) % obstacles: N x 2 当前障碍物位置 % obs_vel: N x 2 当前障碍物速度 % dt: 仿真步长 % steps: 预测步数 % obs_pred: steps x N x 2 每个时刻的障碍物位置 n_obs size(obstacles, 1); obs_pred zeros(steps, n_obs, 2); pos obstacles; for k 1:steps pos pos obs_vel * dt; obs_pred(k, :, :) reshape(pos, [1, n_obs, 2]); end end逻辑说明每个时间步把所有障碍物位置按速度推进一次存到三维矩阵里。评价轨迹时第 $k$ 步的轨迹点要和第 $k$ 步的障碍物位置比距离而不是和当前障碍物位置比。这一点很多人会搞错导致动态障碍物避不开。参数说明obs_vel需要从传感器数据里估计激光雷达做帧间匹配或者用卡尔曼滤波跟踪都能拿到。如果拿不到速度退而求其次用最近两帧位置差除以时间间隔。3.2 滚动窗口与重规划频率的配合DWA本身就是一个滚动窗口规划器每个控制周期重新采样、重新评价、重新选速度。但重规划频率和仿真步长是两回事。仿真步长dt决定轨迹预测的精度重规划频率决定机器人对外界变化的响应速度。常见配置是仿真步长0.1秒重规划频率10赫兹也就是每个仿真步都重规划。如果计算资源紧张可以降到5赫兹但机器人会明显变迟钝。我试过在复杂场景里把重规划频率提到20赫兹避障成功率提升有限计算量翻倍不划算。% 主循环骨架 dt 0.1; predict_time 2.5; replan_interval 1; % 每1个仿真步重规划一次 step_count 0; while ~reached_goal(pose, goal, 0.3) if mod(step_count, replan_interval) 0 % 计算动态窗口 v_window compute_dynamic_window(pose, vel, robot_params, dt); w_window compute_dynamic_window_w(pose, vel, robot_params, dt); % 生成候选轨迹 [trajs, ~] generate_trajectories(pose, v_window, w_window, ... 7, 7, dt, predict_time); % 预测动态障碍物 obs_pred predict_obstacles(obstacles, obs_vel, dt, ... floor(predict_time/dt)); % 评价并选最优 best_score -inf; best_v 0; best_w 0; for i 1:size(trajs, 1) s evaluate_trajectory_dynamic(squeeze(trajs(i,:,:)), ... goal, obs_pred, global_path, weights); if s best_score best_score s; best_v V(i); best_w W(i); end end end % 执行速度指令更新位姿 pose update_pose(pose, best_v, best_w, dt); step_count step_count 1; end逻辑说明主循环里每隔replan_interval步做一次完整的DWA计算其余步只执行上一次的速度指令。这样可以在计算资源和响应速度之间做权衡。evaluate_trajectory_dynamic和静态版本的区别在于障碍物距离计算要用对应时刻的预测位置。参数说明replan_interval取1是最稳的取2或3能省计算但会牺牲响应。reached_goal的阈值0.3米是经验值太小机器人会在目标点附近震荡太大则停不准。3.3 动态窗口计算的边界处理动态窗口计算里有个容易翻车的地方当机器人当前速度接近极限时加速度约束可能让窗口变成空集。比如机器人已经以最大线速度跑加速度限制下窗口上限还是最大速度下限是最大速度减去 $a_{max} \Delta t$窗口非空。但如果同时角速度也到极限两个窗口的笛卡尔积可能没有可行解。处理办法是加一个降级策略如果动态窗口为空逐步放宽加速度约束直到窗口非空。或者直接把速度降到零附近重新规划。我一般用前者因为急停对机械结构冲击大。function v_window compute_dynamic_window(pose, vel, params, dt) v_min max(params.v_min, vel(1) - params.a_max * dt); v_max min(params.v_max, vel(1) params.a_max * dt); if v_min v_max % 降级放宽约束 v_min params.v_min; v_max params.v_max; end v_window [v_min, v_max]; end逻辑说明先按加速度约束算窗口如果下限超过上限说明无解直接退回到机器人物理极限。这个降级策略在实际跑的时候救过我好几次尤其是机器人从高速急转弯的时候。参数说明a_max取机器人实际最大加速度的80%左右留点余量。v_min如果设成0机器人可以原地转设成负值则允许倒车但评价函数里速度分量要改成绝对值。4. 避坑与排查DWA调参路上最常见的五个翻车现场4.1 机器人原地抖动或画龙现象机器人不往前走在原地左右摇摆或者走直线时轨迹像蛇一样。原因角速度采样分辨率太高相邻两个速度对的评分接近机器人反复切换。或者方位角分量的权重过大机器人过度追求朝向目标而忽略前进。解决把n_w从10降到5降低角速度分辨率。同时把速度分量权重从0.1提到0.2以上给前进一点奖励。如果还抖在评价函数里加一个惩罚项惩罚角速度的绝对值。4.2 动态障碍物明明在旁边却撞上去现象静态障碍物避得很好一加动态障碍物就撞。原因评价轨迹时用的是障碍物当前位置而不是对应时刻的预测位置。这是最常见的实现错误。解决检查evaluate_trajectory_dynamic里障碍物距离的计算确保第 $k$ 个轨迹点比的是第 $k$ 个预测障碍物位置。另外确认predict_obstacles的步长和轨迹预测步长一致。4.3 机器人卡在障碍物前面不动现象机器人走到障碍物前停下不绕行也不后退。原因所有候选轨迹的障碍距离分量都低于安全阈值被硬约束淘汰导致没有可行解。或者路径跟随分量权重太大机器人不敢偏离全局路径。解决把障碍距离的硬约束阈值从0.2降到0.15给机器人一点贴边通过的空间。同时降低路径跟随权重让局部避障有更多自由度。如果还卡检查全局路径是不是穿过了障碍物全局规划本身有问题局部怎么调都没用。4.4 计算太慢导致控制周期跟不上现象仿真里跑得好好的放到实机上机器人反应迟钝或者MATLAB卡顿。原因采样数量太多或者评价函数里用了for循环逐点算障碍距离。解决把n_v和n_w各降到5总轨迹数从100降到25。障碍距离计算用pdist2或者矩阵运算替代循环。如果还慢把预测时长从3秒降到2秒减少轨迹点数量。4.5 目标点附近来回震荡现象机器人到了目标点附近但不停止来回转圈。原因目标点判定阈值太小机器人速度降不下来冲过目标点后又往回走。解决把到达阈值从0.1米提到0.3米。同时在评价函数里加一个减速逻辑当机器人离目标小于1米时速度分量的权重逐渐降到零让机器人优先对准目标而不是冲速度。5. 从跑通到跑稳DWA参数整定的进阶手法5.1 用代价地图替代离散障碍物列表前面代码里障碍物是一堆圆心坐标实际场景里激光雷达给的是点云或者栅格地图。把点云转成障碍物列表会丢失形状信息而且计算量大。更好的做法是直接用代价地图每个栅格有一个代价值评价轨迹时查表得到障碍代价。function cost query_cost_map(cost_map, resolution, origin, point) % cost_map: 二维矩阵值越大越危险 % resolution: 每格代表的实际距离 % origin: 地图左下角对应的实际坐标 % point: [x, y] 查询点 idx_x round((point(1) - origin(1)) / resolution) 1; idx_y round((point(2) - origin(2)) / resolution) 1; % 边界检查 idx_x max(1, min(size(cost_map, 2), idx_x)); idx_y max(1, min(size(cost_map, 1), idx_y)); cost cost_map(idx_y, idx_x); end逻辑说明把连续坐标转成栅格索引查表得到代价值。代价地图可以用膨胀层把障碍物周围一圈设成高代价这样机器人会自然远离障碍而不是贴边。参数说明resolution取0.05到0.1米太细计算量大太粗避障精度不够。膨胀半径取机器人半径加0.1米安全余量。5.2 自适应权重让参数随场景变化固定权重在简单场景够用复杂场景就不行了。我一般会加一个简单的自适应逻辑根据机器人周围障碍物密度调整权重。function weights adaptive_weights(base_weights, cost_map, pose, resolution, origin) % 查询机器人周围3x3区域的代价 local_cost 0; for dx -1:1 for dy -1:1 p [pose(1) dx*resolution, pose(2) dy*resolution]; local_cost local_cost query_cost_map(cost_map, resolution, origin, p); end end local_cost local_cost / 9; % 障碍密集时提高距离权重降低速度权重 if local_cost 0.5 weights base_weights .* [0.8, 1.5, 0.5, 1.0]; else weights base_weights; end end逻辑说明查询机器人周围9个格子的平均代价如果超过阈值说明障碍密集把距离权重乘1.5、速度权重乘0.5。这个逻辑很粗糙但有效比固定权重在混合场景里表现好很多。参数说明阈值0.5取决于代价地图的归一化方式需要根据实际地图调。自适应权重的缩放系数不要超过2倍否则参数变化太剧烈会导致机器人行为不稳定。5.3 用仿真回放验证参数一个可复现的测试流程调参最忌讳凭感觉改。我习惯搭一个回放测试固定几张地图和几组障碍物轨迹每次改参数跑一遍记录成功率、平均速度、最小障碍距离三个指标。指标含义目标成功率到达目标且无碰撞的测试次数占比大于95%平均速度全程线速度均值越高越好但不牺牲成功率最小障碍距离全程离障碍物最近的距离大于0.15米测试流程准备5张地图开阔、走廊、密集障碍、动态障碍、混合每张跑20次障碍物初始位置随机。每次改一个参数跑完100次测试对比三个指标。这样调出来的参数比拍脑袋靠谱得多。我自己的习惯是先把成功率调到95%以上再优化平均速度。最小障碍距离低于0.1米说明太激进高于0.3米说明太保守。这套流程跑下来DWA在室内场景基本能稳定工作。希望帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/10/14 11:34:24 · 阅读 1.2k MATLAB动态避障实战：DWA算法从跑通到跑稳的调参指南
简介这份资源提供基于动态窗口法DWA的移动机器人动态避障MATLAB实现代码面向学习机器人路径规划、需要理解实时避障算法的高校学生与研究人员。DWA通过定义动态窗口在速度空间内搜索兼顾避障与目标趋近的最优轨迹资源涵盖环境栅格建模、速度分解与约束、动态窗口计算、路径评估与速度调整等核心模块可帮助读者掌握从理论到代码的完整落地思路。压缩包共5个文件约4.53MB包含2个m源码文件与3个gif演示动图源码对应机器人模型、地图处理与DWA主流程动图直观展示速度一致与不一致时的避障效果便于对照理解算法行为。目前已有4637人学习下载适合作为课程设计、科研入门或算法复现的参考素材读者可在此基础上修改参数、替换地图并扩展至更复杂的动态环境。1. 基于DWA的动态避障代码在MATLAB里到底怎么跑通移动机器人做局部路径规划DWADynamic Window Approach动态窗口法是绕不开的经典方案。它的核心思路很朴素在速度空间里采样一堆可行速度对前向模拟一小段轨迹用评价函数打分挑最优的那组速度下发。听起来简单但真正在MATLAB里把它跑起来、调稳、避得开动态障碍中间隔着一堆参数玄学。这篇笔记面向两类人一是刚接触局部规划、想在MATLAB里快速复现DWA的研究生或工程师二是已经跑过A*或RRT全局规划、想补上局部避障这一环的从业者。我会从速度空间采样讲起一路落到评价函数怎么写、参数怎么调、动态障碍怎么加、翻车了怎么排查。所有代码都是MATLAB原生实现不依赖任何工具箱复制到脚本里就能跑。2. DWA的速度空间与轨迹预测先把数学变成可执行的矩阵2.1 为什么DWA要在速度空间里采样而不是位置空间全局规划器A*、Dijkstra在栅格地图上搜的是一串离散位置点它不关心机器人能不能在两点之间平滑过渡。DWA反过来它在速度空间 $(v, \omega)$ 里做文章——线速度和角速度的组合决定了机器人未来一小段时间的运动轨迹。这个思路的好处是天然满足运动学约束你采样的速度必须落在机器人实际能执行的范围里。速度空间要经过三层约束裁剪才能得到最终用于采样的动态窗口第一层是机器人自身极限$v \in [v_{min}, v_{max}]$$\omega \in [\omega_{min}, \omega_{max}]$。第二层是电机加速度限制在一个仿真周期 $\Delta t$ 内速度变化不能超过 $a_{max} \cdot \Delta t$ 和 $\alpha_{max} \cdot \Delta t$。第三层是障碍物约束如果某个速度对应的轨迹会撞上障碍直接剔除。三层裁剪之后剩下的速度对集合就是动态窗口。采样时在窗口内均匀取 $n_v \times n_\omega$ 个点每个点前向模拟 $T$ 秒通常取2到3秒步长 $\Delta t$通常0.1秒。轨迹预测用的是差速轮模型$$ \begin{cases} x_{t1} x_t v \cos(\theta_t) \Delta t \ y_{t1} y_t v \sin(\theta_t) \Delta t \ \theta_{t1} \theta_t \omega \Delta t \end{cases} $$这个模型假设机器人是差速驱动的如果是阿克曼转向就得换模型。MATLAB里用矩阵运算可以一次性算出所有采样轨迹不用for循环逐条模拟速度能快一个量级。2.2 用MATLAB向量化生成候选轨迹下面这段代码是整个DWA的地基它接收当前位姿、速度窗口和采样参数返回所有候选轨迹的终点位姿和轨迹点矩阵。function [trajs, endpoints] generate_trajectories(pose, v_window, w_window, ... n_v, n_w, dt, predict_time) % pose: [x, y, theta] 当前位姿 % v_window: [v_min, v_max] 线速度窗口 % w_window: [w_min, w_max] 角速度窗口 % n_v, n_w: 采样数量 % dt: 仿真步长 % predict_time: 前向预测时长 v_samples linspace(v_window(1), v_window(2), n_v); w_samples linspace(w_window(1), w_window(2), n_w); [V, W] meshgrid(v_samples, w_samples); V V(:); W W(:); % 拉平成列向量每个元素是一组速度对 steps floor(predict_time / dt); n_traj length(V); % 预分配轨迹矩阵: n_traj x steps x 3 (x, y, theta) trajs zeros(n_traj, steps, 3); x repmat(pose(1), n_traj, 1); y repmat(pose(2), n_traj, 1); theta repmat(pose(3), n_traj, 1); for k 1:steps x x V .* cos(theta) * dt; y y V .* sin(theta) * dt; theta theta W * dt; trajs(:, k, :) cat(3, x, y, theta); end endpoints squeeze(trajs(:, end, :)); % n_traj x 3 end逻辑说明meshgrid把线速度和角速度的采样点做成笛卡尔积拉平后每一行就是一组速度对。循环里用向量化运算同时推进所有轨迹避免了逐条模拟的开销。trajs存的是每条轨迹每个时刻的位姿后面评价函数要用它来判断是否撞障碍。参数说明n_v和n_w一般取5到10太小会导致速度空间覆盖不够、机器人抖动太大则计算量暴涨。dt取0.1秒是经验值和大多数激光雷达或深度相机的更新频率匹配。predict_time取2.0到3.0秒太短看不到远处障碍太长则轨迹末端误差累积严重。提示如果你的机器人是阿克曼转向把运动学更新换成自行车模型其余评价逻辑不变。2.3 评价函数的四个分量怎么配比DWA的灵魂在评价函数。常见做法是四个分量加权求和方位角对齐、障碍距离、速度大小、路径跟随。每个分量归一化之后乘以权重总分最高的速度对被选中。方位角分量衡量机器人朝向和目标方向的偏差偏差越小分越高。障碍距离分量取轨迹上离最近障碍的最小距离距离越大越安全。速度分量直接奖励高速让机器人别磨蹭。路径跟随分量衡量轨迹终点离全局路径的横向偏差保证局部避障不跑偏。function score evaluate_trajectory(traj, goal, obstacles, global_path, weights) % traj: steps x 3 的单条轨迹 % goal: [x, y] 目标点 % obstacles: N x 2 障碍物圆心坐标 % global_path: M x 2 全局路径点 % weights: [w_heading, w_dist, w_vel, w_path] endpoint traj(end, :); % 方位角分量 goal_angle atan2(goal(2) - endpoint(2), goal(1) - endpoint(1)); heading_diff abs(angle_diff(goal_angle, endpoint(3))); heading_score pi - heading_diff; % 偏差越小分越高 % 障碍距离分量 min_dist inf; for i 1:size(traj, 1) d min(sqrt(sum((obstacles - traj(i, 1:2)).^2, 2))); if d min_dist min_dist d; end end if min_dist 0.2 dist_score -inf; % 撞了直接淘汰 else dist_score min_dist; end % 速度分量 vel_score traj(end, 1); % 用末端线速度近似 % 路径跟随分量 path_dist min(sqrt(sum((global_path - endpoint(1:2)).^2, 2))); path_score -path_dist; score weights(1)*heading_score weights(2)*dist_score ... weights(3)*vel_score weights(4)*path_score; end function d angle_diff(a, b) d mod(a - b pi, 2*pi) - pi; end逻辑说明angle_diff把角度差归一化到 $[-\pi, \pi]$避免绕圈问题。障碍距离分量里如果最小距离小于0.2米直接给负无穷相当于硬约束淘汰。路径跟随分量用负距离离全局路径越近分越高。参数说明四个权重没有万能值。我一般从[0.3, 0.4, 0.1, 0.2]起步障碍密集场景把w_dist提到0.5以上开阔场景把w_vel提到0.3让机器人跑快点。归一化方式不同权重差异很大建议先把每个分量除以自己的最大值再加权。3. 动态障碍物建模与滚动窗口让避障真正“动”起来3.1 动态障碍物的三种建模方式与选型静态障碍物在DWA里就是一堆固定圆心和半径的圆。动态障碍物麻烦得多因为它的位置随时间变化而DWA的轨迹预测是未来2到3秒必须预测障碍物在这段时间里在哪。常见做法有三种。第一种是匀速直线模型假设障碍物保持当前速度不变位置按 $p_{t1} p_t v_{obs} \Delta t$ 更新。实现最简单适合行人、其他机器人这类运动规律性强的场景。第二种是恒定加速度模型多一个加速度状态量适合车辆。第三种是概率预测用卡尔曼滤波或粒子滤波给出障碍物未来位置的概率分布DWA评价时用期望距离。实现复杂但更鲁棒。我一般先用匀速模型跑通因为大多数室内场景障碍物速度变化不快。如果发现机器人对突然变向的障碍物反应迟钝再升级到概率预测。function obs_pred predict_obstacles(obstacles, obs_vel, dt, steps) % obstacles: N x 2 当前障碍物位置 % obs_vel: N x 2 当前障碍物速度 % dt: 仿真步长 % steps: 预测步数 % obs_pred: steps x N x 2 每个时刻的障碍物位置 n_obs size(obstacles, 1); obs_pred zeros(steps, n_obs, 2); pos obstacles; for k 1:steps pos pos obs_vel * dt; obs_pred(k, :, :) reshape(pos, [1, n_obs, 2]); end end逻辑说明每个时间步把所有障碍物位置按速度推进一次存到三维矩阵里。评价轨迹时第 $k$ 步的轨迹点要和第 $k$ 步的障碍物位置比距离而不是和当前障碍物位置比。这一点很多人会搞错导致动态障碍物避不开。参数说明obs_vel需要从传感器数据里估计激光雷达做帧间匹配或者用卡尔曼滤波跟踪都能拿到。如果拿不到速度退而求其次用最近两帧位置差除以时间间隔。3.2 滚动窗口与重规划频率的配合DWA本身就是一个滚动窗口规划器每个控制周期重新采样、重新评价、重新选速度。但重规划频率和仿真步长是两回事。仿真步长dt决定轨迹预测的精度重规划频率决定机器人对外界变化的响应速度。常见配置是仿真步长0.1秒重规划频率10赫兹也就是每个仿真步都重规划。如果计算资源紧张可以降到5赫兹但机器人会明显变迟钝。我试过在复杂场景里把重规划频率提到20赫兹避障成功率提升有限计算量翻倍不划算。% 主循环骨架 dt 0.1; predict_time 2.5; replan_interval 1; % 每1个仿真步重规划一次 step_count 0; while ~reached_goal(pose, goal, 0.3) if mod(step_count, replan_interval) 0 % 计算动态窗口 v_window compute_dynamic_window(pose, vel, robot_params, dt); w_window compute_dynamic_window_w(pose, vel, robot_params, dt); % 生成候选轨迹 [trajs, ~] generate_trajectories(pose, v_window, w_window, ... 7, 7, dt, predict_time); % 预测动态障碍物 obs_pred predict_obstacles(obstacles, obs_vel, dt, ... floor(predict_time/dt)); % 评价并选最优 best_score -inf; best_v 0; best_w 0; for i 1:size(trajs, 1) s evaluate_trajectory_dynamic(squeeze(trajs(i,:,:)), ... goal, obs_pred, global_path, weights); if s best_score best_score s; best_v V(i); best_w W(i); end end end % 执行速度指令更新位姿 pose update_pose(pose, best_v, best_w, dt); step_count step_count 1; end逻辑说明主循环里每隔replan_interval步做一次完整的DWA计算其余步只执行上一次的速度指令。这样可以在计算资源和响应速度之间做权衡。evaluate_trajectory_dynamic和静态版本的区别在于障碍物距离计算要用对应时刻的预测位置。参数说明replan_interval取1是最稳的取2或3能省计算但会牺牲响应。reached_goal的阈值0.3米是经验值太小机器人会在目标点附近震荡太大则停不准。3.3 动态窗口计算的边界处理动态窗口计算里有个容易翻车的地方当机器人当前速度接近极限时加速度约束可能让窗口变成空集。比如机器人已经以最大线速度跑加速度限制下窗口上限还是最大速度下限是最大速度减去 $a_{max} \Delta t$窗口非空。但如果同时角速度也到极限两个窗口的笛卡尔积可能没有可行解。处理办法是加一个降级策略如果动态窗口为空逐步放宽加速度约束直到窗口非空。或者直接把速度降到零附近重新规划。我一般用前者因为急停对机械结构冲击大。function v_window compute_dynamic_window(pose, vel, params, dt) v_min max(params.v_min, vel(1) - params.a_max * dt); v_max min(params.v_max, vel(1) params.a_max * dt); if v_min v_max % 降级放宽约束 v_min params.v_min; v_max params.v_max; end v_window [v_min, v_max]; end逻辑说明先按加速度约束算窗口如果下限超过上限说明无解直接退回到机器人物理极限。这个降级策略在实际跑的时候救过我好几次尤其是机器人从高速急转弯的时候。参数说明a_max取机器人实际最大加速度的80%左右留点余量。v_min如果设成0机器人可以原地转设成负值则允许倒车但评价函数里速度分量要改成绝对值。4. 避坑与排查DWA调参路上最常见的五个翻车现场4.1 机器人原地抖动或画龙现象机器人不往前走在原地左右摇摆或者走直线时轨迹像蛇一样。原因角速度采样分辨率太高相邻两个速度对的评分接近机器人反复切换。或者方位角分量的权重过大机器人过度追求朝向目标而忽略前进。解决把n_w从10降到5降低角速度分辨率。同时把速度分量权重从0.1提到0.2以上给前进一点奖励。如果还抖在评价函数里加一个惩罚项惩罚角速度的绝对值。4.2 动态障碍物明明在旁边却撞上去现象静态障碍物避得很好一加动态障碍物就撞。原因评价轨迹时用的是障碍物当前位置而不是对应时刻的预测位置。这是最常见的实现错误。解决检查evaluate_trajectory_dynamic里障碍物距离的计算确保第 $k$ 个轨迹点比的是第 $k$ 个预测障碍物位置。另外确认predict_obstacles的步长和轨迹预测步长一致。4.3 机器人卡在障碍物前面不动现象机器人走到障碍物前停下不绕行也不后退。原因所有候选轨迹的障碍距离分量都低于安全阈值被硬约束淘汰导致没有可行解。或者路径跟随分量权重太大机器人不敢偏离全局路径。解决把障碍距离的硬约束阈值从0.2降到0.15给机器人一点贴边通过的空间。同时降低路径跟随权重让局部避障有更多自由度。如果还卡检查全局路径是不是穿过了障碍物全局规划本身有问题局部怎么调都没用。4.4 计算太慢导致控制周期跟不上现象仿真里跑得好好的放到实机上机器人反应迟钝或者MATLAB卡顿。原因采样数量太多或者评价函数里用了for循环逐点算障碍距离。解决把n_v和n_w各降到5总轨迹数从100降到25。障碍距离计算用pdist2或者矩阵运算替代循环。如果还慢把预测时长从3秒降到2秒减少轨迹点数量。4.5 目标点附近来回震荡现象机器人到了目标点附近但不停止来回转圈。原因目标点判定阈值太小机器人速度降不下来冲过目标点后又往回走。解决把到达阈值从0.1米提到0.3米。同时在评价函数里加一个减速逻辑当机器人离目标小于1米时速度分量的权重逐渐降到零让机器人优先对准目标而不是冲速度。5. 从跑通到跑稳DWA参数整定的进阶手法5.1 用代价地图替代离散障碍物列表前面代码里障碍物是一堆圆心坐标实际场景里激光雷达给的是点云或者栅格地图。把点云转成障碍物列表会丢失形状信息而且计算量大。更好的做法是直接用代价地图每个栅格有一个代价值评价轨迹时查表得到障碍代价。function cost query_cost_map(cost_map, resolution, origin, point) % cost_map: 二维矩阵值越大越危险 % resolution: 每格代表的实际距离 % origin: 地图左下角对应的实际坐标 % point: [x, y] 查询点 idx_x round((point(1) - origin(1)) / resolution) 1; idx_y round((point(2) - origin(2)) / resolution) 1; % 边界检查 idx_x max(1, min(size(cost_map, 2), idx_x)); idx_y max(1, min(size(cost_map, 1), idx_y)); cost cost_map(idx_y, idx_x); end逻辑说明把连续坐标转成栅格索引查表得到代价值。代价地图可以用膨胀层把障碍物周围一圈设成高代价这样机器人会自然远离障碍而不是贴边。参数说明resolution取0.05到0.1米太细计算量大太粗避障精度不够。膨胀半径取机器人半径加0.1米安全余量。5.2 自适应权重让参数随场景变化固定权重在简单场景够用复杂场景就不行了。我一般会加一个简单的自适应逻辑根据机器人周围障碍物密度调整权重。function weights adaptive_weights(base_weights, cost_map, pose, resolution, origin) % 查询机器人周围3x3区域的代价 local_cost 0; for dx -1:1 for dy -1:1 p [pose(1) dx*resolution, pose(2) dy*resolution]; local_cost local_cost query_cost_map(cost_map, resolution, origin, p); end end local_cost local_cost / 9; % 障碍密集时提高距离权重降低速度权重 if local_cost 0.5 weights base_weights .* [0.8, 1.5, 0.5, 1.0]; else weights base_weights; end end逻辑说明查询机器人周围9个格子的平均代价如果超过阈值说明障碍密集把距离权重乘1.5、速度权重乘0.5。这个逻辑很粗糙但有效比固定权重在混合场景里表现好很多。参数说明阈值0.5取决于代价地图的归一化方式需要根据实际地图调。自适应权重的缩放系数不要超过2倍否则参数变化太剧烈会导致机器人行为不稳定。5.3 用仿真回放验证参数一个可复现的测试流程调参最忌讳凭感觉改。我习惯搭一个回放测试固定几张地图和几组障碍物轨迹每次改参数跑一遍记录成功率、平均速度、最小障碍距离三个指标。指标含义目标成功率到达目标且无碰撞的测试次数占比大于95%平均速度全程线速度均值越高越好但不牺牲成功率最小障碍距离全程离障碍物最近的距离大于0.15米测试流程准备5张地图开阔、走廊、密集障碍、动态障碍、混合每张跑20次障碍物初始位置随机。每次改一个参数跑完100次测试对比三个指标。这样调出来的参数比拍脑袋靠谱得多。我自己的习惯是先把成功率调到95%以上再优化平均速度。最小障碍距离低于0.1米说明太激进高于0.3米说明太保守。这套流程跑下来DWA在室内场景基本能稳定工作。希望帮到你。本文还有配套的精品资源点击获取