简介：本资源是一份面向路径规划初学者与移动机器人开发者的A星融合DWA算法实现包，重点解决静态栅格地图与动态障碍环境下的全局寻路与局部避障问题，适合学习MATLAB算法仿真或快速搭建验证环境。资源共13个文件，包含6份docx技术分析文档、4份html说明页面及3张jpg示意图，从算法原理、代码结构到静动态避障策略均有展开，压缩包仅1.15MB，轻量易用，便于按文档对应关系逐一阅读。已有330人学习下载，关注度稳定。内容既包括对A星启发式搜索与DWA动态窗口法的融合思路分析，也提供带详细注释的MATLAB源码与实现说明，可直接用于无人车、服务机器人等场景的静态避障与动态目标规避试验，帮助读者将论文原理落地为可运行代码。

1. A星与DWA融合到底在解决什么：静态全局规划与动态局部避让的单算法短板

调度一台差速底盘 AGV 在人车共行的仓库里跑，墙和货架是静态障碍，但同事推着托盘车、行人会随时出现在通道上。只上 A星（A*），地图一变就得全图重算，动态障碍出现时控制周期根本等不起；只上 DWA，遇到凹形障碍物就会原地打转，全局方向感全丢。A星与DWA融合的解法就是把这两件事拆开干：A星在静态栅格地图上算一条全局参考线，DWA以参考线为方向做短时速度规划与动态避让，两者用一个局部目标点衔接。这篇笔记把融合思路落到 Matlab 带注释源码上，讲清静态避障、动态避障和两者衔接的细节，适合做移动机器人课程设计、AGV 调度 Demo，或者计划从仿真改到实车验证的开发者。

2. 静态避障底座：用A星在Matlab里生成栅格全局路径

2.1 栅格地图与“隐形增肥”：为什么A星不能按真实车身尺寸搜

A星搜索的前提是栅格地图，但直接把真实地图喂给它会出问题。栅格地图里的障碍物是传感器轮廓线，机器人是有体积的，路径一旦贴着墙根走，DWA 在局部避障时根本没有横向余量去闪避那些突然出现的动态障碍。所以静态避障的第一步不是写搜索代码，而是把地图做“隐形增肥”，也就是障碍物膨胀。

我一般这样做：拿到原始逻辑地图后，按机器人外接圆半径加一个安全余量做二值膨胀。半径换算成栅格数，用ceil((robotRadius + safetyMargin) / cellSize)向上取整，再用strel('disk', r)做形态学膨胀。这一步会直接决定后面 DWA 的发挥空间：膨胀太薄，路径贴障碍物太近；膨胀太厚，狭窄通道直接被堵死，A星可能连路都找不到。

function inflateMap = inflateObstacleMap(map, robotRadius, safetyMargin, cellSize) % 按机器人尺寸膨胀静态障碍物 % map : 逻辑栅格地图，true 表示障碍物 % robotRadius : 机器人外接圆半径，单位米 % safetyMargin : 额外安全余量，单位米 % cellSize : 栅格边长，单位米/栅格 r = ceil((robotRadius + safetyMargin) / cellSize); se = strel('disk', r); inflateMap = imdilate(map, se); end

这里imdilate是图像处理工具箱里的二值膨胀函数，没有工具箱也可以用二维卷积加阈值替代。膨胀半径直接决定 A星路径离墙根有多远，实际工程里我习惯至少留出 2～3 个栅格的安全余量，这样后面 DWA 做动态避障时才有足够的横向空间。

栅格代价也可以做“软膨胀”而不是纯二值：离障碍越近的栅格代价越高，A星自然偏向中间走。不过软代价会明显提高搜索耗时，静态地图不大的时候用纯粹二值膨胀就够了。

2.2 A星搜索的Matlab核心代码：代价函数与开闭列表

