机器人 / 系统课时
比较 NavFn 与 Smac 规划路径
比较 NavFn 与 Smac 规划路径
在同一张全局代价地图上,NavFn 与不同的 Smac 规划器可能给出不同路径。本课在 ROS 2 Jazzy 中固定地图和起终点,分别比较网格搜索与带转弯约束的搜索,并判断哪类路径适合差速机器人跟踪。
学习目标
- 说清 NavFn、Smac 2D 与 Smac Hybrid-A* 的搜索状态及适用底盘。
- 在同一 Planner Server 中注册并测量三种规划路径。
- 区分路径几何约束、差速底盘原地旋转能力与实际跟踪限制。
网格搜索与运动学搜索的差别
| 规划器 | 搜索与代价 | 对转向的处理 | 适用情况 |
|---|---|---|---|
| NavFn | 二维代价网格,Dijkstra 或 A* | 不把航向和最小转弯半径纳入搜索 | 圆形或近圆形差速底盘的基础方案 |
| Smac 2D | 考虑通行代价的二维 A* | 仍不是运动学可行路径规划 | 希望路径更主动避开高代价区域 |
| Smac Hybrid-A* | 在位置和离散航向上搜索,使用 Dubins/Reeds-Shepp 运动模型 | 约束非零最小转弯半径,并考虑姿态下的碰撞 | 阿克曼转向或明确限制曲率的底盘 |
三者都使用全局代价地图,但不会保证输出完全相同的曲线。Smac 是一组插件,不等于“都考虑运动学”:SmacPlanner2D 仍然是网格规划器,SmacPlannerHybrid 才显式搜索朝向与转弯动作。

注册三种规划器进行受控比较
前提是 Ubuntu 24.04、ROS 2 Jazzy、Nav2 已安装,地图、定位、TF 和全局代价地图正常工作。把下面的片段示例合并到现有 nav2_params.yaml 的 planner_server 节点;保留原有全局代价地图及其他参数,不要复制出第二个同名节点。三种插件共用同一地图与障碍配置。
planner_server:
ros__parameters:
planner_plugins: ["Navfn", "Smac2D", "SmacHybrid"]
Navfn:
plugin: "nav2_navfn_planner::NavfnPlanner"
use_astar: false
tolerance: 0.25
allow_unknown: false
Smac2D:
plugin: "nav2_smac_planner::SmacPlanner2D"
tolerance: 0.25
allow_unknown: false
downsample_costmap: false
cost_travel_multiplier: 2.0
SmacHybrid:
plugin: "nav2_smac_planner::SmacPlannerHybrid"
tolerance: 0.25
allow_unknown: false
motion_model_for_search: "REEDS_SHEPP"
angle_quantization_bins: 72
minimum_turning_radius: 0.40
max_planning_time: 5.0这里将三者的目标容差统一为 0.25 m、禁止穿过未知区域;NavFn 使用 Dijkstra,Smac 2D 保留 2.0 的高代价惩罚系数。Hybrid-A* 允许倒车,但转弯半径至少为 0.40 m。重新使用这份参数文件启动 Nav2;已有导航进程应先停止,避免重复节点。配置生效后可执行:
ros2 param get /planner_server planner_plugins预期结果:返回的字符串数组包含 Navfn、Smac2D 和 SmacHybrid。若找不到 /planner_server,先检查节点命名空间和生命周期启动状态;若缺少插件,检查是否安装对应 Jazzy 规划器软件包及插件类名。
固定起终点实际计算路径
用 nav2_simple_commander 的 BasicNavigator.getPath() 分别指定 planner_id,只请求路径、不下发移动命令,便于排除控制器影响。以下示例需要已经激活的 Nav2 和仿真 /clock;先在 RViz 中确认示例坐标 (0, 0)、(2, 1) 都位于已知可通行区域,不适合时修改为地图内相同的有效起终点。
import math
import rclpy
from rclpy.parameter import Parameter
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
def make_pose(nav, x, y, yaw):
p = PoseStamped()
p.header.frame_id = "map"
p.header.stamp = nav.get_clock().now().to_msg()
p.pose.position.x, p.pose.position.y = x, y
p.pose.orientation.z = math.sin(yaw / 2.0)
p.pose.orientation.w = math.cos(yaw / 2.0)
return p
def path_length(path):
points = [pose.pose.position for pose in path.poses]
return sum(math.hypot(b.x - a.x, b.y - a.y)
for a, b in zip(points, points[1:]))
def main():
rclpy.init()
nav = BasicNavigator()
# 仿真时统一采用 /clock,避免起终点时间戳与 TF 时间域不一致
nav.set_parameters([Parameter("use_sim_time", value=True)])
nav.waitUntilNav2Active()
start = make_pose(nav, 0.0, 0.0, 0.0)
goal = make_pose(nav, 2.0, 1.0, math.pi / 2)
# 相同地图和位姿,仅改变插件 ID
for name in ("Navfn", "Smac2D", "SmacHybrid"):
path = nav.getPath(start, goal, planner_id=name, use_start=True)
if path is None or not path.poses:
print(f"{name}: 未找到路径")
else:
print(f"{name}: {len(path.poses)} 点 / {path_length(path):.2f} m")
nav.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()预期结果:脚本依次请求三条路径,有解时打印点数和长度,无解时报告“未找到路径”。保存为 compare_planners.py,在已执行 source /opt/ros/jazzy/setup.bash 的终端中运行:
python3 compare_planners.py预期输出形式如下,点数与长度取决于实际地图、起终点、Footprint 和代价地图参数,不能把占位值当作基准成绩:
Navfn: <N> 点 / <L> m
Smac2D: <N> 点 / <L> m
SmacHybrid: <N> 点 / <L> m想观察路径形状,可在 RViz 的 Displays → Add → By topic 中添加 /plan 的 Path 显示(有命名空间时按实际话题调整),将脚本循环中的插件列表临时改为单个名称,分别运行并截图。/plan 只呈现最近一次规划结果,三条轨迹不会自动叠加。
如果某个规划器搜索失败,对应行显示“未找到路径”,不能据此推断算法差;需先排除目标落在障碍内、TF 不可用、地图边界和 0.40 m 转弯约束过严等因素。此脚本的 planner_id 仅影响本次路径请求;正式 NavigateToPose 的规划器仍由行为树中的选择配置决定。
根据可跟踪性而非单纯路径长度选型
对每条路径至少检查三项:在 RViz 中查看它是否穿过障碍膨胀带或贴墙;观察急转弯处是否要求机器人先旋转再前进;实际低速导航时比较 /odom 轨迹与规划路径的偏离、是否出现左右振荡。建议用同一控制器、相同速度上限,反复测试相同目标 3 次,不要一边换规划器一边调整控制器。
若狭窄拐角中 NavFn 有路径、Hybrid-A* 无路径,可能是后者的非零转弯半径模型无法通过;对于可原地转向的差速车,不应盲目减小该参数来冒充零半径模型。圆形底盘可优先试 NavFn 或 Smac 2D;非圆形差速底盘若必须在规划阶段检查旋转姿态,应评估带原地旋转动作集的 SmacPlannerLattice。若规划路径有效但跟踪失败,则进一步核对 Footprint、代价地图膨胀、控制器速度与角速度限制,不能只靠换全局规划器修复。
参数定义可对照 NavFn、Smac 2D 和 Smac Hybrid-A* 的 Jazzy 文档。