企业官网建设流程全解析

简介：这份MATLAB源码包面向机器人路径规划学习者与研发人员，重点解决全局路径规划与局部实时避障的协同问题。代码主体基于DWA（动态窗口法）三维避障框架，融合改进的RRT算法生成的全局参考路径，并设计了障碍物感知机制：当机器人检测到全局路径存在障碍时，自动计算切入点和切出点，实现绕障后平滑回归原路径，兼顾全局效率与局部灵活性。压缩包共14个m文件，大小仅14KB，包含主程序DWA_3D.m、轨迹生成generateTrajectory.m、碰撞检测isCollision.m、动态窗口评估及速度采样等核心模块，各文件附详细中文注释，便于逐行理解算法原理与参数调优。目前已有370人学习下载，适合具备一定MATLAB基础、希望快速复现混合路径规划策略或在此基础上做二次开发的读者参考使用。

1. 一套能直接跑的双闭环路径规划：改进RRT做全局、DWA做局部，避障不再靠运气

“全局规划 局部规划 matlab代码实现”这个组合能解决很具体的痛点：只做全局规划的小车，在仿真里没画出来的纸箱、行人面前只能急停；只做局部规划的小车，又绕不开地图里的U形死角。这套把改进RRT当全局、DWA当局部的MATLAB实现，把两个算法装进同一个可运行工程，注释量很大，地图换一换就能看到一条全局路线上小车实时躲开动态障碍。适合正在做AGV路径仿真、机器人导航课程设计，或者想在现成代码上改参数的开发者——它解决的不是“算法长什么样”，而是“两个算法怎么在程序里握手”。

2. 改进RRT做全局路径：目标偏置采样与剪枝，把弯绕的树拉直

2.1 原始RRT的两个痛点：均匀采样太瞎，生长出来的路径没法直接用

RRT的朴素版本在每次迭代时，在整个地图范围内均匀地撒一个随机点，然后从已有的随机树里找最近节点，朝随机点方向生长一段。这个逻辑有一个很实际的问题：地图越大，撒在终点方向附近的点比例越低，树会长得漫无目的，跑到一两千次迭代还没有接近目标。另一个问题是，RRT生成的路径是由一段段树的分支拼出来的，节点之间是折线连接，车要是沿着这条路径走，走到每个拐点都得停下来转向，速度根本提不起来，而且很多拐点实际上是可以直接跨过去的——中间并没有障碍物。

所以我在拿到这套代码之后，第一件事就是看它对原始RRT做了什么改动。项目正文虽然没写具体改法，但按这套实现的效果来看，目标偏置采样和路径剪枝是两处关键改动，这两处也是当前主流改进RRT的通用做法。

2.2 目标偏置采样：用一个概率把随机树“拽”向终点

目标偏置的实现思路很简单：每次采样时，不是百分之百随机撒点，而是以一定的概率直接把终点当作采样点。这样一来，树在生长的过程中每隔几次迭代就会朝终点方向“冲动”一次，接近终点的速度会快很多。这份资源里对应的采样函数大概是这样的结构：

function [p_sample] = SamplePoint(p_goal, map, bias_prob) % 目标偏置采样：以 bias_prob 概率直接取目标点，其余在地图内均匀采样 if rand() < bias_prob p_sample = p_goal; % 直接朝终点生长 else x = map.x_min + rand() * (map.x_max - map.x_min); y = map.y_min + rand() * (map.y_max - map.y_min); p_sample = [x, y]; end end

逻辑上就两步：掷一个骰子，中奖就返回终点坐标，没中就在地图边界内均匀撒点。这里最重要的参数是bias_prob，它控制树的“目标倾向性”。我一般会把它设在 0.1 到 0.3 之间：设大了，树会快速向终点生长，但同时容易一条直线冲过去，在障碍物密集的区域很难找到绕行的缝隙；设小了，目标倾向不明显，退化成原始RRT。

2.3 碰撞检测：树能不能长，全靠一个函数把关

