前言

• 也是好久没更新了这个系列了hhhh,最近完善一下
• 往期内容：
  • 第一期：【10天速通Navigation2】(一) 框架总览和概念解释
  • 第二期：【10天速通Navigation2】(二) ：ROS2gazebo阿克曼小车模型搭建-gazebo_ackermann_drive等插件的配置和说明
  • 第三期：【10天速通Navigation2】(三) ：Cartographer建图算法配置：从仿真到实车，从原理到实现
  • 第四期：【10天速通Navigation2】(四) ：ORB-SLAM3的ROS2 humble编译和配置
  • 第五期：【10天速通Navigation2】(五) ：基于gazebo仿真的复杂地形的ORB-SLAM3配置
  • 第六期：【10天速通Navigation2】(六) ：Navigation2基础配置与参数解析
• 本教材将贯穿nav2的全部内容，使用ROS2和C++实现一些仿真乃至实车中常见的建图和路径规划算法，例如cartographer,ORB-SLAM,RRT,hybrid-astar。我们将注重与原理讲解和代码实现，去详细讲解每一步的配置过程和代码复现细节。
• 同理本教材默认大家有一些基础的ROS2和C++的编程基础，故不对一些基础部分进行详细说明。
• 本教程使用的环境：
  • ROS2 humble
  • ubuntu 22.04 LTS
• 上一期我们完成了Nav2的基础配置和参数解析，但在最后留下了一个问题：当目标点位于机器人后方时，默认的NavfnPlanner（基于A*或Dijkstra）无法规划出带倒车的路径。这是因为传统2D网格规划器把机器人当成一个"没有朝向"的点，完全不考虑运动学约束。
• 本期我们就来解决这个问题——手写一个Hybrid-A*全局规划器，以插件的形式注册到Nav2中，替换掉默认规划器。内容涵盖：Nav2规划器插件接口、自行车模型运动学、Hybrid-A*公式推导、C++实现逐行解读、路径平滑，以及linorobot2（四轮差速小车）的仿真环境搭建。

---

0 环境安装

• 注意：前几期使用的"轻舟"阿克曼小车模型文件因故丢失，本期开始更换为 linorobot2 仿真平台。
• linorobot2 是一个开源的 ROS2 移动机器人仿真框架，支持 2WD、4WD 和**麦克纳姆轮（mecanum）**三种底盘类型。我们选用 4WD 模式——四轮差速驱动，相比阿克曼转向结构更简单、控制更直接，也适合在室内仿真中验证路径规划算法。
• 以下从零搭建仿真环境。

0-1 创建工作空间

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws
colcon build

0-2 拉取仓库

cd ~/ros2_ws/src
git clone https://github.com/linorobot/linorobot2.git
cd ~/ros2_ws/src/linorobot2
git checkout humble

0-3 编译

# 编译工作空间
cd ~/ros2_ws
source /opt/ros/humble/setup.bash
colcon build

0-4 仿真模型文件

• 本期的 Gazebo 仿真世界是一个 10m×10m 的室内场景，包含 9 个由墙壁隔开的房间、走廊以及散布的柱子和箱子障碍物。完整的 SDF 模型文件见附录 B。
• 将以下内容保存为 linorobot2/linorobot2_gazebo/worlds/my_world.sdf 即可使用。

0-5 运行仿真

• 将以下内容保存为 1_bringup.sh，用于一键启动 Gazebo 仿真环境和小车模型：

#!/bin/bash
# 1_bringup.sh — 启动 Gazebo 仿真 + 4WD 小车
echo "export LINOROBOT2_BASE=4wd" >> ~/.bashrc
source ~/.bashrc

source /opt/ros/humble/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 launch linorobot2_gazebo gazebo.launch.py

• 首次使用前需赋予可执行权限：chmod +x 1_bringup.sh，后续只需 ./1_bringup.sh 即可启动。
• 启动后可以看到我们的10m×10m室内仿真环境，有9个由墙壁隔开的房间，散布着柱子和箱子作为障碍物。小车是四轮差速底盘。
• 注意：直接启动后你会发现雷达点云中始终存在机器人自身底盘的轮廓——这是因为激光雷达安装位置较低，扫描到了车体。参考第六期 0-1 节的做法，我们需要修改小车的 xacro 模型文件，调整雷达的最小扫描距离和安装高度。
• 打开 linorobot2/linorobot2_description/urdf/sensors/lidar.gazebo.xacro，修改雷达的 min_range 参数：

<ray>
  <scan>
    <horizontal>
      <samples>360</samples>
      <resolution>1</resolution>
      <min_angle>-3.14159</min_angle>
      <max_angle>3.14159</max_angle>
    </horizontal>
  </scan>
  <range>
    <min>0.3</min>          <!-- 从 0 改为 0.3m，过滤掉车体反射 -->
    <max>10.0</max>
    <resolution>0.01</resolution>
  </range>
</ray>

• 同时可以适当抬高雷达安装高度（在 base.urdf.xacro 或 lidar.urdf.xacro 中调整 <origin> 的 z 值），进一步避免扫描到车身。
• 同理，深度相机发布的点云（/camera/depth/color/points）也会捕捉到车身底部。在 navigation_sim.yaml 的 global_costmap 和 local_costmap 中，给 voxel_layer 或 obstacle_layer 的观测源设置 min_obstacle_height: 0.1，过滤掉地面高度以下的点云反射：

voxel_layer:
  plugin: "nav2_costmap_2d::VoxelLayer"
  observation_sources: pointcloud
  pointcloud:
    topic: /camera/depth/color/points
    min_obstacle_height: 0.1    # 过滤车身反射（低于10cm的点不标记为障碍物）
    max_obstacle_height: 2.0

• 修改后重新编译即可生效。

0-6 建图

• 将以下内容保存为 2_slam.sh，用于启动 Cartographer SLAM 在线建图：

#!/bin/bash
# 2_slam.sh — 启动 SLAM 建图
source /opt/ros/humble/setup.bash
source ./install/setup.bash
ros2 launch linorobot2_navigation slam.launch.py sim:=true rviz:=true

• 建图过程中用键盘控制小车在环境中巡游一圈，覆盖所有房间和走廊。建图满意后，将以下内容保存为 save_map.sh，用于将当前地图保存为 PGM+YAML 文件：

#!/bin/bash
# save_map.sh — 保存地图（自动按时间戳命名）
source /opt/ros/humble/setup.bash

MAP_NAME="map_$(date +%Y%m%d_%H%M%S)"
MAP_DIR="$(dirname "$0")/src/linorobot2/linorobot2_navigation/maps"

mkdir -p "$MAP_DIR"
cd "$MAP_DIR"
ros2 run nav2_map_server map_saver_cli -f "$MAP_NAME" --ros-args -p save_map_timeout:=10000.