A星的骨架是f(n) = g(n) + h(n)。g 是从起点到当前栅格的实际代价，h 是当前栅格到终点的启发代价。静态避障场景里我常用欧几里得距离做启发函数，因为机器人可以八方向移动，欧氏距离比曼哈顿距离更贴近真实运动代价，搜索出来的路径更直。

下面这段代码是 A星的核心搜索循环。为了便于读懂，我用两个数组模拟开集合，没有引入复杂堆结构，地图在几百乘几百的规模下跑起来完全够用。

function [path, cost] = astar_grid(map, start, goal, useDiag) % A星栅格搜索 % map : 0=可通行, 1=障碍（已膨胀） % start : [row, col] 起点 % goal : [row, col] 终点 % useDiag: 是否允许斜向移动 [rows, cols] = size(map); heuristic = @(a, b) sqrt((a(1)-b(1))^2 + (a(2)-b(2))^2); gScore = inf(rows, cols); gScore(start(1), start(2)) = 0; fScore = inf(rows, cols); fScore(start(1), start(2)) = heuristic(start, goal); openList = zeros(rows * cols, 3); openCount = 0; openCount = openCount + 1; openList(openCount, :) = [start(1), start(2), fScore(start(1), start(2))]; openFlag = false(rows, cols); openFlag(start(1), start(2)) = true; parentR = zeros(rows, cols); parentC = zeros(rows, cols); % 八方向邻域，useDiag=false 时只取前四组 dirs = [-1 0; 1 0; 0 -1; 0 1]; if useDiag dirs = [dirs; -1 -1; -1 1; 1 -1; 1 1]; end while openCount > 0 % 取 f 值最小的结点 [~, minIdx] = min(openList(1:openCount, 3)); current = openList(minIdx, :); row = current(1); col = current(2); if row == goal(1) && col == goal(2) break; end % 从开列表中移除当前结点 openList(minIdx, :) = openList(openCount, :); openCount = openCount - 1; openFlag(row, col) = false; for k = 1:size(dirs, 1) nr = row + dirs(k, 1); nc = col + dirs(k, 2); if nr < 1 || nr > rows || nc < 1 || nc > cols continue; end if map(nr, nc) == 1 continue; end % 斜向移动代价取 sqrt(2)，四方向取 1 stepCost = sqrt(2); if abs(dirs(k, 1)) + abs(dirs(k, 2)) == 1 stepCost = 1; end tentativeG = gScore(row, col) + stepCost; if tentativeG < gScore(nr, nc) - 1e-9 gScore(nr, nc) = tentativeG; fScore(nr, nc) = tentativeG + heuristic([nr nc], goal); parentR(nr, nc) = row; parentC(nr, nc) = col; if ~openFlag(nr, nc) openCount = openCount + 1; openList(openCount, :) = [nr, nc, fScore(nr, nc)]; openFlag(nr, nc) = true; else % 已在开列表中，更新记录 for q = 1:openCount if openList(q, 1) == nr && openList(q, 2) == nc openList(q, 3) = fScore(nr, nc); break; end end end end end end % 回溯路径 path = []; if gScore(goal(1), goal(2)) < inf crow = goal(1); ccol = goal(2); while crow ~= start(1) || ccol ~= start(2) path = [path; crow, ccol]; pr = parentR(crow, ccol); pc = parentC(crow, ccol); crow = pr; ccol = pc; end path = [path; start(1), start(2)]; path = flipud(path); end cost = gScore(goal(1), goal(2)); end

这段代码里有两个容易踩坑的细节。第一个是stepCost：四方向移动代价为 1，斜向移动为sqrt(2)，否则 A星会为了走对角线疯狂绕路。第二个是开列表更新：数组线性扫描在几百栅格规模下没问题，如果地图超过一两千乘一两千，建议用二叉堆来存openList，不然每轮迭代的更新复杂度会成为整体瓶颈。

2.3 从路径点到参考线：拐点剔除与等距插值

