记一次纯手搓 A* (A-Star) 算法的踩坑复盘

记一次纯手搓 A* (A-Star) 算法的踩坑复盘

今天突发奇想，决定不依赖任何参考资料，全凭记忆纯手搓一个基础版的 A* 寻路算法。

本以为是个信手拈来的小任务，结果现实狠狠打脸——做了这么久的算法开发，真要脱离了框架和现成的库从零写起，竟然折腾了一整个下午才弄出初版。在这里记录一下，不仅是为了纪念，更是对自己的一次自省：纸上得来终觉浅，绝知此事要躬行。

在手写和 Debug 的过程中，我总结了 5 个看似基础、但极容易踩坑的核心要点：

1. 寻路不只是 OpenList，CloseList 同样致命

一开始凭着对 BFS 的肌肉记忆写，差点忘了 A* 的核心机制之一是 CloseList（已探索集合）。如果没有 CloseList 或者标记不严谨，算法在复杂的障碍物环境中极容易陷入死循环或重复计算。

2. 回溯路径的极致优化：丢掉指针

传统的教程里，每个节点通常会存一个 Node* parent 指针用来最终回溯路径。但我发现，在网格地图中其实不需要维护繁琐的指针。 优化方案： 只需要在节点中记录它相对于父节点的“移动方向（dir）”。到达终点后，直接用当前坐标减去 dir 就能完美逆推回起点，省内存且逻辑更清爽。不过这个情况只适用于栅格地图，对于其他有向图则无法适用。

3. 没有优先队列，A* 就会退化为 BFS

A* 的灵魂在于 $F = G + H$。如果图省事只用了一个普通的 std::queue 或者 std::vector，那实际上每次取出的并不是代价最小的节点，整个算法就直接退化成了盲目的广度优先搜索（BFS）。必须使用优先队列（Priority Queue），根据 $F$ 值动态排序。

4. 别忘了更新 OpenList 中的“更优路径”

这是我卡了最久的一个 Bug。当探测到一个已经存在于 OpenList 中的相邻节点时，不能直接跳过！必须检查当前路径到达该节点的 $G$ 值是否比它原有的 $G$ 值更小。如果更小，说明找到了一条更近的路，必须更新它的父节点（dir）和 $G$ 值。

5. 启发函数 (Heuristic) 必须与移动方式匹配

最容易犯的数学错误：

• 如果你的地图只能 上下左右 4 方向 移动，启发函数用 曼哈顿距离 (Manhattan Distance)。
• 但如果是 8 方向 移动，如果还用曼哈顿距离，会导致高估代价（不满足 Admissible 条件），可能找不到最短路径。必须改为 切比雪夫距离 (Chebyshev Distance) 或 对角线距离。

---

基本 A* 算法实现

虽然过程有些磕绊，但最终跑通的那一刻成就感还是拉满了。下面是我最终手搓出来的一版基础代码，留作纪念：

#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <functional>
#include <iostream>
#include <queue>
#include <vector>

int MAX_ROW = 1000;
int MAX_COL = 1000;

std::vector<int> map_data(MAX_ROW *MAX_COL);

// f = g + h
struct Node {
  int g;
  int h;
  int x;
  int y;
  int dir;
  int state;
};
struct CompareNode {
  bool operator()(const Node &a, const Node &b) const {
    return a.g + a.h > b.g + b.h; // f 小的优先
  }
};

std::vector<Node> list_(MAX_ROW *MAX_COL);
// std::queue<Node> open;
std::priority_queue<Node, std::vector<Node>, CompareNode> open;

static const int DX[8] = {0, 0, -1, 1, -1, 1, -1, 1};
static const int DY[8] = {-1, 1, 0, 0, -1, -1, 1, 1};
static const int COST[8] = {10, 10, 10, 10, 14, 14, 14, 14};

inline int heuristic(int x, int y, int ex, int ey) {
  int dx = std::abs(x - ex);
  int dy = std::abs(y - ey);
  // Octile: 10 * max(dx,dy) + 4 * min(dx,dy)   [= 10*直线 + 14*对角 的等价形式]
  return 10 * std::max(dx, dy) + 4 * std::min(dx, dy);
}

std::vector<int> astar() {
  list_.assign(MAX_ROW * MAX_COL, Node{0, 0, 0, 0, -1, 0});

  int sx = 20;
  int sy = 20;
  int ex = 800;
  int ey = 900;

  Node n{0, 0, sx, sy, -1, 1};
  open.push(n);
  std::vector<int> path;

  while (!open.empty()) {
    auto now = open.top();
    open.pop();
    auto nn = now.y * MAX_COL + now.x;
    if (list_[nn].state == 2)
      continue;
    list_[nn].state = 2; // 关闭

    if (now.x == ex && now.y == ey) {
      std::cout << "Find\n";
      // 回溯
      int backx = ex, backy = ey;
      while (list_[backy * MAX_COL + backx].dir != -1) {
        const auto &back_node = list_[backy * MAX_COL + backx];
        backx = back_node.x - DX[back_node.dir];
        backy = back_node.y - DY[back_node.dir];
        std::cout << "v" << list_[backy * MAX_COL + backx].dir;
        path.push_back(backy * MAX_COL + backx);
        std::reverse(path.begin(), path.end());
      }
      return path;
    }
    if (now.x == 0 || now.x == MAX_ROW - 1 || now.y == 0 ||
        now.y == MAX_COL - 1) {
      continue;
    }

    for (int i = 0; i < 8; i++) {
      int nx = now.x + DX[i];
      int ny = now.y + DY[i];
      // 在生成邻居时就检查边界
      if (nx < 0 || nx >= MAX_COL || ny < 0 || ny >= MAX_ROW)
        continue;
      int idx = ny * MAX_COL + nx;
      if (map_data[idx] != 0)
        continue;
      if (list_[idx].state != 0)
        continue;
      int new_g = now.g + COST[i];

      Node next{now.g + 1, 0, now.x + DX[i], now.y + DY[i], i, 1};
      next.h = std::abs(next.x - ex) + std::abs(next.y - ey);
      if (list_[idx].state == 0 || new_g < list_[idx].g) {
        open.push(next);
        Node next;
        next.g = new_g;
        next.h = heuristic(nx, ny, ex, ey);
        next.x = nx;
        next.y = ny;
        next.dir = i;
        next.state = 1;
        list_[idx] = next;
        open.push(next); // 允许重复入队，靠 state==2 跳过旧副本
      }
    }
  }
  return path;
}

int main() {
  //
  auto path = astar();
  std::cout << "path size:" << path.size() << "\n";
  for (const auto &p : path) {
    std::cout << p / MAX_COL << "," << p % MAX_COL << "\n";
  }
}

---

小结： 日常工作中我们习惯了调用各种封装好的库（比如 Nav2 里的各类 Planner），但偶尔像这样回归底层，纯手工实现一遍经典算法，依然能发现自己对细节的把控还有提升空间。路漫漫其修远兮，继续敲代码吧！