2.A* Search
- 2020 年 2 月 18 日
- 笔记
A *伪代码
Search( grid, initial_point, goal_point ) :
1.初始化一个空的打开节点列表 2.使用以下内容初始化起始节点：
- 由initial_point给出的x和y值
- g = 0，其中g是每一步的成本
- h由启发函数（当前坐标和目标的函数）给出
- 将新节点添加到打开的节点列表中
- while 打开节点的列表是非空的：
- 按f值对打开的列表进行排序
- 弹出最佳单元格（称为current单元格）。
- 在路径中将单元格的坐标标记在网格中。
- if current单元格是goal单元格：
- 返回单元格
- else将搜索范围扩展到current节点的邻居。这包括以下步骤：
- 检查网格中的每个相邻单元，以确保该单元为空：它尚未关闭且没有障碍。
- 如果单元格为空，请计算成本（g值）和启发式方法，然后将其添加到打开的节点列表中
- 将单元格标记为关闭。
- 如果由于打开的节点列表为空而退出while循环，则表明您用完了新节点以进行探索，但未找到路径。
#include <algorithm> // for sort #include <fstream> #include <iostream> #include <sstream> #include <string> #include <vector> using std::cout; using std::ifstream; using std::istringstream; using std::sort; using std::string; using std::vector; using std::abs; // State classes enum class State { kEmpty, kObstacle, kClosed, kPath, kStart, kFinish }; // Directional deltas const int delta[4][2]{ {-1, 0}, {0, -1}, {1, 0}, {0, 1} }; vector<State> ParseLine(string line) { istringstream sline(line); int n; char c; vector<State> row; while (sline >> n >> c && c == ',') { if (n == 0) { row.push_back(State::kEmpty); } else { row.push_back(State::kObstacle); } } return row; } vector<vector<State>> ReadBoardFile(string path) { ifstream myfile(path); vector<vector<State>> board{}; if (myfile) { string line; while (getline(myfile, line)) { vector<State> row = ParseLine(line); board.push_back(row); } } return board; } /** * Compare the F values of two cells. */ bool Compare(const vector<int> a, const vector<int> b) { int f1 = a[2] + a[3]; // f1 = g1 + h1 int f2 = b[2] + b[3]; // f2 = g2 + h2 return f1 > f2; } /** * Sort the two-dimensional vector of ints in descending order. */ void CellSort(vector<vector<int>>* v) { sort(v->begin(), v->end(), Compare); } // Calculate the manhattan distance int Heuristic(int x1, int y1, int x2, int y2) { return abs(x2 - x1) + abs(y2 - y1); } /** * Check that a cell is valid: on the grid, not an obstacle, and clear. */ bool CheckValidCell(int x, int y, vector<vector<State>>& grid) { bool on_grid_x = (x >= 0 && x < grid.size()); bool on_grid_y = (y >= 0 && y < grid[0].size()); if (on_grid_x && on_grid_y) return grid[x][y] == State::kEmpty; return false; } /** * Add a node to the open list and mark it as open. */ void AddToOpen(int x, int y, int g, int h, vector<vector<int>>& openlist, vector<vector<State>>& grid) { // Add node to open vector, and mark grid cell as closed. openlist.push_back(vector<int>{x, y, g, h}); grid[x][y] = State::kClosed; } /** * Expand current nodes's neighbors and add them to the open list. */ void ExpandNeighbors(const vector<int>& current, int goal[2], vector<vector<int>>& openlist, vector<vector<State>>& grid) { // Get current node's data. int x = current[0]; int y = current[1]; int g = current[2]; // Loop through current node's potential neighbors. for (int i = 0; i < 4; i++) { int x2 = x + delta[i][0]; int y2 = y + delta[i][1]; // Check that the potential neighbor's x2 and y2 values are on the grid and not closed. if (CheckValidCell(x2, y2, grid)) { // Increment g value and add neighbor to open list. int g2 = g + 1; int h2 = Heuristic(x2, y2, goal[0], goal[1]); AddToOpen(x2, y2, g2, h2, openlist, grid); } } } /** * Implementation of A* search algorithm */ vector<vector<State>> Search(vector<vector<State>> grid, int init[2], int goal[2]) { // Create the vector of open nodes. vector<vector<int>> open{}; // Initialize the starting node. int x = init[0]; int y = init[1]; int g = 0; int h = Heuristic(x, y, goal[0], goal[1]); AddToOpen(x, y, g, h, open, grid); while (open.size() > 0) { // Get the next node CellSort(&open); auto current = open.back(); open.pop_back(); x = current[0]; y = current[1]; grid[x][y] = State::kPath; // Check if we're done. if (x == goal[0] && y == goal[1]) { grid[init[0]][init[1]] = State::kStart; grid[goal[0]][goal[1]] = State::kFinish; return grid; } // If we're not done, expand search to current node's neighbors. ExpandNeighbors(current, goal, open, grid); } // We've run out of new nodes to explore and haven't found a path. cout << "No path found!" << "n"; return std::vector<vector<State>>{}; } string CellString(State cell) { switch (cell) { case State::kObstacle: return "山 "; case State::kPath: return "路径 "; case State::kStart: return "起点 "; case State::kFinish: return "终点 "; default: return "0 "; } } void PrintBoard(const vector<vector<State>> board) { for (int i = 0; i < board.size(); i++) { for (int j = 0; j < board[i].size(); j++) { cout << CellString(board[i][j]); } cout << "n"; } } //#include "test.cpp" int main() { int init[2]{ 0, 0 }; int goal[2]{ 4, 5 }; auto board = ReadBoardFile("1.board"); auto solution = Search(board, init, goal); PrintBoard(solution); system("pause"); // Tests /* TestHeuristic(); TestAddToOpen(); TestCompare(); TestSearch(); TestCheckValidCell(); TestExpandNeighbors();*/ }
