ROS 2 Lyrical 第六章 Nav2自主导航完整配置与实操

前言

本章完整迁移ROS1 move_base导航栈内容，全量适配ROS 2 Lyrical + Nav2，替换ROS1专属catkin、XML launch、actionlib、amcl_diff，统一使用colcon、Python Launch、Nav2 Action服务、AMCL2、Costmap2D、Teb/DWB局部规划器。
基于第四章四轮差速小车、第五章SLAM地图与TF基础，完整实现静态地图加载、AMCL粒子定位、全局/局部代价地图、全局/局部路径规划、动态避障、RViz2交互导航、代码批量发送目标点全流程，所有配置文件、启动脚本、C++代码均可在Ubuntu26.04+Lyrical环境直接运行，理论与仿真实操一一对应。

本章学习目标

1. 创建Nav2专用功能包，梳理全套yaml参数配置文件分工
2. 理解全局/局部代价地图原理、机器人占地、障碍物膨胀、传感器观测配置
3. 掌握DWB局部规划器速度、加速度、运动约束调参
4. 编写Nav2一体化Python Launch，整合Gazebo、机器人模型、map_server、AMCL2、Nav2核心节点
5. RViz2导航全套插件可视化：地图、粒子云、代价地图、全局/局部路径、机器人轮廓
6. 2D位姿初始化、2D导航目标交互操作流程
7. AMCL2粒子滤波定位参数调优，动态rqt_reconfigure在线改参
8. 动态障碍物实时避障原理与仿真验证
9. 编写Action客户端代码，程序自动下发多个导航目标点

6.1 创建Nav2导航功能包

6.1.1 ROS2 colcon创建功能包命令

ROS1 catkin_create_pkg 替换为ROS2标准创建指令，依赖Nav2全套组件：

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake chapter6_tutorials \
rclcpp nav2_bringup nav2_amcl nav2_costmap_2d nav2_planner nav2_dwb_controller \
tf2_ros gazebo_ros xacro map_server rviz2 rqt_reconfigure

目录结构规划：

chapter6_tutorials/
├── launch/          # 所有启动文件、yaml参数
├── maps/            # gmapping保存的栅格地图map.pgm+map.yaml
├── src/             # 发送导航目标Action客户端代码
├── CMakeLists.txt
└── package.xml

6.1.2 package.xml关键依赖

<buildtool_depend>ament_cmake</buildtool>
<depend>rclcpp</depend>
<depend>nav2_bringup</depend>
<depend>nav2_amcl</depend>
<depend>nav2_costmap_2d</depend>
<depend>nav2_dwb_controller</depend>
<depend>nav2_planner</depend>
<depend>gazebo_ros</depend>
<depend>xacro</depend>
<depend>map_server</depend>
<depend>rviz2</depend>
<depend>rqt_reconfigure</depend>

6.2 Nav2核心YAML参数配置文件详解

共4组核心配置文件，分别控制公共代价参数、全局代价地图、局部代价地图、局部速度规划器。

6.2.1 costmap_common_params.yaml（全局/局部共用基础参数）

作用：机器人外形、障碍物探测、安全膨胀、雷达观测源通用配置

# 传感器探测障碍物最大距离
obstacle_range: 2.5
# 射线清除自由空间最大距离
raytrace_range: 3.0
# 四轮小车占地轮廓 [x,y]多边形
footprint: [[-0.2,-0.2],[-0.2,0.2],[0.2,0.2],[0.2,-0.2]]
# 障碍物安全膨胀半径，预留避障缓冲
inflation_radius: 0.5
# 代价缩放，数值越大越远离障碍物
cost_scaling_factor: 10.0
# 观测传感器列表
observation_sources: scan
# 2D激光雷达配置
scan:
  sensor_frame: laser_link
  observation_persistence: 0.0
  max_obstacle_height: 0.4
  min_obstacle_height: 0.0
  data_type: LaserScan
  topic: /robot/laser/scan
  marking: true    # 标记障碍物
  clearing: true   # 清除雷达可视自由区域

关键参数说明：

• footprint：机器人物理外轮廓，代价地图会严格避开该多边形区域
• inflation_radius：障碍物向外膨胀缓冲区，防止贴墙碰撞
• marking/clearing：雷达既标记障碍物，又清除视线内空白区域

6.2.2 global_costmap_params.yaml（全局代价地图）

用于全局长距离路径规划，基于静态地图，固定尺寸不跟随机器人移动

global_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  update_frequency: 1.0
  static_map: true          # 加载map_server静态地图
  rolling_window: false     # 不跟随机器人滑动窗口
  width: 20.0
  height: 20.0
  resolution: 0.05

6.2.3 local_costmap_params.yaml（局部代价地图）

用于机器人周边实时避障，滑动窗口随机器人同步移动，处理动态障碍物

local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true      # 窗口跟随机器人移动
  width: 5.0                # 机器人前后左右各2.5m窗口
  height: 5.0
  resolution: 0.02
  transform_tolerance: 0.5
  planner_frequency: 1.0

6.2.4 dwb_local_planner_params.yaml（ROS2标准局部规划器，替代TrajectoryPlannerROS）

