本文介绍了关于A*路径规划算法的一些基本概念与算法步骤。

基本概念

G：起点A到网格中的指定点所花费的代价

H：指定点到终点所花费的代价

F：F = G + H，起点到终点经过指定点的总代价

open_list：用来寻找最短的路径的中间点

closed_list：不会被考虑的点

结束条件：1. 终点出现在open_list；2. open_list为空

详细步骤

从起点S开始，把S作为一个等待检查的方格，放入到open_list中
寻找起点S周围可以到达的方格(最多八个) ，将它们放入到open_list，并设置它们的父方格为S
从open_list中删除起点S，并将放入到closed_list中(closed_list存放的是不再需要检查的方格)
计算每个周围方格的F值
从open_list中选择F值最低的方格，将它从open_list中移除，并且把它可达的方格（障碍和closed_list中的方格舍弃）加入到open_list
如果可达方格不在open_list中，则父方格设置为当前方格；如果可达方格已经在open_list中，已当前方格为中间点得到的G值比原来的更小，则更新父节点为当前节点，F = G’ + H，否则不变
循环从open_list中找F值最小的点来更新，直到结束条件

算法示例

关于最终代价的计算，首先需要将地图进行网格化如图所示，每个方格视作为一个节点，蓝色方格是障碍物，白色方格是可通过区域，绿色方格是起点，红色方格是终点。

素材1.png

其次需要添加一些约束，针对一个节点，可以选择朝其周围的8个方向移动一个方格，其中朝上、下、左、右移动的成本为 10，朝左上、右上、左下、右下移动的成本为 14（10√2 近似值），蓝色区域也就是障碍物区域表示不可移动。

实际代价的计算可以由待检查节点向八个方向出发并计算代价，而预计代价的计算则只考虑横纵向的移动，然后把总数乘以 10，通过实际代价与预计代价的叠加来进行启发，如下图的A*算法代价计算示例。
素材2.png

每个方格都标上了综合代价，实际代价，预计代价的值，方格左上角表示综合代价，左下角即从起点到当前节点的实际代价，右下角即由当前节点到终点的预计代价。最终通过A*算法得到的路径代价计算值为68，按照下图中的方式进行回溯得到路径。

素材3.png

具体实现

快手二面的时候面试官居然让我手撕A*，实在没想到，很久没写最后只写了个大概然后讲了一下思路，面完立马灰溜溜的重新捡起来了一遍

使用unordered_map实现open_list和close_list的快速查询（空间换时间），key是m * row + col，value是节点指针

#include <iostream>
#include <vector>
#include <queue>
#include <unordered_map>
#include <memory>

using namespace std;

class Node {
public:
    int row, col, F, H, G; // 横坐标，纵坐标，综合代价，估计代价，实际代价
    shared_ptr<Node> prev; // 前缀节点
    Node(int _row, int _col) : row(_row), col(_col), F(0), H(0), G(0), prev(nullptr) {}

    Node(int _row, int _col, int _G, int _H, shared_ptr<Node> _prev) : row(_row), col(_col), G(_G), H(_H), prev(_prev) {
        F = G + H;
    }
};

// 优先队列比较函数
struct cmp {
    bool operator()(shared_ptr<Node> a, shared_ptr<Node> b) {
        return a->F > b->F;
    }
};

// 计算估计代价H
int calculate_H(int cur_row, int cur_col, int end_row, int end_col) {
    return abs(end_row - cur_row) + abs(end_col - cur_col);
}

int main() {
    vector<vector<int>> Graph = {{0, 0, 0, 0, 0},
                                 {0, 0, 1, 0, 0},
                                 {0, 0, 1, 0, 0},
                                 {0, 0, 1, 0, 0},
                                 {0, 0, 0, 0, 0}};
    // 行数
    int m = Graph.size();
    // 列数
    int n = Graph[0].size();
    // 目标节点
    auto end = make_shared<Node>(2, 4);
    // 起始节点
    auto start = make_shared<Node>(2, 0, 0, calculate_H(2, 0, end->row, end->col), nullptr);

    priority_queue<shared_ptr<Node>, vector<shared_ptr<Node>>, cmp> open_list;
    unordered_map<int, shared_ptr<Node>> close_set;
    unordered_map<int, shared_ptr<Node>> open_set;
    open_list.push(start);
    open_set[m * start->row + start->col] = start;
    vector<pair<int, int>> neighbors = {{-1, 0}, {1,  0}, {0,  -1}, {0,  1}, {-1, -1}, {1,  1}, {-1, 1}, {1,  -1}};
    // 从当前节点启发式搜索到目标节点
    while (!open_list.empty()) {
        auto cur = open_list.top();

        for (auto &neighbor: neighbors) {
            int new_row = cur->row + neighbor.first;
            int new_col = cur->col + neighbor.second;
            // 如果新节点超出边界或者是障碍物，直接跳过
            if (new_row < 0 || new_row >= m || new_col < 0 || new_col >= n || Graph[new_row][new_col] == 1) {
                continue;
            }
            auto new_node = make_shared<Node>(new_row, new_col,
                                              cur->G + (neighbor.first == 0 || neighbor.second == 0 ? 10 : 14),
                                              calculate_H(new_row, new_col, end->row, end->col), cur);
            int new_key = m * new_row + new_col;
            // 如果新节点在close_list中，直接跳过
            if (close_set.find(new_key) != close_set.end()) {
                continue;
            }
            // 如果新节点是目标节点，输出路径
            if (new_row == end->row && new_col == end->col) {
                auto temp = new_node;
                while (temp != nullptr) {
                    cout << "(" << temp->row << ", " << temp->col << ")" << endl;
                    temp = temp->prev;
                }
                return 0;
            }
            // 检查新节点是否在开放列表中
            auto it = open_set.find(new_key);
            // 如果新节点不在open_list中，将新节点加入open_list
            if (it == open_set.end()) {
                open_list.push(new_node);
                open_set[new_key] = new_node;
            }
                // 如果新节点在open_list中，更新新节点的代价
            else {
                auto old_node = it->second;
                if (new_node->G < old_node->G) {
                    old_node->G = new_node->G;
                    old_node->F = old_node->G + old_node->H;
                    old_node->prev = cur;
                }
            }
        }
        // 将当前节点从open_list中移除，加入close_list
        open_list.pop();
        open_set.erase(m * cur->row + cur->col);
        close_set[m * cur->row + cur->col] = cur;
    }
    return 0;
}