RRT的生长过程，每一次向新节点延伸都要做碰撞检测。常见的实现方式是在两个节点之间按步长密集取点，判断每个点是否落在障碍物区域里。这套代码里对应的是is_collision_free这一类函数，它负责判断从当前节点到新节点的连线是否存在障碍物：

function [free] = IsCollisionFree(p_from, p_to, map, step_len) % 把两点连线按 step_len 步长密集插值，逐一判断是否落在障碍物内 dist = norm(p_to - p_from); n_steps = ceil(dist / step_len); free = true; for k = 0:n_steps p_check = p_from + (p_to - p_from) * (k / n_steps); if IsInObstacle(p_check, map) % 查询栅格地图占用状态 free = false; return; end end end

注意step_len的取值：我一般取栅格边长的 0.5 倍左右。取值太大，细小的障碍物可能被直接“跨过去”，路径看起来没碰障碍，实际运动时车体会撞上；取值太小，碰撞检测的计算量成倍上升。也就是说，碰撞检测函数的开销基本决定了整个RRT规划一帧的耗时，这是后面调实时性时第一个要动的地方。

2.4 路径剪枝：从终点往前回溯，能直连就跳过中间节点

RRT 规划完成后得到的路径，节点数可能很多，路径总长度也可能明显长于实际最短距离。剪枝的做法是从起点开始，尝试直接连接后面的节点，如果连线没有碰撞，就说明中间那些节点完全可以跳过。这套实现里我看到的剪枝逻辑大致如下：

function [path_pruned] = PrunePath(path, map, step_len) % 剪枝：从当前节点开始，尝试直连最远的可行节点 path_pruned = path(1, :); i = 1; while i < size(path, 1) j = size(path, 1); while j > i + 1 if IsCollisionFree(path(i, :), path(j, :), map, step_len) break; % 找到可达的最远节点 end j = j - 1; end path_pruned = [path_pruned; path(j, :)]; % 跳过中间的冗余点 i = j; end end

剪枝完成之后，路径上的节点数会急剧减少，比如原本三十多个节点的路径，可能只剩五到七个关键拐点。这对后续DWA的局部规划是有直接好处的：全局路径越干净，DWA拿到的前视方向就越稳定，不会因为全局路径本身弯弯绕绕而频繁转向。另一个隐藏收益是，路径总长度也会缩短一点，虽然不保证是最短路径，但至少比原始RRT的直接输出好很多。

3. DWA局部重规划：速度窗口采样、轨迹推演与全局路径对接

3.1 DWA在整套系统里扮演什么角色：全局路径是导航图，DWA是方向盘

全局路径规划完成后，得到的是从起点到终点的一串关键节点。但真实场景里这串节点只代表“大方向怎么走”，小车真正往前走的时候，每时每刻都在重新计算“下一步该用什么速度”。这就是DWA的活：在每一帧控制周期内，根据当前速度、当前位置、局部障碍物分布和局部目标点，从一组合法速度里挑一组最优的。所以整套系统的数据流是这样的：全局规划只跑一次，得到一条路径；之后小车每次往前挪动，都在DWA的动态窗口里重新决策。

DWA这个名字里的“动态窗口”，指的是当前速度所决定的可达速度范围。一个正在以 1 m/s 往前开的车，不可能瞬间变成 -1 m/s 后退，也不可能突然获得一个很大的角速度。所以DWA第一步就是把速度空间限制在一个“当前状态可达”的窗口里，而不是整个速度空间。

3.2 轨迹推演：把每组速度先跑一遍，看看会撞到什么

DWA的核心计算是轨迹推演。对窗口内采样的每组(v, w)，模拟小车在接下来一小段时间内的运动轨迹，然后检查这条轨迹是否碰撞、离目标方向有多偏、速度够不够快。对应代码的大致结构如下：