echo "Map saved: $MAP_DIR/${MAP_NAME}.yaml"

---

0-7 导航

• 将以下内容保存为 3_nav.sh，用于一键启动导航（自动加载最新地图、初始化位姿、使用 Hybrid-A* 规划器）：

#!/bin/bash
# 3_nav.sh — 启动导航（自动加载最新地图 + 自动初始化位姿）
pkill -f "component_container" 2>/dev/null
pkill -f "ros2 launch linorobot2_navigation" 2>/dev/null
pkill -f "rviz2" 2>/dev/null
sleep 1

source /opt/ros/humble/setup.bash
source ./install/setup.bash

SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)"
MAP_DIR="$SCRIPT_DIR/src/linorobot2/linorobot2_navigation/maps"
LATEST_MAP=$(ls -t "$MAP_DIR"/map_*.yaml 2>/dev/null | head -1)

if [ -z "$LATEST_MAP" ]; then
    echo "ERROR: No map_*.yaml found in $MAP_DIR"
    echo "Run ./save_map.sh first to create a map."
    exit 1
fi

echo "Loading map: $LATEST_MAP"
ros2 launch linorobot2_navigation navigation.launch.py sim:=true rviz:=true map:="$LATEST_MAP" &
NAV_PID=$!

echo "Waiting for navigation to start..."
sleep 8

echo "Publishing initial pose (0, 0, yaw=0)..."
ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{
  header: {frame_id: 'map'},
  pose: {
    pose: {
      position: {x: 0.0, y: 0.0, z: 0.0},
      orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
    },
    covariance: [0.25, 0.0, 0.0, 0.0, 0.0, 0.0,
                 0.0, 0.25, 0.0, 0.0, 0.0, 0.0,
                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0685]
  }
}"

echo "Initial pose set. Nav is running."
wait $NAV_PID

• 脚本启动后等待 8s 自动发布初始位姿，默认使用 DWB 局部规划器和 Hybrid-A 全局规划器* 进行导航。在 RViz2 中使用 Nav2 Goal 工具点击目标点即可测试。

0-8 调节膨胀系数

• 如果发现规划出的路径离墙壁太近（容易碰撞），需要调大膨胀半径。修改 linorobot2_navigation/config/navigation_sim.yaml 中 global_costmap 的 inflation_layer 参数：

global_costmap:
  global_costmap:
    ros__parameters:
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        enabled: true
        inflation_radius: 0.30        # 膨胀半径 (m)，根据机器人尺寸调整
        cost_scaling_factor: 3.0      # 代价衰减系数

• inflation_radius 越大，障碍物周围的安全区域越大，规划出的路径离墙壁越远；cost_scaling_factor 控制代价向外衰减的速度——值越小衰减越慢（远处也有代价），值越大衰减越快（只有紧贴障碍物处代价高）。

• 此外，将以下内容保存为 kill.sh，用于一键清理所有 Gazebo 和 ROS2 进程（调试时很实用）：

#!/bin/bash
# kill.sh — 一键杀掉 Gazebo 和所有 ROS2 进程
killall -9 gzserver gzclient gazebo 2>/dev/null
pkill -f "component_container" 2>/dev/null
pkill -f "ros2 launch" 2>/dev/null
pkill -f "rviz2" 2>/dev/null
pkill -f "robot_state_publisher" 2>/dev/null
pkill -f "command_timeout" 2>/dev/null
pkill -f "urdf_spawner" 2>/dev/null
pkill -f "spawn_entity" 2>/dev/null
pkill -f "ekf_node" 2>/dev/null
sleep 1
ros2 daemon stop 2>/dev/null
ros2 daemon start 2>/dev/null
echo "All killed"

---

1 Nav2 全局路径规划器接口说明

• 在 Nav2 中，全局规划器是一个插件（plugin），通过 pluginlib 机制动态加载。所有全局规划器都必须继承 nav2_core::GlobalPlanner 这个基类，并实现以下接口：

class GlobalPlanner
{
public:
  virtual void configure(
    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
    std::string name,
    std::shared_ptr<tf2_ros::Buffer> tf,
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) = 0;

  virtual void cleanup() = 0;
  virtual void activate() = 0;
  virtual void deactivate() = 0;

  virtual nav_msgs::msg::Path createPlan(
    const geometry_msgs::msg::PoseStamped & start,
    const geometry_msgs::msg::PoseStamped & goal) = 0;
};

• 五个接口的职责很清晰：

方法	作用	调用时机
configure()	加载参数、获取 costmap 指针、初始化	节点启动时
activate()	激活规划器，开始接受规划请求	生命周期切换到 active 状态
deactivate()	停用规划器	生命周期切换到 inactive 状态
cleanup()	清理资源	节点关闭时
createPlan()	核心方法——给定起点和目标点，返回一条路径	每次收到导航目标时调用

• createPlan() 就是我们要实现的核心。输入是 start（机器人当前位姿）和 goal（目标位姿），输出是一条 nav_msgs::Path——由一系列位姿点组成的路径。costmap 在 configure() 时就拿到了，规划时直接读代价地图来判断障碍物。

• 说人话就是：

Nav2 的架构已经把"框子"搭好了——生命周期管理、costmap、TF、行为树调度全都帮你搞定。你只需要继承一个类、实现五个函数（核心就一个 createPlan），你自己的算法就能像官方规划器一样工作。

• 要让 Nav2 识别我们的插件，还需要两样东西：
  1. plugins.xml：声明插件类名和基类
  2. package.xml 的 export 标签：告诉 pluginlib 去哪里找插件描述文件

<!-- plugins.xml -->
<library path="planner">
  <class name="planner/HybridAStarPlanner"
         type="planner::HybridAStarPlanner"
         base_class_type="nav2_core::GlobalPlanner">
    <description>Hybrid A* global planner with kinematic constraints</description>
  </class>
</library>

<!-- package.xml 导出插件 -->
<export>
  <build_type>ament_cmake</build_type>
  <nav2_core plugin="${prefix}/plugins.xml" />
</export>

• 最后在 Nav2 的参数 YAML 里，把 planner_plugins 换成我们的插件名：

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "planner/HybridAStarPlanner"   # 替换掉原来的 nav2_navfn_planner/NavfnPlanner
      tolerance: 0.5
      # ... 我们自定义的参数 ...
      wheel_base: 0.3
      steering_angle_max: 0.6
      steering_angle_res: 0.3
      arc_length: 0.5

• 接下来，我们就来创建这个规划器插件包，并实现 HybridAStarPlanner。

1-1 创建规划器功能包

• 按照上述接口规范，我们在工作空间中新建一个名为 planner 的功能包。完整的目录树如下：

