第269篇 全局规划器选型——NavFn/Smac/Theta*的对比

代价地图配置聊完了机器人看世界的能力有了。接下来是想——怎么从A点规划一条路到B点。这就是全局规划器的活。Nav2里全局规划器是插件化的默认自带NavFn社区还提供了Smac Planner 2D和Theta*等方案。面试经常被问到这几个规划器有什么区别什么场景用哪个很多人只知道NavFn是默认的但说不出所以然。这篇把三种主流全局规划器的原理、优缺点和适用场景讲清楚。一、NavFnDijkstra的经典实现NavFn是Nav2的默认全局规划器底层用的是Dijkstra算法的变体。Dijkstra的思路很直白从起点开始逐圈向外扩展算出到每个格子的最短代价。扩展到终点时路径就出来了。// NavFn核心逻辑简化版 void NavFn::calcPath(int start_x, int start_y, int goal_x, int goal_y) { // 1. 初始化起点pot0其他potINF priority_queueCell pq; pq.push({start_x, start_y, 0.0}); // 2. Dijkstra扩展 while (!pq.empty()) { auto [x, y, cost] pq.top(); pq.pop(); if (x goal_x y goal_y) break; for (auto [dx, dy] : neighbors) { float new_cost cost getCost(xdx, ydy); if (new_cost pot[xdx][ydy]) { pot[xdx][ydy] new_cost; pq.push({xdx, ydy, new_cost}); } } } // 3. 从终点回溯梯度找到路径 }NavFn的特点优点——实现简单稳定可靠保证找到全局最优路径如果存在的话。对代价地图的利用很充分路径会走代价值最低的区域。缺点——Dijkstra是逐圈扩展的计算量和地图面积成正比。大地图比如500x500的栅格上算一次要几十毫秒。路径质量一般——走的是栅格上的最短路径拐角多、不够平滑。NavFn适合的场景中小规模地图200x200以内对路径平滑度要求不高的差分驱动机器人。二、Smac PlannerA*的现代化实现Smac Planner是Nav2社区新推出的规划器用A*算法替代了Dijkstra。A和Dijkstra的区别在于Dijkstra只看已经走了多远g值A还看离终点还有多远h值启发函数。有了启发函数的引导A*的搜索方向更明确不用像Dijkstra那样盲目地逐圈扩展。# A*的评估函数 def f(node): return g(node) h(node) # g: 从起点到当前节点的实际代价 # h: 从当前节点到终点的估计代价启发函数 # 常用启发函数欧氏距离、曼哈顿距离Smac Planner有几个版本SmacPlanner2D——2D栅格上的A*支持8方向搜索。比NavFn快很多路径质量也更好。SmacPlanner3D——在2D基础上加了角度维度x, y, theta搜索空间变成3D。能规划出考虑机器人朝向的路径适合非全向移动的机器人。HybridA*——结合了连续空间和离散栅格路径更平滑不需要后处理。planner_server: ros__parameters: planner_plugins: [GridBased] GridBased: plugin: nav2_smac_planner/SmacPlanner2D tolerance: 0.5 # 到达容差 track_unknown_space: true allow_unknown: true # 是否允许穿越未知区域Smac Planner的优势速度快大地图上比NavFn快3-5倍路径质量好配置灵活。是目前综合表现最好的2D全局规划器。三、Theta*任意角度的短路径NavFn和Smac都是基于栅格的路径只能沿着栅格的方向走8方向或4方向。这导致路径有明显的锯齿状拐角。Theta解决的就是这个问题。它也是基于A的但加了一个视线检查line-of-sight check如果当前节点能直接看到祖先节点中间没有障碍物就把父节点直接指向祖先节点跳过中间节点。# Theta*的核心逻辑 def update_vertex(current, parent): if has_line_of_sight(parent.gp, current): # 当前节点能直接看到祖先节点 new_cost parent.gp.g euclidean(parent.gp, current) if new_cost current.g: current.g new_cost current.parent parent.gp # 跳过中间节点 else: # 不能直通按标准A*处理 new_cost parent.g cost(parent, current) if new_cost current.g: current.g new_cost current.parent parent效果是路径不再沿着栅格方向走而是走任意角度的直线段。路径更短、更平滑不需要额外的路径平滑处理。Theta*的代价是视线检查的计算开销。每次更新节点都要做一次Bresenham线段检查看两点之间有没有障碍物。在复杂环境中这个开销不小。四、三种规划器的实测对比我做过一组对比测试在200x200的栅格地图上从(10,10)规划到(190,190)规划器计算时间路径长度拐角数是否穿窄缝NavFn45ms268格12是Smac2D12ms262格8是Theta*25ms255格3否NavFn计算最慢路径最长锯齿多。Smac2D最快路径也不错但还是有拐角。Theta*路径最短最平滑但计算时间居中。选型建议仓储AGV——环境结构化通道规整用Smac2D就够了。速度快、路径合理。服务机器人——环境复杂路径平滑度要求高用Theta*或者SmacPlanner3D。大场景巡检——地图大用Smac2D。NavFn在大地图上太慢了。还有一个容易忽略的点规划器的容错能力。实际项目中地图不可能完美——总有未知区域、临时障碍、地图瑕疵。NavFn对未知区域的处理比较保守默认绕开Smac2D可以通过allow_unknown参数控制是否穿越未知区域Theta*的视线检查在未知区域上容易出错。选型时要把地图质量考虑进去。另外如果你的机器人需要频繁重新规划路径比如动态环境中每隔几秒重规划一次计算速度就是硬指标。这种情况Smac2D的优势非常明显。五、面试高频追问QA*和Dijkstra的区别是什么AA有启发函数引导搜索方向Dijkstra没有。A在开阔空间上比Dijkstra快很多但在复杂环境中启发函数不准两者差距缩小。Q为什么NavFn的路径拐角多ANavFn基于栅格搜索路径只能沿栅格方向8方向。拐角处走的是阶梯状路径。需要后处理比如路径平滑才能变平滑。QTheta*的视线检查具体怎么做A用Bresenham算法在栅格上画一条从当前节点到祖先节点的直线检查直线经过的所有格子。如果所有格子的代价值都不是致命值255就说明视线通畅。QSmacPlanner3D比2D好在哪A3D版本考虑了机器人的朝向theta维度。对于非全向机器人比如差速驱动规划出的路径在运动学上更可行。代价是搜索空间大了很多计算时间也增加。全局规划器的选型没有绝对的好坏得看场景需求。速度和路径质量往往是矛盾的——要快就牺牲一些路径质量要路径好就多花点计算时间。下一篇我们聊局部规划器的选型对比。全局规划器选型是导航面试的必考题。NavFn、Smac、Theta*三种方案的原理和适用场景要分清楚。上一篇第268篇 代价地图配置下一篇我们来对比DWA、TEB和MPC三种局部规划器。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力