A*算法原理及C++实现

Big David

已于 2024-03-15 15:31:57 修改

阅读量1.9k

点赞数 10

分类专栏： Planning模块决策规划控制文章标签：算法 A星

于 2023-11-23 18:53:16 首次发布

本文链接：https://blog.csdn.net/BigDavid123/article/details/134582599

版权

决策规划控制同时被 2 个专栏收录

48 篇文章 39 订阅

订阅专栏

Planning模块

34 篇文章 30 订阅

订阅专栏

A*算法原理

全局路径规划算法，根据给定的起点和终点在全局地图上进行总体路径规划。

导航中使用A*算法计算出机器人到目标位置的最优路线，一般作为规划的参考路线

在这里插入图片描述

// 定义地图上的点
struct Point
{
    int x,y; // 栅格行列
    Point(int x, int y):x(x),y(y){}; // 参数列表初始化
    double distance(Point& p)        // 求距离
    {
        return sqrt((x-p.x)*(x-p.x)+(y-p.y)*(y-p.y)); // 欧几里得距离
    }
};

// 定义节点
struct Node
{
    Point point; // 栅格点
    double g,h,f;// 代价值,f总价值,g到起点的代价值，h到终点的估计代价（启发式函数）
    Node *parent;// 父节点指针
    Node(Point point, double g, double h, Node* parent = nullptr):point(point), g(g), h(h), f(g+h), parent(parent)
    {}
};

在这里插入图片描述

// 定义地图
 vector<vector<int>> gridmap = {
         {0, 1, 0, 0, 0},
         {0, 0, 1, 0, 0},
         {0, 0, 1, 1, 0},
         {0, 0, 1, 0, 0},
         {0, 0, 1, 1, 0}
 };
 // 定义起点和终点
 Point start{0, 0};
 Point goal{4, 4};

A*算法的寻路原理
在这里插入图片描述

A的结束条件

A算法的寻路详细步骤
在这里插入图片描述

手撕A*代码

#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
using namespace std;
// 定义地图上的点
struct Point
{
    int x,y; // 栅格行列
    Point(int x, int y):x(x),y(y){}; // 参数列表初始化
    double distance(Point& p)        // 求距离
    {
        return sqrt((x-p.x)*(x-p.x)+(y-p.y)*(y-p.y)); // 欧几里得距离
    }
};


// 定义节点
struct Node
{
    Point point; // 栅格点
    double g,h,f;// 代价值,f总价值,g到起点的代价值，h到终点的估计代价（启发式函数）
    Node *parent;// 父节点指针
    Node(Point point, double g, double h, Node* parent = nullptr):point(point), g(g), h(h), f(g+h), parent(parent)
    {}
};

