57、C++项目实战：机器人路径规划（A*算法）

路径规划，说白了就是让机器人自己找到一条从起点到终点的路。别撞墙，别绕远，最好还能走最短路径。我最早接触这个问题是在一个仓储机器人项目里，当时小车在货架间穿梭，如果路径选不好，要么堵车，要么撞货架。嗯，后来我选了A*算法，效果还不错。

A*算法是启发式搜索的经典代表。它不像广度优先那样傻傻地往四周扩散，而是带着“方向感”去搜索。这个方向感，就来自一个评估函数。

核心思想：f = g + h

A*算法的灵魂就这一个公式：

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

• g(n)：从起点到当前节点n的实际代价。说白了就是已经走了多远。
• h(n)：从当前节点n到终点的估计代价。这是启发式函数，是A*的“直觉”。
• f(n)：总估计代价。A*每次选f值最小的节点去扩展。

我刚开始学的时候，总觉得h(n)随便写个距离就行。后来踩了坑才发现，h(n)的选择直接影响算法效率和正确性。如果h(n)永远不大于真实距离，A*保证能找到最短路径。这叫“可采纳性”。

启发式函数怎么选？

不同的地图，适合不同的h(n)。我整理了一个表格，方便你对照：

地图类型	推荐启发式函数	说明
网格地图（四方向）	曼哈顿距离	|dx| + |dy|，只能上下左右走
网格地图（八方向）	切比雪夫距离	max(|dx|, |dy|)，可以斜着走
任意方向移动	欧几里得距离	sqrt(dx² + dy²)，直线距离
路网/图结构	直线距离	两点间直线，作为下界估计

我在仓储项目里用的是网格地图，机器人只能上下左右走，所以选了曼哈顿距离。你想想看，如果选了欧几里得距离，估计值可能比实际小太多，搜索范围会大得离谱。

数据结构：Open List 和 Close List

A*算法需要维护两个核心列表：

• Open List：待探索的节点。每次从中取出f值最小的节点。
• Close List：已经探索过的节点。避免重复走回头路。

Open List 的选型很关键。我见过有人用vector，每次手动找最小值。地图小还行，地图一大就卡死了。我个人习惯用优先队列（priority_queue），C++标准库就有，省心。

C++实现：核心代码

先定义节点结构：

struct Node {
    int x, y;          // 坐标
    int g;             // 起点到当前点的代价
    int h;             // 当前点到终点的估计代价
    int f;             // f = g + h
    Node* parent;      // 记录父节点，用于回溯路径

    Node(int x, int y) : x(x), y(y), g(0), h(0), f(0), parent(nullptr) {}
};

比较器，用于优先队列：

struct CompareNode {
    bool operator()(const Node* a, const Node* b) {
        return a->f > b->f;  // 小顶堆：f值小的优先
    }
};

A*主函数框架：

std::vector<Node*> aStarSearch(int startX, int startY, int endX, int endY,
                                const std::vector<std::vector<int>>& grid) {
    // 1. 初始化起点和终点
    Node* startNode = new Node(startX, startY);
    Node* endNode = new Node(endX, endY);

    // 2. 优先队列（Open List）
    std::priority_queue<Node*, std::vector<Node*>, CompareNode> openList;
    openList.push(startNode);

    // 3. Close List（用二维数组标记）
    std::vector<std::vector<bool>> closedList(grid.size(),
        std::vector<bool>(grid[0].size(), false));

    // 4. 方向数组（四方向）
    int dx[] = {-1, 1, 0, 0};
    int dy[] = {0, 0, -1, 1};

    while (!openList.empty()) {
        Node* current = openList.top();
        openList.pop();

        // 到达终点
        if (current->x == endX && current->y == endY) {
            return reconstructPath(current);
        }

        closedList[current->x][current->y] = true;

        // 探索四个邻居
        for (int i = 0; i < 4; i++) {
            int nx = current->x + dx[i];
            int ny = current->y + dy[i];

            // 边界检查、障碍物检查、是否已关闭
            if (nx < 0 || nx >= grid.size() || ny < 0 || ny >= grid[0].size())
                continue;
            if (grid[nx][ny] == 1)  // 1表示障碍物
                continue;
            if (closedList[nx][ny])
                continue;

            Node* neighbor = new Node(nx, ny);
            neighbor->g = current->g + 1;  // 每步代价为1
            neighbor->h = manhattanDistance(nx, ny, endX, endY);
            neighbor->f = neighbor->g + neighbor->h;
            neighbor->parent = current;

            openList.push(neighbor);
        }
    }

    // 没找到路径
    return {};
}

曼哈顿距离计算：

int manhattanDistance(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}

路径回溯：

std::vector<Node*> reconstructPath(Node* node) {
    std::vector<Node*> path;
    while (node != nullptr) {
        path.push_back(node);
        node = node->parent;
    }
    std::reverse(path.begin(), path.end());
    return path;
}

A*算法流程图

下面这张图展示了A*算法的完整流程，从起点开始，一步步找到终点：

避坑指南

还有几个常见问题：

• 内存泄漏：new 出来的 Node 记得 delete。我建议用智能指针，或者用 vector 统一管理生命周期。
• 障碍物边界：检查邻居时，一定要先判断坐标是否越界，再访问 grid 数组。顺序反了会崩。
• 启发式函数不一致：如果 h(n) 有时大于真实距离，A* 可能找不到最短路径。我一般用曼哈顿距离，安全。

性能优化建议

地图大了之后，A* 的搜索空间会爆炸。我分享几个优化思路：

1. 使用二叉堆：C++ priority_queue 底层就是堆，够用。如果追求极致性能，可以用斐波那契堆。
2. 跳点搜索（JPS）：在网格地图上，JPS 可以跳过大量对称路径，速度提升几十倍。我后来把项目改成了 JPS，效果立竿见影。
3. 分层规划：先粗粒度规划大方向，再细粒度规划局部路径。适合超大场景。
4. 缓存路径：如果起点和终点经常不变，可以缓存路径结果，避免重复计算。

总结

A* 算法是路径规划的基石。它用 f = g + h 这个简单的公式，平衡了搜索的广度和方向性。你想想看，没有启发式函数，就是 Dijkstra；没有 g 值，就是贪心最佳优先。A* 把两者结合，既保证最优，又提高效率。

我在实际项目中，A* 的代码量其实不大，核心就几十行。但真正用好它，需要理解启发式函数的选择、数据结构的搭配、以及各种边界情况的处理。嗯，希望这篇文章能帮你少走弯路。

---