A星直接输出的是栅格中心点序列，相邻两个点可能只差 45 度方向。这条折线直接交给 DWA 当前视目标点时会有两个问题：一是目标点索引切换产生剧烈转角，二是路径长度变化不均匀导致前视距离失真。我一般会对路径做两步后处理。

第一步是拐点剔除：如果三个连续点p1-p2-p3中p1->p2和p2->p3的方向差小于一个阈值，就删掉p2。第二步是等距插值：沿剔除后的折线按固定步长重新采样，输出一条密度均匀的参考线。这样 DWA 在取局部目标点时，按弧长找点会比按索引找点稳定得多。

function refPath = smoothGlobalPath(path, stepLen) % 拐点剔除 + 等距插值 % path : A星输出的栅格点序列 [row, col] % stepLen: 插值步长，单位与网格坐标一致 % 1. 剔除共线或近似共线的中间点 kept = [path(1, :)]; for i = 2 : size(path, 1) - 1 v1 = path(i, :) - kept(end, :); v2 = path(i+1, :) - path(i, :); crossV = v1(1)*v2(2) - v1(2)*v2(1); if abs(crossV) > 0.01 kept = [kept; path(i, :)]; end end kept = [kept; path(end, :)]; % 2. 等距插值 refPath = kept(1, :); for i = 2 : size(kept, 1) segLen = norm(kept(i, :) - kept(i-1, :)); n = floor(segLen / stepLen); for k = 1 : n t = k / max(n, 1); pt = (1-t) * kept(i-1, :) + t * kept(i, :); refPath = [refPath; pt]; end end refPath = [refPath; kept(end, :)]; end

这个函数的crossV判断是关键：等于 0 表示三点共线，但浮点计算里取一个 0.01 的容差就够了。插值步长stepLen我习惯取机器人外接圆直径的一半，取太大前视目标点会跳过关键拐弯，取太小则参考线点数过多，DWA 每周期都要搜最近点，徒增计算量。

3. 动态避障引擎：DWA的速度采样、轨迹预测与评价函数

3.1 速度采样窗口：一个周期内能踩到哪些速度和角速度

DWA 的核心假设是：机器人每个控制周期只能在一个有限的速度集合里选出一个(v, w)。这个集合不是全速度空间，而是受三组约束夹出来的“动态窗口”——最大速度边界、最大加速度边界、障碍物安全边界。

其中加速度约束是最关键的。上一周期速度是v_old，控制周期是dt，那么这一周期的速度只能落在[v_old - accelV*dt, v_old + accelV*dt]之间，角速度同理。这保证了规划出来的速度指令在物理上可执行，不会出现“上一帧 0.3 m/s，下一帧直接给 1.2 m/s”这种执行器根本跟不上的命令。

function [vRange, wRange] = dwv_velocity_window(v, w, params) % 动态窗口采样范围 % params 需要包含: vMax, wMax, accelV, accelW, dt vLow = max(0, v - params.accelV * params.dt); vHigh = min(params.vMax, v + params.accelV * params.dt); wLow = max(-params.wMax, w - params.accelW * params.dt); wHigh = min(params.wMax, w + params.accelW * params.dt); % 按固定步长生成候选速度对 vRange = linspace(vLow, vHigh, params.vSamples); wRange = linspace(wLow, wHigh, params.wSamples); end

注意vLow我用的是max(0, ...)，不允许倒车。如果你的平台需要原地后退避障，可以把下限改成-vMax，但代价是 DWA 会在倒车和前进之间频繁切换，视觉上非常不稳。一般差速平台还是前向避障更可靠。

速度采样数量vSamples、wSamples是精度和耗时的折中。我做仿真时取 12×12 到 16×16 的网格足够，每个周期要跑 144 到 256 次轨迹预测。实车再结合嵌入式计算能力决定要不要降采样。

3.2 轨迹预测与三项评价函数

选出一组(v, w)后，需要预测机器人以这个速度跑一小段时间会走到哪。常见做法是用运动学模型做短时积分，预测horizon步，每一步更新位姿[x, y, theta]。差分驱动机器人的模型很简单：线速度沿车头方向平移，角速度改变航向角。

