路径规划——A-Star算法_a*(a-star)算法是一种静态路网中求解最短路最有效的方法。公式表示为:f(n)=g(n)+h

什么是A-Star算法

A*（A-Star)算法是一种静态路网中求解最短路最有效的方法。这是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法。该算法的成本计算公式表示为：
$$f(n)=g(n)+h(n)$$
其中,$f(n)$是节点$n$从初始点到目标点的估价函数，$g(n)$是在状态空间中从初始节点到$n$节点的实际代价，$h(n)$是从$n$到目标节点最佳路径的估计代价。
  一般情况下，$g(n)$是从起点到当前节点$n$的移动量。相邻节点间水平或垂直方向的移动量一边为1，对角线方向的移动量则为1.4（勾股定理）；$h(n)$则是从当前节点$n$到终点的移动量的估算值，一般采用“曼哈顿距离”。
  路径规划问题，则可以将搜索区域简化为二维数组，二维数组中的每一个元素都表示网格中的一个节点，节点则被标记为可以通过或不可以通过，则最终的路径就是从起点到终点的节点的集合。

算法流程

C++代码实现

AStar类定义文件，AStar.h

#pragma once
#include <vector>
#include <list>

struct Point
{
    int x,y;
    int F,G,H;
    Point *parent;
    Point(int _x,int _y):x(_x),y(_y),F(0),G(0),H(0),parent(nullptr) { }
};

class AStar {
public:
    AStar();
    void InitAStar(std::vector<std::vector<int>> &_maze);

    std::list<Point *> GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner);

private:
    Point *FindPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner);
    std::vector<Point *> GetSurroundPoints(const Point *point, bool isIgnoreCorner) const;
    bool IsReachable(const Point *point, const Point *target, bool isIgnoreCorner) const;
    Point *IsInList(const std::list<Point *> &list, const Point *point) const;
    //从开启列表中返回F值最小的节点
    Point *GetSmallestFPoint();
    //计算FGH值
    int CalcG(Point *temp_start, Point *point);
    int CalcH(Point *point, Point *end);
    int CalcF(Point *point);

private:
    std::vector<std::vector<int>> maze;
    std::list<Point *> openList;  //开启列表
    std::list<Point *> closeList; //关闭列表

    //cost 值
    int kCost1_;
    int kCost2_;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

AStar类的实现，AStar.cpp

#include "AStar.h"
#include <math.h>

AStar::AStar() {
    kCost1_ = 10;
    kCost2_ = 14;
}

void AStar::InitAStar(std::vector<std::vector<int>> &_maze) {
    maze = _maze;
}

int AStar::CalcG(Point *temp_start, Point *point) {
    int extraG = (abs(point->x - temp_start->x) + abs(point->y - temp_start->y)) == 1 ? kCost1_ : kCost2_;
    //如果是初始节点，则其父节点是空
    int parentG = point->parent == nullptr ? 0 : point->parent->G;
    return parentG + extraG;
}

int AStar::CalcH(Point *point, Point *end) {
    //用简单的欧几里得距离计算H，这个H的计算有很多种方法
    return sqrt((double) (end->x - point->x) * (double) (end->x - point->x) +
                (double) (end->y - point->y) * (double) (end->y - point->y)) * kCost1_;
}

int AStar::CalcF(Point *point) {
    return point->G + point->H;
}

Point *AStar::GetSmallestFPoint() {
    if (!openList.empty()) {
        auto resPoint = openList.front();
        for (auto &point:openList)
            if (point->F < resPoint->F)
                resPoint = point;
        return resPoint;
    }
    return nullptr;
}

Point *AStar::FindPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner) {
    openList.push_back(new Point(startPoint.x, startPoint.y));
    while (!openList.empty()) {
        //找到F值最小的点
        auto curPoint = GetSmallestFPoint();
        openList.remove(curPoint);
        closeList.push_back(curPoint);
        // 找到当前周围八个格中可以通过的格子
        auto surroundPoints = GetSurroundPoints(curPoint, isIgnoreCorner);
        for (auto &target:surroundPoints) {
            // 对某一个格子，如果它不在开启列表中，加入到开启列表，
            // 设置当前格为其父节点，计算F G H
            if (!IsInList(openList, target)) {
                target->parent = curPoint;
                target->G = CalcG(curPoint, target);
                target->H = CalcH(target, &endPoint);
                target->F = CalcF(target);
                openList.push_back(target);
            }
                // 对某一个格子，它在开启列表中，计算G值, 如果比原来的大, 就什么都不做,
                // 否则设置它的父节点为当前点,并更新G和F
            else {
                int tempG = CalcG(curPoint, target);
                if (tempG < target->G) {
                    target->parent = curPoint;
                    target->G = tempG;
                    target->F = CalcF(target);
                }
            }
            Point *resPoint = IsInList(openList, &endPoint);
            if (resPoint)
                //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝
                return resPoint;
        }
    }
    return nullptr;
}

std::list<Point *> AStar::GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner) {
    Point *result = FindPath(startPoint, endPoint, isIgnoreCorner);
    std::list<Point *> path;
    //返回路径，如果没找到路径，返回空链表
    while (result) {
        path.push_front(result);
        result = result->parent;
    }
    // 清空临时开闭列表，防止重复执行GetPath导致结果异常
    openList.clear();
    closeList.clear();
    return path;
}

Point *AStar::IsInList(const std::list<Point *> &list, const Point *point) const {
    // 判断某个节点是否在列表中，这里不能比较指针，
    // 因为每次加入列表是新开辟的节点，只能比较坐标
    for (auto p:list)
        if (p->x == point->x && p->y == point->y)
            return p;
    return nullptr;
}

bool AStar::IsReachable(const Point *point, const Point *target, bool isIgnoreCorner) const {
    //如果点与当前节点重合、超出地图、是障碍物、或者在关闭列表中，返回false
    if (target->x < 0 || target->x > maze.size() - 1
        || target->y < 0 || target->y > maze[0].size() - 1
        || maze[target->x][target->y] == 1
        || target->x == point->x && target->y == point->y
        || IsInList(closeList, target))
        return false;
    else {
        //非斜角可以通过
        if (abs(point->x - target->x) + abs(point->y - target->y) == 1)
            return true;
        else {
            //斜对角要判断是否绊住
            if (maze[point->x][target->y] == 0 && maze[target->x][point->y] == 0)
                return true;
            else
                return isIgnoreCorner;
        }
    }
}

std::vector<Point *> AStar::GetSurroundPoints(const Point *point, bool isIgnoreCorner) const {
    std::vector<Point *> surroundPoints;
    // 获取八个放方向的节点信息
    for (int x = point->x - 1; x <= point->x + 1; x++)
        for (int y = point->y - 1; y <= point->y + 1; y++)
            if (IsReachable(point, new Point(x, y), isIgnoreCorner))
                surroundPoints.push_back(new Point(x, y));
    return surroundPoints;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132

测试代码

#include <iostream>
#include "Astar.h"
using namespace std;

int main() {
    //初始化地图，用二维矩阵代表地图，1表示障碍物，0表示可通
    vector<vector<int>> maze = {
            {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
            {1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1},
            {1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1},
            {1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1},
            {1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1},
            {1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1},
            {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}
    };
    AStar astar;
    astar.InitAStar(maze);
    //设置起始和结束点
    Point start(1, 1);
    Point end(6, 10);
    //A*算法找寻路径
    list<Point *> path = astar.GetPath(start, end, false);
    //打印
    for (auto &p:path)
        cout << '(' << p->x << ',' << p->y << ')' << endl;
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

测试结果