## Navigation2单点导航避障

### 1、程序功能说明

运行程序后，rviz中会加载地图。在rviz界面中，用【2D Pose Estimate】工具给定小车初始位姿，然后用【2D Goal Pose】工具给定小车一个目标点。小车结合自身环境，会规划出一条路径并且根据规划的路径移动到目的地，期间如果遇到障碍物，会自助避障，到达目的地后停车。

### 2、Navigation2简介

#### 2.1 简介

Navigation2整体架构图

![](/media/202512/faf3a321d8e64cf180a2dea3391d25b66285.png)

Navigation2具有下列工具：

*   加载、提供和存储地图的工具（地图服务器Map Server）
    
*   在地图上定位机器人的工具 (AMCL)
    
*   避开障碍物从A点移动到B点的路径规划工具（Nav2 Planner）
    
*   跟随路径过程中控制机器人的工具（Nav2 Controller）
    
*   将传感器数据转换为机器人世界中的成本地图表达的工具（Nav2 Costmap 2D）
    
*   使用行为树构建复杂机器人行为的工具（Nav2 行为树和BT Navigator）
    
*   发生故障时计算恢复行为的工具（Nav2 Recoveries）
    
*   跟随顺序航点的工具（Nav2 Waypoint Follower）
    
*   管理服务器生命周期的工具和看门狗(Nav2 Lifecycle Manager)
    
*   启用用户自定义算法和行为的插件（Nav2 Core）

Navigation 2（Nav 2）是ROS 2中自带的导航框架，其目的是能够通过一种安全的方式使移动机器人从A点移动到B点。所以，Nav 2可以完成动态路径规划、计算电机速度、避开障碍物和恢复结构等行为。

Nav 2使用行为树（BT，Behavior Trees）调用模块化服务器来完成一个动作。动作可以是计算路径、控制工作（control efforts）、恢复或其他与导航相关的动作。这些动作都是通过动作服务器与行为树（BT）进行通信的独立节点。

#### 2.2 相关资料