function traj = dwv_trajectory(pose, v, w, dt, horizon) % 差分驱动模型轨迹预测 % pose : [x, y, theta] % horizon : 预测步数 traj = zeros(horizon + 1, 3); traj(1, :) = pose; for i = 1 : horizon traj(i+1, 1) = traj(i, 1) + v * dt * cos(traj(i, 3)); traj(i+1, 2) = traj(i, 2) + v * dt * sin(traj(i, 3)); traj(i+1, 3) = traj(i, 3) + w * dt; end end

轨迹预测出来以后，要用三项评价函数给每条候选轨迹打分：朝向代价、障碍距离代价、速度代价。

• 朝向代价衡量轨迹终点朝向与局部目标点方向的一致程度，方向越对齐分越高。
• 障碍距离代价取整条预测轨迹与最近障碍物的最小距离，距离越小分越低，直接压掉会撞的轨迹。
• 速度代价鼓励机器人保持较高速度，避免它为了绕一个小障碍就停到 0。

function score = dwv_evaluate(traj, localGoal, obstacleMap, v, params) % 三项评价合一 % obstacleMap: 膨胀后的静态障碍 + 动态障碍投影 % 轨迹终点朝向与目标方向 terminal = traj(end, :); targetDir = atan2(localGoal(2) - terminal(2), localGoal(1) - terminal(1)); headingErr = abs(angdiff(terminal(3), targetDir)); headingScore = 1 - headingErr / pi; % 轨迹与障碍最小距离 minDist = dwv_min_clearance(traj, obstacleMap); if minDist < params.safeDist minDist = minDist / params.safeDist; % 归一化到 0~1 else minDist = 1; end % 速度偏好 velScore = v / params.vMax; score = params.alpha * headingScore + params.beta * minDist + params.gamma * velScore; end

angdiff是机器人工具箱里的角度差函数，没有的话自己写两行差值取模就行。dwv_min_clearance我通常是把轨迹点映射到栅格坐标系，逐个查局部障碍图上的值，取最小；也可以用距离变换预计算一张最近障碍距离场，查询就是 O(1)。

3.3 DWA参数的边界经验：权重、预测时长和采样步长

DWA 参数调起来很敏感，这也是很多人说它“玄学”的原因。其实只要理解每组参数在评价函数里的量级，就没那么难。下面是我在差速底盘 Demo 上反复试出来的参考范围：

最重要的一个心得是：三个权重不要孤立调，先把alpha固定成 1，然后只调beta和gamma，让三项得分量级保持大致相当。否则beta绝对值过大时，DWA 会变得“过分礼貌”，离障碍两米就开始刹车，机器人在空旷场地也走不出速度；alpha过小则变成“犹豫症”，机器人到了局部目标点附近总是内外晃。

另一个边界是horizon。预测时间太长，动态障碍物已经走出传感器视野，预测位置误差大；预测太短，DWA 只有刹车反应，来不及规划绕行。我一般把预测时长设成 2.5～4 秒，效果相对均衡。

4. A星与DWA的融合实现：局部目标点选取与主循环代码

4.1 融合的关键不是“两套算法相加”，而是局部目标点怎么取

A星和DWA单独都能跑，但直接拼在一起会立刻露馅：如果把地图终点直接喂给 DWA，DWA 在复杂静态环境里照样局部最优；如果每帧都用 A星重规划，动态避障周期根本等不及。真正的融合点在一个叫“局部目标点”的小函数上。

局部目标点的取法是这样：机器人每帧定位自己的当前位置，在 A星全局参考线上找到最近点，然后从最近点沿参考线向前取一个“前视距离”lookAhead处的点，这个点就是 DWA 当帧要冲的方向。DWA 不需要知道终点在哪，它只要追着这个前视点走，遇到动态障碍就临时绕开；绕完以后，参考线会把它重新拉回全局趋势上。