ros2_ws/
├── src/
│   ├── linorobot2/          ← 仿真平台（机器人模型 + Gazebo + 导航启动）
│   │   ├── linorobot2_description/   ← URDF/Xacro 模型
│   │   ├── linorobot2_gazebo/        ← Gazebo 启动文件和世界文件
│   │   └── linorobot2_navigation/    ← Nav2 启动文件和参数配置
│   │       ├── config/navigation_sim.yaml  ← 全局规划器替换入口
│   │       ├── launch/
│   │       └── maps/
│   └── planner/             ← **我们的自定义规划器包**
│       ├── CMakeLists.txt            ← 编译配置
│       ├── package.xml               ← 包描述 + 插件导出声明
│       ├── plugins.xml               ← pluginlib 注册文件
│       ├── README.md
│       ├── include/planner/
│       │   └── hybrid_a_star.hpp     ← 头文件
│       └── src/
│           └── hybrid_a_star.cpp     ← 实现文件

• CMakeLists.txt——关键的三件事：声明依赖、编译共享库、导出插件描述文件。

cmake_minimum_required(VERSION 3.5)
project(planner)

# 依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(nav2_core REQUIRED)        # GlobalPlanner 基类
find_package(nav2_costmap_2d REQUIRED)  # 代价地图
find_package(pluginlib REQUIRED)        # 插件加载

# 编译为共享库（pluginlib 要求）
add_library(planner SHARED src/hybrid_a_star.cpp)
ament_target_dependencies(planner
  rclcpp nav2_core nav2_costmap_2d nav2_util
  nav_msgs geometry_msgs tf2_ros pluginlib)

# 导出插件描述文件
pluginlib_export_plugin_description_file(nav2_core plugins.xml)

install(TARGETS planner ARCHIVE DESTINATION lib LIBRARY DESTINATION lib)
install(DIRECTORY include/ DESTINATION include/)
install(FILES plugins.xml DESTINATION share/${PROJECT_NAME}/)
ament_package()

• 注意 add_library 必须是 SHARED（动态库）——pluginlib 在运行时通过 dlopen 加载 .so 文件，静态库不行。

• package.xml——最关键的是 <export> 标签，它告诉 pluginlib 去 plugins.xml 里找插件注册信息：

<package format="3">
  <name>planner</name>
  <version>0.1.0</version>
  <description>Custom global planners for Nav2: Hybrid-A*, and more.</description>

  <depend>rclcpp</depend>
  <depend>nav2_core</depend>
  <depend>nav2_costmap_2d</depend>
  <depend>pluginlib</depend>
  <!-- ... 其他依赖 ... -->

  <export>
    <build_type>ament_cmake</build_type>
    <nav2_core plugin="${prefix}/plugins.xml" />   <!-- 关键行 -->
  </export>
</package>

• plugins.xml——声明插件类名和基类映射：

<library path="planner">
  <class name="planner/HybridAStarPlanner"
         type="planner::HybridAStarPlanner"
         base_class_type="nav2_core::GlobalPlanner">
    <description>Hybrid A* global planner with kinematic constraints</description>
  </class>
</library>

• 三个文件的协作关系：

package.xml 告诉构建系统"这个包导出了一个 nav2_core 插件"→ plugins.xml 告诉 pluginlib "找 libplanner.so 里的 planner::HybridAStarPlanner"→ PLUGINLIB_EXPORT_CLASS 宏把这个类注册到 C++ 的类型系统中。三步缺一不可。

• 编译完成后，Nav2 的 planner_server 通过参数 YAML 中的 GridBased.plugin: "planner/HybridAStarPlanner" 就能动态加载我们的规划器。可以用以下命令确认当前使用的规划器：

ros2 param get /planner_server GridBased.plugin
# 输出: planner/HybridAStarPlanner

---

2 Hybrid-A*

2-1 为什么需要 Hybrid-A*

• 上一期我们使用的默认规划器 NavfnPlanner，底层是 A* 或 Dijkstra 算法，在 2D 网格上搜索最短路径。它把机器人看成一个2D 粒子——没有朝向、没有转向半径、没有运动学约束。

• 这在实践中会导致两个问题：

  1. 规划的路径不可执行：路径可能出现急转弯，阿克曼转向的机器人根本转不过来
  2. 不会倒车：当目标点在机器人后方时（比如泊车场景），传统 2D A* 规划的路径需要机器人"原地旋转"才能跟上，而真实的车只能通过倒车+前进的组合来完成任务

• Hybrid-A*（Dolgov et al., 2010）就是为解决这个问题而生的。它把搜索空间从 2D $(x,y)$ 扩展到了 3D $(x,y,\theta )$，在搜索时每一步的扩展都符合车辆的运动学模型。这样生成的路径天然满足转向约束，而且支持前进/倒车两种运动方向。

• 说人话就是：

传统 A* 给机器人指了一条"人走的路"——不管你的车能不能拐过去、需不需要倒车。Hybrid-A* 给机器人指的是一条"车能开的路"——每个弯的半径都考虑了转向极限，必要的时候会规划倒车。

2-2 自行车运动学模型

• Hybrid-A* 在扩展状态时，使用自行车模型（bicycle model）来模拟车辆运动。对于阿克曼转向的车辆：

• 状态变量 $[x,y,\theta ]$ 表示车辆后轴中心的位置和朝向。给定转向角 $\delta$（前轮转角）和弧长 $l$（沿圆弧行驶的距离），状态更新为：