function [traj] = GenerateTrajectory(x, y, theta, v, w, dt, horizon) % 以当前位姿 (x,y,theta) 和速度 (v,w) 推演 horizon 时间内的轨迹 n_steps = round(horizon / dt); traj = zeros(n_steps, 3); for k = 1:n_steps x = x + v * cos(theta) * dt; % 沿当前朝向前进 y = y + v * sin(theta) * dt; theta = theta + w * dt; % 角速度积分 traj(k, :) = [x, y, theta]; end end

这里dt是推演步长，horizon是推演时长。horizon很关键：设得太大，轨迹预测时间过长，远处的障碍物会过分影响当前决策，小车会显得“畏畏缩缩”；设得太小，预测轨迹太短，近处的障碍物可能来不及被发现。通常horizon取 1.5 到 3 秒之间，具体看车的最大速度和减速能力。

3.3 评价函数：三项打分，决定哪组速度胜出

每一组速度对应的轨迹推演完之后，都要算一个分数，分数由三部分构成：轨迹终点朝向与目标方向的夹角代价、轨迹与最近障碍物的距离代价、速度大小本身的代价。这套代码里的评价函数大致是：

function [score] = EvaluateTrajectory(traj, goal, obstacles, v, w, weights) % 三项加权评分：朝向代价、障碍距离代价、速度代价 terminal_heading = atan2(goal(2) - traj(end,2), goal(1) - traj(end,1)); heading_cost = abs(angdiff(traj(end,3), terminal_heading)); dist_cost = MinDistToObstacles(traj, obstacles); speed_cost = 1.0 / (1.0 + abs(v)); % 速度越接近目标速度，代价越低 score = weights(1) * heading_cost ... + weights(2) * dist_cost ... - weights(3) * speed_cost; end

权重weights的整定是DWA里最“玄学”的部分。我从这份图纸上的经验来看，需要遵守一个原则：障碍距离代价在任何情况下都不能为 0，否则小车会贴着障碍物走。我一般这样起调：朝向代价权重 0.05，障碍距离权重 0.2，速度权重 0.1，然后根据仿真效果调。如果小车频繁左右摇摆，多半是朝向代价权重太高；如果小车贴着障碍物走，则是障碍距离权重偏低。

3.4 全局路径怎么喂给DWA：不是整条路径，而是“前视目标点”

RRT规划出来的路径是一串节点，但DWA每一帧只需要一个局部目标点，不能把整条路径直接丢给DWA。正确做法是在全局路径上找一个“当前目标点”，这个点离当前位置有一定前视距离。代码逻辑如下：

function [goal_local] = GetLocalGoal(path, pos, lookahead) % 找到路径上距离当前位置最近的点，再沿路径向前取 lookahead 距离点 dists = sqrt(sum((path - pos).^2, 2)); [~, idx] = min(dists); arc_len = 0; for k = idx:size(path, 1) - 1 arc_len = arc_len + norm(path(k+1, :) - path(k, :)); if arc_len >= lookahead goal_local = path(k+1, :); return; end end goal_local = path(end, :); % 如果路径快到终点，直接取终点 end

lookahead的取值对系统行为影响非常大：取值过小，小车会非常“近视”，每次只盯着脚下一点点距离，动态障碍一出现就容易急转；取值过大，小车会忽略近处的障碍，DWA的局部避障效果就体现不出来。一般来说，lookahead取小车最大速度对应制动距离的 1.2 到 1.5 倍比较合适。

4. 联调避坑：RRT+DWA最常见的五个翻车现场

4.1 DWA选了一条绕远的路，全局路径被无视

现象：全局规划明明是一条从左绕过障碍的路径，小车起步后却向右拐，绕了大半圈才回来。

原因：局部目标点选取时，直接取了路径上的最近点而不是前视点，最近点在小车的侧后方，导致DWA算出来的“朝向代价最小”的速度方向跟全局路径背道而驰。

解决：检查GetLocalGoal的返回值，确认它取的是“路径上前视距离之外的点”，而不是离车最近的路径点。前视距离至少要大于车的转弯半径，否则局部目标点出现在侧后方，DWA必然“叛逆”。

