1.前言
在机器人运动过程中的最优路径的选择的算法有Dijkstra和A*算法，其中A*算法是Dijkstra算法的一种改进策略.适用于已知起点和终点，求起点到终点的最短距离.对于不知道终点缺需要寻找最短路径的问题仍然使用Dijkstra算法具有普适性和天然优势.
2.A*算法迭代案例
如图所示为一张简易地图，其中绿色方块是起点（A表示），中间蓝色的是障碍物，红色的方块（B表示）是目的地.为了方便，用一个二维数组表示地图，我们将地图划分成一个个的小方块.
二维数组在游戏中的应用是很多的，比如贪吃蛇和俄罗斯方块基本原理就是移动方块.而大型游戏的地图，则将各种“地貌”铺在这样的小方块上.
寻路步骤
1. 从起点A开始，把它作为待处理的方格存入一个“开启列表”，开启列表就是一个等待检查方格的列表.
2.寻找起点A周围可以到达的方格，将它们放入“开启列表”，并设置它们的“父方格”为A.
3.从“开启列表”中删除起点A，并将起点A放入“关闭列表”，关闭列表中存放的都是不需要再次检查的顶点.
图中浅绿色描边的方块表示已经加入“开启列表”等待检查.淡蓝色描边的起点A表示已经放入“关闭列表”，它不需要再执行检查.从“开启列表”中找出相对最靠谱的方块，什么是最靠谱？它们通过公式来计算.
表示从起点A移动到网格上指定方格的移动耗费（可沿斜方向移动）.
表示从指定的方格移动到终点B的预计耗费（
有很多计算方法，这里设定为“曼哈顿距离”）.
从“开启列表”中选择值最低的方格C（绿色起始块A右边的方块），然后对它进行如下处理：
4.把它从“开启列表”中删除，并加入“关闭列表”中.
5.检查它所有相邻并且可以到达（障碍物和“关闭列表”的方格都不考虑）的方格.如果这些方格还不在“开启列表”中的话，将它们加入“开启列表”，计算这些方格的G，H和F值各是多少，并设置它们的“父节点”为C.
6.如果某个相邻方格D已经在“开启列表”中，检查如果用新的路径（就是经过C的路径）到达它的话，G值是否会更低一些，如果新的G值更低，那就把它的“父方格”改为目前选中的方格C，然后重新计算它的F和G值（H值不需要重新计算，因为对于每个方块，H值是不改变的）.如果新的G值比较高，就说明经过C再到D不是一个明智的选择，因为它需要更远的路，这时我们什么也不做.
如图4，我们选中了C因为它的F值最小，我们把它从“开启列表”中删除，并把它加入“关闭列表”.它右边上下三个都是墙，所以不考虑它们.它左边是起始方块，已经加入到“关闭列表”了，也不考虑.所以它周围的候选方块就只剩4个.然后我们看看C下面的那个格子，它目前的G是14，如果通过C到达它的话，G将会是10+10，这比14要大，所以我们什么也不做.
然后我们继续从“开启列表”中找出F值最小的，但是我们发现C上面和下面的同时为54，这时怎么办？这时随便取哪一个都行，比如选C下面的方块D.
D右边以及右上角都是墙，所以不考虑，但是为什么右下角没有被加入“开启列表”？因为如果它下面的那块也不可以走，想要到达C右下角就需要从“方块的角”走了，在程序中设置是否允许这样走.（图中的示例不允许这样走）.
就这样，我们从“开启列表”找出F值最小的，将它从“开启列表”中移除，添加到“关闭列表”.再继续找出它周围可以到达的方块，如此循环下去...
那么什么售后停止呢？——当我们发现“开始列表”中出现了目标终点方块的时候，说明路径已经找到了.
如何找回路径
如上图所示，除了起始方块，每一个曾经或者现在还在“开启列表”中的方块，它都有一个“父方块”，通过“父方块”可以索引到最初的“起始方块”，这是路径.
3.A*算法伪代码
4.A*算法C++代码实现
Astar.h文件：
#pragma once
/*
// A*算法对象类
*/
#include<vector>
#include<list>
const int kCost1 = 10; //直移一格消耗
const int kCost2 = 14; //斜移一格消耗
struct Point {
int x, y; //点坐标，这里为了方便按照C++的数组来计算，x代表横排，y代表竖列
int F, G, H; //F = G + H
Point* parent; //parent的坐标，这里没有用指针，从而简化代码
Point(int _x, int _y) : x(_x), y(_y), F(0), G(0), H(0), parent(NULL) {
}
};
class Astar {
public:
void InitAstar(std::vector<std::vector<int>>& _maze);
std::list<Point*> GetPath(Point& startPoint, Point& endPoint, bool isIgnoreConer);
private:
Point *findPath(Point& startPoint, Point& endPoint, bool isIgnoreCorner);
std::vector<Point*>getSurroundPoints(const Point* point, bool isIgnoreCorner) const;
bool isCanreach(const Point* point, const Point* target, bool isIgnoreCorner) const; //判断某点是否可以用于下一步判断
Point *isInList(const std::list<Point*>& list, const Point *point)const;//判断开启/关闭列表中是否包含某点
Point* getLeastFpoint(); //从开启列表中返回F值最小的节点
//计算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; //关闭列表
};
Astar.cpp文件：
#include<math.h>
#include "Astar.h"
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));
int parentG = point->parent == NULL ? 0 : point->parent->G; //如果是初始节点，则其父节点为空
return parentG + extraG;
}
int Astar::calcH(Point* point, Point* end) {
//用简单的欧几里得距离计算，这个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::getLeastFpoint() {
if (!openList.empty()) {
auto resPoint = openList.front();
for (auto& point : openList)
if (point->F < resPoint->F)
resPoint = point;
return resPoint;
}
return NULL;
}
Point* Astar::findPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner) {
openList.push_back(new Point(startPoint.x, startPoint.y)); //置入起点，拷贝开辟一个节点，内外隔离
while (!openList.empty()) {
auto curPoint = getLeastFpoint(); //找到F值最小的点
openList.remove(curPoint); //从开启列表中删除
closeList.push_back(curPoint); //放到关闭列表
// 1. 找到当前周围8个格中可以通过的格子
auto surroundPoints = getSurroundPoints(curPoint, isIgnoreCorner);
for (auto& target : surroundPoints) {
// 2. 对某一个格子，如果它不在开启列表中，加入到开启列表，设置当前格为其父节点，计算F G H
if (!isInList(openList, target)) {
target->parent = curPoint;
target->G = calcG(curPoint, target);
target->H = calcH(curPoint, &endPoint);
target->F = calcF(target);
openList.push_back(target);
}
// 3. 对某一格子，它在开启列表中，计算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)
return resPoint; //返回列表中的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝
}
}
return NULL;
}
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 NULL;
}
bool Astar::isCanreach(const Point* point, const Point* target, bool isIgnoreCorner) const {
if (target->x < 0 || target->x > maze.size() - 1
|| target->y < 0 || target->y > maze[0].size() - 1
|| target->x == point->x && target->y == point->y
|| isInList(closeList, target))//如果点与当前节点重合、超出地图、是障碍物、或者在关闭列表中，返回false
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 (isCanreach(point, new Point(x, y), isIgnoreCorner))
surroundPoints.push_back(new Point(x, y));
return surroundPoints;
}
main.cpp文件：
#include <iostream>
#include "Astar.h"
bool InPath(const int& row, const int& col, const std::list<Point*>& path) {
for (const auto& p : path) {
if (row == p->x && col == p->y) {
return true;
}
}
return false;
}
int main() {
// 初始化地图，用二维数组代表地图，1表示障碍物，0表示可通
std::vector<std::vector<int>> map = { {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(map);
//设置起点和结束点
Point start(1, 1);
Point end(6, 10);
//A*算法找寻路径
std::list<Point*> path = astar.GetPath(start, end, false);
//打印结束
for (auto& p : path) {
std::cout << "(" << p->x << "," << p->y << ")";
}
std::cout << "\n";
for (int row = 0; row < map.size(); row++) {
for (int col = 0; col < map[0].size(); col++) {
if (InPath(row, col, path)) {
if (map[row][col] != 0) {
std::cout << "e ";
}
else {
std::cout << "*";
}
}
else {
std::cout << map[row][col] << " ";
}
}
std::cout << "\n";
}
return 0;
}
