机器人 / 系统课时

比较 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 才显式搜索朝向与转弯动作。

NavFn、Smac 2D 与 Smac Hybrid-A* 的路径和转弯约束对比

注册三种规划器进行受控比较

前提是 Ubuntu 24.04、ROS 2 Jazzy、Nav2 已安装,地图、定位、TF 和全局代价地图正常工作。把下面的片段示例合并到现有 nav2_params.yaml 的 planner_server 节点;保留原有全局代价地图及其他参数,不要复制出第二个同名节点。三种插件共用同一地图与障碍配置。

yaml
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;已有导航进程应先停止,避免重复节点。配置生效后可执行:

bash
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) 都位于已知可通行区域,不适合时修改为地图内相同的有效起终点。

python
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 的终端中运行:

bash
python3 compare_planners.py

预期输出形式如下,点数与长度取决于实际地图、起终点、Footprint 和代价地图参数,不能把占位值当作基准成绩:

text
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 文档。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

比较 NavFn 与 Smac 规划路径

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通