4.2 全局路径贴着障碍物，DWA还没开始避就已经撞了

现象：RRT 剪枝完成后，路径上一段线段紧贴障碍物边缘，全局路径在栅格地图上看着没碰撞，但小车实际宽度比栅格大，一开过去就刮蹭。

原因：RRT 的碰撞检测只判断了路径线段是否穿过障碍栅格，没有给障碍物做膨胀。路径贴着障碍物边缘空出来的“安全距离”，不够车体宽度的一半。

解决：在建图之后对障碍物做膨胀处理，膨胀半径取车体宽度一半加 5 到 10 厘米的安全余量。所有碰撞检测都基于膨胀后的栅格地图跑，而不是原始障碍物地图。

4.3 小车蛇形走位，DWA每帧都在换方向

现象：整段路径跑下来，小车的朝向一直在左右修正，轨迹像一条蛇，速度也上不去。

原因：局部目标点切换太频繁。lookahead取值太小，小车每往前走一点就要重新瞄准一个新目标点，目标点方向变化略微超过转向响应速度，就会来回修正。另外，全局路径节点过密也会有这个问题。

解决：把lookahead调大到 1.2 倍制动距离以上；同时可以给DWA加一个朝向死区：只有当局部目标点与当前朝向的夹角超过一定阈值（比如 15 度）时才触发转向，否则保持当前角速度。

4.4 一帧仿真卡十几秒，实时性完全没法看

现象：打开仿真跑起来，全局规划耗时接近一秒，DWA每帧决策也要几百毫秒，小车动一下停一下。

原因：三个地方同时拖慢：剪枝碰撞检测里step_len取得太小，整条路径反复插值判断；轨迹推演里速度采样分辨率太高，上百组速度每一组都推演几千个点；还有全局规划在每次迭代里都重复做全图最近邻搜索。

解决：先降低速度采样分辨率，比如线速度 20 档、角速度 20 档，总共 400 组，每组推演 30 个点，这个量级在 MATLAB 里能跑到每帧几十毫秒。再把碰撞检测的step_len调大到地图栅格边长的 0.5 倍。最后，全图搜索可以用 KD 树或直接对栅格地图做距离变换，最近邻查询从遍历变成查表。

4.5 环境一变化就重新全局规划，DWA被反复打断

现象：动态障碍物挡住去路时，DWA 绕行到一半，全局路径因为起点变化被重新规划，新路径和旧路径差异很大，小车原地愣住。

原因：代码把“动态障碍影响”判断得太敏感，一旦DWA偏离全局路径过远，就触发全局重规划。全局规划本身是耗时的，频繁重规划导致DWA的连续性完全被破坏。

解决：给全局重规划加一个触发条件：只有当前方路径被完全堵死（DWA在该位置找不到任何可达速度）时才触发全局重规划；DWA绕行偏离全局路径一定范围内，不触发。这样全局规划基本只跑一次，DWA负责所有局部调整，分工才是干净的。

5. 仿真验证：造一张带动态障碍的地图，跑通整个双闭环

5.1 构造栅格地图与障碍物表示

要把这套代码真正跑起来，第一步是构造一份合适的地图。以我调这种代码的习惯，地图一般用 2D 栅格矩阵表示，使用0表示自由空间，使用1表示障碍物。这份实现里的地图结构大概是这样的：

function [map] = CreateMap(width, height, inflate_radius) % 生成一张栅格地图，width/height 为栅格数量 map.grid = zeros(height, width); % 0 = 自由, 1 = 占用 % 画几块矩形障碍物 map = AddRectObstacle(map, 20, 20, 35, 8); map = AddRectObstacle(map, 50, 35, 8, 25); map = AddRectObstacle(map, 70, 10, 25, 10); % 障碍物膨胀，给出安全距离 map = InflateObstacles(map, inflate_radius); map.x_min = 0; map.x_max = width; map.y_min = 0; map.y_max = height; end