$$\{\begin{array}{l}\kappa =\frac{\tan \delta }{L}\\ \Delta \theta =\kappa \cdot l\end{array}$$

其中 $L$ 是轴距（wheelbase），$\kappa$ 是曲率（curvature），$\Delta \theta$ 是朝向变化量。

• 当 $\Delta \theta \approx 0$（直线行驶）时：
  $$\{\begin{array}{l}{x}^{\prime }=x+l\cdot \cos \theta \\ y^{\prime }=y+l\cdot \sin \theta \\ {\theta }^{\prime }=\theta \end{array}$$

• 当 $\Delta \theta \ne 0$（转向行驶）时，车辆沿着半径为 $R=1/\kappa$ 的圆弧运动，圆心 $(cx,cy)$ 在垂直于当前朝向的方向上：
  $$\{\begin{array}{l}cx=x-R\cdot \sin \theta \\ cy=y+R\cdot \cos \theta \\ x^{\prime }=cx+R\cdot \sin (\theta +\Delta \theta )\\ y^{\prime }=cy-R\cdot \cos (\theta +\Delta \theta )\\ {\theta }^{\prime }=\theta +\Delta \theta \end{array}$$

• 对照代码，kinodynamicExpand() 就是这两个公式的 C++ 翻译：

State kinodynamicExpand(const State & s, double steer_angle, double arc_len)
{
  State next = s;
  double curvature = std::tan(steer_angle) / wheel_base_;   // κ = tan(δ) / L
  double dtheta = curvature * arc_len;                       // Δθ = κ · l

  if (std::abs(dtheta) < 1e-6) {
    // 直线
    next.x += arc_len * std::cos(s.theta);
    next.y += arc_len * std::sin(s.theta);
  } else {
    // 圆弧
    double radius = 1.0 / curvature;
    double cx = s.x - radius * std::sin(s.theta);
    double cy = s.y + radius * std::cos(s.theta);
    next.x = cx + radius * std::sin(s.theta + dtheta);
    next.y = cy - radius * std::cos(s.theta + dtheta);
    next.theta = s.theta + dtheta;
  }
  return next;
}

• 说人话就是：

给定当前位置、方向盘打多少度、往前走多远，算出车子最终会到哪个位置和朝向。这就是"运动学模拟"——比传统 A* 的"往前走一格"多了曲率和朝向的概念。

2-3 Hybrid-A* 算法推导

2-3-1 搜索空间离散化

• Hybrid-A* 在三维空间 $(x,y,\theta )$ 中搜索。$x$ 和 $y$ 由 costmap 的分辨率决定（如 0.05m），$\theta$ 被离散化为 theta_bins_ 个等分（如 72 份，每份 5°）。
• 连续位姿 $(x,y,\theta )$ 通过 poseToGrid() 映射到离散格点 $(gx,gy,g\theta )$：

gx = (int)((x - origin_x) / resolution);
gy = (int)((y - origin_y) / resolution);
gt = (int)(theta / (2π / theta_bins));

2-3-2 状态扩展——主搜索循环

• 这是 Hybrid-A* 的心脏。先看代码再看解释：

// ---- 生成转向角采样队列 ----
std::vector<double> steer_angles;
for (double a = -steering_angle_max_; a <= steering_angle_max_ + 1e-6;
     a += steering_angle_res_) {
  steer_angles.push_back(a);
}

• 例如 ${\delta }_{max}=0.6$ rad（约 34°），${\delta }_{res}=0.3$ rad，则采样得到 [-0.6, -0.3, 0, 0.3, 0.6] 共 5 个转向角。72 个 $\theta$ bin × 每个状态扩展 5 个转向 × 2 个方向 = 每步最多扩展 10 个子状态。

• 主循环的核心逻辑：

while (!open.empty() && iterations < max_iterations_) {
  // 超时保护
  if ((clock_->now() - start_time).seconds() > max_planning_time_) break;

  auto [f, idx] = open.top(); open.pop();  // 取出 f 值最小的状态
  if (closed[idx].closed) continue;         // 跳过已处理的过期条目
  closed[idx].closed = true;

  State & current = closed[idx];
  iterations++;

  // ---- 目标检测 (2-3-4) ----
  if (dist_to_goal < goal_xy_tolerance_ && dtheta < goal_theta_tolerance_) {
    goal_idx = idx; break;
  }

  // ---- 展开子状态 ----
  for (double steer : steer_angles) {
    // 前进
    State next = kinodynamicExpand(current, steer, arc_length_);
    poseToGrid(next.x, next.y, next.theta,
               next.grid_x, next.grid_y, next.grid_theta);

    if (visited.count({next.grid_x, next.grid_y, next.grid_theta})) continue;
    if (!isStateValid(next)) continue;

    // 计算代价
    double edge_cost = arc_length_ + 0.2 * std::abs(steer) * arc_length_;
    next.g = current.g + edge_cost;                    // 2-3-3
    next.h = heuristic(next, goal_x, goal_y, goal_yaw); // 2-3-3
    next.parent_idx = idx;

    closed.push_back(next);
    open.emplace(next.f(), closed.size() - 1);
    visited.insert({next.grid_x, next.grid_y, next.grid_theta});

    // 倒车（步长减半、代价加重）
    State next_rev = kinodynamicExpand(current, steer, -arc_length_ * 0.5);
    poseToGrid(next_rev.x, next_rev.y, next_rev.theta,
               next_rev.grid_x, next_rev.grid_y, next_rev.grid_theta);

    if (!visited.count({next_rev.grid_x, next_rev.grid_y, next_rev.grid_theta})
        && isStateValid(next_rev)) {
      double rev_cost = arc_length_ * 0.5 * 1.5;      // 倒车惩罚系数 1.5×
      next_rev.g = current.g + rev_cost;
      next_rev.h = heuristic(next_rev, goal_x, goal_y, goal_yaw);
      next_rev.parent_idx = idx;
      closed.push_back(next_rev);
      open.emplace(next_rev.f(), closed.size() - 1);
      visited.insert({next_rev.grid_x, next_rev.grid_y, next_rev.grid_theta});
    }
  }
}

• 逐段解读：

  • steer_angles：均匀采样转向角，作为状态扩展的"动作空间"。${\delta }_{res}$ 越大动作越少、搜索越快但路径越粗糙；${\delta }_{res}$ 越小路径越精细但搜索爆炸。
  • open 优先队列：std::priority_queue 按 $f=g+h$ 排序，每次弹出最有希望的状态——这就是 A* 的核心机制。由于 priority_queue 不支持更新已入队元素，我们用 closed[idx].closed 标记跳过过期条目。
  • visited 哈希集：对 $(gx,gy,g\theta )$ 三维格点去重。注意这里的 key 是离散化后的整数索引，不是浮点位姿——同一格点内的连续位姿被视为同一个状态，防止在同一个"方格"里无限展开。
  • closed 向量：存储所有已展开的状态对象。用 parent_idx 链式回溯即可重建完整路径。
  • 前进与倒车：不同点在于弧长和代价——倒车步长减半（0.5×arc_length），代价加重（1.5× 惩罚系数），让规划器倾向于前进，只有在必要时（如泊车）才使用倒车。

• 说人话就是：

每次从队列里捞出"最有可能最快到终点"的状态，按不同的方向盘角度向前和向后各踩一脚油门，生出几个子状态。子状态如果没撞墙、没走过，就放进队列。反复循环直到摸到终点。

2-3-3 代价函数

• A* 的灵魂：$f=g+h$，决定搜索的效率和路径质量。

• $g$ 值——已走过路程的代价：

$$g_{next}=g_{current}+arc\_length\cdot (1+0.2\cdot |\delta |)$$

• 其中 $0.2\cdot |\delta |$ 是转向惩罚：方向盘打得越大，代价越高。这鼓励规划器走直线而不是频繁扭动。

• $h$ 值——启发式，估计还要走多远。必须"乐观"（admissible）——不能高估真实代价，否则 A* 找到的不再是最优解。我们设计为三项加权和：

$$h=d_{2D}+0.5\cdot |\Delta {\theta }_{to\_goal}|\cdot R_{min}+0.3\cdot |\Delta {\theta }_{goal\_orient}|\cdot R_{min}$$

其中：

• $d_{2D}$：欧氏距离（普通 A* 的启发式）
• $|\Delta {\theta }_{to\_goal}|\cdot R_{min}$："转对方向"的最小弧长——你必须朝向目标才能接近它
• $|\Delta {\theta }_{goal\_orient}|\cdot R_{min}$："对准目标朝向"的最小弧长——你不仅要到达位置，还要以正确的姿势停下来

double heuristic(const State & s, double goal_x, double goal_y, double goal_theta) {
  double dx = goal_x - s.x, dy = goal_y - s.y;
  double dist_2d = std::sqrt(dx*dx + dy*dy);          // ① 欧氏距离
  double angle_to_goal = std::atan2(dy, dx);

  double dtheta = normalize(angle_to_goal - s.theta); // ② 朝向差
  double min_radius = wheel_base_ / std::tan(steering_angle_max_);
  double turn_cost = std::abs(dtheta) * min_radius;

  double goal_dtheta = normalize(goal_theta - s.theta); // ③ 目标朝向差
  return dist_2d + turn_cost * 0.5 + std::abs(goal_dtheta) * min_radius * 0.3;
}

• 这就是 Hybrid-A* 比普通 A* 聪明的地方：启发式不仅考虑了"还有多远"，还考虑了"方向对不对"。这让搜索在目标附近不会因为朝向偏差而浪费大量迭代。

2-3-4 目标检测

• 传统 A* 到达目标格子就算成功。Hybrid-A* 必须以正确的姿势到达——位置和朝向都要满足容差：

double dist_to_goal = std::sqrt(
  std::pow(current.x - goal.pose.position.x, 2) +
  std::pow(current.y - goal.pose.position.y, 2));
double dtheta = std::fmod(current.theta - goal_yaw + M_PI, 2.0 * M_PI);
// ... 归一化到 [0, π]

if (dist_to_goal < goal_xy_tolerance_ && dtheta < goal_theta_tolerance_) {
  goal_idx = idx;
  break;  // 到达目标！
}

• 条件：$\Vert (x,y)-(x_{goal},y_{goal})\Vert <0.3m$ 且 $|\theta -{\theta }_{goal}|<0.3$ rad。这正是泊车场景所需要的——“位置到了、车头方向也要对”。

2-4 C++ 完整实现

2-4-1 状态结构体

struct State
{
  double x, y, theta;              // 连续位姿
  int grid_x, grid_y, grid_theta;  // 离散化索引
  double g, h;                     // 代价: 已走路程 + 启发式估计
  int parent_idx;                  // 父状态在 closed 列表中的下标
  bool closed;                     // 是否已关闭
  double f() const { return g + h; }
};

• 注意这里用 parent_idx（下标索引）而不是指针——因为 closed 是 std::vector<State>，用索引避免了指针失效问题。

2-4-2 碰撞检测

bool isPoseFree(double x, double y, double)
{
  unsigned int mx, my;
  if (!costmap_->worldToMap(x, y, mx, my)) return false;  // 超出地图边界
  unsigned char cost = costmap_->getCost(mx, my);
  return cost < nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE &&
         (allow_unknown_ || cost != nav2_costmap_2d::NO_INFORMATION);
}

• INSCRIBED_INFLATED_OBSTACLE 是 costmap 中标定好的"内切膨胀障碍物"代价阈值——比这个阈值高的都被视作不可通过。allow_unknown_ 控制是否允许穿越未知区域。

2-4-3 启发式函数

double heuristic(const State & s, double goal_x, double goal_y, double goal_theta)
{
  double dx = goal_x - s.x, dy = goal_y - s.y;
  double dist_2d = std::sqrt(dx*dx + dy*dy);        // 欧氏距离
  double angle_to_goal = std::atan2(dy, dx);

  double dtheta = normalize(angle_to_goal - s.theta);
  double min_radius = wheel_base_ / std::tan(steering_angle_max_);
  double turn_cost = std::abs(dtheta) * min_radius; // 转向弧长估计

  double goal_dtheta = normalize(goal_theta - s.theta);
  return dist_2d + turn_cost * 0.5 + std::abs(goal_dtheta) * min_radius * 0.3;
}

2-4-4 路径平滑

• Hybrid-A* 的每次状态扩展本身就是圆弧——路径天生不是折线。但由于状态在 $(x,y,\theta )$ 三维空间中被离散化（比如 $\theta$ 被切成 72 份），相邻状态之间的微小朝向跳变会在路径上产生肉眼可见的"锯齿"。路径平滑的目的就是消除这些离散化伪影。

• 思路很简单：让每个路径点稍微挪动一下，既不要离原来位置太远，又尽量和前后邻居"排成一条线"。用公式表达就是：

$$\{\begin{array}{l}{x}_i\leftarrow x_i+\alpha (x_i^{orig}-x_i)+\beta (x_{i-1}+x_{i+1}-2x_i)\\ y_i\leftarrow y_i+\alpha (y_i^{orig}-y_i)+\beta (y_{i-1}+y_{i+1}-2y_i)\end{array}$$

• 拆开看两项的含义：

项	公式	它在干什么
data term（数据项）	$\alpha (x_i^{orig}-x_i)$	把 $x_i$ 往它原始位置 $x_i^{orig}$ 拉——“别跑太远，保持对搜索结果的忠实度”
smoothness term（平滑项）	$\beta (x_{i-1}+x_{i+1}-2x_i)$	把 $x_i$ 往前后邻居的中点 $(x_{i-1}+x_{i+1})/2$ 推——“三点排成一条直线”

• 举个例子你就懂了。假设三个连续路径点的 $x$ 坐标为 $(1.0,2.5,4.0)$，中间点 $x_1=2.5$ 明显偏了（理想共线应该是 2.5，但这里邻居中点是 $(1.0+4.0)/2=2.0$）：

  • data term：$x_1^{orig}-x_1=2.5-2.5=0$（当前恰好就在原始位置，没有拉扯）

• 换个更明显的例子：坐标 $(0,1,2)$，原先是 $(0,0.5,2)$——中间点原始在 0.5，当前被拉到了 1：

  • data term：$\alpha (0.5-1)=-0.5\alpha$，把中间点往回拉 0.5
  • smoothness term：$\beta (0+2-2\cdot 1)=0$，说明它已经共线了

• 两个力同时作用于每个点，迭代 50 次后达到平衡。$\alpha =0.4$、$\beta =0.3$ 意味着数据忠实度比平滑度稍重要——路径大致保持原样，但锯齿被抹平。

void smoothPath(nav_msgs::msg::Path & path)
{
  const double alpha = 0.4, beta = 0.3;
  const int iterations = 50;

  // 备份原始位置，data term 需要参照
  std::vector<double> x_orig, y_orig;
  for (const auto & pose : path.poses) {
    x_orig.push_back(pose.pose.position.x);
    y_orig.push_back(pose.pose.position.y);
  }

  std::vector<double> x = x_orig, y = y_orig;
  for (int iter = 0; iter < iterations; ++iter) {
    for (size_t i = 1; i < path.poses.size() - 1; ++i) {
      double dx_data   = x_orig[i] - x[i];                 // 数据项: 往回拉
      double dx_smooth = x[i-1] + x[i+1] - 2.0 * x[i];     // 平滑项: 往中间推
      x[i] += alpha * dx_data + beta * dx_smooth;

      double dy_data   = y_orig[i] - y[i];
      double dy_smooth = y[i-1] + y[i+1] - 2.0 * y[i];
      y[i] += alpha * dy_data + beta * dy_smooth;
    }
  }
  // 回写平滑后的坐标，并根据新位置重新计算每个点的朝向
}

• 注意首尾两个点不参与平滑——它们是起点和终点，不能动。还有，平滑后每个点的朝向需要根据前后点的方向重新计算：${\theta }_i=\text{atan2}(y_{i+1}-y_i,x_{i+1}-x_i)$。

• 说人话就是：

想象路径是一根橡皮筋，上面有很多珠子（路径点）。每颗珠子被两个力拉扯：一个力说"别离开你最初的位置"（data term），另一个力说"跟你前后的珠子排成一条直线"（smoothness term）。50 轮拉扯之后，所有珠子都找到了平衡位置——路径还是原来那条路，但变得更流畅了。

2-4-5 完整代码

• 头文件 planner/hybrid_a_star.hpp：

#ifndef PLANNER__HYBRID_A_STAR_HPP_
#define PLANNER__HYBRID_A_STAR_HPP_

#include <chrono>
#include <cmath>
#include <memory>
#include <string>
#include <vector>
#include <unordered_set>
#include <queue>
#include <algorithm>

#include "rclcpp/rclcpp.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "tf2_ros/buffer.h"

namespace planner
{

class HybridAStarPlanner : public nav2_core::GlobalPlanner
{
public:
  HybridAStarPlanner() = default;
  ~HybridAStarPlanner() = default;

  void configure(
    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
    std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;

  void cleanup() override;
  void activate() override;
  void deactivate() override;

  nav_msgs::msg::Path createPlan(
    const geometry_msgs::msg::PoseStamped & start,
    const geometry_msgs::msg::PoseStamped & goal) override;

private:
  struct State
  {
    double x, y, theta;
    int grid_x, grid_y, grid_theta;
    double g, h;
    int parent_idx;
    bool closed;
    double f() const { return g + h; }
  };

  struct StateHash
  {
    size_t operator()(const std::tuple<int, int, int> & s) const
    {
      return std::get<0>(s) ^ (std::get<1>(s) << 10) ^ (std::get<2>(s) << 20);
    }
  };

  State kinodynamicExpand(const State & s, double steer_angle, double arc_length);
  bool isStateValid(const State & s);
  bool isPathFree(const State & from, const State & to, int steps = 10);
  bool isPoseFree(double x, double y, double theta);
  double heuristic(const State & s, double goal_x, double goal_y, double goal_theta);
  void poseToGrid(double x, double y, double theta, int & gx, int & gy, int & gt);
  nav_msgs::msg::Path buildPath(const std::vector<State> & closed, int goal_idx);
  void smoothPath(nav_msgs::msg::Path & path);

  std::shared_ptr<tf2_ros::Buffer> tf_;
  nav2_costmap_2d::Costmap2D * costmap_;
  rclcpp::Logger logger_{rclcpp::get_logger("HybridAStarPlanner")};
  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
  rclcpp::Clock::SharedPtr clock_;
  std::string name_, global_frame_;

  double tolerance_;
  double steering_angle_res_;
  double steering_angle_max_;
  double wheel_base_;
  double arc_length_;
  double goal_xy_tolerance_;
  double goal_theta_tolerance_;
  bool allow_unknown_;
  int max_iterations_;
  int theta_bins_;
  double max_planning_time_;

  unsigned int size_x_, size_y_;
  double origin_x_, origin_y_, resolution_;
};

}  // namespace planner

#endif  // PLANNER__HYBRID_A_STAR_HPP_

• 实现文件 planner/hybrid_a_star.cpp（核心部分已在上面逐段解读，完整代码在工程里）：

#include "planner/hybrid_a_star.hpp"
#include <tuple>

namespace planner
{

void HybridAStarPlanner::configure(
  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
  std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
  node_ = parent;
  name_ = name;
  tf_ = tf;
  costmap_ = costmap_ros->getCostmap();
  global_frame_ = costmap_ros->getGlobalFrameID();

  auto node = node_.lock();
  clock_ = node->get_clock();
  logger_ = node->get_logger();

  // 声明参数及其默认值
  nav2_util::declare_parameter_if_not_declared(node, name_ + ".tolerance", rclcpp::ParameterValue(0.5));
  nav2_util::declare_parameter_if_not_declared(node, name_ + ".steering_angle_res", rclcpp::ParameterValue(0.3));
  nav2_util::declare_parameter_if_not_declared(node, name_ + ".steering_angle_max", rclcpp::ParameterValue(0.6));
  nav2_util::declare_parameter_if_not_declared(node, name_ + ".wheel_base", rclcpp::ParameterValue(0.3));
  nav2_util::declare_parameter_if_not_declared(node, name_ + ".arc_length", rclcpp::ParameterValue(0.5));
  nav2_util::declare_parameter_if_not_declared(node, name_ + ".goal_xy_tolerance", rclcpp::ParameterValue(0.3));
  nav2_util::declare_parameter_if_not_declared(node, name_ + ".goal_theta_tolerance", rclcpp::ParameterValue(0.3));
  nav2_util::declare_parameter_if_not_declared(node, name_ + ".allow_unknown", rclcpp::ParameterValue(true));
  nav2_util::declare_parameter_if_not_declared(node, name_ + ".max_iterations", rclcpp::ParameterValue(50000));
  nav2_util::declare_parameter_if_not_declared(node, name_ + ".theta_bins", rclcpp::ParameterValue(72));
  nav2_util::declare_parameter_if_not_declared(node, name_ + ".max_planning_time", rclcpp::ParameterValue(5.0));

  node->get_parameter(name_ + ".tolerance", tolerance_);
  node->get_parameter(name_ + ".steering_angle_res", steering_angle_res_);
  node->get_parameter(name_ + ".steering_angle_max", steering_angle_max_);
  node->get_parameter(name_ + ".wheel_base", wheel_base_);
  node->get_parameter(name_ + ".arc_length", arc_length_);
  node->get_parameter(name_ + ".goal_xy_tolerance", goal_xy_tolerance_);
  node->get_parameter(name_ + ".goal_theta_tolerance", goal_theta_tolerance_);
  node->get_parameter(name_ + ".allow_unknown", allow_unknown_);
  node->get_parameter(name_ + ".max_iterations", max_iterations_);
  node->get_parameter(name_ + ".theta_bins", theta_bins_);
  node->get_parameter(name_ + ".max_planning_time", max_planning_time_);

  size_x_ = costmap_->getSizeInCellsX();
  size_y_ = costmap_->getSizeInCellsY();
  origin_x_ = costmap_->getOriginX();
  origin_y_ = costmap_->getOriginY();
  resolution_ = costmap_->getResolution();

  RCLCPP_INFO(logger_, "HybridAStar configured: wheelbase=%.2f, steer_max=%.2f, arc=%.2f",
    wheel_base_, steering_angle_max_, arc_length_);
}

// ... (kinodynamicExpand, heuristic, createPlan, smoothPath 等已在文中逐段解读)

}  // namespace planner

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(planner::HybridAStarPlanner, nav2_core::GlobalPlanner)

• 最后一行 PLUGINLIB_EXPORT_CLASS 是 pluginlib 的宏——这行不能少，它把我们写的类注册到 Nav2 的插件体系中，让 Nav2 能在运行时通过插件名 planner/HybridAStarPlanner 找到并加载它。

---

3 编译与使用

• 将 planner 包放到 ros2_ws/src 下，编译：

cd ~/ros2_ws
colcon build --symlink-install --packages-select planner
source install/setup.bash

• 在 Nav2 参数文件中指定使用我们的规划器：

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "planner/HybridAStarPlanner"
      wheel_base: 0.3          # 麦克纳姆小车轴距
      steering_angle_max: 0.6  # 最大转向角 (rad)
      steering_angle_res: 0.3  # 转向角采样步长
      arc_length: 0.5          # 每步扩展弧长 (m)
      goal_xy_tolerance: 0.3   # 目标位置容差 (m)
      goal_theta_tolerance: 0.3 # 目标朝向容差 (rad)
      max_iterations: 50000
      max_planning_time: 5.0

• 启动导航后，发布一个位于小车后方的目标点，你会看到 Hybrid-A* 规划的路径包含了一段平滑的倒车+前进轨迹，而不是生硬的原地旋转。

---

总结

• 本期我们完成了从"理论推导"到"Nav2 插件落地"的完整链路——手写了一个 Hybrid-A* 全局规划器，替换掉默认的 NavfnPlanner。

1. Nav2 规划器插件接口：继承 nav2_core::GlobalPlanner，实现 configure/activate/deactivate/cleanup/createPlan 五个方法，通过 pluginlib 注册，一行 YAML 即可替换。
2. Hybrid-A 核心*：在 3D 状态空间 $(x,y,\theta )$ 中搜索，每一步扩展都使用自行车运动学模型。启发式不仅考虑距离，还考虑朝向差异——让规划器能找到"车能开的路径"，包括倒车。
3. 工程细节：visited set 去重防止搜索爆炸、arc_length 参数控制搜索粒度、目标检测同时判断位置和朝向、梯度下降平滑消除路径折线。
4. 完整可运行：代码已放在 ros2_ws/src/planner 中，编译后修改 Nav2 参数即可切换规划器，配合 linorobot2 仿真环境直接验证效果。

• 本期实现了基于自行车运动学模型的 Hybrid-A* 全局规划器，下一期我们将继续深入——手写 RRT（快速随机探索树）全局规划器，同样以 Nav2 插件的形式加载~
• 感谢支持！！！！
• 如有错误，欢迎指出！！！！！！

---

附录 B：仿真世界 SDF 文件

• 以下为完整的 10m×10m 室内仿真世界模型文件：

<sdf version='1.7'>
  <world name='default'>
    <light name='sun' type='directional'>
      <cast_shadows>1</cast_shadows>
      <pose>0 0 10 0 -0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range><constant>0.9</constant><linear>0.01</linear><quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>

    <model name='ground_plane'>
      <static>1</static>
      <link name='link'>
        <collision name='collision'>
          <geometry><plane><normal>0 0 1</normal><size>100 100</size></plane></geometry>
          <surface><friction><ode><mu>100</mu><mu2>50</mu2></ode></friction></surface>
        </collision>
        <visual name='visual'>
          <cast_shadows>0</cast_shadows>
          <geometry><plane><normal>0 0 1</normal><size>100 100</size></plane></geometry>
          <material>
            <script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Grey</name></script>
          </material>
        </visual>
      </link>
    </model>

    <gravity>0 0 -9.8</gravity>
    <physics type='ode'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>1</shadows>
    </scene>

    <!-- ==================== 外墙 10m x 10m ==================== -->
    <model name='wall_bottom'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>10 0.15 0.5</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>10 0.15 0.5</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/BricksBrown</name></script></material></visual>
    </link><pose>0 -5 0.25 0 0 0</pose></model>

    <model name='wall_top'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>10 0.15 0.5</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>10 0.15 0.5</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/BricksBrown</name></script></material></visual>
    </link><pose>0 5 0.25 0 0 0</pose></model>

    <model name='wall_left'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>0.15 10 0.5</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>0.15 10 0.5</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/BricksBrown</name></script></material></visual>
    </link><pose>-5 0 0.25 0 0 0</pose></model>

    <model name='wall_right'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>0.15 10 0.5</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>0.15 10 0.5</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/BricksBrown</name></script></material></visual>
    </link><pose>5 0 0.25 0 0 0</pose></model>

    <!-- ==================== 内部墙壁（3x3=9个房间，走廊~2.3m宽） ==================== -->

    <!-- 列1: x=-2.5，第1、3个房间有墙，第2个房间开放（通道） -->
    <model name='v1_bottom'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>0.1 1.8 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>0.1 1.8 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/White</name></script></material></visual>
    </link><pose>-2.5 -4.1 0.2 0 0 0</pose></model>

    <model name='v1_top'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>0.1 2.0 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>0.1 2.0 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/White</name></script></material></visual>
    </link><pose>-2.5 4.0 0.2 0 0 0</pose></model>

    <!-- 列2: x=0，第1个房间有墙，第2、3个房间开放 -->
    <model name='v2_bottom'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>0.1 1.8 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>0.1 1.8 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/White</name></script></material></visual>
    </link><pose>0 -4.1 0.2 0 0 0</pose></model>

    <model name='v2_mid'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>0.1 1.6 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>0.1 1.6 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/White</name></script></material></visual>
    </link><pose>0 0.8 0.2 0 0 0</pose></model>

    <!-- 列3: x=2.5，第2、3个房间有墙，第1个房间开放 -->
    <model name='v3_top'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>0.1 3.6 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>0.1 3.6 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/White</name></script></material></visual>
    </link><pose>2.5 3.2 0.2 0 0 0</pose></model>

    <!-- 行1: y=-2.5，第2、3个房间有墙 -->
    <model name='h1_mid'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>1.5 0.1 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>1.5 0.1 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/White</name></script></material></visual>
    </link><pose>0.75 -2.5 0.2 0 0 0</pose></model>

    <model name='h1_right'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>2.0 0.1 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>2.0 0.1 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/White</name></script></material></visual>
    </link><pose>3.5 -2.5 0.2 0 0 0</pose></model>

    <!-- 行2: y=0，第1、3个房间有墙 -->
    <model name='h2_left'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>1.6 0.1 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>1.6 0.1 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/White</name></script></material></visual>
    </link><pose>-3.3 0 0.2 0 0 0</pose></model>

    <model name='h2_right'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>2.0 0.1 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>2.0 0.1 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/White</name></script></material></visual>
    </link><pose>3.5 0 0.2 0 0 0</pose></model>

    <!-- 行3: y=2.5，第1、2个房间有墙 -->
    <model name='h3_left'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>1.8 0.1 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>1.8 0.1 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/White</name></script></material></visual>
    </link><pose>-3.4 2.5 0.2 0 0 0</pose></model>

    <model name='h3_mid'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>1.8 0.1 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>1.8 0.1 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/White</name></script></material></visual>
    </link><pose>-0.4 2.5 0.2 0 0 0</pose></model>


    <!-- ==================== 障碍物 ==================== -->

    <!-- 左下房间：2个柱子和1个箱子 -->
    <model name='obs_cylinder1'><static>1</static><link name='link'>
      <collision name='c'><geometry><cylinder><radius>0.15</radius><length>0.4</length></cylinder></geometry></collision>
      <visual name='v'><geometry><cylinder><radius>0.15</radius><length>0.4</length></cylinder></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>-3.5 -3.5 0.2 0 0 0</pose></model>

    <model name='obs_cylinder2'><static>1</static><link name='link'>
      <collision name='c'><geometry><cylinder><radius>0.12</radius><length>0.4</length></cylinder></geometry></collision>
      <visual name='v'><geometry><cylinder><radius>0.12</radius><length>0.4</length></cylinder></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>-1.5 -3.5 0.2 0 0 0</pose></model>

    <model name='obs_box1'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>0.4 0.4 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>0.4 0.4 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>-2.5 -3.5 0.2 0 0 0</pose></model>

    <!-- 中下房间：2个柱子和1个箱子 -->
    <model name='obs_cylinder3'><static>1</static><link name='link'>
      <collision name='c'><geometry><cylinder><radius>0.15</radius><length>0.4</length></cylinder></geometry></collision>
      <visual name='v'><geometry><cylinder><radius>0.15</radius><length>0.4</length></cylinder></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>1.5 -3.5 0.2 0 0 0</pose></model>

    <model name='obs_cylinder4'><static>1</static><link name='link'>
      <collision name='c'><geometry><cylinder><radius>0.12</radius><length>0.4</length></cylinder></geometry></collision>
      <visual name='v'><geometry><cylinder><radius>0.12</radius><length>0.4</length></cylinder></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>1.5 -1.5 0.2 0 0 0</pose></model>

    <model name='obs_box2'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>0.5 0.5 0.5</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>0.5 0.5 0.5</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>3.5 -3.5 0.25 0 0 0</pose></model>

    <!-- 中间房间（中心区域）障碍物密集 -->
    <model name='obs_box3'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>0.5 0.5 0.5</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>0.5 0.5 0.5</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>-1.5 1.2 0.25 0 0 0</pose></model>

    <model name='obs_cylinder5'><static>1</static><link name='link'>
      <collision name='c'><geometry><cylinder><radius>0.15</radius><length>0.4</length></cylinder></geometry></collision>
      <visual name='v'><geometry><cylinder><radius>0.15</radius><length>0.4</length></cylinder></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>1.2 1.2 0.2 0 0 0</pose></model>

    <model name='obs_cylinder6'><static>1</static><link name='link'>
      <collision name='c'><geometry><cylinder><radius>0.1</radius><length>0.4</length></cylinder></geometry></collision>
      <visual name='v'><geometry><cylinder><radius>0.1</radius><length>0.4</length></cylinder></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>0 3.5 0.2 0 0 0</pose></model>

    <!-- 右上房间障碍物 -->
    <model name='obs_box4'><static>1</static><link name='link'>
      <collision name='c'><geometry><box><size>0.5 0.3 0.4</size></box></geometry></collision>
      <visual name='v'><geometry><box><size>0.5 0.3 0.4</size></box></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>3.5 3.5 0.2 0 0 0</pose></model>

    <model name='obs_cylinder7'><static>1</static><link name='link'>
      <collision name='c'><geometry><cylinder><radius>0.12</radius><length>0.4</length></cylinder></geometry></collision>
      <visual name='v'><geometry><cylinder><radius>0.12</radius><length>0.4</length></cylinder></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>-3.5 1.2 0.2 0 0 0</pose></model>

    <model name='obs_cylinder8'><static>1</static><link name='link'>
      <collision name='c'><geometry><cylinder><radius>0.15</radius><length>0.4</length></cylinder></geometry></collision>
      <visual name='v'><geometry><cylinder><radius>0.15</radius><length>0.4</length></cylinder></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>-1.0 3.5 0.2 0 0 0</pose></model>

    <!-- 通道中孤立的障碍物 -->
    <model name='obs_cylinder9'><static>1</static><link name='link'>
      <collision name='c'><geometry><cylinder><radius>0.1</radius><length>0.4</length></cylinder></geometry></collision>
      <visual name='v'><geometry><cylinder><radius>0.1</radius><length>0.4</length></cylinder></geometry>
      <material><script><uri>file://media/materials/scripts/gazebo.material</uri><name>Gazebo/Wood</name></script></material></visual>
    </link><pose>3.5 0.8 0.2 0 0 0</pose></model>

    <gui fullscreen='0'>
      <camera name='user_camera'>
        <pose>0 -8 10 0 0.8 1.57</pose>
        <view_controller>orbit</view_controller>
        <projection_type>perspective</projection_type>
      </camera>
    </gui>

  </world>
</sdf>