控制机器人线速度、角速度、加速度、运动约束，差速非全向机器人配置：

DWBLocalPlanner:
  max_vel_x: 0.2
  min_vel_x: 0.05
  max_vel_theta: 0.15
  min_vel_theta: -0.15
  acc_lim_x: 2.5
  acc_lim_theta: 3.2
  holonomic_robot: false    # 差速轮非全向机器人
  sim_time: 1.0
  vx_samples: 20
  vtheta_samples: 30

• holonomic_robot: false：小车无法横向平移，仅前进后退转向；麦克纳姆轮设为true

6.3 一体化Python启动文件（gazebo+机器人+Nav2全套）

文件路径：chapter6_tutorials/launch/nav2_full.launch.py
替代ROS1 XML launch，整合仿真、机器人模型、地图、AMCL、代价地图、规划器、RViz2

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    pkg_tutorial = get_package_share_directory("chapter6_tutorials")
    pkg_gazebo = get_package_share_directory("gazebo_ros2")
    pkg_nav2 = get_package_share_directory("nav2_bringup")

    # 启动参数：机器人xacro模型、地图文件
    model_arg = DeclareLaunchArgument(
        "model",
        default_value=os.path.join(get_package_share_directory("robot1_description"),
                                   "urdf/robot1_base_04.xacro")
    )
    map_yaml = os.path.join(pkg_tutorial, "maps/map.yaml")

    # 1. 加载Gazebo室内仿真世界
    gazebo_world = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_gazebo, "launch/willowgarage_world.launch.py")
        ),
        launch_arguments={"use_sim_time": "true"}.items()
    )

    # 2. 机器人状态发布，解析xacro模型
    robot_state_pub = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description": Command(["xacro ", LaunchConfiguration("model")]),
                     "use_sim_time": True}]
    )

    # 3. Gazebo生成机器人实体
    spawn_robot = Node(
        package="gazebo_ros2",
        executable="spawn_entity.py",
        arguments=["-topic", "robot_description", "-entity", "robot", "-z", "0.1"],
        output="screen"
    )

    # 4. 地图服务器加载SLAM静态地图
    map_server = Node(
        package="map_server",
        executable="map_server",
        arguments=[map_yaml],
        parameters=[{"use_sim_time": True}]
    )

    # 5. AMCL2自适应蒙特卡洛定位（差速底盘）
    amcl_node = Node(
        package="nav2_amcl",
        executable="amcl",
        parameters=[
            os.path.join(pkg_tutorial, "launch/amcl_params.yaml"),
            {"use_sim_time": True}
        ]
    )

    # 6. Nav2核心规划+代价地图 bringup启动文件
    nav2_bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_nav2, "launch/navigation.launch.py")
        ),
        launch_arguments={
            "params_file": os.path.join(pkg_tutorial, "launch/nav2_params.yaml"),
            "use_sim_time": "true"
        }.items()
    )

    # 7. RViz2导航预设界面
    rviz2 = Node(
        package="rviz2",
        executable="rviz2",
        arguments=["-d", os.path.join(pkg_tutorial, "launch/navigation.rviz")]
    )

    return LaunchDescription([
        model_arg, gazebo_world, robot_state_pub, spawn_robot,
        map_server, amcl_node, nav2_bringup, rviz2
    ])

6.3.1 AMCL2核心参数 amcl_params.yaml

amcl:
  odom_model_type: diff
  transform_tolerance: 0.2
  gui_publish_rate: 10.0
  laser_max_beams: 30
  min_particles: 500
  max_particles: 5000
  kld_err: 0.05
  laser_model_type: likelihood_field
  laser_likelihood_max_dist: 2.0
  update_min_d: 0.2
  update_min_a: 0.5

• min/max_particles：粒子数量越多定位越准，CPU占用越高
• laser_model_type：likelihood_field室内地图匹配精度优于beam模型

6.3.2 完整启动命令

colcon build --packages-select chapter6_tutorials
source install/setup.bash
ros2 launch chapter6_tutorials nav2_full.launch.py

6.4 RViz2导航全套可视化插件使用

6.4.1 基础界面总览

启动后RViz2左侧Displays面板加载全部导航可视化组件，正常节点全部显示绿色OK，异常红色告警。
配图对应：ros2_ch6_rviz_overview

6.4.2 2D Pose Estimate（快捷键P，初始化机器人定位）

功能：手动给定机器人初始位姿，AMCL粒子云收敛定位
操作：点击工具，在地图拖拽箭头指向机器人真实朝向
话题：/initialpose 消息类型 geometry_msgs/msg/PoseWithCovarianceStamped
配图：ros2_ch6_pose_estimate

6.4.3 2D Nav Goal（快捷键G，下发自主导航目标）

功能：指定终点坐标与朝向，Nav2自动规划路径、避障行驶
话题：/move_base_simple/goal 消息 geometry_msgs/msg/PoseStamped
配图：ros2_ch6_nav_goal

6.4.4 Static Map静态地图

显示gmapping保存的室内栅格地图，黑色=障碍物，白色=可通行，灰色=未知
话题：/map nav_msgs/msg/OccupancyGrid
配图：ros2_ch6_static_map

