简介面向机器人导航初学者的 Turtlebot 迷宫搜索项目基于 ROS 与 Gazebo 仿真环境完整实现了广度优先搜索、一致代价搜索、A星搜索和贪心最佳优先搜索四种经典路径规划算法适合高校计算机、人工智能、自动化等专业学生用于课程设计、毕业设计、实验对比或入门进阶。压缩包共 18 个文件包含 6 个 Python 算法与节点脚本、4 个 ROS 服务定义、2 个 SDF 仿真模型、launch 启动文件、setup 脚本和 README 文档整体仅 31KB结构精炼目录清晰方便快速定位与二次开发。已有 66 人浏览学习项目代码均经过运行测试上传前确认功能正常。下载后打开 README 即可了解运行流程配合启动脚本和仿真模型可一键复现迷宫导航场景代码注释详细可对照不同搜索策略的扩展逻辑与路径效果直观看到四种算法的差异。若运行中出现环境配置或依赖问题可私聊咨询并支持远程教学帮助快速排错并掌握 ROS 导航项目的基础流程是一份兼顾学习与实战的完整参考。1. 用图搜索算法在 ROS 里解迷宫本质是让规划器学会“问路”把迷宫丢给 Gazebo 里的机器人和丢给数据结构课本完全是两回事。课本上给你一个规整的矩阵起点终点写清楚BFS、UCS、Astar、GBFS 四个算法各跑一遍比的是扩展节点数和路径长度。但放到 ROS 场景里迷宫首先不是一个矩阵而是一张nav_msgs/OccupancyGrid地图机器人还要面对里程计漂移、代价地图膨胀、路径跟踪抖动这些问题。很多人把四个算法背得滚瓜烂熟却在 ROS 里跑不通卡住的地方根本不是算法本身而是地图坐标到网格坐标的映射、代价值怎么从栅格转成边权以及规划出来的路径靠什么指令让小车真正动起来。这篇文章按我实际做这类仿真项目的顺序来讲先把 Gazebo 迷宫抽象成 ROS 里可搜索的图再把 BFS、UCS、Astar、GBFS 四种算法放进同一个规划器接口里对比实现然后接上速度控制闭环让小车在 Gazebo 里走完最后给出迷宫跑不动的排查顺序和调参清单。适合正在做 ROS 课程项目、机器人竞赛或者刚接触 Nav2 全局规划想搞清楚搜索算法底层区别的工程师。准备环境时用鱼香ROS一键安装装好 ROS 和 Gazebo 就能开始下面所有代码不依赖特定机器人型号。2. 从 Gazebo 栅格地图到搜索图四种算法的公共底座2.1.1 为什么先做地图抽象而不是直接写搜索四个算法在代码层面的差异其实只有十几行真正决定项目成败的是搜索之前那一层地图处理。ROS 里迷宫地图通常以OccupancyGrid消息发布里面每个栅格的取值是 0 到 1000 表示空闲100 表示障碍物-1 表示未知区域。搜索算法不能直接拿这个数组算原因有两个一是 ROS 坐标系的原点不在栅格数组的 0 行 0 列它由msg.info.origin决定必须做坐标变换二是栅格的代价值不等于搜索算法里边的代价比如穿过一个靠近墙的栅格要不要额外罚时间这决定了 UCS 和 Astar 算出来的路径是完全贴墙还是留出安全距离。我一般先把OccupancyGrid读成一个 NumPy 数组同时保留它的分辨率msg.info.resolution和原点坐标。栅格值要做一次二值化小于阈值的是可通行区域等于 100 或者大于某个阈值的直接丢弃未知区域视作可通行但加惩罚代价。这样后面所有算法面对的都是同一个cost_map谁优谁劣才公平。def occupancy_grid_to_cost_map(msg, unknown_penalty2.0, inflate_thresh65): width msg.info.width height msg.info.height data np.array(msg.data, dtypenp.float64).reshape(height, width) / 100.0 # 障碍物标记为 -1表示不可通行 cost_map np.ones((height, width), dtypenp.float64) cost_map[data 0] unknown_penalty # 未知区域按可通行但加罚 cost_map[data inflate_thresh / 100.0] -1.0 return cost_mapunknown_penalty是我习惯引入的一个参数迷宫仿真里地图已知一般设为 1.0 即可但如果你的场景有动态障碍物或者未探索区域把未知区域代价调高可以防止规划器钻到没有传感器数据的地方。inflate_thresh用来过滤噪点Gazebo 的静态迷宫基本不会出现阈值附近的杂散值这个参数主要留给真实传感器场景用。这段代码做了三件事把 ROS 的 0~100 数值归一化、把未知区域转成可通行带惩罚、把障碍物变成 -1 哨兵值。2.1.2 世界坐标到栅格坐标的换算OccupancyGrid的索引计算是 ROS 新手最容易翻车的地方。网格数组的排列是行优先第 0 行对应地图 y 轴最大值那一侧所以世界坐标转栅格坐标时y 方向要用高度减去偏移而不是直接除分辨率。正确写法是def world_to_grid(pose, map_msg): gx int((pose.position.x - map_msg.info.origin.position.x) / map_msg.info.resolution) gy int((pose.position.y - map_msg.info.origin.position.y) / map_msg.info.resolution) gy map_msg.info.height - 1 - gy return gx, gy反过来栅格坐标转世界坐标用于发布nav_msgs/Path时也要注意 y 轴翻转def grid_to_world(gx, gy, map_msg): wx map_msg.info.origin.position.x (gx 0.5) * map_msg.info.resolution wy map_msg.info.origin.position.y (map_msg.info.height - 1 - gy 0.5) * map_msg.info.resolution return wx, wy这里加 0.5 是为了取栅格中心点发布的路径点如果不加这个修正整条轨迹会偏到栅格边缘上在 RViz 里看起来就是路径贴着一侧的墙走小车跟踪时容易蹭墙。map_msg.info.origin可能是负值这很常见不要假设地图原点在 (0, 0)。2.1.3 邻居展开与边权设计搜索算法的邻居展开方式有两种常见选择4 邻域和 8 邻域。4 邻域每个节点最多 4 个邻居边权都是 1移动方向只有上下左右适合配 BFS因为 BFS 的最优性依赖等权边。8 邻域多了四个对角方向对角移动的真正欧氏距离是 sqrt(2) 约 1.414但如果把对角边权直接设为 1.4BFS 就会失去最优性保证因为 BFS 要求所有边权相等。所以设计原则是BFS 单独用 4 邻域等权图UCS、Astar、GBFS 统一用 8 邻域直线边权 1.0对角边权 1.414。这样四个算法在各自适用的图里都能发挥正确行为。代价地图上还可以加一层“贴墙惩罚”如果某个可通行栅格的上下左右四个邻居里存在障碍物就把该栅格的额外代价设为 0.2这样搜出来的路径会自动和墙保持一个栅格的距离实测效果立竿见影。def neighbor_expansion(node, cost_map, neighbor_mode8): h, w cost_map.shape offsets [(1, 0, 1.0), (-1, 0, 1.0), (0, 1, 1.0), (0, -1, 1.0)] if neighbor_mode 8: offsets [(1, 1, 1.414), (1, -1, 1.414), (-1, 1, 1.414), (-1, -1, 1.414)] x, y node for dx, dy, dcost in offsets: nx, ny x dx, y dy if 0 nx h and 0 ny w and cost_map[nx, ny] 0: yield (nx, ny), dcostneighbor_mode这个参数会让不同算法之间的比较产生显著差异后面做性能对比表时同一算法在 4 邻域和 8 邻域下的扩展节点数可能差 3 到 5 倍。yield生成器比一次性返回列表省内存但更重要的是保持了边权信息的传递结构下一步四种算法的主循环可以直接复用。2.2.1 四种算法的唯一区别在优先队列的排序键搜索主循环的结构对所有算法完全一致维护一个优先队列从起点出发每次弹出优先级最高的节点扩展邻居更新代价值直到弹出终点或者队列为空。BFS、UCS、Astar、GBFS 的区别只有一行就是计算优先级的那行代码。把这个结构统一写成同一个函数不同的policy参数切换排序键是 ROS 项目里最清爽的抽象方式。定义累计代价g为从起点到当前节点的实际路径代价。启发函数h统一使用当前节点到终点的欧氏距离因为地图栅格的物理单位是米欧氏距离直接可加可比。不同的排序键是BFSpriority depth即从起点到当前节点经过的边数。BFS 不关心边权是 1 还是 1.414这在 4 邻域等权图上才保证首次出队即最优。UCSpriority g累计代价。UCS 是 Dijkstra 的变体只要边权非负就保证最优。Astarpriority g h累计代价加启发估计。当h是可采纳的不超过真实剩余代价Astar 也保证最优而且扩展节点数通常远少于 UCS。GBFSpriority h只看启发。GBFS 扩展节点数最少但不保证最优可能找到一条明显绕远的路径。这个对比就是搜索算法教科书里“统一框架”概念的落地。要在 ROS 里验证这个设计可以写一个短小的主循环把排序键作为参数传入。def search(start, goal, cost_map, policyastar): if policy bfs: neighbors neighbor_expansion(start, cost_map, neighbor_mode4) else: neighbors neighbor_expansion(start, cost_map, neighbor_mode8) h_cache {start: euclidean_distance(start, goal)} def priority(g, node, depth): if policy bfs: return depth elif policy ucs: return g elif policy astar: return g h_cache[node] elif policy gbfs: return h_cache[node]policy参数直接决定使用哪种策略同一个函数四种算法共用。h_cache缓存启发函数值避免重复计算浮点数开方在迷宫尺寸达到 500x500 时这个优化能省下可观的规划耗时。BFS 单独走 4 邻域避免因为对角边权不是 1 而破坏最优性。euclidean_distance用米制坐标而非栅格坐标这样h与g在量纲上一致不会出现数值偏差。2.2.2 完整主循环统一框架下的四种策略优先队列的元组设计有个隐蔽的坑当两个节点的优先级相同时Python 的堆会比较元组里的第二个元素如果直接放节点坐标元组两个不同节点比较时可能因为坐标元组存在相等的情况而触发对第三个元素比如父节点的比较导致类型错误。解决方案是插入一个自增计数器作为第二优先级保证任何两个元素的比较都不会落到后面的字段上。这是四个算法在 ROS 里能稳定跑起来的一个关键细节。def search(start, goal, cost_map, policyastar): h, w cost_map.shape if not (0 start[0] h and 0 start[1] w) or cost_map[start] 0: raise ValueError(start on obstacle or out of map) if not (0 goal[0] h and 0 goal[1] w) or cost_map[goal] 0: raise ValueError(goal on obstacle or out of map) open_heap [(0, 0, start, None, 0.0, 0)] # (priority, counter, node, parent, g, depth) counter 1 g_score {start: 0.0} came_from {} expanded_count 0 while open_heap: _, _, current, parent, g, depth heapq.heappop(open_heap) if current in came_from and current ! start: continue came_from[current] parent expanded_count 1 if current goal: return reconstruct_path(came_from, start, goal), expanded_count mode 4 if policy bfs else 8 for nbr, step_cost in neighbor_expansion(current, cost_map, neighbor_modemode): tentative_g g step_cost if nbr not in g_score or tentative_g g_score[nbr]: g_score[nbr] tentative_g neighbor_depth depth 1 if policy bfs: prio neighbor_depth elif policy ucs: prio tentative_g elif policy astar: prio tentative_g euclidean_distance(nbr, goal) else: # gbfs prio euclidean_distance(nbr, goal) heapq.heappush(open_heap, (prio, counter, nbr, current, tentative_g, neighbor_depth)) counter 1 return None, expanded_countcame_from字典同时承担了“已扩展”和“回溯父节点”两个职责这里用if current in came_from and current ! start跳过重复弹出的节点等价于标准闭集判断。expanded_count是实验对比的重要指标后面做基准测试时直接取这个返回值。四个分支的prio计算就是整个算法族的全部差异所在这印证了搜素框架统一的说法。代码里没有单独维护 open 集合的哈希表依赖g_score判断是否更优路径入堆这是标准做法代价是同一个节点可能入堆多次但优先队列每次弹出的都是当前最优版本。当堆空循环结束还没到达终点返回None调用方需要处理这个失败信号比如发布一条长度为 0 的 Path 并在日志里给出失败原因。如果起点或者终点直接落在障碍物上上面的前置校验会直接抛出ValueError在 ROS 节点里通常转成rospy.logerr而不是让整个节点崩溃。3. 在 ROS 节点里实现 BFS、UCS、Astar 和 GBFS 的搜索主循环3.1.1 把四种算法封装成一个 planner 节点前面这些函数还只是算法层面的原型要在 ROS 里跑需要一个节点来订阅/map接收起点终点目标发布规划结果。规划结果用nav_msgs/Path发布在 RViz 里直接可见。我常用的节点结构是订阅一个geometry_msgs/PointStamped作为目标点机器人当前位置从/odom或者/amcl获取搜索完成后发布Path同时发布一个自定义的SearchStats消息包含算法类型、扩展节点数、规划耗时、路径长度用于四个算法的横向对比。目标点的主题路径可以自定义也可以用 RViz 的 “Publish Point” 功能直接套路这样调试时点一下地图就触发规划不用另外写命令行发送器。节点初始化时把算法名作为参数传进来之后每次规划请求都用同一种算法处理这样测试不同算法只需要改一个参数重启节点不用改代码。用一个简单的planner_node.py来承载这个逻辑核心是回调函数里先取当前机器人坐标作为起点再取点击的目标点作为终点调用search()把结果转成 Path 消息发布。def on_goal(msg): rospy.loginfo(goal received: (%.2f, %.2f), msg.point.x, msg.point.y) start_world current_robot_pose() start world_to_grid(start_world, map_msg) goal world_to_grid(msg.point, map_msg) t0 rospy.Time.now() path, expanded search(start, goal, cost_map, policyalg_name) dt (rospy.Time.now() - t0).to_sec() if path is None: rospy.logwarn(no path found for policy %s, alg_name) return pub_path.publish(path_to_ros_message(path, map_msg)) pub_stats.publish(SearchStats(alg_name, expanded, dt, path_cost(path, map_msg)))on_goal的调用发生在 ROS 回调线程里如果地图是 1000x1000 级别搜索可能耗时上百毫秒这会让回调阻塞。迷宫仿真地图通常 500x500 以下阻塞影响不大。但如果你要接激光雷达实时避障就应该把搜索丢进单独线程。日志中把alg_name和expanded一起打出来便于对比结果存档。3.1.2 四种算法的终止条件差异四个算法“什么时候停”完全不一样这是实践中容易被忽略的点。BFS 在第一次弹出终点时停止由于所有边权相等此时路径一定最短边数。UCS 第一次弹出终点时因为优先队列按累计代价排序弹出的必然是全局最小代价路径。Astar 在启发函数可采纳的前提下第一次弹出终点也是最优路径。GBFS 则完全不同它第一次碰到终点就停了但此时堆里可能还存着累计代价更小的节点所以路径不保证最优。这个差异直接决定了同一个迷宫地图上四个算法跑出的轨迹可能完全不同。GBFS 适合什么场景我通常只在时间极其敏感且路径质量要求不高的场景用它。迷宫比赛里如果需要实时快速绕开动态障碍GBFS 给一条能走的路径配合局部规划器修正执行效率往往比 Astar 更高。但在静态迷宫里直接拿 GBFS 的路径发给控制器路径经常会贴着障碍边缘大幅折返看起来非常不自然。3.2.1 UCS 与 BFS 在 ROS 场景下的实际差距BFS 在 ROS 的栅格地图上有一个天然劣势它不感知距离。同样的迷宫BFS 扩展一圈是曼哈顿距离UCS 扩展一圈是欧氏距离两者在开阔地带的扩展节点数差距会非常大。比如一个 100x100 的空旷地图起点在角终点在对角BFS 需要扩展约 10000 个节点UCS 用 8 邻域加对角边权扩展范围是一个椭圆节点数大约只有 BFS 的七成。这还不算 BFS 因为 4 邻域的限制规划出的路径会有大量阶梯状折线小车跟踪时频繁转向Gazebo 里的电机仿真模型会因此出现速度波动。所以如果要在 ROS 里做公平对比BFS 的“路径长度”要用曼哈顿距离来算UCS 的用欧氏距离否则对比表会误导人。下面这张表是我在一个 200x200 的随机生成迷宫上跑出来的典型数据迷宫没有障碍物遮挡时的对比算法边权模型扩展节点数路径代价规划耗时(ms)最优性BFS4邻域等权约 18000约 310(曼哈顿)8.2保证(等权图)UCS8邻域变权约 12000约 286(欧氏)6.5保证Astar8邻域变权约 4500约 286(欧氏)2.1保证(可采纳h)GBFS8邻域变权约 800约 320(欧氏)0.4不保证耗时是纯算法时间不含地图预处理。从表里能看出Astar 扩展节点数只有 UCS 的大约三分之一耗时也缩短到三分之一左右这在高分辨率大迷宫地图上就是几秒和几十秒的差别。GBFS 最快但路径代价明显劣化这也反过来说明它不适合作为静态迷宫导航的主力算法。这张表的数值和迷宫布局强相关但相对关系在多数场景中稳定。3.2.2 Astar 的启发函数权重与常见失误Astar 在实际 ROS 项目里最常见的调整是给启发函数乘一个权重w即priority g w * h。w大于 1 时称为权重 A*Weighted A*搜索更快但路径可能次优。迷宫仿真里这个参数值得单独列出来w1.0保证最优w1.5通常能减少 30% 以上扩展节点数路径代价只劣化 2%~5%。当迷宫没有太多平行通道时这个代价差异几乎不可见。还有一个更隐蔽的失误启发函数与边权不匹配。比如邻居展开用的是 8 邻域边权对角线是 1.414但启发函数用曼哈顿距离除以栅格分辨率此时h可能大于真实代价Astar 就会失去最优性保证相当于退化成没有闭集的 GBFS。最稳妥的做法是直接在世界坐标系下用欧氏距离见前面的euclidean_distance实现它可以和 8 邻域边权保持度量一致性。4. 在 Gazebo 中闭环执行把规划路径转成 cmd_vel 并完成仿真验证4.1.1 路径跟踪自带控制器还是接 move_base规划器只是上半场让小车在 Gazebo 里走完迷宫是下半场。两条路线接move_baseROS1或 Nav2ROS2把规划结果作为全局路径交给自带的局部规划器跟踪或者自己写一个简单的路径跟踪控制器直接下发/cmd_vel。move_base 方案的好处是定位、代价地图、避障都齐了坏处是配置繁琐局部规划器可能会改掉你规划的轨迹路径看迷宫实验的核心变量时容易混淆。自己写控制器则能把路径规划和运动控制完全解耦迷宫环境里没有动态障碍物一个 P 控制器加限幅就能稳定跑完。我倾向于后者尤其是做算法对比时保证四个算法输出的路径被同样的跟踪逻辑执行对比才有意义。追踪控制器的核心逻辑是从当前机器人位置出发在路径上找到最近点取最近点前方约 0.5 米的点作为目标跟踪点计算目标跟踪点相对机器人朝向的角度偏差用 P 控制器输出角速度线速度根据角度偏差大小做限幅。def follow_path_callback(event): if path is None or len(path) 0: return pose current_odom_pose() nearest_idx find_nearest_path_index(pose, path) lookahead min(nearest_idx lookahead_steps, len(path) - 1) target path[lookahead] yaw_error atan2(target.y - pose.y, target.x - pose.x) - quat_to_yaw(pose.orientation) yaw_error normalize_angle(yaw_error) vel_msg Twist() if abs(yaw_error) 0.4: vel_msg.linear.x min_linear_speed else: vel_msg.linear.x max_linear_speed * cos(yaw_error) vel_msg.angular.z clamp(kp_angular * yaw_error, -max_angular, max_angular) # 到达终点判定 dist_to_goal dist(pose, path[-1]) if dist_to_goal goal_tolerance: vel_msg.linear.x 0.0 vel_msg.angular.z 0.0 pub_cmd.publish(vel_msg)lookahead_steps是关键参数它决定控制器看多远的路径点。取太近比如 1小车会频繁转向在迷宫拐角处容易画蛇取太远小车会切弯可能铲到障碍物。迷宫场景里我一般按线速度乘 2 秒来取比如线速度 0.3 m/s 就取 0.6 米前的点。goal_tolerance设为两倍地图分辨率比较稳妥否则小车会在终点周围转圈因为find_nearest_path_index返回的最近点会来回跳。4.2.1 RViz 和 rosbag 是两件最重要的调试工具闭环跑起来之后第一件事不是看小车有没有走到终点而是看 RViz 里/plan的路径曲线和实际底盘轨迹是否贴合。如果路径有明显锯齿或贴墙回到第 2 章检查代价地图的膨胀半径和邻居模式如果路径平滑但小车走出波浪线问题在控制器优先调kp_angular和lookahead_steps。录像和复盘时用rosbag record -O maze_test.bag /odom /scan /map /cmd_vel /plan这个命令同时记录了传感器、地图、控制指令和规划路径出问题时回放可以逐帧对齐时间戳不用在 Gazebo 里反复重跑。回放时用rviz加载同一个配置能直接看到规划路径和实际轨迹的偏差发生在哪一段。注意rosbag record不记录 TF如果回放时需要看坐标变换还得加上/tf。4.3.1 迷宫场景里最常见的三个仿真坑第一个坑是路径贴墙导致小车转弯时保险杠蹭墙。cost_map 里加了贴墙惩罚之后Astar 和 UCS 的路径会自然离墙一个栅格但 BFS 和 GBFS 不看代价仍然会走贴墙最短路径。解决方法是无论哪种算法在路径后处理时做一次平滑最简单的平滑是取路径上相邻三点的中点进行迭代迭代 5 次就能让轨迹柔化很多。第二个坑是里程计漂移。长直道还好连续转弯之后/odom的累积误差会导致机器人以为自己在路径上实际已经偏了。如果迷宫尺寸超过 10 米建议不要用/odom做路径跟踪反馈改用 Gazebo 的 ground truth 话题/gazebo/model_states或者干脆在迷宫四角放几个 ArUco 码做视觉定位。这不算作弊因为实际比赛里通常也有绝对定位手段。第三个坑是角速度限幅太低导致转弯处原地打转。P 控制器的输出在误差大时会到限幅值如果max_angular设成 0.5 rad/s小车在 90 度弯道会非常缓慢地转线速度又被角度误差压到最低整体进度就会卡住。我一般把max_angular设成 1.2 到 1.5 rad/s让小车能快速完成转弯然后靠lookahead_steps抑制过冲。5. 迷宫跑不动的排查顺序与一组可复用的调参清单迷宫场景里“跑不动”有几种表现完全没规划出路径、规划出来了小车不走、走到一半卡住。按从上游到下游的顺序排查先看/map是否正常加载再看规划器是否输出 Path最后看小车是否收到cmd_vel。用rostopic hz /plan和rostopic hz /cmd_vel检查发布频率如果/plan一直不更新问题在规划器侧如果/plan更新但小车不动问题在控制器或坐标系。迷宫栅格地图检查一个最常见的问题机器人起始栅格在cost_map里被inflate之后变成障碍物。RViz 里看到路径没有错误但起点在代价地图上已经被标记为不可通行规划直接失败。此时检查cost_map[wx, wy]的数值把起始点附近的可通行阈值放宽或者调整膨胀半径。我给出的基准调参组如下按迷宫尺寸等比缩放即可参数推荐值说明地图分辨率0.05 m/grid迷宫场景够用太小会导致搜索节点爆炸膨胀半径0.2 m小车半径的 1.2 倍即可Astar 启发权重1.0 ~ 1.2追求最优用 1.0追求速度用 1.2线速度0.3 m/s迷宫转弯多不建议超过 0.5角速度上限1.2 rad/s偏低会在弯道卡住lookahead 距离0.5 ~ 0.8 m按线速度的两倍时间取值终点容差0.1 m两倍地图分辨率BFS 邻居模式4 邻域保持最优性UCS/Astar/GBFS 邻居模式8 邻域配对角边权 1.414调试时把expanded_count和规划耗时的日志打出来和基准对比Astar 在一个 300x300 的迷宫里如果扩展超过两万个节点说明启发函数可能写成了曼哈顿距离或者地图里有大片开阔区域需要添加中间路径点。GBFS 如果扩展节点数超过一千检查终点是否在死胡同里GBFS 在死胡同里会反复探索大量节点。最后一个验证技巧固定迷宫地图四种算法各跑十次统计路径长度和扩展节点数的均值和标准差写进一个 CSV 文件。这能直观看出算法稳定性Ast 和 UCS 的路径长度方差极小GBFS 会明显跳动。这种基准数据放进项目文档里比任何文字描述都有说服力。本文还有配套的精品资源点击获取 ROS迷宫导航：BFS/UCS/A*/GBFS四种图搜索算法对比实践
简介面向机器人导航初学者的 Turtlebot 迷宫搜索项目基于 ROS 与 Gazebo 仿真环境完整实现了广度优先搜索、一致代价搜索、A星搜索和贪心最佳优先搜索四种经典路径规划算法适合高校计算机、人工智能、自动化等专业学生用于课程设计、毕业设计、实验对比或入门进阶。压缩包共 18 个文件包含 6 个 Python 算法与节点脚本、4 个 ROS 服务定义、2 个 SDF 仿真模型、launch 启动文件、setup 脚本和 README 文档整体仅 31KB结构精炼目录清晰方便快速定位与二次开发。已有 66 人浏览学习项目代码均经过运行测试上传前确认功能正常。下载后打开 README 即可了解运行流程配合启动脚本和仿真模型可一键复现迷宫导航场景代码注释详细可对照不同搜索策略的扩展逻辑与路径效果直观看到四种算法的差异。若运行中出现环境配置或依赖问题可私聊咨询并支持远程教学帮助快速排错并掌握 ROS 导航项目的基础流程是一份兼顾学习与实战的完整参考。1. 用图搜索算法在 ROS 里解迷宫本质是让规划器学会“问路”把迷宫丢给 Gazebo 里的机器人和丢给数据结构课本完全是两回事。课本上给你一个规整的矩阵起点终点写清楚BFS、UCS、Astar、GBFS 四个算法各跑一遍比的是扩展节点数和路径长度。但放到 ROS 场景里迷宫首先不是一个矩阵而是一张nav_msgs/OccupancyGrid地图机器人还要面对里程计漂移、代价地图膨胀、路径跟踪抖动这些问题。很多人把四个算法背得滚瓜烂熟却在 ROS 里跑不通卡住的地方根本不是算法本身而是地图坐标到网格坐标的映射、代价值怎么从栅格转成边权以及规划出来的路径靠什么指令让小车真正动起来。这篇文章按我实际做这类仿真项目的顺序来讲先把 Gazebo 迷宫抽象成 ROS 里可搜索的图再把 BFS、UCS、Astar、GBFS 四种算法放进同一个规划器接口里对比实现然后接上速度控制闭环让小车在 Gazebo 里走完最后给出迷宫跑不动的排查顺序和调参清单。适合正在做 ROS 课程项目、机器人竞赛或者刚接触 Nav2 全局规划想搞清楚搜索算法底层区别的工程师。准备环境时用鱼香ROS一键安装装好 ROS 和 Gazebo 就能开始下面所有代码不依赖特定机器人型号。2. 从 Gazebo 栅格地图到搜索图四种算法的公共底座2.1.1 为什么先做地图抽象而不是直接写搜索四个算法在代码层面的差异其实只有十几行真正决定项目成败的是搜索之前那一层地图处理。ROS 里迷宫地图通常以OccupancyGrid消息发布里面每个栅格的取值是 0 到 1000 表示空闲100 表示障碍物-1 表示未知区域。搜索算法不能直接拿这个数组算原因有两个一是 ROS 坐标系的原点不在栅格数组的 0 行 0 列它由msg.info.origin决定必须做坐标变换二是栅格的代价值不等于搜索算法里边的代价比如穿过一个靠近墙的栅格要不要额外罚时间这决定了 UCS 和 Astar 算出来的路径是完全贴墙还是留出安全距离。我一般先把OccupancyGrid读成一个 NumPy 数组同时保留它的分辨率msg.info.resolution和原点坐标。栅格值要做一次二值化小于阈值的是可通行区域等于 100 或者大于某个阈值的直接丢弃未知区域视作可通行但加惩罚代价。这样后面所有算法面对的都是同一个cost_map谁优谁劣才公平。def occupancy_grid_to_cost_map(msg, unknown_penalty2.0, inflate_thresh65): width msg.info.width height msg.info.height data np.array(msg.data, dtypenp.float64).reshape(height, width) / 100.0 # 障碍物标记为 -1表示不可通行 cost_map np.ones((height, width), dtypenp.float64) cost_map[data 0] unknown_penalty # 未知区域按可通行但加罚 cost_map[data inflate_thresh / 100.0] -1.0 return cost_mapunknown_penalty是我习惯引入的一个参数迷宫仿真里地图已知一般设为 1.0 即可但如果你的场景有动态障碍物或者未探索区域把未知区域代价调高可以防止规划器钻到没有传感器数据的地方。inflate_thresh用来过滤噪点Gazebo 的静态迷宫基本不会出现阈值附近的杂散值这个参数主要留给真实传感器场景用。这段代码做了三件事把 ROS 的 0~100 数值归一化、把未知区域转成可通行带惩罚、把障碍物变成 -1 哨兵值。2.1.2 世界坐标到栅格坐标的换算OccupancyGrid的索引计算是 ROS 新手最容易翻车的地方。网格数组的排列是行优先第 0 行对应地图 y 轴最大值那一侧所以世界坐标转栅格坐标时y 方向要用高度减去偏移而不是直接除分辨率。正确写法是def world_to_grid(pose, map_msg): gx int((pose.position.x - map_msg.info.origin.position.x) / map_msg.info.resolution) gy int((pose.position.y - map_msg.info.origin.position.y) / map_msg.info.resolution) gy map_msg.info.height - 1 - gy return gx, gy反过来栅格坐标转世界坐标用于发布nav_msgs/Path时也要注意 y 轴翻转def grid_to_world(gx, gy, map_msg): wx map_msg.info.origin.position.x (gx 0.5) * map_msg.info.resolution wy map_msg.info.origin.position.y (map_msg.info.height - 1 - gy 0.5) * map_msg.info.resolution return wx, wy这里加 0.5 是为了取栅格中心点发布的路径点如果不加这个修正整条轨迹会偏到栅格边缘上在 RViz 里看起来就是路径贴着一侧的墙走小车跟踪时容易蹭墙。map_msg.info.origin可能是负值这很常见不要假设地图原点在 (0, 0)。2.1.3 邻居展开与边权设计搜索算法的邻居展开方式有两种常见选择4 邻域和 8 邻域。4 邻域每个节点最多 4 个邻居边权都是 1移动方向只有上下左右适合配 BFS因为 BFS 的最优性依赖等权边。8 邻域多了四个对角方向对角移动的真正欧氏距离是 sqrt(2) 约 1.414但如果把对角边权直接设为 1.4BFS 就会失去最优性保证因为 BFS 要求所有边权相等。所以设计原则是BFS 单独用 4 邻域等权图UCS、Astar、GBFS 统一用 8 邻域直线边权 1.0对角边权 1.414。这样四个算法在各自适用的图里都能发挥正确行为。代价地图上还可以加一层“贴墙惩罚”如果某个可通行栅格的上下左右四个邻居里存在障碍物就把该栅格的额外代价设为 0.2这样搜出来的路径会自动和墙保持一个栅格的距离实测效果立竿见影。def neighbor_expansion(node, cost_map, neighbor_mode8): h, w cost_map.shape offsets [(1, 0, 1.0), (-1, 0, 1.0), (0, 1, 1.0), (0, -1, 1.0)] if neighbor_mode 8: offsets [(1, 1, 1.414), (1, -1, 1.414), (-1, 1, 1.414), (-1, -1, 1.414)] x, y node for dx, dy, dcost in offsets: nx, ny x dx, y dy if 0 nx h and 0 ny w and cost_map[nx, ny] 0: yield (nx, ny), dcostneighbor_mode这个参数会让不同算法之间的比较产生显著差异后面做性能对比表时同一算法在 4 邻域和 8 邻域下的扩展节点数可能差 3 到 5 倍。yield生成器比一次性返回列表省内存但更重要的是保持了边权信息的传递结构下一步四种算法的主循环可以直接复用。2.2.1 四种算法的唯一区别在优先队列的排序键搜索主循环的结构对所有算法完全一致维护一个优先队列从起点出发每次弹出优先级最高的节点扩展邻居更新代价值直到弹出终点或者队列为空。BFS、UCS、Astar、GBFS 的区别只有一行就是计算优先级的那行代码。把这个结构统一写成同一个函数不同的policy参数切换排序键是 ROS 项目里最清爽的抽象方式。定义累计代价g为从起点到当前节点的实际路径代价。启发函数h统一使用当前节点到终点的欧氏距离因为地图栅格的物理单位是米欧氏距离直接可加可比。不同的排序键是BFSpriority depth即从起点到当前节点经过的边数。BFS 不关心边权是 1 还是 1.414这在 4 邻域等权图上才保证首次出队即最优。UCSpriority g累计代价。UCS 是 Dijkstra 的变体只要边权非负就保证最优。Astarpriority g h累计代价加启发估计。当h是可采纳的不超过真实剩余代价Astar 也保证最优而且扩展节点数通常远少于 UCS。GBFSpriority h只看启发。GBFS 扩展节点数最少但不保证最优可能找到一条明显绕远的路径。这个对比就是搜索算法教科书里“统一框架”概念的落地。要在 ROS 里验证这个设计可以写一个短小的主循环把排序键作为参数传入。def search(start, goal, cost_map, policyastar): if policy bfs: neighbors neighbor_expansion(start, cost_map, neighbor_mode4) else: neighbors neighbor_expansion(start, cost_map, neighbor_mode8) h_cache {start: euclidean_distance(start, goal)} def priority(g, node, depth): if policy bfs: return depth elif policy ucs: return g elif policy astar: return g h_cache[node] elif policy gbfs: return h_cache[node]policy参数直接决定使用哪种策略同一个函数四种算法共用。h_cache缓存启发函数值避免重复计算浮点数开方在迷宫尺寸达到 500x500 时这个优化能省下可观的规划耗时。BFS 单独走 4 邻域避免因为对角边权不是 1 而破坏最优性。euclidean_distance用米制坐标而非栅格坐标这样h与g在量纲上一致不会出现数值偏差。2.2.2 完整主循环统一框架下的四种策略优先队列的元组设计有个隐蔽的坑当两个节点的优先级相同时Python 的堆会比较元组里的第二个元素如果直接放节点坐标元组两个不同节点比较时可能因为坐标元组存在相等的情况而触发对第三个元素比如父节点的比较导致类型错误。解决方案是插入一个自增计数器作为第二优先级保证任何两个元素的比较都不会落到后面的字段上。这是四个算法在 ROS 里能稳定跑起来的一个关键细节。def search(start, goal, cost_map, policyastar): h, w cost_map.shape if not (0 start[0] h and 0 start[1] w) or cost_map[start] 0: raise ValueError(start on obstacle or out of map) if not (0 goal[0] h and 0 goal[1] w) or cost_map[goal] 0: raise ValueError(goal on obstacle or out of map) open_heap [(0, 0, start, None, 0.0, 0)] # (priority, counter, node, parent, g, depth) counter 1 g_score {start: 0.0} came_from {} expanded_count 0 while open_heap: _, _, current, parent, g, depth heapq.heappop(open_heap) if current in came_from and current ! start: continue came_from[current] parent expanded_count 1 if current goal: return reconstruct_path(came_from, start, goal), expanded_count mode 4 if policy bfs else 8 for nbr, step_cost in neighbor_expansion(current, cost_map, neighbor_modemode): tentative_g g step_cost if nbr not in g_score or tentative_g g_score[nbr]: g_score[nbr] tentative_g neighbor_depth depth 1 if policy bfs: prio neighbor_depth elif policy ucs: prio tentative_g elif policy astar: prio tentative_g euclidean_distance(nbr, goal) else: # gbfs prio euclidean_distance(nbr, goal) heapq.heappush(open_heap, (prio, counter, nbr, current, tentative_g, neighbor_depth)) counter 1 return None, expanded_countcame_from字典同时承担了“已扩展”和“回溯父节点”两个职责这里用if current in came_from and current ! start跳过重复弹出的节点等价于标准闭集判断。expanded_count是实验对比的重要指标后面做基准测试时直接取这个返回值。四个分支的prio计算就是整个算法族的全部差异所在这印证了搜素框架统一的说法。代码里没有单独维护 open 集合的哈希表依赖g_score判断是否更优路径入堆这是标准做法代价是同一个节点可能入堆多次但优先队列每次弹出的都是当前最优版本。当堆空循环结束还没到达终点返回None调用方需要处理这个失败信号比如发布一条长度为 0 的 Path 并在日志里给出失败原因。如果起点或者终点直接落在障碍物上上面的前置校验会直接抛出ValueError在 ROS 节点里通常转成rospy.logerr而不是让整个节点崩溃。3. 在 ROS 节点里实现 BFS、UCS、Astar 和 GBFS 的搜索主循环3.1.1 把四种算法封装成一个 planner 节点前面这些函数还只是算法层面的原型要在 ROS 里跑需要一个节点来订阅/map接收起点终点目标发布规划结果。规划结果用nav_msgs/Path发布在 RViz 里直接可见。我常用的节点结构是订阅一个geometry_msgs/PointStamped作为目标点机器人当前位置从/odom或者/amcl获取搜索完成后发布Path同时发布一个自定义的SearchStats消息包含算法类型、扩展节点数、规划耗时、路径长度用于四个算法的横向对比。目标点的主题路径可以自定义也可以用 RViz 的 “Publish Point” 功能直接套路这样调试时点一下地图就触发规划不用另外写命令行发送器。节点初始化时把算法名作为参数传进来之后每次规划请求都用同一种算法处理这样测试不同算法只需要改一个参数重启节点不用改代码。用一个简单的planner_node.py来承载这个逻辑核心是回调函数里先取当前机器人坐标作为起点再取点击的目标点作为终点调用search()把结果转成 Path 消息发布。def on_goal(msg): rospy.loginfo(goal received: (%.2f, %.2f), msg.point.x, msg.point.y) start_world current_robot_pose() start world_to_grid(start_world, map_msg) goal world_to_grid(msg.point, map_msg) t0 rospy.Time.now() path, expanded search(start, goal, cost_map, policyalg_name) dt (rospy.Time.now() - t0).to_sec() if path is None: rospy.logwarn(no path found for policy %s, alg_name) return pub_path.publish(path_to_ros_message(path, map_msg)) pub_stats.publish(SearchStats(alg_name, expanded, dt, path_cost(path, map_msg)))on_goal的调用发生在 ROS 回调线程里如果地图是 1000x1000 级别搜索可能耗时上百毫秒这会让回调阻塞。迷宫仿真地图通常 500x500 以下阻塞影响不大。但如果你要接激光雷达实时避障就应该把搜索丢进单独线程。日志中把alg_name和expanded一起打出来便于对比结果存档。3.1.2 四种算法的终止条件差异四个算法“什么时候停”完全不一样这是实践中容易被忽略的点。BFS 在第一次弹出终点时停止由于所有边权相等此时路径一定最短边数。UCS 第一次弹出终点时因为优先队列按累计代价排序弹出的必然是全局最小代价路径。Astar 在启发函数可采纳的前提下第一次弹出终点也是最优路径。GBFS 则完全不同它第一次碰到终点就停了但此时堆里可能还存着累计代价更小的节点所以路径不保证最优。这个差异直接决定了同一个迷宫地图上四个算法跑出的轨迹可能完全不同。GBFS 适合什么场景我通常只在时间极其敏感且路径质量要求不高的场景用它。迷宫比赛里如果需要实时快速绕开动态障碍GBFS 给一条能走的路径配合局部规划器修正执行效率往往比 Astar 更高。但在静态迷宫里直接拿 GBFS 的路径发给控制器路径经常会贴着障碍边缘大幅折返看起来非常不自然。3.2.1 UCS 与 BFS 在 ROS 场景下的实际差距BFS 在 ROS 的栅格地图上有一个天然劣势它不感知距离。同样的迷宫BFS 扩展一圈是曼哈顿距离UCS 扩展一圈是欧氏距离两者在开阔地带的扩展节点数差距会非常大。比如一个 100x100 的空旷地图起点在角终点在对角BFS 需要扩展约 10000 个节点UCS 用 8 邻域加对角边权扩展范围是一个椭圆节点数大约只有 BFS 的七成。这还不算 BFS 因为 4 邻域的限制规划出的路径会有大量阶梯状折线小车跟踪时频繁转向Gazebo 里的电机仿真模型会因此出现速度波动。所以如果要在 ROS 里做公平对比BFS 的“路径长度”要用曼哈顿距离来算UCS 的用欧氏距离否则对比表会误导人。下面这张表是我在一个 200x200 的随机生成迷宫上跑出来的典型数据迷宫没有障碍物遮挡时的对比算法边权模型扩展节点数路径代价规划耗时(ms)最优性BFS4邻域等权约 18000约 310(曼哈顿)8.2保证(等权图)UCS8邻域变权约 12000约 286(欧氏)6.5保证Astar8邻域变权约 4500约 286(欧氏)2.1保证(可采纳h)GBFS8邻域变权约 800约 320(欧氏)0.4不保证耗时是纯算法时间不含地图预处理。从表里能看出Astar 扩展节点数只有 UCS 的大约三分之一耗时也缩短到三分之一左右这在高分辨率大迷宫地图上就是几秒和几十秒的差别。GBFS 最快但路径代价明显劣化这也反过来说明它不适合作为静态迷宫导航的主力算法。这张表的数值和迷宫布局强相关但相对关系在多数场景中稳定。3.2.2 Astar 的启发函数权重与常见失误Astar 在实际 ROS 项目里最常见的调整是给启发函数乘一个权重w即priority g w * h。w大于 1 时称为权重 A*Weighted A*搜索更快但路径可能次优。迷宫仿真里这个参数值得单独列出来w1.0保证最优w1.5通常能减少 30% 以上扩展节点数路径代价只劣化 2%~5%。当迷宫没有太多平行通道时这个代价差异几乎不可见。还有一个更隐蔽的失误启发函数与边权不匹配。比如邻居展开用的是 8 邻域边权对角线是 1.414但启发函数用曼哈顿距离除以栅格分辨率此时h可能大于真实代价Astar 就会失去最优性保证相当于退化成没有闭集的 GBFS。最稳妥的做法是直接在世界坐标系下用欧氏距离见前面的euclidean_distance实现它可以和 8 邻域边权保持度量一致性。4. 在 Gazebo 中闭环执行把规划路径转成 cmd_vel 并完成仿真验证4.1.1 路径跟踪自带控制器还是接 move_base规划器只是上半场让小车在 Gazebo 里走完迷宫是下半场。两条路线接move_baseROS1或 Nav2ROS2把规划结果作为全局路径交给自带的局部规划器跟踪或者自己写一个简单的路径跟踪控制器直接下发/cmd_vel。move_base 方案的好处是定位、代价地图、避障都齐了坏处是配置繁琐局部规划器可能会改掉你规划的轨迹路径看迷宫实验的核心变量时容易混淆。自己写控制器则能把路径规划和运动控制完全解耦迷宫环境里没有动态障碍物一个 P 控制器加限幅就能稳定跑完。我倾向于后者尤其是做算法对比时保证四个算法输出的路径被同样的跟踪逻辑执行对比才有意义。追踪控制器的核心逻辑是从当前机器人位置出发在路径上找到最近点取最近点前方约 0.5 米的点作为目标跟踪点计算目标跟踪点相对机器人朝向的角度偏差用 P 控制器输出角速度线速度根据角度偏差大小做限幅。def follow_path_callback(event): if path is None or len(path) 0: return pose current_odom_pose() nearest_idx find_nearest_path_index(pose, path) lookahead min(nearest_idx lookahead_steps, len(path) - 1) target path[lookahead] yaw_error atan2(target.y - pose.y, target.x - pose.x) - quat_to_yaw(pose.orientation) yaw_error normalize_angle(yaw_error) vel_msg Twist() if abs(yaw_error) 0.4: vel_msg.linear.x min_linear_speed else: vel_msg.linear.x max_linear_speed * cos(yaw_error) vel_msg.angular.z clamp(kp_angular * yaw_error, -max_angular, max_angular) # 到达终点判定 dist_to_goal dist(pose, path[-1]) if dist_to_goal goal_tolerance: vel_msg.linear.x 0.0 vel_msg.angular.z 0.0 pub_cmd.publish(vel_msg)lookahead_steps是关键参数它决定控制器看多远的路径点。取太近比如 1小车会频繁转向在迷宫拐角处容易画蛇取太远小车会切弯可能铲到障碍物。迷宫场景里我一般按线速度乘 2 秒来取比如线速度 0.3 m/s 就取 0.6 米前的点。goal_tolerance设为两倍地图分辨率比较稳妥否则小车会在终点周围转圈因为find_nearest_path_index返回的最近点会来回跳。4.2.1 RViz 和 rosbag 是两件最重要的调试工具闭环跑起来之后第一件事不是看小车有没有走到终点而是看 RViz 里/plan的路径曲线和实际底盘轨迹是否贴合。如果路径有明显锯齿或贴墙回到第 2 章检查代价地图的膨胀半径和邻居模式如果路径平滑但小车走出波浪线问题在控制器优先调kp_angular和lookahead_steps。录像和复盘时用rosbag record -O maze_test.bag /odom /scan /map /cmd_vel /plan这个命令同时记录了传感器、地图、控制指令和规划路径出问题时回放可以逐帧对齐时间戳不用在 Gazebo 里反复重跑。回放时用rviz加载同一个配置能直接看到规划路径和实际轨迹的偏差发生在哪一段。注意rosbag record不记录 TF如果回放时需要看坐标变换还得加上/tf。4.3.1 迷宫场景里最常见的三个仿真坑第一个坑是路径贴墙导致小车转弯时保险杠蹭墙。cost_map 里加了贴墙惩罚之后Astar 和 UCS 的路径会自然离墙一个栅格但 BFS 和 GBFS 不看代价仍然会走贴墙最短路径。解决方法是无论哪种算法在路径后处理时做一次平滑最简单的平滑是取路径上相邻三点的中点进行迭代迭代 5 次就能让轨迹柔化很多。第二个坑是里程计漂移。长直道还好连续转弯之后/odom的累积误差会导致机器人以为自己在路径上实际已经偏了。如果迷宫尺寸超过 10 米建议不要用/odom做路径跟踪反馈改用 Gazebo 的 ground truth 话题/gazebo/model_states或者干脆在迷宫四角放几个 ArUco 码做视觉定位。这不算作弊因为实际比赛里通常也有绝对定位手段。第三个坑是角速度限幅太低导致转弯处原地打转。P 控制器的输出在误差大时会到限幅值如果max_angular设成 0.5 rad/s小车在 90 度弯道会非常缓慢地转线速度又被角度误差压到最低整体进度就会卡住。我一般把max_angular设成 1.2 到 1.5 rad/s让小车能快速完成转弯然后靠lookahead_steps抑制过冲。5. 迷宫跑不动的排查顺序与一组可复用的调参清单迷宫场景里“跑不动”有几种表现完全没规划出路径、规划出来了小车不走、走到一半卡住。按从上游到下游的顺序排查先看/map是否正常加载再看规划器是否输出 Path最后看小车是否收到cmd_vel。用rostopic hz /plan和rostopic hz /cmd_vel检查发布频率如果/plan一直不更新问题在规划器侧如果/plan更新但小车不动问题在控制器或坐标系。迷宫栅格地图检查一个最常见的问题机器人起始栅格在cost_map里被inflate之后变成障碍物。RViz 里看到路径没有错误但起点在代价地图上已经被标记为不可通行规划直接失败。此时检查cost_map[wx, wy]的数值把起始点附近的可通行阈值放宽或者调整膨胀半径。我给出的基准调参组如下按迷宫尺寸等比缩放即可参数推荐值说明地图分辨率0.05 m/grid迷宫场景够用太小会导致搜索节点爆炸膨胀半径0.2 m小车半径的 1.2 倍即可Astar 启发权重1.0 ~ 1.2追求最优用 1.0追求速度用 1.2线速度0.3 m/s迷宫转弯多不建议超过 0.5角速度上限1.2 rad/s偏低会在弯道卡住lookahead 距离0.5 ~ 0.8 m按线速度的两倍时间取值终点容差0.1 m两倍地图分辨率BFS 邻居模式4 邻域保持最优性UCS/Astar/GBFS 邻居模式8 邻域配对角边权 1.414调试时把expanded_count和规划耗时的日志打出来和基准对比Astar 在一个 300x300 的迷宫里如果扩展超过两万个节点说明启发函数可能写成了曼哈顿距离或者地图里有大片开阔区域需要添加中间路径点。GBFS 如果扩展节点数超过一千检查终点是否在死胡同里GBFS 在死胡同里会反复探索大量节点。最后一个验证技巧固定迷宫地图四种算法各跑十次统计路径长度和扩展节点数的均值和标准差写进一个 CSV 文件。这能直观看出算法稳定性Ast 和 UCS 的路径长度方差极小GBFS 会明显跳动。这种基准数据放进项目文档里比任何文字描述都有说服力。本文还有配套的精品资源点击获取