Navigation2 文档：[https://navigation.ros.org/index.html](https://navigation.ros.org/index.html)

Navigation2 github：[https://github.com/ros-planning/navigation2](https://github.com/ros-planning/navigation2)

Navigation2 对应的论文：[https://arxiv.org/pdf/2003.00368.pdf](https://arxiv.org/pdf/2003.00368.pdf)

### 3、运行案例

#### 3.1 使用前说明

本节课程以树莓派5为例。树莓派5和Jetson-nano主板，需要在宿主机中打开终端，然后输入进入docker容器的指令，进入到docker容器后的终端里边输入本节课程提及的指令，宿主机进入docker容器的教程可以参考本产品教程**【机器人配置与操作指引】中的【5、进入小车Docker（Jetson-Nano和树莓派5用户看这里）】**的内容。RDKX5、Orin主板直接打开终端，输入本节课程提及的指令即可。

#### 3.2 单点导航功能

**注意：**

*   **jetson nano、树莓派**系列主控需要先进入docker容器**（步骤请参见【Docker课程】--- 【4、Docker启动脚本】）**
    
    下面所有指令的docker终端都需要进入同一个docker容器后执行**（步骤请参见【Docker课程】--- 【3、Docker提交与多终端进入】）**
    
*   本章节课程需要至少已经存在一张已经建好的地图，参考**【5、gmapping建图】、【6、cartographer建图】、【7、slam\_toolx建图】**任意一节SLAM建图课程

机器人车机终端启动底层传感器命令：
`
ros2 launch yahboomcar_nav laser_bringup_launch.py
`

![](/media/202512/cb4f62ee1046435ab5babd5cf41df9ae5105.png)

输入指令启动rviz可视化建图 （rviz可视化功能在车机端或虚拟机端启动都可以，**任选一种**方式启动，禁止虚拟机端和车机端重复启动：），下图是在车机端输入，
`
ros2 launch yahboomcar_nav display_nav_launch.py
`

![](/media/202512/07efeaa26da44b318d6b9e63c28372c22508.png)

此时还没有显示地图加载，因为还没有启动导航的程序，所以没有地图加载。接下来运行导航节点，终端输入

```
#导航算法二选一
#teb
ros2 launch yahboomcar_nav navigation_teb_launch.py 
#dwa
ros2 launch yahboomcar_nav navigation_dwa_launch.py 
```

![](/media/202512/52b7efcdfa6448998956f6487bbd6d973419.png)
此时可以看到地图加载进去了，然后我们点击【2D Pose Estimate】，给小车设置初始位姿，根据小车在实际环境中的位置，在rviz中用鼠标点击拖动，小车模型移动我们设置的位置。如下图所示，雷达扫描的区域与实际障碍物大致重合则表示位姿准确。位姿初始化完成后，rviz界面中会出现机器人模型和膨胀区域

![](/media/202512/0927003659374e21be3ea3c36f242d4d8612.png)
单点导航，点击【2D Goal Pose】工具，然后在rviz中使用鼠标选择一个目标点和目标朝向，然后释放

![](/media/202512/2295f3d575514d6c93ff66f6475aebe51054.png)
小车结合周围的情况，规划出一条路径并且沿着路径移动到目标点。

![](/media/202512/37386b618a30459fa33b7509a1834afd8115.png)
机器人成功抵达目标点后，车机终端会提示 Goal succeeded,代表导航成功

![](/media/202512/515f3229c57f476d8339543266a942605273.png)

### 4、查看节点通讯图
终端输入：`ros2 run rqt_graph rqt_graph`
如果一开始没有显示，选择【Nodes/Topics(all)】，然后点击左上角的刷新按钮，原图尺寸过大，可以在本节课程文件夹下查看

![](/media/202512/3a2f41d6458e42f68ab7dcfbac7ae1156625.png)

### 5、查看TF树
终端输入：`ros2 run rqt_tf_tree rqt_tf_tree`
如果一开始没有显示，点击左上角的刷新符号刷新页面，原图尺寸过大，可以在本节课程文件夹下查看

![](/media/202512/fd6791c83e3a4ccb9dac1659b739973a1484.png)

### 6、导航讲解

#### 6.1 导航流程核心步骤

##### 6.1.1 阶段 1：系统初始化与地图加载

*   **地图获取**
    
*   从`map_server`加载预构建的栅格地图
    
*   地图包含静态障碍物信息，作为导航的基础环境模型。

**代价地图初始化**

*   **全局代价地图（Global Costmap）**：基于静态地图，用于全局路径规划。
    
*   **局部代价地图（Local Costmap）**：实时融合激光雷达的`scan`话题用于动态障碍物避障。

##### 6.1.2 阶段 2：传感器数据处理与环境感知

**传感器数据接入**

*   激光雷达（`/scan`）、IMU（`/imu/data`）、里程计（`/odom`）数据通过 ROS 话题输入。
*   
**代价地图更新**

*   局部代价地图实时更新动态障碍物信息，每个栅格根据传感器数据计算占用概率和代价（如距离障碍物越近，代价越高）。
    
*   膨胀处理（Inflation）：将障碍物边缘扩展，避免机器人过于靠近障碍物。
*   
##### 6.1.3 阶段 3：路径规划（全局规划与局部规划）

**全局路径规划**

*   输入：起点（机器人当前位姿）、终点（目标位姿）、全局代价地图。
    
*   规划器算法Dijkstra生成从起点到终点的最优路径（一系列离散的坐标点）。

**局部路径规划与跟踪**

局部规划器DWBLocalPlanner根据全局路径和局部代价地图，生成机器人可执行的短时间内的控制指令。

##### 6.1.4 阶段 4：控制执行与行为决策

**控制器输出**

*   控制器将局部路径转换为机器人的线速度（`linear.x`）和角速度（`angular.z`），通过`/cmd_vel`话题发布。
    
*   速度平滑处理：避免机器人运动突变，提高稳定性。

**行为树决策流程**

行为树定义导航任务的优先级和状态转移逻辑，典型流程：

1.  **检查目标是否可达**：若全局规划失败，触发 “恢复行为”（如重新规划）。
    
2.  **执行局部避障**：当局部代价地图检测到障碍物时，暂时偏离全局路径。
    
3.  **到达目标**：当机器人位姿与目标位姿误差小于阈值时，任务完成。

**恢复行为机制**

当导航遇到异常（如路径被完全阻塞），触发预设的恢复策略（如旋转搜索新路径、后退重新规划）。

#### 6.2 关键技术原理

##### 6.2.1 代价地图

*   **概率栅格表示**：每个栅格存储占用概率（0-1），通过传感器数据更新（如激光雷达的射线检测）。
    
*   **多层代价叠加**：静态代价（地图障碍物）+ 动态代价（实时传感器检测的障碍物）+ 膨胀代价（安全距离）。

Costmap是机器人收集传感器信息建立和更新的二维或三维地图，可以从下图简要了解。

![](/media/202512/c7d93b705c714e4084e554fb5e44cc629656.png)

上图中，红色部分代表costmap中的障碍物，蓝色部分表示通过机器人内切圆半径膨胀出的障碍，红色多边形是footprint(机器人轮廓的垂直投影)。为了避免碰撞，footprint不应该和红色部分有交叉，机器人中心不应该与蓝色部分有交叉。 ROS的代价地图（costmap）采用网格（grid）形式，每个网格的值（cell cost）从0~255。分成三种状态：被占用（有障碍）、自由区域（无障碍）、未知区域； 具体状态和值对应有下图：

上图可分为五部分，其中红色多边形区域为机器人的轮廓：

*   Lethal（致命的）:机器人的中心与该网格的中心重合，此时机器人必然与障碍物冲突。
    
*   Inscribed（内切）：网格的外切圆与机器人的轮廓内切，此时机器人也必然与障碍物冲突。
    
*   Possibly circumscribed（外切）：网格的外切圆与机器人的轮廓外切，此时机器人相当于靠在障碍物附近，所以不一定冲突。
    
*   Freespace（自由空间）：没有障碍物的空间。
    
*   Unknown（未知）：未知的空间。

全局代价地图：

![](/media/202512/5f7b42f6631d4755803545f7991b07c47971.png)

局部代价地图：

![](/media/202512/3a70deb558f84b7e9cfd83e743365f237369.png)

##### 6.2.2 AMCL定位算法

AMCL(自适应蒙特卡洛定位) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。

如下图所示，如果里程计没有误差，完美的情况下，我们可以直接使用里程计信息（上半图）推算出机器人（base\_frame）相对里程计坐标系的位置。但现实情况，里程计存在漂移以及无法忽略的累计误差，所以AMCL采用下半图的方法，即先根据里程计信息初步定位base\_frame，然后通过测量模型得到base\_frame相对于map\_frame（全局地图坐标系），也就知道了机器人在地图中的位姿。（注意，这里虽然估计的是base到map的转换，但最后发布的是map到odom的转换，可以理解为里程计的漂移。）

![image-20250813104236605](/media/202512/7164e9b716fe4eb3b0b110dbc8fd8dfd9642.png)

在机器人导航时的TF树中，其中map（地图坐标系）—odom（里程计坐标系）之间坐标变换由AMCL定位算法发布，而odom（里程计坐标系）—besel_footprint（机器人底盘中心点投影坐标系）之间的坐标变换由**robot\_localization**功能包下的**ekf_filter_node**节点发布，其作用是发布IMU传感器和编码器原始里程融合后的里程计信息。

![image-20250813104411811](/media/202512/bf3e01c8bb88465cbbb1a7057d5b55be2619.png)

在前边查看节点通信图中，我们可找到关于ekf_filter_node的通信数据流

其中/driver_node节点为机器人底盘节点，发布了**/imu/data_raw**(IMU传感器原始数据)、**/odom_raw**（编码器原始里程计数据）

imu_filter节点订阅了机器人底盘发布的**/imu/data_raw**(IMU传感器原始数据)，imu_filter节点对IMU传感器原始数据进行滤波后发布/imu/data话题

**ekf_filter_node**节点订阅了/**odom**话题和**/imu/data**话题数据，将多传感器数据融合后发布**/odom**话题

![image-20250813104848585](/media/202512/ce17917fd36e47c2acf17cd5eee68ee82537.png)

#### 6.3 路径规划

##### 6.3.1 全局路径规划

全局路径规划作用是在已知环境地图中，根据起点和终点计算一条最优或可行的全局路径，不考虑实时障碍物，以全局最优为目标，计算从起点到终点的路径，如下图所示，箭头连成的路径即为全局路径

![image-20250813105347557](/media/202512/0aab94d69da74c8190b5feebe6c07e729607.png)

##### 6.3.2 局部路径规划

局部路径规划作用是在机器人运动过程中，根据实时激光雷达传感器数据动态调整路径，避开障碍物（障碍物会在代价地图中显示）。如下图所是，蓝色路径为局部路径。

![image-20250813105609389](/media/202512/ea22da4dffa048a7a58c542ffba42c9c5955.png)

#### 6.4 导航参数配置

**jetson nano、树莓派需要先进入docker**

配置文件路径：
`
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_nav/params/teb_nav_params.yaml
`
TEB算法导航功能默认配置参数如下：

```
amcl:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间
    alpha1: 0.1          # 里程计旋转误差模型参数
    alpha2: 0.1          # 里程计平移误差模型参数
    alpha3: 0.3          # 里程计平移误差模型参数
    alpha4: 0.3          # 里程计旋转误差模型参数
    alpha5: 0.1          # 里程计旋转误差模型参数
    base_frame_id: "base_footprint"  # 机器人基座坐标系
    beam_skip_distance: 0.5          # 光束跳过距离阈值
    beam_skip_error_threshold: 0.9   # 光束跳过误差阈值
    beam_skip_threshold: 0.3         # 光束跳过阈值
    do_beamskip: false               # 是否启用光束跳过
    global_frame_id: "map"            # 全局坐标系
    lambda_short: 0.1                # 激光模型短距离权重
    laser_likelihood_max_dist: 2.0   # 激光似然场最大距离
    laser_max_range: 100.0           # 激光最大有效距离
    laser_min_range: -1.0            # 激光最小有效距离
    laser_model_type: "likelihood_field"  # 激光模型类型（似然场）
    max_beams: 50                    # 每次处理的最大激光束数
    max_particles: 4000              # 最大粒子数
    min_particles: 1000              # 最小粒子数
    odom_frame_id: "odom"            # 里程计坐标系
    pf_err: 0.05                     # 粒子滤波器误差阈值
    pf_z: 0.99                        # 粒子滤波器权重指数
    recovery_alpha_fast: 0.0          # 快速恢复参数
    recovery_alpha_slow: 0.0          # 慢速恢复参数
    resample_interval: 1             # 重采样间隔
    robot_model_type: "nav2_amcl::DifferentialMotionModel"  # 机器人运动模型
    save_pose_rate: 0.5              # 保存位姿的速率
    sigma_hit: 0.2                   # 激光模型标准差
    tf_broadcast: true               # 是否广播TF变换
    transform_tolerance: 1.0          # TF变换容差
    update_min_a: 0.15               # 最小角度变化触发更新
    update_min_d: 0.15               # 最小距离变化触发更新
    z_hit: 0.5                       # 激光模型命中权重
    z_max: 0.05                      # 激光模型最大距离权重
    z_rand: 0.5                      # 激光模型随机权重
    z_short: 0.05                    # 激光模型短距离权重
    scan_topic: scan                 # 激光扫描话题

amcl_map_client:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间

bt_navigator:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间
    global_frame: map    # 全局坐标系
    robot_base_frame: base_footprint  # 机器人基座坐标系
    odom_topic: /odom   # 里程计话题
    plugin_lib_names:   # 行为树插件库列表
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      # ...（其他行为树节点插件）

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间

controller_server:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间
    controller_frequency: 30.0  # 控制器频率
    min_x_velocity_threshold: 0.001  # X方向最小速度阈值
    min_y_velocity_threshold: 0.0    # Y方向最小速度阈值
    min_theta_velocity_threshold: 0.001  # 旋转最小速度阈值
    progress_checker_plugin: "progress_checker"  # 进度检查器插件
    goal_checker_plugin: "goal_checker"          # 目标检查器插件
    controller_plugins: ["FollowPath"]           # 控制器插件列表
    # 进度检查器参数
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"  # 简单进度检查器
      required_movement_radius: 0.3  # 要求移动半径
      movement_time_allowance: 3.5   # 移动时间容限
    # 目标检查器参数
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"  # 简单目标检查器
      xy_goal_tolerance: 0.25  # XY位置容差
      yaw_goal_tolerance: 0.25  # 航向角容差
      stateful: True           # 是否保持状态
    # TEB局部规划器参数
    FollowPath:
      plugin: "teb_local_planner::TebLocalPlannerROS"  # TEB局部规划器
      footprint_model.type: circular  # 机器人足迹模型（圆形）
      footprint_model.radius: 0.1     # 机器人半径
      min_obstacle_dist: 0.1          # 最小障碍物距离
      inflation_dist: 0.1             # 膨胀距离
      costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"  # 代价地图转换插件
      costmap_converter_spin_thread: True  # 转换器是否使用独立线程
      costmap_converter_rate: 20      # 转换器频率
      enable_homotopy_class_planning: True  # 启用同伦类规划
      enable_multithreading: True     # 启用多线程
      optimization_verbose: False    # 优化过程是否详细输出
      teb_autoresize: True           # 自动调整轨迹点数量
      min_samples: 3                 # 最小轨迹点数
      max_samples: 20                 # 最大轨迹点数
      max_global_plan_lookahead_dist: 1.0  # 全局规划前瞻距离
      visualize_hc_graph: True        # 是否可视化同伦图
      max_vel_x: 0.26                 # 最大线速度
      min_vel_x: -0.26                # 最小线速度（后退）
      max_vel_theta: 3.0              # 最大角速度
      min_vel_theta: -3.0             # 最小角速度
      acc_lim_x: 2.5                  # X方向加速度限制
      acc_lim_theta: 3.2              # 旋转加速度限制

costmap_converter:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间

local_costmap:
  local_costmap:
    ros__parameters:
      use_sim_time: False  # 是否使用仿真时间
      update_frequency: 5.0  # 更新频率
      publish_frequency: 2.0  # 发布频率
      global_frame: odom      # 全局坐标系
      robot_base_frame: base_footprint  # 机器人基座坐标系
      rolling_window: true     # 是否使用滚动窗口
      width: 3                 # 代价地图宽度（米）
      height: 3                # 代价地图高度（米）
      resolution: 0.05          # 分辨率（米/像素）
      robot_radius: 0.1         # 机器人半径
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]  # 图层插件
      inflation_layer:          # 膨胀层参数
        plugin: "nav2_costmap_2d::InflationLayer"  # 膨胀层插件
        cost_scaling_factor: 5.0  # 代价缩放因子
        inflation_radius: 0.11    # 膨胀半径
      obstacle_layer:           # 障碍物层参数
        plugin: "nav2_costmap_2d::ObstacleLayer"  # 障碍物层插件
        enabled: True           # 是否启用
        observation_sources: scan  # 观测源（激光）
        scan:                   # 激光参数
          topic: /scan           # 激光话题
          max_obstacle_height: 2.0  # 最大障碍物高度
          obstacle_min_range: 0.1    # 最小障碍物距离
          clearing: True         # 是否用于清除空间
          marking: True          # 是否用于标记障碍
          data_type: "LaserScan" # 数据类型
      voxel_layer:              # 体素层参数
        plugin: "nav2_costmap_2d::VoxelLayer"  # 体素层插件
        enabled: True           # 是否启用
        publish_voxel_map: True # 是否发布体素地图
        origin_z: 0.0           # Z轴原点
        z_resolution: 0.2       # Z轴分辨率
        z_voxels: 10            # Z轴体素数
        max_obstacle_height: 2.0  # 最大障碍物高度
        mark_threshold: 0       # 标记阈值
        observation_sources: pointcloud  # 观测源（点云）
        pointcloud:              # 点云参数
          topic: /intel_realsense_r200_depth/points  # 点云话题
          max_obstacle_height: 2.0  # 最大障碍物高度
          clearing: True         # 是否用于清除空间
          marking: True          # 是否用于标记障碍
          data_type: "PointCloud2"  # 数据类型
      static_layer:             # 静态层参数
        map_subscribe_transient_local: True  # 使用瞬态本地订阅
      always_send_full_costmap: True  # 总是发送完整代价地图

local_costmap_client:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间

local_costmap_rclcpp_node:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间

global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: False  # 是否使用仿真时间
      footprint_padding: 0.03  # 足迹填充
      update_frequency: 1.0    # 更新频率
      publish_frequency: 1.0    # 发布频率
      global_frame: map         # 全局坐标系
      robot_base_frame: base_footprint  # 机器人基座坐标系
      robot_radius: 0.1        # 机器人半径
      resolution: 0.05          # 分辨率
      plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]  # 图层插件
      obstacle_layer:           # 障碍物层参数
        plugin: "nav2_costmap_2d::ObstacleLayer"  # 障碍物层插件
        enabled: True           # 是否启用
        observation_sources: scan  # 观测源（激光）
        footprint_clearing_enabled: true  # 是否启用足迹清除
        max_obstacle_height: 2.0  # 最大障碍物高度
        combination_method: 1    # 组合方法
        scan:                   # 激光参数
          topic: /scan           # 激光话题
          obstacle_range: 3.0    # 障碍物检测范围
          obstacle_min_range: 0.3  # 最小障碍物距离
          raytrace_range: 4.0    # 光线追踪范围
          max_obstacle_height: 1.0  # 最大障碍物高度
          min_obstacle_height: 0.0  # 最小障碍物高度
          clearing: True         # 是否用于清除空间
          marking: True          # 是否用于标记障碍
          data_type: "LaserScan" # 数据类型
          inf_is_valid: false    # 无限距离是否有效
      voxel_layer:              # 体素层参数
        plugin: "nav2_costmap_2d::VoxelLayer"  # 体素层插件
        enabled: True           # 是否启用
        footprint_clearing_enabled: true  # 是否启用足迹清除
        max_obstacle_height: 2.0  # 最大障碍物高度
        publish_voxel_map: True # 是否发布体素地图
        origin_z: 0.0           # Z轴原点
        z_resolution: 0.05      # Z轴分辨率
        z_voxels: 16            # Z轴体素数
        unknown_threshold: 15   # 未知区域阈值
        mark_threshold: 0       # 标记阈值
        observation_sources: pointcloud  # 观测源（点云）
        combination_method: 1    # 组合方法
        pointcloud:              # 点云参数
          topic: /intel_realsense_r200_depth/points  # 点云话题
          max_obstacle_height: 2.0  # 最大障碍物高度
          min_obstacle_height: 0.0  # 最小障碍物高度
          obstacle_range: 2.5     # 障碍物检测范围
          raytrace_range: 3.0     # 光线追踪范围
          clearing: True         # 是否用于清除空间
          marking: True          # 是否用于标记障碍
          data_type: "PointCloud2"  # 数据类型
      static_layer:             # 静态层参数
        plugin: "nav2_costmap_2d::StaticLayer"  # 静态层插件
        map_subscribe_transient_local: True  # 使用瞬态本地订阅
        enabled: true            # 是否启用
        subscribe_to_updates: true  # 是否订阅更新
        transform_tolerance: 0.1  # 变换容差
      inflation_layer:          # 膨胀层参数
        plugin: "nav2_costmap_2d::InflationLayer"  # 膨胀层插件
        enabled: true            # 是否启用
        cost_scaling_factor: 5.0  # 代价缩放因子
        inflation_radius: 0.11    # 膨胀半径
        inflate_unknown: false   # 是否膨胀未知区域
        inflate_around_unknown: true  # 是否在未知区域周围膨胀
      always_send_full_costmap: True  # 总是发送完整代价地图

global_costmap_client:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间
  
global_costmap_rclcpp_node:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间

map_server:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间
    yaml_filename: "yahboomcar.yaml"  # 地图YAML文件路径

map_saver:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间
    save_map_timeout: 5.0  # 保存地图超时时间
    free_thresh_default: 0.25  # 空闲阈值默认值
    occupied_thresh_default: 0.65  # 占用阈值默认值
    map_subscribe_transient_local: True  # 使用瞬态本地订阅

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0  # 期望规划器频率
    use_sim_time: False  # 是否使用仿真时间
    planner_plugins: ["GridBased"]  # 规划器插件列表
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"  # Navfn规划器
      tolerance: 0.5  # 目标点容差
      use_astar: false  # 是否使用A*算法
      allow_unknown: true  # 是否允许未知区域

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间

behavior_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw  # 代价地图话题
    footprint_topic: local_costmap/published_footprint  # 足迹话题
    cycle_frequency: 10.0  # 行为执行频率
    behavior_plugins: ["backup"]  # 行为插件列表
    backup:  # 后退行为参数
      plugin: "nav2_behaviors/BackUp"  # 后退行为插件
      backup_speed: -1.0  # 后退速度
      backup_duration: 1.5  # 后退持续时间
      simulate_ahead_time: 2.0  # 模拟提前时间
    global_frame: odom  # 全局坐标系
    robot_base_frame: base_footprint  # 机器人基座坐标系
    transform_timeout: 0.1  # 变换超时
    use_sim_time: False  # 是否使用仿真时间
    simulate_ahead_time: 2.0  # 模拟提前时间
    max_rotational_vel: 1.0  # 最大旋转速度
    min_rotational_vel: 0.4  # 最小旋转速度
    rotational_acc_lim: 3.2  # 旋转加速度限制

robot_state_publisher:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间

waypoint_follower:
  ros__parameters:
    loop_rate: 20  # 循环频率
    stop_on_failure: false  # 失败时是否停止
    waypoint_task_executor_plugin: "wait_at_waypoint"  # 航点任务执行器插件
    wait_at_waypoint:  # 航点等待参数
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"  # 航点等待插件
      enabled: True  # 是否启用
      waypoint_pause_duration: 200  # 航点暂停时间（毫秒）
dwa算法导航功能默认配置参数如下：

amcl:
  ros__parameters:
    use_sim_time: False  # 是否使用仿真时间 (Gazebo等)
    alpha1: 0.2          # 运动模型中的平移噪声参数 (x方向)
    alpha2: 0.2          # 运动模型中的平移噪声参数 (y方向)
    alpha3: 0.2          # 运动模型中的旋转噪声参数
    alpha4: 0.2          # 运动模型中的平移噪声参数 (末端旋转)
    alpha5: 0.2          # 运动模型中的旋转噪声参数 (末端旋转)
    base_frame_id: "base_footprint"  # 机器人基座坐标系
    beam_skip_distance: 0.5          # 光束跳过距离(米)
    beam_skip_error_threshold: 0.9   # 光束跳过错误阈值
    beam_skip_threshold: 0.3          # 光束跳过阈值
    do_beamskip: false                # 是否启用光束跳过功能
    global_frame_id: "map"            # 全局地图坐标系
    lambda_short: 0.1                 # 短距离测量的权重
    laser_likelihood_max_dist: 2.0    # 激光似然场最大距离(米)
    laser_max_range: 100.0            # 激光最大测距范围(米)
    laser_min_range: -1.0             # 激光最小测距范围(米)
    laser_model_type: "likelihood_field"  # 激光模型类型 (似然场模型)
    max_beams: 50                     # 每次更新使用的最大激光束数
    max_particles: 4000               # 最大粒子数
    min_particles: 1000               # 最小粒子数
    odom_frame_id: "odom"             # 里程计坐标系
    pf_err: 0.05                      # 粒子滤波器错误阈值
    pf_z: 0.99                        # 粒子滤波器z值
    recovery_alpha_fast: 0.0          # 快速恢复alpha值
    recovery_alpha_slow: 0.0          # 慢速恢复alpha值
    resample_interval: 1              # 重采样间隔
    robot_model_type: "nav2_amcl::DifferentialMotionModel"  # 机器人运动模型 (差速驱动)
    save_pose_rate: 0.5               # 保存位姿的速率(Hz)
    sigma_hit: 0.2                    # 激光测量噪声标准差
    tf_broadcast: true                 # 是否广播TF变换
    transform_tolerance: 1.0          # TF变换容忍时间(秒)
    update_min_a: 0.2                 # 最小角度变化触发更新(弧度)
    update_min_d: 0.25                # 最小距离变化触发更新(米)
    z_hit: 0.5                        # 命中测量的权重
    z_max: 0.05                       # 最大范围测量的权重
    z_rand: 0.5                       # 随机测量的权重
    z_short: 0.05                     # 短距离测量的权重
    scan_topic: scan                  # 激光扫描数据的话题名称
    
    # 初始位置设置 (示例，当前被注释)
    # set_initial_pose: true
    # initial_pose:
    #   x: 0.0
    #   y: 0.0
    #   z: 0.0
    #   yaw: 0.0

# AMCL相关组件的参数
amcl_map_client:
  ros__parameters:
    use_sim_time: False

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: False

# 行为树导航器配置
bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map                  # 全局坐标系
    robot_base_frame: base_footprint   # 机器人基座坐标系
    odom_topic: /odom                  # 里程计数据的话题
    default_bt_xml_filename: "navigate_to_pose_w_replanning_and_recovery.xml"  # 默认行为树文件
    plugin_lib_names:                  # 行为树插件库列表
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      - nav2_smooth_path_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_assisted_teleop_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_drive_on_heading_bt_node
      - nav2_clear_costmap_service_bt_node
      - nav2_is_stuck_condition_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_goal_updated_condition_bt_node
      - nav2_globally_updated_goal_condition_bt_node
      - nav2_is_path_valid_condition_bt_node
      - nav2_initial_pose_received_condition_bt_node
      - nav2_reinitialize_global_localization_service_bt_node
      - nav2_rate_controller_bt_node
      - nav2_distance_controller_bt_node
      - nav2_speed_controller_bt_node
      - nav2_truncate_path_action_bt_node
      - nav2_truncate_path_local_action_bt_node
      - nav2_goal_updater_node_bt_node
      - nav2_recovery_node_bt_node
      - nav2_pipeline_sequence_bt_node
      - nav2_round_robin_node_bt_node
      - nav2_transform_available_condition_bt_node
      - nav2_time_expired_condition_bt_node
      - nav2_path_expiring_timer_condition
      - nav2_distance_traveled_condition_bt_node
      - nav2_single_trigger_bt_node
      - nav2_goal_updated_controller_bt_node
      - nav2_is_battery_low_condition_bt_node
      - nav2_navigate_through_poses_action_bt_node
      - nav2_navigate_to_pose_action_bt_node
      - nav2_remove_passed_goals_action_bt_node
      - nav2_planner_selector_bt_node
      - nav2_controller_selector_bt_node
      - nav2_goal_checker_selector_bt_node
      - nav2_controller_cancel_bt_node
      - nav2_path_longer_on_approach_bt_node
      - nav2_wait_cancel_bt_node
      - nav2_spin_cancel_bt_node
      - nav2_back_up_cancel_bt_node
      - nav2_assisted_teleop_cancel_bt_node
      - nav2_drive_on_heading_cancel_bt_node
      - nav2_is_battery_charging_condition_bt_node

# 行为树导航器节点参数
bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: False

# 控制器服务器配置
controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 10.0        # 控制器运行频率(Hz)
    min_x_velocity_threshold: 0.001   # X方向最小速度阈值(m/s)
    min_y_velocity_threshold: 0.5      # Y方向最小速度阈值(m/s)
    min_theta_velocity_threshold: 0.001  # 旋转最小速度阈值(rad/s)
    controller_plugins: ["FollowPath"]  # 使用的控制器插件列表

# DWB控制器参数
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"  # 控制器类型
      debug_trajectory_details: True    # 是否调试轨迹细节
      min_vel_x: 0.0                    # 最小X方向速度(m/s)
      min_vel_y: 0.0                    # 最小Y方向速度(m/s)
      max_vel_x: 0.3                    # 最大X方向速度(m/s)
      max_vel_y: 0.0                    # 最大Y方向速度(m/s)
      max_vel_theta: 0.6                # 最大旋转速度(rad/s)
      min_speed_xy: 0.0                 # 最小XY平面速度(m/s)
      max_speed_xy: 0.3                 # 最大XY平面速度(m/s)
      min_speed_theta: 0.0              # 最小旋转速度(rad/s)
      acc_lim_x: 0.25                   # X方向加速度限制(m/s²)
      acc_lim_y: 0.0                    # Y方向加速度限制(m/s²)
      acc_lim_theta: 0.4                # 旋转加速度限制(rad/s²)
      decel_lim_x: -0.25                # X方向减速度限制(m/s²)
      decel_lim_y: 0.0                  # Y方向减速度限制(m/s²)
      decel_lim_theta: -0.4             # 旋转减速度限制(rad/s²)
      vx_samples: 20                    # X方向速度采样数
      vy_samples: 0                     # Y方向速度采样数
      vtheta_samples: 40                # 旋转速度采样数
      sim_time: 1.5                     # 轨迹模拟时间(s)
      linear_granularity: 0.05          # 线速度粒度(m/s)
      angular_granularity: 0.025        # 角速度粒度(rad/s)
      transform_tolerance: 0.2           # TF变换容忍时间(s)
      xy_goal_tolerance: 0.15           # XY目标容差(m)
      trans_stopped_velocity: 0.25      # 停止时的平移速度(m/s)
      short_circuit_trajectory_evaluation: True  # 是否使用短路轨迹评估
      stateful: True                    # 是否保持状态
      critics:                          # 轨迹评估器列表
        - "RotateToGoal"
        - "Oscillation"
        - "BaseObstacle"
        - "GoalAlign"
        - "PathAlign"
        - "PathDist"
        - "GoalDist"
      BaseObstacle.scale: 0.02          # 基础障碍物评估器缩放因子
      PathAlign.scale: 16.0             # 路径对齐评估器缩放因子
      PathAlign.forward_point_distance: 0.1  # 路径对齐前视点距离(m)
      GoalAlign.scale: 24.0             # 目标对齐评估器缩放因子
      GoalAlign.forward_point_distance: 0.1  # 目标对齐前视点距离(m)
      PathDist.scale: 32.0              # 路径距离评估器缩放因子
      GoalDist.scale: 24.0              # 目标距离评估器缩放因子
      RotateToGoal.scale: 64.0          # 旋转到目标评估器缩放因子
      RotateToGoal.slowing_factor: 5.0  # 旋转到目标减速因子
      RotateToGoal.lookahead_time: -1.0 # 旋转到目标前视时间(s)

# 控制器服务器节点参数
controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

# 局部代价地图配置
local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0            # 更新频率(Hz)
      publish_frequency: 2.0           # 发布频率(Hz)
      global_frame: odom                # 全局坐标系
      robot_base_frame: base_footprint  # 机器人基座坐标系
      use_sim_time: False
      rolling_window: true              # 是否使用滚动窗口
      width: 3                          # 地图宽度(m)
      height: 3                         # 地图高度(m)
      resolution: 0.05                  # 分辨率(m/像素)
      robot_radius: 0.1                 # 机器人半径(m)
      plugins: ["obstacle_layer", "inflation_layer"]  # 使用的图层
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"  # 膨胀层插件
        inflation_radius: 0.22          # 膨胀半径(m)
        cost_scaling_factor: 6.0        # 成本缩放因子
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"  # 障碍物层插件
        enabled: True                   # 是否启用障碍物层
        observation_sources: scan       # 观测源
        scan:
          topic: /scan                  # 激光扫描话题
          max_obstacle_height: 2.0      # 最大障碍物高度(m)
          obstacle_min_range: 0.1       # 障碍物最小范围(m)
          clearing: True                # 是否清除障碍物
          marking: True                 # 是否标记障碍物
          data_type: "LaserScan"        # 数据类型
      static_layer:
        map_subscribe_transient_local: True  # 静态地图订阅传输方式
      always_send_full_costmap: True    # 是否始终发送完整代价地图
  
  # 局部代价地图客户端参数
  local_costmap_client:
    ros__parameters:
      use_sim_time: False
  
  # 局部代价地图节点参数
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

# 全局代价地图配置
global_costmap:
  global_costmap:
    ros__parameters:
      footprint_padding: 0.03           # 机器人轮廓填充(m)
      update_frequency: 1.0             # 更新频率(Hz)
      publish_frequency: 1.0            # 发布频率(Hz)
      global_frame: map                  # 全局坐标系
      robot_base_frame: base_footprint   # 机器人基座坐标系
      use_sim_time: False
      robot_radius: 0.1                 # 机器人半径(m)
      resolution: 0.05                  # 分辨率(m/像素)
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]  # 使用的图层
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"  # 障碍物层插件
        enabled: True                   # 是否启用障碍物层
        observation_sources: scan       # 观测源
        scan:
          topic: /scan                  # 激光扫描话题
          raytrace_range: 3.0           # 光线追踪范围(m)
          obstacle_min_range: 0.1        # 障碍物最小范围(m)
          max_obstacle_height: 2.0      # 最大障碍物高度(m)
          clearing: True                # 是否清除障碍物
          marking: True                 # 是否标记障碍物
          data_type: "LaserScan"        # 数据类型
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"  # 静态层插件
        map_subscribe_transient_local: True  # 静态地图订阅传输方式
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"  # 膨胀层插件
        cost_scaling_factor: 6.0        # 成本缩放因子
        inflation_radius: 0.22          # 膨胀半径(m)
      always_send_full_costmap: True    # 是否始终发送完整代价地图
  
  # 全局代价地图客户端参数
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  
  # 全局代价地图节点参数
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

# 地图服务器配置
map_server:
  ros__parameters:
    use_sim_time: False
    yaml_filename: "yahboomcar.yaml"    # 地图文件路径

# 地图保存器配置
map_saver:
  ros__parameters:
    use_sim_time: False
    save_map_timeout: 5000              # 保存地图超时时间(ms)
    free_thresh_default: 0.25           # 自由空间阈值
    occupied_thresh_default: 0.65        # 占用空间阈值

# 规划器服务器配置
planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0    # 预期规划器频率(Hz)
    use_sim_time: False
    planner_plugins: ["GridBased"]      # 使用的规划器插件
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"  # 规划器类型
      tolerance: 0.5                    # 目标点容差(m)
      use_astar: false                  # 是否使用A*算法
      allow_unknown: true               # 是否允许在未知区域规划

# 规划器服务器节点参数
planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

# 行为服务器配置
behavior_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw  # 代价地图话题
    footprint_topic: local_costmap/published_footprint  # 机器人轮廓话题
    cycle_frequency: 10.0                # 行为执行频率(Hz)
    behavior_plugins: ["spin", "backup", "wait"]  # 行为插件列表
    spin:
      plugin: "nav2_behaviors/Spin"     # 旋转行为插件
      spin_type: "SPIN_IN_PLACE"        # 旋转类型 (原地旋转)
      rotation_velocity: 1.0            # 旋转速度(rad/s)
      max_rotational_vel: 1.0           # 最大旋转速度(rad/s)
      min_rotational_vel: 0.4           # 最小旋转速度(rad/s)
      rotational_acc_lim: 3.2           # 旋转加速度限制(rad/s²)
    backup:
      plugin: "nav2_behaviors/BackUp"   # 后退行为插件
      backup_speed: -0.2                # 后退速度(m/s)
      backup_duration: 2.0              # 后退持续时间(s)
      simulate_ahead_time: 2.0          # 模拟提前时间(s)
    wait:
      plugin: "nav2_behaviors/Wait"     # 等待行为插件
      wait_duration: 1.0                # 等待时间(s)
    global_frame: odom                  # 全局坐标系
    robot_base_frame: base_footprint    # 机器人基座坐标系
    transform_timeout: 0.1              # 变换超时时间(s)
    use_sim_time: False
    simulate_ahead_time: 2.0            # 模拟提前时间(s)
    max_rotational_vel: 1.0             # 最大旋转速度(rad/s)
    min_rotational_vel: 0.4             # 最小旋转速度(rad/s)
    rotational_acc_lim: 3.2             # 旋转加速度限制(rad/s²)

# 机器人状态发布器参数
robot_state_publisher:
  ros__parameters:
    use_sim_time: False

# 航点跟随器配置
waypoint_follower:
  ros__parameters:
    use_sim_time: False
    loop_rate: 20                       # 循环频率(Hz)
    stop_on_failure: false              # 失败时是否停止
    waypoint_task_executor_plugin: "wait_at_waypoint"  # 航点任务执行器插件
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"  # 等待插件
      enabled: True                     # 是否启用
      waypoint_pause_duration: 200      # 航点暂停时间(ms)

```