这里最需要注意的是最近点索引不能倒退。机器人绕行时可能短暂偏离参考线，如果每帧都用欧氏距离重新找最近点，索引可能在几个点之间跳来跳去，局部目标点也跟着前后抖动，DWA 就会摆头。

function [localGoal, idxNearest] = pick_local_goal(refPath, pose, idxPrev, lookAhead) % 从参考线上取DWA局部目标点 % refPath : 插值后的全局路径，[x, y] 或 [row, col] % pose : 当前位姿 % idxPrev : 上一帧最近点索引，防止倒退 % 1. 在当前索引附近搜索最近点，限制搜索范围 ±20 searchRange = max(1, idxPrev - 20) : min(size(refPath, 1), idxPrev + 20); dist2 = sum((refPath(searchRange, :) - pose(1:2)).^2, 2); [~, minLocal] = min(dist2); idxNearest = searchRange(minLocal); % 2. 从最近点向前累积弧长，直到达到前视距离 localGoal = refPath(idxNearest, :); cumLen = 0; for i = idxNearest + 1 : size(refPath, 1) cumLen = cumLen + norm(refPath(i, :) - refPath(i-1, :)); if cumLen >= lookAhead localGoal = refPath(i, :); break; end end end

这个函数有两点值得抠：搜索范围限制在±20点内，能挡住动态障碍造成的瞬时偏离；前视距离lookAhead我习惯取 0.8～1.5 米，和机器人最大速度挂钩，lookAhead ≈ vMax * 1.5s。

4.2 融合主循环的Matlab代码骨架

把静态 A星、动态窗口、评价函数和局部目标点串起来，就是这个融合算法的主循环。伪代码层面并不复杂：先跑一次 A星得到全局参考线，然后每个控制周期依次算局部目标点、生成速度窗口、预测轨迹、打分选最优、执行速度并更新位姿。

function runFusionPlanner(map, dynamicObstacles, start, goal, params) % A星 + DWA 融合规划主循环 % dynamicObstacles: 动态障碍物结构数组，每帧刷新位置和速度 % 1. 静态A星全局规划 inflateMap = inflateObstacleMap(map, params.robotRadius, params.safetyMargin, params.cellSize); rawPath = astar_grid(inflateMap, start, goal, true); globalPath = smoothGlobalPath(rawPath, params.refStep); % 2. 主循环 pose = [start, 0]; % 用角度 0 作为初始航向 idxNearest = 1; for step = 1 : params.maxSteps % 2.1 动态障碍物投影到局部栅格代价图 localMap = projectObstacles(inflateMap, dynamicObstacles, step); % 2.2 取局部目标点 [localGoal, idxNearest] = pick_local_goal(globalPath, pose, idxNearest, params.lookAhead); % 2.3 DWA速度采样与评价 [vRange, wRange] = dwv_velocity_window(pose(3), 0, params); bestScore = -inf; bestV = 0; bestW = 0; for v = vRange for w = wRange traj = dwv_trajectory(pose, v, w, params.dt, params.horizon); score = dwv_evaluate(traj, localGoal, localMap, v, params); if score > bestScore bestScore = score; bestV = v; bestW = w; end end end % 2.4 执行并更新位姿 pose = updatePose(pose, bestV, bestW, params.dt); record(step) = pose; % 2.5 到达判定 if norm(pose(1:2) - goal) < params.goalTol break; end end end

这个骨架我故意没把projectObstacles写全，因为它和你的传感器模型强相关。常见做法是把动态障碍物当前位置膨胀成一个局部障碍图，和静态膨胀图叠加，然后 DWA 的距离代价在这个叠加图上查询。注意动态障碍物每帧要刷新，不能把上一帧位置留在图里。

4.3 为什么要让DWA“忘记”终点：全局与局部职责划分

