简介
代码为二维平面A星算法的基础代码，没有改进，启发式代价的距离使用的是最简单的曼哈顿距离，代码为本人依据网上代码手敲，限于本人代码水平有限，若有问题，欢迎大佬指正。
A*算法原理
A*算法通过计算和量化每走一步到一个点的代价，来选择最优路径，其可以走直线也可以走斜线，一定可以找到最优路径，开销小，适用于大型地图寻路。
上述说到量化走出一步到一个点的代价，包括启发式代价（预期代价）和已付出代价，公式如下：
f为评估代价，即为起点到这个点的代价g和这个点到终点的预期代价之和
g的计算是通过到这个点的方式累加的，若直线代价为10，斜线代价为14（符合勾股定理即可，比例为根号2），若到这个点是从起点走3段直线，2段斜线，则g值为58。
h值是当前点到目标点的距离代价，通常距离可以为曼哈顿距离、欧几里得距离、对角线距离，示意图如下：
通过不断比较f值迭代，最终找到最优路径。
A*算法改进方向简介
改进方向有以下几点：
1、启发函数——曼哈顿距离等
2、权重系数——动态加权等
3、搜索邻域——基于8邻域搜索改进
4、搜索策略——双向搜索、JPS策略等
5、路径平滑处理——贝塞尔曲线、B样条曲线等
C++代码
以下为C++代码：
#include<iostream>
#include<vector>
using namespace std;
//地图行和列数，修改地图行列需要将下面二维数组地图一并修改
#define ROWS 10
#define COLS 10
//直线代价和斜线代价，取1和1.4的话后面的f，g，h数据类型需要改为浮点型
#define ZXDJ 10
#define XXDJ 14
enum dir { p_up, p_down, p_left, p_right, p_uleft, p_uright, p_dleft, p_dright };
//点结构体，每个点需要有行号，列号，以及代价值
struct MyPoint
{
int row, col; //数组下标 表示地图位置
int f = 0;
int g = 0;
int h = 0;//量化评估
};
//八叉树结构体
struct treeNode
{
MyPoint Pos;//树节点
vector<treeNode*> child;//维护孩子节点的动态数组
treeNode* pParent;//父节点只有一个，不需要数组来存放
};
//创建树节点
treeNode* createTreeNode(int row, int col)
{
treeNode* pNew = new (treeNode);
pNew->Pos.row = row;
pNew->Pos.col = col;
return pNew;
}
//获取h值函数
int getH(MyPoint pos, MyPoint endPos)
{
int y = ((pos.row > endPos.row) ? (pos.row - endPos.row) : (endPos.row - pos.row));
int x = ((pos.col > endPos.col) ? (pos.col - endPos.col) : (endPos.col - pos.col));
return ZXDJ * (x + y);
}
int main()
{
//设置地图
int map[ROWS][COLS] = {
{0,0,0,0,1,0,0,0,0,0},
{0,0,1,0,1,0,0,0,0,0},
{0,0,1,1,1,0,0,0,0,0},
{0,1,0,1,1,0,0,0,0,0},
{0,1,1,0,1,0,0,0,0,0},
{0,0,1,1,1,0,0,0,0,0},
{0,0,0,0,1,0,0,0,0,0},
{0,0,0,0,1,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,1,0,0,0,0,0},
};
//（closelist）辅助地图，bool类型的二维数组，用来判断此点有没有走过，0 False 没有走过 1 True 已经走过
bool pathMap[ROWS][COLS] = { 0 };
//设置起点终点
MyPoint begPos = { 2,1 };
MyPoint EndPos = { 6,7 };
//建立八叉树结构，根节点即起点
treeNode* pRoot = createTreeNode(begPos.row, begPos.col);
//标记起点走过，先将起点标记为走过
pathMap[begPos.row][begPos.col] = true;
//准备数组（openlist）
vector<treeNode*> buff;
vector<treeNode*>::iterator it; //遍历迭代器
vector<treeNode*>::iterator it_MIN; //最小记录迭代器
//从起点开始
treeNode* pCurrent = pRoot; //当前点
treeNode* pChild = NULL;
bool IsFineEnd = false;
while (1)
{
//找出周围能够走到点 <=8
for (int i = 0; i < 8; i++)
{
int flag1 = 1;
pChild = createTreeNode(pCurrent->Pos.row, pCurrent->Pos.col);
switch (i)
{
case p_up:
pChild->Pos.row--;
pChild->Pos.g =pCurrent->Pos.g + ZXDJ;
break;
case p_down:
pChild->Pos.row++;
pChild->Pos.g = pCurrent->Pos.g + ZXDJ;
break;
case p_left:
pChild->Pos.col--;
pChild->Pos.g = pCurrent->Pos.g + ZXDJ;
break;
case p_right:
pChild->Pos.col++;
pChild->Pos.g = pCurrent->Pos.g + ZXDJ;
break;
case p_uleft:
pChild->Pos.row--;
pChild->Pos.col--;
pChild->Pos.g = pCurrent->Pos.g + XXDJ;
break;
case p_uright:
pChild->Pos.row--;
pChild->Pos.col++;
pChild->Pos.g = pCurrent->Pos.g + XXDJ;
break;
case p_dleft:
pChild->Pos.row++;
pChild->Pos.col--;
pChild->Pos.g = pCurrent->Pos.g + XXDJ;
break;
case p_dright:
pChild->Pos.row++;
pChild->Pos.col++;
pChild->Pos.g = pCurrent->Pos.g + XXDJ;
break;
}//end of switch
//检查pChlid可行性
if (map[pChild->Pos.row][pChild->Pos.col] == 0 && pathMap[pChild->Pos.row][pChild->Pos.col] == false) //不是障碍物且没有走过
{
pChild->Pos.h = getH(pChild->Pos, EndPos); //计算启发式代价
pChild->Pos.f = pChild->Pos.g + pChild->Pos.h; //计算综合代价
//遍历buff（即openlist），判断这点是否buff（openlist）中，若在则比较更新
for (it = buff.begin(); it != buff.end(); it++)
{
if (((*it)->Pos.row == pChild->Pos.row && (*it)->Pos.col == pChild->Pos.col))
{
flag1 = 0;
//在openlist中，新路径到这点的g值小于buff（openlist）数组中路径到这点的g值，则更新，否则不更新释放即可
if (pChild->Pos.g < (*it)->Pos.g)
{
(*it)->Pos.g = pChild->Pos.g;
(*it)->Pos.h = pChild->Pos.h;
(*it)->Pos.f = pChild->Pos.f;
break;
}
else
{
delete pChild;
}
}
}
//若不再openlist中，说明这点从未开过，加入buff（openlist）数组
if (flag1)
{
pCurrent->child.push_back(pChild); //将子节点加入八叉树
pChild->pParent = pCurrent; //给定子节点的父节点,方便后续打印路径
buff.push_back(pChild); //加入数组
}
}
else
{
delete pChild;
}//end of if else
}//end of for
//寻找最小f
it_MIN = buff.begin();
for (it = buff.begin(); it != buff.end(); it++)
{
if ((*it)->Pos.f < (*it_MIN)->Pos.f)
{
it_MIN = it;
}
}
//前进
pCurrent = *it_MIN;
pathMap[pCurrent->Pos.row][pCurrent->Pos.col] = true;//更新辅助地图（closelist）
buff.erase(it_MIN);//将走过的点删除
//判断成功与否
if (pCurrent->Pos.row == EndPos.row && pCurrent->Pos.col == EndPos.col)
{
IsFineEnd = true;
break;
}
if (buff.size() == 0)
{
break;
}
}//end of while
//显示结果
if (IsFineEnd)
{
printf("已经找到终点啦！\n");
while (true)
{
printf("(%d,%d)", pCurrent->Pos.row, pCurrent->Pos.col);
pCurrent = pCurrent->pParent; //沿着节点往上走
if (pCurrent->Pos.row == begPos.row && pCurrent->Pos.col == begPos.col)
{
printf("(%d,%d)", pCurrent->Pos.row, pCurrent->Pos.col);
break;
}
}
}
else
{
printf("没有找到可行路径！\n");
}
return 0;
}Matlab代码
以下为matlab主程序代码：
close;
clear;
clc;
% 定义栅格地图的行数、列数
m = 5;
n = 7;
%起点
start_node = [2,3];
%终点
target_node = [6,3];
%障碍物
obs = [4,2;4,3;4,4;4,5];
%画栅格横线
for i = 1:m
plot([0,n], [i, i], 'k');
hold on
end
%画栅格竖线
for j = 1:n
plot([j, j], [0, m], 'k');
end
axis equal%等比坐标轴，使得每个坐标轴都具有均匀的刻度间隔
%xy轴上下限
xlim([0, n]);
ylim([0, m]);
% 绘制障碍物、起止点颜色块
fill([start_node(1)-1, start_node(1), start_node(1), start_node(1)-1],...
[start_node(2)-1, start_node(2)-1 , start_node(2), start_node(2)], 'g');
fill([target_node(1)-1, target_node(1), target_node(1), target_node(1)-1],...
[target_node(2)-1, target_node(2)-1 , target_node(2), target_node(2)], 'r');
for i = 1:size(obs,1)%返回矩阵行数
temp = obs(i,:);
fill([temp(1)-1, temp(1), temp(1), temp(1)-1],...
[temp(2)-1, temp(2)-1 , temp(2), temp(2)], 'b');
end
% 初始化closeList
closeList = start_node;
closeList_path = {start_node,start_node};
closeList_cost = 0;
child_nodes = child_nodes_cal(start_node, m, n, obs, closeList); %子节点搜索函数
% 初始化openList
openList = child_nodes;
for i = 1:size(openList,1)
openList_path{i,1} = openList(i,:);
openList_path{i,2} = [start_node;openList(i,:)];%从初始点到第i个子节点
end
for i = 1:size(openList, 1)
g = norm(start_node - openList(i,:));%norm求范数，返回最大奇异值；abs求绝对值
h = abs(target_node(1) - openList(i,1)) + abs(target_node(2) - openList(i,2));
%终点横坐标距离加纵坐标距离
f = g + h;
openList_cost(i,:) = [g, h, f];
end
%开始搜索
% 从openList开始搜索移动代价最小的节点
[~, min_idx] = min(openList_cost(:,3));%输出openlist_cost表中最小值的位置
parent_node = openList(min_idx,:);%父节点为代价最小节点
%进入循环
flag = 1;
while flag
% 父节点除去障碍物，超越地图边界和已走过的孩子节点，此处写了一个函数child_nodes_cal
child_nodes = child_nodes_cal(parent_node, m, n, obs, closeList);
% 判断这些子节点是否在openList中，若在，则比较更新；没在则追加到openList中
for i = 1:size(child_nodes,1)
child_node = child_nodes(i,:);
[in_flag,openList_idx] = ismember(child_node, openList, 'rows');%ismember函数表示子节点在open表中则返回1，判断flag,输出此子节点在openlist表中的位置
g = openList_cost(min_idx, 1) + norm(parent_node - child_node);%按照新父节点计算此子节点的g,h值
h = abs(child_node(1) - target_node(1)) + abs(child_node(2) - target_node(2));
f = g+h;
if in_flag % 若在，比较更新g和f
if g < openList_cost(openList_idx,1)
openList_cost(openList_idx, 1) = g;%将openlist_cost表中第id个位置的第一个数更新为以新父节点计算的g值
openList_cost(openList_idx, 3) = f;
openList_path{openList_idx,2} = [openList_path{min_idx,2}; child_node];
end
else % 若不在，追加到openList
openList(end+1,:) = child_node;
openList_cost(end+1, :) = [g, h, f];
openList_path{end+1, 1} = child_node;
openList_path{end, 2} = [openList_path{min_idx,2}; child_node];
end
end
% 从openList移除移动代价最小的节点到 closeList
closeList(end+1,: ) = openList(min_idx,:);
closeList_cost(end+1,1) = openList_cost(min_idx,3);
closeList_path(end+1,:) = openList_path(min_idx,:);
openList(min_idx,:) = [];%openlist表中已跳出的最小值位置设为空
openList_cost(min_idx,:) = [];
openList_path(min_idx,:) = [];
% 重新搜索：从openList搜索移动代价最小的节点（重复步骤）
[~, min_idx] = min(openList_cost(:,3));
parent_node = openList(min_idx,:);
% 判断是否搜索到终点
if parent_node == target_node
closeList(end+1,: ) = openList(min_idx,:);
closeList_cost(end+1,1) = openList_cost(min_idx,1);
closeList_path(end+1,:) = openList_path(min_idx,:);
flag = 0;
end
end
%画路径
path_opt = closeList_path{end,2};
path_opt(:,1) = path_opt(:,1)-0.5;
path_opt(:,2) = path_opt(:,2)-0.5;
scatter(path_opt(:,1), path_opt(:,2), 'k');%绘制散点图
plot(path_opt(:,1), path_opt(:,2), 'k');以下为寻找符合要求子节点函数matlab代码：
function child_nodes = child_nodes_cal(parent_node, m, n, obs, closeList)
child_nodes = [];
field = [1,1; n,1; n,m; 1,m];
% 第1个子节点
child_node = [parent_node(1)-1, parent_node(2)+1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))%判断点是否在多边形内
if ~ismember(child_node, obs, 'rows')
child_nodes = [child_nodes; child_node];
end
end
% 第2个子节点
child_node = [parent_node(1), parent_node(2)+1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
if ~ismember(child_node, obs, 'rows')
child_nodes = [child_nodes; child_node];
end
end
% 第3个子节点
child_node = [parent_node(1)+1, parent_node(2)+1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
if ~ismember(child_node, obs, 'rows')
child_nodes = [child_nodes; child_node];
end
end
% 第4个子节点
child_node = [parent_node(1)-1, parent_node(2)];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
if ~ismember(child_node, obs, 'rows')
child_nodes = [child_nodes; child_node];
end
end
% 第5个子节点
child_node = [parent_node(1)+1, parent_node(2)];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
if ~ismember(child_node, obs, 'rows')
child_nodes = [child_nodes; child_node];
end
end
% 第6个子节点
child_node = [parent_node(1)-1, parent_node(2)-1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
if ~ismember(child_node, obs, 'rows')
child_nodes = [child_nodes; child_node];
end
end
% 第7个子节点
child_node = [parent_node(1), parent_node(2)-1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
if ~ismember(child_node, obs, 'rows')
child_nodes = [child_nodes; child_node];
end
end
% 第8个子节点
child_node = [parent_node(1)+1, parent_node(2)-1];
if inpolygon(child_node(1), child_node(2), field(:,1), field(:,2))
if ~ismember(child_node, obs, 'rows')
child_nodes = [child_nodes; child_node];
end
end
%排除已经存在于closeList的节点
delete_idx = [];
for i = 1:size(child_nodes, 1)
if ismember(child_nodes(i,:), closeList , 'rows')
delete_idx(end+1,:) = i;
end
end
child_nodes(delete_idx, :) = [];我在matlab中的地图设置的特别小，可以通过matlab的调试功能，一步步步进，观察各个变量的变化，来深刻理解A*算法。C++代码用到了现成的容器，也可以自己写一个数据结构，h值是很好得到的，主要是要理解每个点的g值是怎么来的，g值是怎么更新的。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
