简介:面向机器人导航初学者的 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 到 100,0 表示空闲,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_penalty=2.0, inflate_thresh=65): width = msg.info.width height = msg.info.height data = np.array(msg.data, dtype=np.float64).reshape(height, width) / 100.0 # 障碍物标记为 -1,表示不可通行 cost_map = np.ones((height, width), dtype=np.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.4,BFS 就会失去最优性保证,因为 BFS 要求所有边权相等。
所以设计原则是:BFS 单独用 4 邻域等权图,UCS、Astar、GBFS 统一用 8 邻域,直线边权 1.0,对角边权 1.414。这样四个算法在各自适用的图里都能发挥正确行为。代价地图上还可以加一层“贴墙惩罚”:如果某个可通行栅格的上下左右四个邻居里存在障碍物,就把该栅格的额外代价设为 0.2,这样搜出来的路径会自动和墙保持一个栅格的距离,实测效果立竿见影。
def neighbor_expansion(node, cost_map, neighbor_mode=8): 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统一使用当前节点到终点的欧氏距离,因为地图栅格的物理单位是米,欧氏距离直接可加可比。不同的排序键是:
- BFS:
priority = depth,即从起点到当前节点经过的边数。BFS 不关心边权是 1 还是 1.414,这在 4 邻域等权图上才保证首次出队即最优。 - UCS:
priority = g,累计代价。UCS 是 Dijkstra 的变体,只要边权非负就保证最优。 - Astar:
priority = g + h,累计代价加启发估计。当h是可采纳的(不超过真实剩余代价),Astar 也保证最优,而且扩展节点数通常远少于 UCS。 - GBFS:
priority = h,只看启发。GBFS 扩展节点数最少,但不保证最优,可能找到一条明显绕远的路径。
这个对比就是搜索算法教科书里“统一框架”概念的落地。要在 ROS 里验证这个设计,可以写一个短小的主循环,把排序键作为参数传入。
def search(start, goal, cost_map, policy="astar"): if policy == "bfs": neighbors = neighbor_expansion(start, cost_map, neighbor_mode=4) else: neighbors = neighbor_expansion(start, cost_map, neighbor_mode=8) 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, policy="astar"): 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_mode=mode): 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, policy=alg_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) | 最优性 |
|---|---|---|---|---|---|
| BFS | 4邻域等权 | 约 18000 | 约 310(曼哈顿) | 8.2 | 保证(等权图) |
| UCS | 8邻域变权 | 约 12000 | 约 286(欧氏) | 6.5 | 保证 |
| Astar | 8邻域变权 | 约 4500 | 约 286(欧氏) | 2.1 | 保证(可采纳h) |
| GBFS | 8邻域变权 | 约 800 | 约 320(欧氏) | 0.4 | 不保证 |
耗时是纯算法时间,不含地图预处理。从表里能看出,Astar 扩展节点数只有 UCS 的大约三分之一,耗时也缩短到三分之一左右,这在高分辨率大迷宫地图上就是几秒和几十秒的差别。GBFS 最快但路径代价明显劣化,这也反过来说明它不适合作为静态迷宫导航的主力算法。这张表的数值和迷宫布局强相关,但相对关系在多数场景中稳定。
3.2.2 Astar 的启发函数权重与常见失误
Astar 在实际 ROS 项目里最常见的调整是给启发函数乘一个权重w,即priority = g + w * h。w大于 1 时称为权重 A*(Weighted A*),搜索更快但路径可能次优。迷宫仿真里这个参数值得单独列出来:w=1.0保证最优,w=1.5通常能减少 30% 以上扩展节点数,路径代价只劣化 2%~5%。当迷宫没有太多平行通道时,这个代价差异几乎不可见。
还有一个更隐蔽的失误:启发函数与边权不匹配。比如邻居展开用的是 8 邻域,边权对角线是 1.414,但启发函数用曼哈顿距离除以栅格分辨率,此时h可能大于真实代价,Astar 就会失去最优性保证,相当于退化成没有闭集的 GBFS。最稳妥的做法是直接在世界坐标系下用欧氏距离,见前面的euclidean_distance实现,它可以和 8 邻域边权保持度量一致性。
4. 在 Gazebo 中闭环执行:把规划路径转成 cmd_vel 并完成仿真验证
4.1.1 路径跟踪:自带控制器还是接 move_base
规划器只是上半场,让小车在 Gazebo 里走完迷宫是下半场。两条路线:接move_base(ROS1)或 Nav2(ROS2),把规划结果作为全局路径,交给自带的局部规划器跟踪;或者自己写一个简单的路径跟踪控制器,直接下发/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 会明显跳动。这种基准数据放进项目文档里,比任何文字描述都有说服力。
本文还有配套的精品资源,点击获取