很多人在第一次做融合时，都会不自觉地想把终点塞给 DWA 当长期目标，理由是“这样 DWA 就不需要 A星了”。但这样做有个致命问题：DWA 的评价函数是局部视角，它只评估未来 3 秒内的轨迹，遇到一个凹形障碍会在原地来回试探，因为 3 秒内它看不出绕过凹槽需要先往反方向走。

A星在这个融合里的作用，不是给 DWA 一个“最终目的地”，而是给 DWA 一个“当前阶段性的方向”。前视目标点只领先机器人一小段路，DWA 的压力大大减小，它不需要全局视野，只需要保证这一段路不撞。等它绕过一个动态障碍，参考线上那个前视点会自动变成绕行后的下一点，全局趋势就回来了。

所以这个融合方案本质上是“主从式”：A星是主线，DWA 是执行器。DWA 的职责边界非常清楚，不需要理解整张地图。理解这一点后，你就不会在融合代码里给 DWA 加一堆全局启发项了，那只会让它变回一个性能更差的全局规划器。

5. 融合算法排雷：5个最常见的失败模式与参数修正

5.1 路径贴墙、车贴墙：静态障碍膨胀半径不够

现象：A星算出的路径紧贴障碍物边界，DWA 在静态墙边反复急转，遇到稍大一点的动态障碍直接退无可退。 原因：inflateObstacleMap的膨胀半径只算了机器人内切圆，没有算外接圆。差速底盘一般是长方形，斜着过通道时车角会扫到栅格外。 解决：膨胀半径用外接圆半径，不能只算车宽的一半。以 0.6 m 宽、0.8 m 长的车体为例，外接圆半径大约是sqrt((0.3)^2 + (0.4)^2) ≈ 0.5 m，栅格 0.05 m 时r = ceil((0.5 + 0.05) / 0.05) = 11。同时检查 A星是否启用了斜向移动，45 度斜穿时外接圆增大，膨胀半径要额外加 1～2 栅格。

5.2 原地摆头不前进：前视距离与heading权重失衡

现象：机器人到达局部目标点附近后，速度降成 0，左右来回转，就是不往前走。 原因：局部目标点太近，DWA 的朝向项在每个候选速度下都差不多，真正决定分数的是障碍项和速度项的噪声。于是最优速度在微小波动里反复横跳，车头来回摆。 解决：把lookAhead拉长到至少vMax * 1.5 m以上；同时检查alpha权重不要低于beta。我自己的经验是alpha:beta:gamma = 1:1.5:0.8起步，不行再微调alpha加 0.2。另外一个直接有效的办法是判定局部目标点距离小于 0.3 m 且速度小于 0.05 m/s 时，直接把航向对准目标点，做一步硬转向。

5.3 对动态障碍“过分礼貌”：速度下限与障碍代价

现象：行人从 3 米外路过，机器人立刻急刹停在原地，等人走后才重新起步，整个路径耗时翻倍。 原因：beta权重高，再加上vLow下限是 0，DWA 倾向于用停车换取安全分数。这是典型的评价函数被“安全项”绑架。 解决：给速度项加一个下限惩罚，或者把vLow提到 0.05～0.1 m/s。更实用的是把动态障碍的代价从“距离代价”改成“预测碰撞代价”：只有轨迹和障碍物预测轨迹的最近距离小于安全距离时才扣分，而不是离障碍任何一个方向近都扣。实际调参时beta降到 1.5 左右，gamma提到 1.2 左右，避障就会果断很多。

5.4 绕行后回不到参考线：最近点索引回退

现象：机器人成功绕过一个动态障碍，但之后的路径不再是 A星参考线，而是偏离到旁边一条平行的“野路”上，越走越偏。 原因：绕行时离参考线太远，pick_local_goal里的最近点搜索重新搜到了绕行前的索引位置，前视目标点也被拽回去，DWA 在两条方向之间被反复拉扯。 解决：这就是我在 4.1 节写idxPrev限制的原因。每帧搜索最近点只允许在上一帧索引前后 ±20 个点范围内搜。如果动态障碍导致参考线偏离太远，再补一个横向偏差判断：当机器人离参考线距离超过2 * lookAhead时，把最近点索引强制向后推进一段，并重新调用 A星对当前位置到终点的局部重规划。注意重规划不能太频繁，不然又退化回单 A星方案了。

