简介本资源是一套面向机器人导航开发初学者与ROS实践者的路径规划算法实战代码包聚焦全局与局部协同规划的核心问题解决真实场景中静态路径生成与动态避障难以兼顾的痛点。压缩包共18个文件14KB含15个YAML配置文件用于costmap、DWA参数、全局规划器等模块调优、1个XMLROS包描述、1个launch启动脚本集成move_base核心节点及1个说明文本结构清晰、开箱即用。已有3604人学习下载表明其在教学演示与快速验证环节具有较高实用价值。读者可直接运行仿真环境完整观察A生成全局路径后由DWA实时修正运动轨迹的全过程深入理解启发式搜索与动态窗口法的耦合逻辑并基于配置文件快速调整机器人运动学约束、障碍物膨胀半径及评分权重等关键参数是掌握ROS导航栈底层原理与工程调参能力的优质实践入口。 我先说个反直觉的结论很多刚开始做机器人导航的同学习惯性地觉得全局路径规划已经用A找到了一条最优路径那我直接照着走不就行了结果一上真机就翻车。原因很简单——你的车不是理想质点你的地图也不是完全静态的A算出来的那条折线既没考虑车辆运动学约束也来不及响应半路闪出来的障碍物。所以成熟导航方案几乎都是全局规划器加局部规划器的组合拳这篇就讲这个组合里最经典的一对全局用A局部用DWA并且给出一份不依赖ROS环境、装好numpy和matplotlib就能直接跑的完整Python代码。GitHub上类似的demo不少但要么只有片段要么封装太深读不懂我这篇是站在工程复现的角度把每一步的原理、代码、参数坑一次说透。1. 为什么路径规划要做成全局局部两层而不是一个算法走到底这个问题我当年也想不通。A*明明能找到一条从起点到终点的最短路径为什么还要再叠一个DWA后来在真机底盘上跑过一次才明白这两层规划器解决的问题根本不是一个维度。1.1 A*负责宏观路线但它活在静态地图里A的本质是在离散栅格上做图搜索它输出的是一串栅格点序列。这串序列对应的世界坐标在工程上通常是相邻点之间有固定间距的折线。但机器人不是棋子不能平移、旋转、瞬移。你让一个差速底盘沿着A的折线走到拐角处必须停下来原地转个90°否则就会因为转弯半径不够直接蹭墙。更麻烦的是A在规划时只认那一张静态地图。地图里没画的障碍物它一概不知道。实际仓库里的叉车、AGV或者实验室里走动的行人这些动态障碍物一出现A这条最优路径立刻就失效了。如果完全依赖全局路径机器人要么撞上去要么紧急停下等人来救。所以全局规划器解决的是从地图层面看宏观上怎么走到终点的问题。它要求的是全局最优、计算稳定但它的输出不能直接喂给电机。1.2 DWA负责微观动作但它看不见远方DWADynamic Window Approach的思路是在当前时刻机器人受限于自身速度、加速度和最大速度边界能达成的速度组合其实是一个有限的动态窗口。我们在这个窗口里采样若干组(v, w)对每组速度用运动学模型预演未来一段时间的轨迹再用评价函数挑一条最好的把对应的v和w发给底盘。这个机制的优点是实时性极强每步都在考虑碰撞、朝向、速度特别适合动态环境。但它的缺点是目光短浅——它只往前预测两三秒根本不知道前方5米处有一堵墙更不知道绕过这堵墙该往哪边走。如果直接拿全局终点喂给DWA机器人在复杂地形里大概率会陷入局部最优或者在墙边上反复横跳出不来。1.3 两级规划的分工与接口局部目标点把A和DWA接起来的就是局部目标点。流程是这样A先把全局路径算出来得到一串点运行时DWA不再直接瞄准全局终点而是从全局路径上取出一个当前机器人附近、又不会太近的点作为局部目标。机器人每走一步都重新取一次这个局部目标点DWA围绕它做实时避障。这个架构在ROS Navigation Stack里就是global_plannerlocal_planner的标准做法。你在仿真里看起来好像只是两个算法叠加实际上它解决了单层规划解决不了的核心矛盾既要看得远又要反应快。2. 环境建模与运动学模型写代码前的两张图纸再急也要先把这两样东西搞明白否则后续的代码你看了只会觉得哦好像是这样但出了问题完全不知道怎么调。2.1 栅格地图与障碍物膨胀A*是离散搜索算法所以第一步要把连续的世界坐标离散化成栅格。我习惯用一个二维布尔数组obstacle_map来表示地图False表示可通行True表示被障碍物占据。这里有一个特别重要、但很多人第一次实现时绝对会漏掉的操作障碍物膨胀。机器人的尺寸不是零它是一个半径为robot_radius的圆。就算A*规划的路径点没有压到障碍物栅格如果路径点离障碍物墙壁太近机器人实际通过时也会蹭到墙。工程上的解法是在构建栅格地图时直接把障碍物周围的栅格全部标记为不可通行膨胀范围等于机器人半径换算成栅格数的距离。def _construct_obstacle_map(self): radius_grid int(round(self.robot_radius / self.resolution)) self.obstacle_map [[False] * self.y_w for _ in range(self.x_w)] for (wx, wy) in zip(self.ox, self.oy): gx self.world2grid_x(wx) gy self.world2grid_y(wy) for ix in range(gx - radius_grid, gx radius_grid 1): for iy in range(gy - radius_grid, gy radius_grid 1): if 0 ix self.x_w and 0 iy self.y_w: self.obstacle_map[ix][iy] True注意膨胀半径不是随便给的它必须大于等于机器人半径。给的太小路径会贴着障碍物走真机过不去给的太大路径会绕远路遇到狭窄通道时甚至直接判死路。我在实际项目中通常取robot_radius / resolution后再向上取整这样能保证栅格覆盖完整。2.2 差速底盘运动学模型A*用不到运动学模型但DWA离不开它。差速机器人在二维平面上的位姿是(x, y, yaw)给一个线速度v和角速度w经过dt时间后的位姿更新是x_new x v * cos(yaw) * dt y_new y v * sin(yaw) * dt yaw_new yaw w * dt这个模型看着简单但它是DWA所有轨迹预测的地基。DWA给每个候选速度画出未来轨迹用的就是这组公式。如果你的底盘是阿克曼转向或者麦克纳姆轮运动学模型会有区别但DWA的整体框架完全不变。这篇文章先用差速底盘因为它最通用、也最容易说清楚。3. A*全局路径规划的完整代码与关键细节A*代码我从零写了一遍没有抄现成库这样每一行都知道在干嘛出问题了也好排查。3.1 Node节点与搜索主循环先定义Node节点存栅格坐标、代价g、启发值f和父节点指针。注意这里我没有单独存h因为fghh在每次计算时直接算就行。class AStarNode: def __init__(self, x, y): self.x x self.y y self.g 0.0 self.f 0.0 self.parent None def __lt__(self, other): return self.f other.f搜索主循环用Python的heapq实现优先队列。每次从堆里弹出f最小的节点如果已经在closed列表里就跳过否则扩展八个邻域。八个方向包括对角线所以移动代价要用math.hypot(dx, dy)计算斜着走是根号2。def plan(self, sx, sy, gx, gy): s AStarNode(self.world2grid_x(sx), self.world2grid_y(sy)) e AStarNode(self.world2grid_x(gx), self.world2grid_y(gy)) if not self.is_free(s.x, s.y) or not self.is_free(e.x, e.y): print(起点或终点在障碍物内) return [] open_heap [] closed {} heapq.heappush(open_heap, s) while open_heap: cur heapq.heappop(open_heap) key (cur.x, cur.y) if key in closed: continue closed[key] cur if cur.x e.x and cur.y e.y: path [] while cur: path.append((self.grid2world_x(cur.x), self.grid2world_y(cur.y))) cur cur.parent return path[::-1] for dx, dy in ((-1, 0), (1, 0), (0, -1), (0, 1), (-1, -1), (-1, 1), (1, -1), (1, 1)): nx, ny cur.x dx, cur.y dy if not self.is_free(nx, ny): continue if (nx, ny) in closed: continue nb AStarNode(nx, ny) nb.g cur.g math.hypot(dx, dy) nb.f nb.g math.hypot(nx - e.x, ny - e.y) nb.parent cur heapq.heappush(open_heap, nb) print(A* 未找到可行路径) return []代码里有个工程化简值得说明我没有维护某个节点是否已经在open堆里以及它当前的g值而是直接允许重复push。这样可能出现同一个节点在堆里有多个条目但因为heapq每次弹出的是f最小的所以第一次被弹出并加入closed的节点一定是该节点最优的g值后面再弹出相同节点时直接跳过即可。这在A*里叫lazy deletion代码短、逻辑简单在中小尺寸地图上性能完全够用。3.2 启发函数为什么选欧几里得距离搜索方向选8邻域时启发函数必须选欧几里得距离math.hypot(nx - e.x, ny - e.y)。原因很简单8邻域允许斜着走如果还用曼哈顿距离h会严重低估实际代价导致搜索时优先往错误方向扩张浪费大量计算。从我实测看在100x100的栅格地图上用欧几里得距离比曼哈顿距离的搜索节点数能少30%以上路径也更直。但如果你是4邻域搜索只能上下左右走曼哈顿距离才是更合适的启发函数因为4邻域的实际移动代价本身就是曼哈顿距离。启发函数必须紧跟移动代价的定义这是很多人写A*调不通的根源。3.3 路径回溯与坐标还原找到终点后从终点的父节点一路回溯到起点再把每个栅格坐标还原成世界坐标。这一步要特别注意grid2world_x和grid2world_y的公式gx * resolution min_x不是(gx 0.5) * resolution min_x。用栅格中心点还是左上角取决于你生成地图时的约定保持一致就行。我习惯用栅格中心这样A*输出的路径点在可视化里看起来更自然。4. DWA局部路径规划的完整代码采样、预测、评价三件套DWA的核心逻辑可以拆成三步生成动态窗口、轨迹预测、评价函数选优。下面这段代码我也单独实现了。4.1 动态窗口速度空间和加速度约束决定能去哪当前机器人的速度是(v, w)但由于加速度限制下一个控制周期内它的速度只能在[v - max_accel * dt, v max_accel * dt]范围内变化本文还有配套的精品资源点击获取