重载相等运算符并且编写节点哈希函数的版本

#include <iostream>
#include <vector>
#include <queue>
#include <unordered_set>
#include <memory>

using namespace std;

class Node {
public:
    int row, col, F, H, G; // 横坐标，纵坐标，综合代价，估计代价，实际代价
    shared_ptr<Node> prev; // 前缀节点
    Node(int _row, int _col) : row(_row), col(_col), F(0), H(0), G(0), prev(nullptr) {}

    Node(int _row, int _col, int _G, int _H, shared_ptr<Node> _prev) : row(_row), col(_col), G(_G), H(_H), prev(_prev) {
        F = G + H;
    }

    bool operator==(const Node &other) const {
        return row == other.row && col == other.col;
    }
};

// 节点哈希函数
struct NodeHash {
    size_t operator()(const shared_ptr<Node> &node) const {
        return hash<int>()(node->row) ^ hash<int>()(node->col);
    }
};

// 优先队列比较函数
struct cmp {
    bool operator()(shared_ptr<Node> a, shared_ptr<Node> b) {
        return a->F > b->F;
    }
};

// 计算估计代价H
int calculate_H(int cur_row, int cur_col, int end_row, int end_col) {
    return abs(end_row - cur_row) + abs(end_col - cur_col);
}

// 检查节点是否在关闭列表中
bool inCloseList(unordered_set<shared_ptr<Node>, NodeHash> &close_list, shared_ptr<Node> node) {
    return close_list.find(node) != close_list.end();
}

int main() {
    vector<vector<int>> Graph = {{0, 0, 0, 0, 0},
                                 {0, 0, 1, 0, 0},
                                 {0, 0, 1, 0, 0},
                                 {0, 0, 1, 0, 0},
                                 {0, 0, 0, 0, 0}};
    // 行数
    int m = Graph.size();
    // 列数
    int n = Graph[0].size();
    // 目标节点
    auto end = make_shared<Node>(2, 4);
    // 起始节点
    auto start = make_shared<Node>(2, 0, 0, calculate_H(2, 0, end->row, end->col), nullptr);

    priority_queue<shared_ptr<Node>, vector<shared_ptr<Node>>, cmp> open_list;
    unordered_set<shared_ptr<Node>, NodeHash> close_set;
    unordered_set<shared_ptr<Node>, NodeHash> open_set;
    open_list.push(start);
    open_set.insert(start);
    vector<pair<int, int>> neighbors = {{-1, 0}, {1,  0}, {0,  -1}, {0,  1}, {-1, -1}, {1,  1}, {-1, 1}, {1,  -1}};
    // 从当前节点启发式搜索到目标节点
    while (!open_list.empty()) {
        auto cur = open_list.top();

        for (auto &neighbor: neighbors) {
            int new_row = cur->row + neighbor.first;
            int new_col = cur->col + neighbor.second;
            // 如果新节点超出边界或者是障碍物，直接跳过
            if (new_row < 0 || new_row >= m || new_col < 0 || new_col >= n || Graph[new_row][new_col] == 1) {
                continue;
            }
            auto new_node = make_shared<Node>(new_row, new_col,
                                              cur->G + (neighbor.first == 0 || neighbor.second == 0 ? 10 : 14),
                                              calculate_H(new_row, new_col, end->row, end->col), cur);
            // 如果新节点在close_set中，直接跳过
            if (inCloseList(close_set, new_node)) {
                continue;
            }
            // 如果新节点是目标节点，输出路径
            if (new_row == end->row && new_col == end->col) {
                auto temp = new_node;
                while (temp != nullptr) {
                    cout << "(" << temp->row << ", " << temp->col << ")" << endl;
                    temp = temp->prev;
                }
                return 0;
            }
            // 检查新节点是否在开放列表中
            auto it = open_set.find(new_node);
            // 如果新节点不在open_list中，将新节点加入open_list
            if (it == open_set.end()) {
                open_list.push(new_node);
                open_set.insert(new_node);
            }
                // 如果新节点在open_list中，更新新节点的代价
            else {
                auto old_node = *it;
                if (new_node->G < old_node->G) {
                    old_node->G = new_node->G;
                    old_node->F = old_node->G + old_node->H;
                    old_node->prev = cur;
                }
            }
        }
        // 将当前节点从open_list中移除，加入close_set
        open_list.pop();
        open_set.erase(cur);
        close_set.insert(cur);
    }
    return 0;
}

A* path planning algorithm

基本概念

详细步骤

算法示例

具体实现