6.4.5 Particle Cloud粒子云（AMCL定位置信度）

红色箭头粒子集合：

• 粒子分散：定位不确定性高
• 粒子聚拢：定位精准
  配图：ros2_ch6_particle_cloud

6.4.6 Robot Footprint机器人轮廓

红色多边形框，匹配costmap_common中footprint参数，直观查看安全边界
话题：/local_costmap/robot_footprint
配图：ros2_ch6_footprint

6.4.7 Local Costmap局部代价地图

彩色栅格：黄色=障碍物，蓝色=膨胀安全区，机器人周边5m滑动窗口，实时检测动态障碍
配图：ros2_ch6_local_costmap

6.4.8 Global Costmap全局代价地图

整张静态地图代价图层，用于长距离全局路径规划
配图：ros2_ch6_global_costmap

6.4.9 路径可视化

• Global Plan：绿色长线，全局最优路径
• Local Plan：蓝色短线，机器人当前局部跟踪轨迹
• Current Goal：红色箭头，导航终点位姿
  配图：ros2_ch6_global_path、ros2_ch6_local_path

6.5 rqt_graph导航全节点通信拓扑

完整数据流：
teleop_twist_keyboard / nav2_controller → /cmd_vel → Gazebo差速驱动
gazebo → /odom → AMCL2 / costmap
map_server → /map → Nav2规划器
/scan激光数据 → 局部/全局代价地图
amcl输出全局位姿 → 路径规划
配图：ros2_ch6_nav_rqt_graph

6.6 动态参数调优 rqt_reconfigure

无需重启节点，在线修改AMCL、DWB规划器、代价地图全部参数
启动工具：

ros2 run rqt_reconfigure rqt_reconfigure

可调节参数：最大速度、粒子数量、障碍物膨胀半径、更新频率等
配图：ros2_ch6_rqt_reconf

6.7 动态障碍物避障实操

1. Gazebo界面Insert Model插入立方体箱子，放置机器人预设路径中间
2. RViz2局部代价地图实时更新障碍物彩色区域
3. Nav2自动重新生成全局+局部路径，绕开障碍行驶
   对比图：无障碍直线路径 / 有障碍绕行路径
   配图：ros2_ch6_obstacle_avoid

6.8 C++ Action客户端：程序自动下发导航目标

文件 chapter6_tutorials/src/send_goal.cpp，ROS2 nav2_msgs Action接口（替换ROS1 move_base_msgs）

#include <rclcpp/rclcpp.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

using NavigateToPose = nav2_msgs::action::NavigateToPose;
using ClientGoalHandle = rclcpp_action::ClientGoalHandle<NavigateToPose>;

class NavGoalSender : public rclcpp::Node
{
public:
  NavGoalSender() : Node("nav_goal_client")
  {
    client_ = rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose");
  }
  void send_goal(double x, double y, double yaw)
  {
    if (!client_->wait_for_action_server(std::chrono::seconds(5))) {
      RCLCPP_ERROR(get_logger(), "Nav2 Action服务器未启动");
      return;
    }
    NavigateToPose::Goal goal;
    goal.pose.header.frame_id = "map";
    goal.pose.header.stamp = this->get_clock()->now();
    goal.pose.pose.position.x = x;
    goal.pose.pose.position.y = y;
    goal.pose.pose.orientation.w = cos(yaw/2);
    goal.pose.pose.orientation.z = sin(yaw/2);
    auto send_future = client_->async_send_goal(goal);
  }
private:
  rclcpp_action::Client<NavigateToPose>::SharedPtr client_;
};

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<NavGoalSender>();
  // 发送目标点 x=1.0 y=1.0 朝向0度
  node->send_goal(1.0, 1.0, 0.0);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

CMakeLists编译配置：

find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(nav2_msgs REQUIRED)
add_executable(send_goal src/send_goal.cpp)
ament_target_dependencies(send_goal rclcpp rclcpp_action nav2_msgs)
install(TARGETS send_goal DESTINATION lib/${PROJECT_NAME})

运行：

ros2 run chapter6_tutorials send_goal

执行后机器人自动驶向设定坐标，终端打印到达/失败状态。
配图：ros2_ch6_code_goal

本章小结

1 Nav2拆分全局/局部代价地图，分别处理远距离全局规划与周边实时避障，footprint与inflation_radius是安全导航核心参数；
2 DWB局部规划器替代ROS1 TrajectoryPlanner，精准控制差速机器人速度、加速度运动约束；
3 AMCL2粒子滤波依靠激光扫描匹配已知地图实现全局定位，粒子数量平衡精度与算力；
4 RViz2全套可视化组件完整覆盖地图、定位、代价地图、规划路径，是导航调试核心工具；
5 支持手动交互下发目标与代码Action客户端自动多点巡航两种使用模式；
6 仿真中动态障碍物可实时触发重规划，适配室内人流、临时杂物真实场景；
7 rqt_reconfigure支持不重启在线调参，大幅缩短导航参数调试周期。

下一章预告：MoveIt2机械臂运动规划、机械臂URDF建模、碰撞检测、轨迹规划、Gazebo机械臂仿真实操。