做移动机器人自主导航的同学肯定纠结过同一个问题地图上那些墙、柱子、箱子是静态的全局规划一次就能给出满意的路径但走廊里突然钻出一个人或者AGV旁边飘过来一个托盘如果核心算法还是只知道“从起点找一条到终点的最短路线”那就很容易撞上去。这个标题里的方案恰好是工程上最实用的组合之一用JPS算法A*的改进版本做全局路径规划负责在静态栅格地图上给出最优路线再用DWA动态窗口法做局部避障负责对动态障碍物实时反应。两者叠加就形成一套“先看全局再盯脚下”的混合控制算法。后面还有Matlab代码配合很适合竞赛练手和方案验证。1. 动静障碍物场景下的导航问题拆解1.1 动静障碍物并存为什么单独用A*和单独用DWA都不行先想一个典型的室内场景一个轮式机器人的任务是从房间A走到房间B地图里有固定的桌子和墙也有正在移动的人、门帘、临时摆放的箱子。如果只用A*这种全局规划器它拿到的是“某一时刻的静态地图”快照规划路径时完全没有动态障碍物的概念等机器人走到某个节点障碍物可能已经换了位置路径就失效了。反过来如果只用DWA这种局部规划器它只看局部窗口内的障碍分布机器人眼前有路就能走但整体方向没有全局约束很容易在类似迷宫的地形里绕远路甚至陷入某个凹形障碍区出不来。所以纯A负责“远方”DWA负责“脚下”两者取长补短才是一个完整的自主导航闭环。更准确地说在移动机器人导航里“静态障碍物”归全局规划处理“动态障碍物”归局部避障处理这个分工本身就是混合控制算法设计的前提。而我之所以在这个项目里选JPS而不是传统A是因为JPS保持了A*的最优性搜索节点数量却少得多在桌面级算力上跑一千像素的地图也毫无压力。这个选择在后面会详细展开。1.2 标题里的“改进A”到底指什么很多刚开始接触这个题目的人会问JPS是改进A*其实JPSJump Point Search跳点搜索严格来说是A算法在栅格地图上的一种加速策略。A每次从open list里取出节点时会上上下下、左左右右扩展所有相邻栅格JPS则先在地图里识别出一些“跳点”只在跳点之间连线扩展中间大量可以直线通过的栅格直接被跳过。路径结果和A*基本一致但扩展节点少一个数量级搜索速度自然快了。“全局路径规划”和“局部避障”还有一层含义上的区别。全局路径规划是离线或低频在线运行的可以花几百毫秒去搜索整张地图局部避障必须高频运行比如每100毫秒要重新采样一次速度所以算法必须轻、快、有实时性。这个项目里的混合控制本质上就是把这两种不同时间尺度的决策放到一个主循环里由DWA去跟踪JPS给出的参考路径同时避开动态障碍物。理解了这一点整段代码的结构也就清楚了先构建地图调用JPS生成一条全局路径然后把路径点切给DWA做局部目标跟踪DWA一边生成速度指令一边刷新动态障碍信息。2. 全局规划核心JPS为什么值得替换A*2.1 A*到JPS的演进逻辑从“逐个扩展”到“跳跃扩展”传统A在栅格地图上的做法是从起点开始把周围8个邻居都塞进open list计算f g h然后选出f最小的点继续扩展。这个逻辑没有错但代价是毫无差别地扩展了大量节点。一张50×50的地图还好什么地图只有几百个格子一旦分辨率提高到0.05米一个20米×20米的场地就是400×400 16万个格子如果每个节点都扩展8个邻居A的open list会被塞爆在Matlab这种以易用性见长但算力相对“娇气”的环境里跑一次可能要几秒钟。JPS的思路很直接因为栅格地图里大量空间是“完全空旷”的从点A到点B直走过去沿途根本不需要拐弯那为什么还要把这一整条直线上的每个格子都当成候选节点呢于是它引入“跳点”的概念——只在路径方向会发生改变、必须拐弯的格子停下来扩展。判断一个格子是不是跳点核心看两点一是它是否有“强制邻居”forced neighbor也就是旁边存在障碍物迫使路径必须回到这个方向上来二是它是否到达了障碍物边界或者路径的尽头。满足任一条件就把这个格子选为跳点然后水平方向找跳点竖直方向找跳点斜线方向继续跳直到碰墙或找到跳点。这样扩展的节点数量从“矩形区域内的所有格子”降到“矩形边框上的关键转折点”在空旷场景下差距非常明显。我实测过一张60×80、障碍物覆盖率大概20%的栅格地图传统A*扩展了3000多个节点JPS不到200个搜索耗时从接近900毫秒降到60毫秒左右。做一个对比表格更直观。指标传统A*JPS跳点搜索节点扩展数量多逐格扩展少仅跳点扩展最优性保持最优保持与A*等价的最优解适用地图任意图结构栅格地图无障碍区域越大越有利内存占用较高open list大明显降低地图动态变化需要重规划同样需要重规划但很快2.2 JPS跳点判定的细节强制邻居与主方向真正写代码时跳点判定不能想当然。假定机器人允许8方向运动主方向是水平或竖直时如果它沿某个方向前进旁边有一个障碍物堵住了一个分支方向并且那个分支方向可以通过绕行越过障碍物那么这个格子就会成为强制邻居。更准确地说从当前节点n朝着方向d扩展时如果存在某个邻居m不沿d轴方向但m到n的距离比从n通过其他邻居到达m更近且m没有被障碍物阻挡那么m就是一个强制邻居。这个规则初看绕实际上翻译成栅格坐标就是往前走的时候注意左右两侧是不是刚好出现障碍物挡在斜向邻居位置如果挡了就必须在当前格停下并考虑转向。斜线方向的跳点判定更复杂一点因为斜着走会同时影响x和y两个轴。在8方向栅格地图里标准的JPS递归搜索是main方向为斜向时先沿斜向继续跳同时递归检查水平分量和垂直分量有没有跳点任一方向找到跳点当前斜向点就是跳点。这个概念实现起来不难但容易出bug的地方在于“越界检查”和“回头检查”。我曾经在Matlab里写过一版JPS搜索出来后路径明显绕路调试了一上午才发现是递归时没有禁止从跳点再次反方向回跳导致路径在原地图上出现了一圈多余回路。解决方法是给每个扩展方向加一个“上一跳方向”的判断反向的扩展直接剪掉。2.3 Matlab实现JPS的代码骨架给出一段可以直接跑通主流程的Matlab代码骨架。这里只列出核心部分地图用二维矩阵表示0代表可通行1代表障碍物。跳点递归函数思路可以参考上面的规则为了篇幅主函数里我把JPS的核心搜索部分压缩成了函数调用。% jps_demo.m % 全局路径规划JPS改进A*输出path function path jps_demo(map, start, goal) % map: 0可通行1障碍物 % start/goal: [row, col] if map(start(1), start(2)) 1 || map(goal(1), goal(2)) 1 error(起点或终点在障碍物上); end rows size(map, 1); cols size(map, 2); % 初始化 openList struct(pos, start, g, 0, h, heuristic(start, goal), parent, []); closedMap zeros(rows, cols); parentMap zeros(rows, cols, 2); while ~isempty(openList) % 选f值最小节点 idx pickMin(openList); cur openList(idx); openList(idx) []; if isequal(cur.pos, goal) path backtrace(parentMap, start, goal); return; end closedMap(cur.pos(1), cur.pos(2)) 1; % 关键找出所有跳点及其代价 jumpPoints findJumpPoints(map, cur.pos, goal); for i 1 : size(jumpPoints, 1) jp jumpPoints(i, 1:2); jpCost jumpPoints(i, 3); if closedMap(jp(1), jp(2)) continue; end gNew cur.g jpCost; idxInOpen findOpenIndex(openList, jp); if idxInOpen 0 openList(end1) struct(pos, jp, g, gNew, ... h, heuristic(jp, goal), parent, cur.pos); parentMap(jp(1), jp(2), :) cur.pos; elseif gNew openList(idxInOpen).g openList(idxInOpen).g gNew; parentMap(jp(1), jp(2), :) cur.pos; end end end path []; warning(JPS未找到可行路径); end function h heuristic(pos, goal) % 八方向地图欧氏距离距离估计即可 h sqrt((pos(1)-goal(1))^2 (pos(2)-goal(2))^2); end真正完整版本还需要补充findJumpPoints、pickMin和backtrace三个子函数。这里我特别说明一下findJumpPoints是最容易出错的地方它要从当前节点出发朝8个方向做递归跳跃搜索把遇到的第一批跳点和对应代价跳跃距离返回。代价计算建议用欧氏距离因为栅格对角线长度是1.414水平和垂直方向是1直接数格子数会低估斜线路径代价。3. 局部避障核心DWA动态窗口法到底在优化什么3.1 速度采样窗口与轨迹模拟DWA的思想非常“工程化”与其在障碍物空间里做复杂规划不如直接考虑机器人下一瞬间能踩出哪些速度组合然后把每条速度对应的未来轨迹都预演一遍选一条不会撞、又能靠近目标的速度发出去。这里的“动态窗口”指的是当前速度约束下机器人真正能达到的速度范围线速度不能超过最大加速度角速度也不能超过最大角加速度。把这些速度对(v, ω)遍历采样再用运动学模型向外推一段时间就能得到一组候选轨迹。运动学模型通常是差速模型轨迹是一段圆弧。给定初始位置(x, y, θ)、线速度v、角速度ω和预测时间T可以递推x_t x v/ω * (sin(θ ω*T) - sin(θ))前提是ω不为0如果ω接近0轨迹就是直线。用这段预测轨迹去查地图里的障碍物分布轨迹经过的栅格如果有障碍物则这条速度组合不可行。选速度时还要注意采样粒度越细轨迹越多越有可能找到更平滑的避障路线但Matlab里每多一组速度就要多算一组圆弧点计算量成倍增长。所以我通常先根据当前速度和加速度约束缩小窗口再按一个较粗的分辨率采样评估后如果所有轨迹都不可行再把窗口分辨率提高一倍而不是一上来就暴力遍历几千组速度。3.2 评价函数目标方向、障碍距离、速度三个维度怎么平衡DWA的经典评价函数是三部分加权求和一是目标方向项轨迹终点朝向和目标方向的夹角余弦夹角越小得分越高二是障碍距离项预测轨迹到最近障碍物的距离太近的直接淘汰三是速度项鼓励机器人跑得更快避免磨磨蹭蹭。总得分就是把这三项乘以各自的权重再相加权重越大代表越看重这个目标。在实际项目中权重设计比想象中更影响行为。我常用的一组初值是目标方向权重0.6障碍距离权重0.4速度权重0.2但要注意当走廊特别窄时障碍距离权重必须调高否则机器人在离障碍不到20厘米的地方还会试图加速冲刺非常吓人。反过来如果全局路径很长需要在空旷区域跑得快速度项的权重可以适当提到0.3。还有一个容易被忽略的细节目标方向项应该参考“局部目标点”而不是终点。局部目标点是全局路径上离机器人最近点往前走一点的那个点比如往前取第3个路径点。这样DWA不会因为终点太远而一路指向终点导致遇到障碍物时转向迟缓和路径贴着障碍物走。这个“参考路径点”的取法是全局规划与局部避障能够真正协同工作的关键。3.3 动态障碍物在DWA里是怎么被处理的DWA本身并不区分静态和动态障碍物它的输入是当前时刻的环境costmap。只要传感器能看到障碍物并且障碍物在预测轨迹的时间范围内可能挡住路径评价函数就会把这个速度惩罚掉。所以更准确地说DWA是“反应式避障”动态障碍物每帧刷新位置DWA每帧重新采样只要刷新频率够快机器人就能绕开突然出现的人或手推车。但反应式避障有一个盲区它只能看到当前检测范围内的障碍如果一个人高速冲向机器人侧面DWA预测轨迹时如果只按当前障碍位置推断等障碍进入前方区域的时候可能已经来不及减速。这就是为什么动态障碍物场景下最好在DWA外面再加一层障碍物速度预测用连续几帧的障碍物位置估计速度和方向把障碍物未来位置按时间展开到预测轨迹的时间轴上机器人计算碰撞时不是只看当前时刻障碍在不在轨迹上而是看未来每个时间点上机器人位置和障碍物预测位置是否重合。Matlab里实现起来也很简单维护一个障碍物列表每帧更新位置预测时做线性外推即可。4. 混合控制算法的整体调度与Matlab代码框架4.1 全局与局部规划的对接方式整套系统的主循环是固定周期的例如0.1秒执行一次。循环开始时先读取机器人的真实或仿真位姿再读取当前传感器感知到的动态障碍物列表然后从JPS在启动时生成的全局路径里找到机器人当前位置对应的目标点把目标点、当前位姿、障碍物列表一起交给DWADWA输出(v, ω)速度指令机器人按这个速度往前移动0.1秒刷新位姿进入下一轮。全局路径不会每帧重新规划因为它依赖的静态地图没有变化重规划反而浪费算力只有当目标点被动态障碍物挡住、DWA连续若干次无法输出可行速度或者机器人偏离全局路径太远时才需要触发一次全局重规划。这个调度逻辑看起来简单真正实现时却有顺序上的讲究。我踩过的一个坑是一开始我把全局路径更新放在DWA输出之后导致DWA拿到的是上一帧全局路径上的旧目标点当机器人高速绕过障碍物时目标点总是滞后于是出现明显的S形抖动。把顺序调整为“先更新目标点再跑DWA”之后抖动立刻缓解。这个顺序问题在代码里就是几行的事但实测效果判若云泥。4.2 数据流、场景与模块划分用表格把各模块的输入输出整理出来后续写代码时可以照抄这个接口模块输入输出运行频率JPS全局规划静态栅格地图、起点、终点全局路径点序列低频启动或重规划目标点选择器全局路径、当前位姿局部目标点高频DWA局部规划当前位姿、局部目标点、障碍物列表、运动学参数线速度、角速度高频运动学模型速度指令、位姿更新后的位姿高频可视化地图、路径、轨迹、障碍物仿真画面与主循环同步如果要严格对照“动静障碍物混合场景”建议给障碍物分成两类静态障碍物写进map矩阵JPS和DWA都能看到动态障碍物单独用数组动态更新只传给DWA。这样比较符合真实系统中的costmap分层也方便调试把动态障碍物数组清空系统就退化成纯全局规划把动态障碍物开成匀速直线运动就能测试局部避障的灵敏度。4.3 可以直接落地的Matlab主循环骨架下面这段代码是我在实际项目基础上去掉业务模块后的通用骨架你可以直接套到自己的地图和运动学模型上。% main_hybrid_navigation.m clear; clc; close all; % 1. 构建地图0可通行1静态障碍物 map zeros(60, 80); map(30:35, 10:70) 1; % 走廊中间放一堵墙 map(20:22, 40:42) 1; % 小柱子 start [10, 5]; goal [55, 70]; % 2. JPS全局路径规划 globalPath jps_demo(map, start, goal); % 对路径做一次均匀重采样方便DWA跟踪 if isempty(globalPath) error(全局路径规划失败请检查地图和起点终点); end % 3. DWA参数 dwaParams struct(... dt, 0.1, ... % 控制周期 predictTime, 1.5, ... % 轨迹预测时间 maxVel, 0.5, ... % 最大线速度 m/s maxYawRate, 1.2, ... % 最大角速度 rad/s maxAcc, 0.2, ... % 最大线加速度 maxYawAcc, 0.4, ... % 最大角加速度 wGoal, 0.6, ... % 目标方向权重 wObstacle, 0.4, ... % 障碍距离权重 wVel, 0.2, ... % 速度权重 robotRadius, 0.25); % 机器人半径 % 4. 初始化动态障碍物列表每帧刷新 dynamicObstacles []; % 5. 开始导航 pose [start(1), start(2), 0]; % [x, y, theta] figure(1); for t 0 : 0.1 : 100 % 模拟动态障碍物这里演示一个围绕地图移动的障碍物 dynamicObstacles updateDynamicObstacles(t); % 从全局路径中取局部目标点 localGoal selectLocalGoal(globalPath, pose); % DWA计算速度指令 [v_cmd, w_cmd] dwa_control(pose, localGoal, map, ... dynamicObstacles, dwaParams); % 用运动学模型更新位姿 pose kinematics(pose, v_cmd, w_cmd, dwaParams.dt); % 可视化 visualizeNavigation(map, globalPath, pose, dynamicObstacles, localGoal); drawnow; enddwa_control和selectLocalGoal是两个需要自己补全的函数。selectLocalGoal里我建议先找全局路径上与当前位姿最近的路径点下标index再取index3所在点作为局部目标如果index3已经超出路径长度就直接取终点。这样的好处是目标点始终在机器人前方不会因为DWA正常绕行而出现目标点跑到身后的情况。5. 参数调参与避坑实录5.1 一套能用的默认参数与调整方向给出参数表不是让你原样照抄而是给你一个“从这套参数开始调试”的基准。地图分辨率不同、机器人尺寸不同参数差很多参数默认值调整方向说明地图分辨率0.05 m/格越小地图越精细但JPS耗时急剧增加机器人半径0.25 m小于实际半径会出现擦墙风险大于会浪费空间预测时间1.5 s调大能提前绕障但会过多惩罚远端障碍容易绕远路目标权重0.6调大更贴路径调小更灵活障碍权重0.4调大更保守更适合窄通道速度权重0.2调大更快但急停和抖动变多最大加速度0.2 m/s²调大更灵敏但速度突变容易引起轨迹震荡动态障碍预测开启缺少时快速横穿障碍物极易来不及避让我个人的习惯是先调预测时间和障碍权重把“撞障碍”问题解决再调目标权重和速度权重把“绕远路、磨蹭不前”问题解决。不要上来就同时动五个参数那样出了问题根本不知道是哪个参数引起的。5.2 典型问题与排查方法我把仿真中高频出现的问题整理成了表基本覆盖了90%的翻车场景现象可能原因排查思路JPS规划返回空路径起点或终点被静态障碍物占据或地图边界未作处理先检查start和goal处的map值再打印findJumpPoints返回的跳点数量路径穿墙跳点搜索时忽略了对角线穿墙情况检查斜向跳跃时水平分量和垂直分量所在格是否都可通行有一个是障碍就不能斜穿机器人走S形抖动局部目标点滞后或目标权重过高在selectLocalGoal里取更靠前的路径点或适当降低wGoal静止在障碍物前不动所有速度候选都被障碍惩罚没有可行轨迹调小障碍权重或增大最大加速度开启“动态窗口扩大”机制当无解时允许急停/倒退避障时绕远路预测时间过长远处障碍也被提前惩罚缩短predictTime或给障碍距离项加一个最大截断距离比如超过1米都算1分机器人在走廊里来回摆动角速度采样粒度太粗或权重不合适提高角速度采样分辨率降低速度权重减少频繁加减速这里有个小技巧值得单独说当DWA输出“无可行速度”时不要简单报错。可以让机器人先原地旋转同时把动态障碍物列表做一个短时间预测如果障碍物只是路过它就走了如果障碍物一直堵着再触发全局重规划。这样处理下来机器人在有人迎面走来这类场景里会表现出明显的“让行”能力而不是呆在原地傻等或野蛮撞人。5.3 从“能跑”到“跑得稳”的几条实操建议第一路径平滑很重要。JPS输出的路径是一串折线直接交给DWA跟踪机器人每个转折点都会减速显得很笨拙。我通常在JPS之后加一次三次样条或贝塞尔平滑把折线变成可控的曲线。平滑时注意不要偏移到障碍物里所以平滑后的路径必须再做一次碰撞检测碰撞点多的局部区域可以保持原始折线。第二仿真中不要忽略坐标单位的换算。Matlab里很多同学直接按“栅格索引”规划又把速度单位按米每秒设置结果机器人前进0.5米实际在图上只走了1个栅格速度指令和路径完全对不上。我的做法是JPS在栅格坐标系里跑DWA在米制坐标系里跑两者之间统一乘一个分辨率系数res。地图上一切换这个系数就是最容易出bug的地方我特意在代码里写成了一个全局常量res 0.05。第三动态障碍物测试场景要覆盖“低速横向穿越”“高速迎面而来”“在目标点附近停留”三种情况。很多方案在低速横穿场景下表现很好一遇到高速迎面而来就暴露问题原因是预测时间太短或权重不协调。调试时我习惯把动态障碍物速度从0.2米/秒逐步加大到1.0米/秒每加一档都观察轨迹偏移量这样能快速定位DWA的最大可避让速度边界。6. 这套方案还能往哪个方向扩展6.1 与状态估计、传感器融合结合的现实意义到目前为止DWA的输入是理想位姿动态障碍物也是仿真位置直接给定的真实机器人不可能直接拿到这么完美的输入。在实际部署时位姿一般来自里程计和激光雷达或视觉SLAM融合障碍物检测来自2D激光点云聚类或相机目标检测。混合控制算法本身不用改但DWA的障碍物列表要换成“传感器实时发布的可通行空间”全局地图要换成“占据栅格地图”。这个改造并不难难的是在Matlab之外找一个能对接传感器数据的框架很多朋友会把这套算法先用Matlab验证逻辑再移植到ROS里做真机测试这也是我推荐的路线。6.2 局部规划器之间的横向对比如果只谈局部避障DWA并不是唯一选择。TEBTimed Elastic Band通过优化轨迹上的时间戳和位姿序列来规划出带速度信息的局部轨迹在动态场景中更平滑MPC模型预测控制则能显式处理控制量约束和控制周期鲁棒性更强。但DWA的优势是简单、直观、在普通PC上也能跑到几十赫兹几乎没有调模型的门槛。如果你手里的算力比较紧张或者不想折腾一堆库的版本依赖DWA是最容易先跑通的选择。6.3 我的个人体会先跑通再谈优化组合拳比单点炫技更重要我在用Matlab做这套仿真时最大的体会是一个导航系统好不好用往往不取决于某个算法单独有多强而取决于全局规划器和局部规划器之间的细节配合。很多同学花大量时间调JPS的高阶优化最后发现机器人还是撞障碍物因为问题根本不在JPS而在DWA里目标点选择滞后、障碍权重太低、预测时间太长。反过来DWA调得再细腻JPS给出的路径穿墙或者绕远整个系统也白搭。所以调试时我会先画一张“路径轨迹障碍物”组合图每一帧都看再定位是哪个模块出了问题。这个习惯帮我省了非常多的时间也给后面扩展留了余地。如果下一步要跑真机我会继续沿用这套“JPSDWA”的混合控制框架只是把仿真里的位姿和障碍物输入换成传感器实读数据再加一层速度平滑过滤就能走得很稳。
把清单整理给你)