5.5 刹车距离明显不够：动态障碍物位置外推

现象：动态障碍物从侧面切入，DWA 直到障碍物已经进入安全距离才开始急刹，刹车后仍会贴上。 原因：DWA 的距离代价只查了障碍物当前帧的位置，没有做运动预测。预测轨迹是未来 3 秒的，但障碍物还停在当前时刻，隐患很大。 解决：在projectObstacles里把动态障碍物按当前速度外推，每个预测时刻的障碍位置都投影到代价图上。我用一个很轻量的做法：

% 代价图里动态障碍按当前速度外推 for i = 1 : params.horizon t = i * params.dt; obsPosNow = dynamicObstacles(k).pos + dynamicObstacles(k).vel * t; localMap(px, py) = true; % 投影膨胀 end

这个外推只做了一阶运动假设，对匀速直线行走的人够用；如果测试场景里有突然变向的行人，可以在速度估计上加低通滤波，别直接信传感器原始速度。东一秒西一秒的噪声速度会让 DWA 以为障碍在乱跳，反而更不敢起步。

6. 把融合过程“看”出来：一屏三图的调试习惯与权重归一化技巧

说到 DWA 参数调试，最忌讳对着日志文件里的数字瞎猜。我习惯在 Matlab 里开一个“一屏三图”：左上角画 A星全局路径与机器人当前位置，右上角画 DWA 当前帧所有候选轨迹，被选中的那条加粗高亮，左下角画障碍物代价图叠加当前动态障碍位置，右下角实时刷新三项评价分数的柱状图。每一帧图都告诉我，当前这个速度是不是被某个不合理障碍项压下去的。

subplot(2,2,1); imagesc(inflateMap); hold on; plot(globalPath(:,2), globalPath(:,1), 'b'); plot(pose(2), pose(1), 'ro'); title('全局路径'); subplot(2,2,2); hold off; for k = 1 : length(scores) plot(trajSet{k}(:, 2), trajSet{k}(:, 1), 'color', [0.7 0.7 0.7]); hold on; end plot(bestTraj(:, 2), bestTraj(:, 1), 'r', 'LineWidth', 2); title('DWA候选轨迹'); subplot(2,2,3); imagesc(localMap); hold on; plot(pose(2), pose(1), 'ro'); title('障碍代价图'); subplot(2,2,4); bar([alpha, beta, gamma]); title('评价权重');

这个调试习惯帮我省掉了很多无效工作时间。以前有一段时期调一个差速底盘的绕障场景，卡了整整两天都翻车，后来发现是beta的障碍距离没有归一化，绝对值被传感器距离数据放大了一百多倍，评分直接把速度项淹没了。于是我又养成了一个更重要的习惯：先归一化，再调权重。

归一化做法很简单，把三项得分都压到 0~1 区间再乘权重。朝向项本来就是归一化的，障碍项用minDist / safeDist归一化，速度项用v / vMax归一化。这样三个权重才有可比性，初始值全部设成 1，再根据行为微调，而不是从某个“网上找来的参数表”盲抄。权重单位统一之后，换地图、换车速都不会出现量级崩掉的问题。

我把这套流程跑完以后，A星负责静态路线，DWA 处理动态变化，融合算法在模拟仓库环境里已经能稳定区分静态货架和动态行人。说实话，把索引回退和动态障碍外推这两个点补上之后，算法表现才真正可用。如果你也打算做类似方向，建议先复现 2、3 章的独立模块，再用第 4 章的主循环拼起来，最后按第 5 章的坑逐个对表查一遍，能少走很多弯路。希望帮到你。

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