路径规划——A-Star算法

什么是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_;
};

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;
}

测试代码

#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;
}

测试结果