自动驾驶-决策规划算法十一：A*算法(C++)
本文介绍无人驾驶决策规划算法中的A*算法，及A*与Dijkstra、BestFS算法的对比。
和之前的算法文章中不同，为了方便表示格栅化地图，本文的地图用二维数组的数据结构表达，而为了方便表达路径点之间的关系，此关系用树（八叉树）的数据结构表达。
语言用的是C++，图形基于easyX库输出。
如有错误或者遗漏之处，欢迎指出。
附赠自动驾驶学习资料和量产经验：链接
基本概念：
/*
A*算法：一种启发式搜索，每走一步都要计算代价，选择代价最小的走法；
f = g + h；
f：总代价；
g：起点到中间点已经付出的代价，已知；也可以用上个点到当前点所走过的距离，因为起点到上个点走过的距离都是一样的；
h：中间点到终点的代价，估算；
//有些场合还要考虑权重，加个w表示权重；
h的计算通常有三种：曼哈顿距离和欧式距离；
曼哈顿距离：两个点之间的横坐标距离加上纵坐标距离；
欧式距离：两个点之间的直线距离；如果允许用float或double变量，可以用；
对角距离：就是两个斜着相邻点之间的距离与横纵坐标综合计算出来的距离，介于曼哈顿距离和欧式距离之间；
//h的计算不考虑障碍物；
为了方便，把地图定义为二维地图，然后格栅化，这样地图就由很多个以正方形为最小单位的格子构成；
每个格子可以向周围8个方向移动，也就是可以走直线，也可以走斜线；
为了方便，把每步走直线的代价设定为10，每步走斜线的代价设定为14，这样可以用int类型来计算；实际项目中应根据需要定义；
如果让h始终为0，则f完全等于g，这就是Dijkstra算法了；
如果h(n)始终小于等于节点n到终点的代价，A*算法才能保证一定能够找到最短路径；
如果h(n)的值比节点n到终点的代价要大，则A*算法不能保证找到最短路径；
所以A*算法求出的不一定是最短路径；
如果让g始终为0，f完全等于h，也就是只考虑h，那么这就是最佳优先算法；
最佳优先算法：有时候可以先计算出每个节点到终点的距离，让h成为已知量，这样可以大幅加快算法速度，减少遍历的点数；但是如果起点和终点之间有障碍物，就不一定能找到最短路径；
所以，A*算法可以通过调节h,来控制算法的速度和精确度，这也是A*算法比较灵活的地方；
在一些场合，并不一定要找到最短路径，只是希望能尽快找到一条路径，这时候可以把h设大一点；
而如果需要精确避障、对速度要求不高，则可以把h设小一点；
几种全局路径规划算法比较：
Dijkstra算法和动态规划算法，更合适处理多个城市之间给定起点到终点的最短路径问题，城市与城市之间是否连通、之间的距离都是已知的；
如果要求解任意两点之间最短距离，可以用Floyd算法，是动态规划的一种；
在一些场合，并不一定要找到最短路径，只是希望能尽快找到一条路径，可以用最佳优先算法；
蚁群算法更适合处理多个城市之间TSP商旅问题，城市与城市之间是否连通、之间的距离都是已知的，也就是从给定的起点遍历所有城市后回到起点的最短路径；
A*算法更合适处理一大块区域中给定起点和终点，没有具体的中间结点或城市，需要避开障碍物求解最短路径问题，这块区域通常需要先格栅化，有点类似有限元的思想；
A*算法常用于机器人寻路、游戏中的寻路；
*/
Astar.h
#pragma once
#include<iostream>
#include<graphics.h>
#include<vector>
#include<graphics.h>
#include<string>
#include<ctime>
using namespace std;
constexpr auto ZXCOST = 10;//每步走直线的代价
constexpr auto XXCOST = 14;//每步走斜线的代价
constexpr auto MAPTYPE = 2;//地图类型
constexpr auto ROWS = 60;//地图行数 //60
constexpr auto COLS = 50;//地图列数 //50
constexpr auto Swidth = 1200;//1200
constexpr auto Sheight = 1400;//1400
constexpr auto ratio = 2;//绘图比例
enum algoType//算法类型
{
Astar,//Astar算法
Dijkstra,//Dijkstra算法
BestFS//最佳优先算法
};
enum direct//方向
{
p_up,//上
p_down,//下
p_left,//左
p_right,//右
p_l_up,//左上
p_r_up,//右上
p_l_down,//左下
p_r_down//右下
};
class Point//地图中的最小单元，也就是点
{
public:
int row;//在地图中的行，不是坐标
int col;//在地图中的列，不是坐标
int f = 0;//总代价
int g = 0;//起点到该点的代价，已知
int h = 0;//该点到终点的代价，估算
};
class Map//地图
{
public:
Map();//初始化地图
void showMap();//打印地图
void drawMap();//绘制地图
void drawRec(int row, int col, COLORREF color = WHITE);//根据行数和列数，求矩形四条边位置，然后绘制矩形
void delay(int time);//延时
public:
int myMap[ROWS][COLS] = { 0 };//地图用二维数组表示
Point beginP = { 1, 1 };//{ ROWS - 2, 1 };//{1, 1};//起点
Point endP = { ROWS - 2, COLS - 2};//{6, 7};//终点 //这样初始化后，后面的f g h会赋值为0；
int disX = (Swidth - (COLS - 1) * ZXCOST * ratio) / 2;//左上点离窗口左边的距离，让地图居中
int disY = (Sheight - (ROWS - 1) * ZXCOST * ratio) / 2;//左上点离窗口上边的距离，让地图居中
};
class treeNode//用树的结构存储点与点之间的关系，这里是树的结点
{
public:
treeNode(int row, int col);//初始化结点
public:
Point pos;
vector<treeNode*> child;//指向孩子的指针，每个结点可以有多个孩子，所以直接用容器表示；
treeNode* pParent = nullptr;//指向父结点的指针，每个结点只有一个父结点；
};
class tree//用树的结构存储点与点之间的关系：每个点的孩子表示这个点可走的下个点
{
public:
tree();
~tree();
void updateTree();//更新树
void showResult();//打印结果
bool bBeginPoint(treeNode* point);//是否是起点
bool bEndPoint(treeNode* point);//是否是终点
public:
Map map0;//地图对象
treeNode* pRoot = nullptr;//根结点
treeNode* pCurrent = nullptr;//当前结点
treeNode* pNext = nullptr;//当前结点的相邻结点
vector<treeNode*> openList;//用来找最小f的数组，存储遍历过的点的指针；
bool closeList[ROWS][COLS] = { false };//标记走过的点，初始化为没走过
vector<treeNode*> result;//存储打印数据
//bool bEnd = false;//是否到达终点
int cost = 0;//累计代价
int costFinal = 0;//最终路径的总代价
int type = Astar;//算法类型
};
class ShortestPath //最短路径
{
public:
void shortestPath();
public:
tree tr;
};
Astar.cpp
#include"Astar.h"
Map::Map()//初始化地图
{
/*for (int i = 0; i < ROWS; i++)
{
for (int j = 0; j < COLS; j++)
{
myMap[i][j] = 0;
}
}*/
//设置障碍物
if (MAPTYPE == 0)
{
for (int i = 0; i < ROWS; i++)
{
for (int j = 0; j < COLS; j++)
{
if (j == 20 && i <= 40)
{
myMap[i][j] = 1;
}
}
}
}
if (MAPTYPE == 1)
{
for (int i = 0; i < ROWS; i++)
{
for (int j = 0; j < COLS; j++)
{
if (j == 4 && i != 6)
{
myMap[i][j] = 1;
}
if (j == 12 && i != 20)
{
myMap[i][j] = 1;
}
if (j == 20 && i != 40)
{
myMap[i][j] = 1;
}
if (j == 35 && i != 20)
{
myMap[i][j] = 1;
}
if (i == 4 && j >= 2 && j <= 4)
{
myMap[i][j] = 1;
}
if (i == 3 && j >= 1 && j <= 2)
{
myMap[i][j] = 1;
}
if (i == ROWS - 10 && j >= COLS - 10 && j < COLS)
{
myMap[i][j] = 1;
}
}
}
}
else if (MAPTYPE == 2)
{
for (int i = 0; i < ROWS; i++)
{
for (int j = 0; j < COLS; j++)
{
if (j == 3 && i >= 0 && i <= 10)
{
myMap[i][j] = 1;
}
if (j == 5 && i != 7 && i != 6)
{
myMap[i][j] = 1;
}
if (j == 11 && i >= 0 && i <= 25)
{
myMap[i][j] = 1;
}
if (j == COLS - 9 && i >= ROWS - 16)
{
myMap[i][j] = 1;
}
if (i == 40 && j >= 13 && j < COLS)
{
myMap[i][j] = 1;
}
if (i == 30 && j >= 5 && j <= 40)
{
myMap[i][j] = 1;
}
}
}
}
else if (MAPTYPE == 3)
{
for (int i = 0; i < ROWS; i++)
{
for (int j = 0; j < COLS; j++)
{
if (j == 10 && (i >= 0 && /*i <= 20 || i >= 23 &&*/ i <= 50))
{
myMap[i][j] = 1;
}
if (j == 30 && (i >= 10 && /*i <= 50 || i >= 53 &&*/ i < ROWS))
{
myMap[i][j] = 1;
}
}
}
}
showMap();
drawMap();
}
void Map::showMap()//打印地图
{
cout << "起点位置：(" << beginP.row << ", " << beginP.col << "); 终点位置：(" << endP.row << ", " << endP.col << "), 地图如下：" << endl;
myMap[beginP.row][beginP.col] = 2;//起点
myMap[endP.row][endP.col] = 9;//终点
for (int i = 0; i < ROWS; i++)
{
for (int j = 0; j < COLS; j++)
{
cout << myMap[i][j] << " ";
}
cout << endl;
}
cout << endl;
}
void Map::drawMap()//绘制地图
{
setbkcolor(WHITE); //设置背景颜色
setlinecolor(BLACK); //设置边框颜色
setlinestyle(PS_SOLID, 2); // 设置线的样式为实线2px
cleardevice();
for (int i = 0; i < ROWS; i++)
{
for (int j = 0; j < COLS; j++)
{
if (myMap[i][j] == 1)
{
drawRec(i, j, BROWN);//障碍物褐色
}
else if (i == beginP.row && j == beginP.col)
{
drawRec(i, j, RED);//起点红色
}
else if (i == endP.row && j == endP.col)
{
drawRec(i, j, BLUE);//终点蓝色
}
else
{
drawRec(i, j);//默认白色
}
}
}
}
void Map::drawRec(int row, int col, COLORREF color)//根据行数和列数，求矩形四条边位置，然后绘制矩形
{
setfillcolor(color);//设置填充颜色
int pointX = disX + col * ZXCOST * ratio;//X对应列
int pointY = disY + row * ZXCOST * ratio;//Y对应行
int left = pointX - ZXCOST / 2 * ratio;
int top = pointY - ZXCOST / 2 * ratio;
int right = left + ZXCOST * ratio;
int bottom = top + ZXCOST * ratio;
fillrectangle(left, top, right, bottom);
}
void Map::delay(int time) //延时函数，单位ms
{
clock_t now = clock();
while (clock() - now < time)
{
}
}
treeNode::treeNode(int row, int col)//初始化结点
{
pos.row = row;
pos.col = col;
}
tree::tree()//初始化树
{
pRoot = new treeNode(map0.beginP.row, map0.beginP.col);//起点放入树中根结点位置
closeList[map0.beginP.row][map0.beginP.col] = true;//起点初始化为走过
cout << "根结点：(" << pRoot->pos.row << ", " << pRoot->pos.col << ")" << endl;
pCurrent = pRoot;//初始化当前结点为根结点
}
tree::~tree()
{
if (pRoot != nullptr)
{
delete pRoot;
pRoot = nullptr;
}
if (pNext != nullptr)
{
delete pNext;
pNext = nullptr;
}
}
void tree::updateTree()//更新树
{
while (true)//整个寻路过程
{
BeginBatchDraw();
//openList.clear();//每轮不需要清空，因为后面会erase()；
for (int i = 0; i < 8; i++)//循环遍历每个点周围的8个点
{
pNext = new treeNode(pCurrent->pos.row, pCurrent->pos.col);//下个点初始化为当前点，因为还不知道往哪个方向走；
//这个要放在for里，不能放在构造函数里，因为每次要从中心点开始走！
//不能写成pNext = pCurrent;
//cout << "pCurrent: " << pNext->pos.row << ", " << pNext->pos.col << endl;
switch (i)
{
case p_up:
{
pNext->pos.row--;
pNext->pos.g += ZXCOST;
break;
}
case p_down:
{
pNext->pos.row++;
pNext->pos.g += ZXCOST;
break;
}
case p_left:
{
pNext->pos.col--;
pNext->pos.g += ZXCOST;
break;
}
case p_right:
{
pNext->pos.col++;
pNext->pos.g += ZXCOST;
break;
}
case p_l_up:
{
pNext->pos.row--;
pNext->pos.col--;
pNext->pos.g += XXCOST;
break;
}
case p_r_up:
{
pNext->pos.row--;
pNext->pos.col++;
pNext->pos.g += XXCOST;
break;
}
case p_l_down:
{
pNext->pos.row++;
pNext->pos.col--;
pNext->pos.g += XXCOST;
break;
}
case p_r_down:
{
pNext->pos.row++;
pNext->pos.col++;
pNext->pos.g += XXCOST;
break;
}
default:
break;
}
//cout << "pNext: " << pNext->pos.row << ", " << pNext->pos.col << endl;
//system("pause");
if (pNext->pos.row < 0 || pNext->pos.row >= ROWS || pNext->pos.col < 0 || pNext->pos.col >= COLS)
{
cout << "超出地图边界" << endl;
continue;
}
//cout << closeList[pNext->pos.row][pNext->pos.col] << ", pNxet: " << pNext->pos.row << ", " << pNext->pos.col << endl;
if (map0.myMap[pNext->pos.row][pNext->pos.col] == 1 || closeList[pNext->pos.row][pNext->pos.col]) //是障碍物或者走过
{
continue;
}
//计算h值
int X = abs(pNext->pos.row - map0.endP.row);
int Y = abs(pNext->pos.col - map0.endP.col);
pNext->pos.h = (X + Y) * ZXCOST;//曼哈顿距离
//计算f值
if (type == Astar)
{
pNext->pos.f = cost + pNext->pos.h;//Astar算法，这里g只能表示单步的代价值，而cost才是从起点到该点的累计代价值
}
else if (type == Dijkstra)
{
pNext->pos.f = cost;//Dijkstra算法
}
else if (type == BestFS)
{
pNext->pos.f = pNext->pos.h;//最佳优先算法
}
pCurrent->child.push_back(pNext);//存入树中，作为当前点的孩子
pNext->pParent = pCurrent;//下个点的父结点指向当前点
bool flag = false;
for (auto iter = openList.begin(); iter != openList.end(); iter++)
{
if ((*iter)->pos.row == pNext->pos.row && (*iter)->pos.col == pNext->pos.col)
{
flag = true;//防止重复存入相同的点
break;
}
}
if (!flag)
{
openList.push_back(pNext);//存入openList数组中，用于寻找最小f的点
}
//cout << "pNext: " << pNext->pos.row << ", " << pNext->pos.col << endl;
//system("pause");
}
/*for (auto iter = openList.begin(); iter != openList.end(); iter++)
{
cout << (*iter)->pos.row << ", " << (*iter)->pos.col << endl;
}*/
//system("pause");
//从openList数组中选取f最小的点
auto itMin = openList.begin();
for (auto it = openList.begin(); it != openList.end(); it++)
{
if ((*it)->pos.f < (*itMin)->pos.f)
{
itMin = it;
}
}
pCurrent = *itMin;//走到这个点
closeList[pCurrent->pos.row][pCurrent->pos.col] = true;//标记为走过
map0.myMap[pCurrent->pos.row][pCurrent->pos.col] = 5;//显示为5
cost += (*itMin)->pos.g;//中间试探点的累计路径长度
cout << "f值最小的点是：(" << (*itMin)->pos.row << ", " << (*itMin)->pos.col << "), f=" << (*itMin)->pos.f << ", g=" << (*itMin)->pos.g << ", h=" << (*itMin)->pos.h << ", 累计代价是：" << cost << endl;
itMin = openList.erase(itMin);//从openList数组里删除这个点
/*for (auto iter = openList.begin(); iter != openList.end(); iter++)
{
cout << (*iter)->pos.row << ", " << (*iter)->pos.col << endl;
}*/
//system("pause");
/*for (int i = 0; i < ROWS; i++)
{
for (int j = 0; j < COLS; j++)
{
cout << closeList[i][j] << " ";
}
cout << endl;
}
cout << endl;*/
//判断是否找到终点
if (bEndPoint(pCurrent))
{
cout << "已到达终点" << endl << endl;
showResult();
break;//到达终点，退出while循环
}
else
{
map0.drawRec(pCurrent->pos.row, pCurrent->pos.col, YELLOW);//用黄色填充试探的路径点
}
if (openList.empty())//判断是否走完了整个地图
{
cout << "没有找到终点" << endl << endl;
break;//走完了整个地图都没找到终点，也退出while循环
}
EndBatchDraw();
//map0.delay(100);//用于实时展示
}
}
void tree::showResult()//打印结果
{
while (pCurrent)//循环遍历树，直到pCurrent为空，也就是到根结点为止
{
BeginBatchDraw();
//cout << "(" << pCurrent->pos.row << ", " << pCurrent->pos.col << ") ";//这样直接打印是逆序的，从终点到起点；
result.insert(result.begin(), pCurrent);//头插存入容器，不然只能逆序打印
costFinal += pCurrent->pos.g;//累计路径长度
if (!bBeginPoint(pCurrent) && !bEndPoint(pCurrent))//如果不是起点和终点
{
map0.myMap[pCurrent->pos.row][pCurrent->pos.col] = 6;//用6标记最终的路径点
map0.drawRec(pCurrent->pos.row, pCurrent->pos.col, GREEN);//用绿色填充最终的路径点
}
pCurrent = pCurrent->pParent;//指针向上移动到父结点
EndBatchDraw();
}
map0.showMap();
string typeName;
if (type == Astar)
{
typeName = "Astar";
}
else if (type == Dijkstra)
{
typeName = "Dijkstra";
}
else if (type == BestFS)
{
typeName = "BestFS";
}
cout << typeName << "算法求得的最短路径是：" << endl;
for (auto it = result.begin(); it != result.end(); it++)
{
cout << "(" <<(*it)->pos.row << ", " << (*it)->pos.col << ")";
if (!bEndPoint(*it))//不是终点就打印箭头
{
cout << " -> ";
}
}
cout << endl;
cout << "总路径长度是：" << costFinal << endl;
}
bool tree::bBeginPoint(treeNode* point)//是否是起点
{
if (point->pos.row == map0.beginP.row && point->pos.col == map0.beginP.col)
{
return true;
}
return false;
}
bool tree::bEndPoint(treeNode* point)//是否是终点
{
if (point->pos.row == map0.endP.row && point->pos.col == map0.endP.col)
{
return true;
}
return false;
}
void ShortestPath::shortestPath()//求解路径
{
system("pause");
tr.updateTree();
}
int main()
{
initgraph(Swidth, Sheight, EW_SHOWCONSOLE);// easyx初始化，同时创建图形窗口和命令窗口
ShortestPath sPath;
sPath.shortestPath();
system("pause");
closegraph();
return 0;
}
测试结果：
红色点代表起点，蓝色点代表终点，褐色点代表障碍物，黄色点代表搜索过程中试探到的点，绿色点代表此算法求出的最终路径点；
简单地图：
较复杂地图：
更复杂的地图：
可以看出，在各种地图下，只要从起点到终点存在可到达的路径，三种算法都有解；
算法速度方面，Dijkstra算法速度通常最慢，BestFS算法速度通常最快，而A*算法速度通常介于两者之间；
算法精确度方面，Dijkstra算法通常能准确地求出最短路径，而BestFS算法和A*算法求得的不一定是最短路径，但A*算法求出的路径通常会比BestFS算法的路径长度更短；
算法遍历格栅数方面，Dijkstra算法遍历到的格栅数通常最多，几乎要遍历整个地图，因此速度相对最慢；而BestFS算法遍历到的格栅数通常最少，因此最快；A*算法通常介于两者之间；
所以，在实际问题中，应根据实际情况选取合适的算法、选取合适的h值（启发函数），来综合考虑到算法的精确度和速度。