2、全局路径规划基础：A*算法原理、ROS2实现、Rviz可视化调试

全局路径规划，说白了就是给自动驾驶车画一条从起点到终点的「大路线」。你想想看，车要自己跑，总得先知道往哪个方向走吧？A*算法就是干这个的。我个人习惯把它叫做「带脑子的广度优先搜索」——它比Dijkstra聪明，比贪心算法靠谱。

2.1 A*算法的核心思想

A*算法本质上是一个启发式搜索算法。它用了一个很巧妙的评价函数：

f(n) = g(n) + h(n)

其中：

• g(n)：从起点到当前节点n的实际代价（已经走过的路）
• h(n)：从当前节点n到终点的估计代价（启发式函数，预测还要走多远）
• f(n)：总代价，算法每次选f(n)最小的节点去扩展

嗯，这里要注意。h(n)的选择直接决定了算法的性能。我在项目中遇到过用欧几里得距离做启发式，结果在复杂停车场里绕了半天——因为忽略了障碍物。后来我改用曼哈顿距离，效果好了不少。

2.2 A*算法的执行流程

我习惯把A*的流程拆成三步走：

1. 初始化：把起点放入Open List（待探索列表），Close List（已探索列表）置空。
2. 循环搜索：从Open List中取出f值最小的节点。如果它就是终点，搜索结束。否则，把它移入Close List，并检查它的所有邻居。
3. 路径回溯：从终点开始，沿着父节点指针一路回溯到起点，就得到了全局路径。

为什么会这样？因为A*每次都在「当前看起来最有可能」的方向上探索。它不像Dijkstra那样盲目地往四周扩散，而是带着「目标感」在走。

2.3 ROS2中的A*实现

在ROS2里实现A*，我建议用nav2的插件机制。nav2本身提供了nav2_navfn_planner，但说实话，那个实现比较老。我个人更推荐自己写一个插件，这样调试起来更灵活。

下面是一个简化的A*核心代码框架：

// AStarPlanner.cpp 核心片段
class AStarPlanner : public nav2_core::GlobalPlanner {
public:
  void initialize(...) override {
    // 加载参数：地图分辨率、启发式类型等
    // 初始化代价地图指针
  }

  nav_msgs::msg::Path createPlan(...) override {
    // 1. 将起点和终点转为栅格坐标
    // 2. 初始化Open List和Close List
    // 3. 主循环
    while (!open_list.empty()) {
      auto current = open_list.top();
      open_list.pop();

      if (current == goal_grid) {
        // 找到路径，回溯
        return reconstructPath(current);
      }

      // 扩展邻居
      for (auto& neighbor : getNeighbors(current)) {
        if (close_list.contains(neighbor)) continue;
        if (isOccupied(neighbor)) continue;

        double tentative_g = current.g + distance(current, neighbor);
        if (tentative_g < neighbor.g) {
          neighbor.g = tentative_g;
          neighbor.h = heuristic(neighbor, goal_grid);
          neighbor.f = neighbor.g + neighbor.h;
          neighbor.parent = &current;
          open_list.push(neighbor);
        }
      }
      close_list.insert(current);
    }
    // 无路径返回
    return nav_msgs::msg::Path();
  }
};

2.4 Rviz可视化调试

调试A*算法，光看代码输出是不够的。你得亲眼看到路径是怎么生成的。Rviz就是干这个的。

我建议你在Rviz里显示以下信息：

显示内容	话题类型	作用
全局路径	nav_msgs/Path	显示规划出的最终路径
Open List节点	visualization_msgs/Marker (点云)	显示算法探索过的候选节点
Close List节点	visualization_msgs/Marker (点云)	显示已确认不可行的节点
代价地图	nav_msgs/OccupancyGrid	显示障碍物分布

嗯，这里有个小技巧。你可以把Open List和Close List用不同颜色显示——比如Open List用绿色，Close List用红色。这样一眼就能看出算法在往哪个方向探索。我在调试一个狭窄通道场景时，就是靠这个发现算法在通道口反复横跳，最后调整了启发式函数才解决。

2.5 核心逻辑流程图

下面我用一张SVG图来总结A*算法的核心逻辑。这张图我画了很多遍，每次给新人讲都用它：

这张图把A*的循环逻辑讲得很清楚。你注意看那个「循环」箭头——算法会一直重复取节点、判断终点、扩展邻居，直到找到路径或者Open List耗尽。我在实际项目中，经常把这张图打印出来贴在工位上，调试时对照着看，思路特别清晰。

2.6 实际调试中的常见问题

最后，我总结几个我在项目中踩过的坑：

• 路径锯齿严重：A*在栅格地图上容易走出「之」字形。解决办法是加路径平滑，或者用JPS（Jump Point Search）替代。
• 规划时间过长：地图太大时，A*会探索很多无用节点。我建议加一个「搜索半径」限制——离终点太远的节点直接跳过。
• 动态障碍物问题：A*是静态规划器。如果障碍物在动，你得配合局部规划器（比如DWA）一起用。全局规划只管大方向，局部规划负责避障。

可可精品资料库