// 自定义Node*排序规则
struct NodeCompare{
    bool operator()(Node* n1, Node* n2){
        return (n1->f) < (n2->f); // 表示升序排列
    }
};
// 基于栅格地图的路径规划A*算法,返回由Point组成的路径path,输入为地图，起点和终点
vector<Point> AstarPathPlanning(vector<vector<int>> &gridmap, Point& start, Point& goal)
{
    // 获取地图参数
    int row = gridmap.size(); // 行，表示地图的宽度
    int col = gridmap[0].size(); // 列，表示地图的长
    // 定义openlist, closelist
    vector<Node *> openlist; // openlist 表示待搜索的节点
    vector<Node *> closelist;// closelist表示已搜索的节点
    openlist.push_back(new Node(start, start.distance(start), start.distance(goal))); // 将起点加入openlist中，作为初始化
    int count1 = 1;
    // 进入循环，开始搜索,搜索到终点则返回路径
    vector<Point> path;
    while (!openlist.empty()) // 当openlist为空，表示所有可搜索节点已经被搜索，此时循环结束
    {
        // 获取当前搜索节点current,即openlist中f最小节点
        sort(openlist.begin(), openlist.end(), NodeCompare{}); // 先对openlist排序，这里自定义排序规则(从小到大)
        Node* current = *openlist.begin(); // *openlist.begin()排序后即为f最小的迭代器位置
        // 将current对应的元素从openlist中删除
        openlist.erase(openlist.begin());
        // 将current加入到closelist中
        closelist.push_back(current);
        // 对当前搜索节点current进行分类讨论
        // 1-current是终点，则返回路径，表示找到路径
        if (current->point.x == goal.x && current->point.y == goal.y)
        {
            while (current != nullptr) // 利用父节点，从终点向起点回溯最短路径,因为起点没有父节点，所以起点current父节点为nullptr
            {
                path.push_back(current->point);
                current = current->parent;
            }
            reverse(path.begin(), path.end()); // 路径是反的，翻转路径
            int count2 = 0; // delete 次数
            for (auto o : openlist)
            {
                delete o;
                count2++;
            }
            for (auto c : closelist)
            {
                delete c;
                count2++;
            }
            cout << "new times: " << count1 << endl;
            cout << "delete times: " << count2 << endl;
            return path;
        }
        // 2-current 不是终点，需要讨论其邻近的节点neighbors
        int x = current->point.x;
        int y = current->point.y;
        vector<Point> neighbors = { // 8个邻近节点的坐标
                {x-1,y-1}, {x-1,y}, {x-1,y+1},
                {x,y-1},     {x,y+1},
                {x+1,y-1}, {x+1,y}, {x+1,y+1}
        };
        // 遍历所有的临近节点,每一个邻近节点n必须满足在地图范围内同时不是障碍物
        for (auto n : neighbors)
        {
            if ((n.x >= 0 && n.x < row) && (n.y >= 0 && n.y < col) && gridmap[n.x][n.y]==0)
            {
                // 1 n在closelist中，表示已经搜索过了，此时直接跳过
                bool incloselist = false;
                for (auto c : closelist)
                {
                    if (c->point.x == n.x && c->point.y == y)
                    {
                        incloselist = true;
                        break;
                    }
                }
                if (incloselist)
                {
                    continue;
                }
                // 2 n是否在openlist中进行讨论
                bool inopenlist = false;
                for (auto o : openlist)
                {
                    if (o->point.x == n.x && o->point.y == n.y)
                    {
                        inopenlist = true;
                        // n 在openlist中，对比f值，更新代价值和父节点parent
                        double g = current->g + n.distance(current->point); // 临近节点n到起点的距离 = 当前搜索节点current到起点的距离 + 当前搜索节点current到邻近节点n距离
                        double h = n.distance(goal); // 临近节点n到终点的估计距离代价
                        double f = g + h;
                        if (f < (o->f))
                        {
                            o->f = f;
                            o->parent = current;
                        }
                        break;
                    }
                }
                if (!inopenlist) // n不在openlist中，对比f值，计算代价值，添加到openlist中，下次备选
                {
                    double g = current->g + n.distance(current->point); // 临近节点n到起点的距离 = 当前搜索节点current到起点的距离 + 当前搜索节点current到邻近节点n距离
                    double h = n.distance(goal); // 临近节点n到终点的估计距离代价
                    double f = g + h;
                    openlist.push_back(new Node(n,g,h,current));
                    count1++;
                }
            }
        }

    }


    // 搜索完成没有路径，表示路径规划失败,此时返回空路径
    return path;
}
int main()
{
    // 定义地图
    vector<vector<int>> gridmap = {
            {0, 1, 0, 0, 0},
            {0, 0, 1, 0, 0},
            {0, 0, 1, 1, 0},
            {0, 0, 1, 0, 0},
            {0, 0, 1, 1, 0}
    };
    // 定义起点和终点
    Point start{0, 0};
    Point goal{4, 4};
    vector<Point> path = AstarPathPlanning(gridmap, start, goal);
    cout << path.size() << endl;
    for (auto p : path)
    {
        if (p.x == goal.x && p.y == goal.y)
        {
            cout << "(" << p.x << ',' << p.y << ")" << endl;
        }
        else
        {
            cout << "(" << p.x << ',' << p.y << ")" << "->";
        }
    }
    return 0;
}

A*算法总结

把起点加入 open list 。
重复如下过程：
a. 遍历 open list ，查找 F 值最小的节点，把它作为当前要处理的节点。
b. 把这个节点移到 close list 。
c. 对当前方格的 8 个相邻方格的每一个方格？
◆ 如果它是不可抵达的或者它在 close list 中，忽略它。否则，做如下操作。
◆ 如果它不在 open list 中，把它加入 open list ，并且把当前方格设置为它的父亲，记录该方格的 F ， G 和 H 值。
◆ 如果它已经在 open list 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G 值表示这是更好的路径。如果是这样，把它的父亲设置为当前方格，并重新计算它的 G 和 F 值。如果你的 open list 是按 F 值排序的话，改变后你可能需要重新排序。
d. 停止，当你
◆ 把终点加入到了 open list 中，此时路径已经找到了，或者
◆ 查找终点失败，并且 open list 是空的，此时没有路径。
3.保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。