AddRectObstacle(map, x, y, w, h)的作用是把一张矩形区域置为占用状态，InflateObstacles则把所有障碍物向四周扩展一圈。这一步如果不做，前面避坑记录里说的“车体刮蹭”问题会在第一次仿真里就出现。地图尺寸我一般用 100×100 栅格起步，跑通之后再加大。

5.2 动态障碍物模拟与重规划触发

静态地图跑通之后，再验证DWA的实时避障能力。做法是让某个障碍物在仿真过程中缓慢移动，模拟突然出现的行人或货架：

function [map] = MoveDynamicObstacle(map, obstacle_id, dx, dy) % 把动态障碍物从原位置平移一段距离 old_pos = map.dynamic_obstacles{obstacle_id}.pos; map.dynamic_obstacles{obstacle_id}.pos = old_pos + [dx, dy]; % 重新生成栅格占用状态，并做膨胀处理 map = UpdateOccupancy(map); end

在仿真主循环里，小车每往前移动一小段距离，就检查一次当前速度是否还能安全行驶。如果DWA在动态窗口内找不到任何无碰撞的速度组合，就认为前方被动态障碍堵死，此时才触发一次全局重规划。每帧仿真结束后，记录一下小车当前位姿、速度、是否发生碰撞，方便后面统计指标。

5.3 一次完整运行的指标统计：路径长度、规划耗时、重规划次数

跑完一次完整的仿真，这份实现里会输出一张运行统计表。我一般记录这几项：

我在验证时一般固定同一份静态地图，先记录全局规划耗时和路径长度，再开启动态障碍，记录 DWA 的帧耗时和重规划次数。如果重规划次数超过 5 次，基本说明前视距离、膨胀参数或速度采样范围没调好，DWA 在每次避障上都“躲不过去”，才导致频繁搬救兵。这套流程走下来，双闭环到底稳不稳，几个数字就能看出来。

6. 从仿真到落地：模块拆分与调参顺序的最后一公里

仿真跑通之后，想把这套代码换到自己场景里，关键是做两件工程化的事情。第一，把地图、全局规划、局部规划拆成三个独立模块，我一般会用三个文件分别承载：map_server.m负责地图加载、膨胀和动态障碍更新；global_planner.m只负责跑改进 RRT、剪枝、输出关键路径节点；local_planner.m只负责 DWA 采样、推演和评价。这样替换地图和替换算法互不干扰。

第二是调参顺序，这条我踩过坑：不要一上来同时调 RRT 的偏置概率和 DWA 的权重。先把静态地图下的 RRT 调通，确认剪枝后的路径不贴障碍、路径节点控制在十个以内；再固定全局路径，调 DWA 的lookahead和评价权重，让小车沿路径走直线时没有蛇形；最后才开动态障碍，调 DWA 的采样范围和碰撞检测步长。每次只动一个参数，跑一次仿真看统计表，比同时改五个参数看效果要高效得多。

另外有一点要注意的是，MATLAB 的复杂循环适合调算法验证，不适合上实时平台。验证完效果之后，要把同样的逻辑翻译成 C++ 或直接换到 ROS 框架里用。我在做这一步时，会把 MATLAB 端的地图矩阵导出为文本文件，方便目标平台直接读取，避免两边地图数据对不上这种低级的坑。

这套代码给我最大的教训是：全局规划和局部规划不是两个孤立的算法，而是通过“前视目标点”这个接口耦合在一起的。全局路径再漂亮，DWA 拿不到稳定、合理的前视目标点，一样会走成蛇形；DWA 调得再好，全局路径贴着障碍物，也永远躲不开碰撞。从那以后，我每次拿到类似的路径规划代码，都会先画一张数据流图，确认全局规划的输出是谁在消费、局部规划的地图是谁在更新，再开始调参，而不会一头扎进权重堆里试运气。希望这套双闭环实现的思路和这些避坑记录，能在你的项目里帮你少走一圈弯路。

本文还有配套的精品资源，点击获取