【路径规划】基于matlab A_star算法机器人栅格地图路径规划【含Matlab源码 116期】

一、获取代码方式

获取代码方式1：
通过订阅紫极神光博客付费专栏，凭支付凭证，私信博主，可获得此代码。

获取代码方式2：
通过紫极神光博客主页开通CSDN会员，凭支付凭证，私信博主，可获得此代码。

获取代码方式3：
完整代码已上传我的资源：【路径规划】基于matlab A_star算法机器人栅格地图路径规划【含Matlab源码 116期】

备注：开通CSDN会员，仅只能免费获得1份代码（有效期为开通日起，三天内有效）；
订阅紫极神光博客付费专栏，可免费获得2份代码（有效期为订阅日起，三天内有效）；

二、A_star算法简介

1 A Star算法及其应用现状
进行搜索任务时提取的有助于简化搜索过程的信息被称为启发信息.启发信息经过文字提炼和公式化后转变为启发函数.启发函数可以表示自起始顶点至目标顶点间的估算距离, 也可以表示自起始顶点至目标顶点间的估算时间等.描述不同的情境、解决不同的问题所采用的启发函数各不相同.我们默认将启发函数命名为H (n) .以启发函数为策略支持的搜索方式我们称之为启发型搜索算法.在救援机器人的路径规划中, A Star算法能结合搜索任务中的环境情况, 缩小搜索范围, 提高搜索效率, 使搜索过程更具方向性、智能性, 所以A Star算法能较好地应用于机器人路径规划相关领域.

2 A Star算法流程
承接2.1节, A Star算法的启发函数是用来估算起始点到目标点的距离, 从而缩小搜索范围, 提高搜索效率.A Star算法的数学公式为:F (n) =G (n) +H (n) , 其中F (n) 是从起始点经由节点n到目标点的估计函数, G (n) 表示从起点移动到方格n的实际移动代价, H (n) 表示从方格n移动到目标点的估算移动代价.

如图2所示, 将要搜寻的区域划分成了正方形的格子, 每个格子的状态分为可通过(walkable) 和不可通过 (unwalkable) .取每个可通过方块的代价值为1, 且可以沿对角移动 (估值不考虑对角移动) .其搜索路径流程如下:

图2 A Star算法路径规划
Step1:定义名为open和closed的两个列表;open列表用于存放所有被考虑来寻找路径的方块, closed列表用于存放不会再考虑的方块;
Step2:A为起点, B为目标点, 从起点A开始, 并将起点A放入open列表中, closed列表初始化为空;
Step3:查看与A相邻的方格n (n称为A的子点, A称为n的父点) , 可通过的方格加入到open列表中, 计算它们的F, G和H值.将A从open移除加入到closed列表中;
Step4:判断open列表是否为空, 如果是, 表示搜索失败, 如果不是, 执行下一步骤;
Step5:将n从open列表移除加入到closed列表中, 判断n是否为目标顶点B, 如果是, 表示搜索成功, 算法运行结束;
Step6:如果不是, 则扩展搜索n的子顶点:
a.如果子顶点是不可通过或在close列表中, 忽略它.
b.子顶点如果不在open列表中, 则加入open列表, 并且把当前方格设置为它的父亲, 记录该方格的F, G和H值.
Step7:跳转到步骤Step4;
Step8:循环结束, 保存路径.从终点开始, 每个方格沿着父节点移动直至起点, 即是最优路径.A Star算法流程图如图3所示.

图3 A Star算法流程

三、部分源代码

%% A* democlc;
clear;
% pause(3);
%% init
n=30;
starNum=1;
% starNum = randi(n*n,[1,1]);
goalNum=172;
goalNum = randi(n*n,[1,1]);
banper=0.25;
%% map init
figure('name','A*','NumberTitle','off','MenuBar','none');
global point
for ii=1:n*npoint(ii).num = ii;point(ii).father=[];point(ii).Gcost=[];point(ii).Hcost=[];
end
%% banper
banList=[randi(n*n,[1,floor(banper*n*n)])];
load banList
banList(find(banList==goalNum))=[];
for jj = 1:length(banList)if banList(jj)~=goalNum || banList(jj)~=starNumpoint(banList(jj)).Gcost = Inf;end
end
point(starNum).Gcost=0;
point(starNum).father = point(starNum).num;
point(starNum).Hcost=getHcost(point(starNum),point(goalNum),n);%% A*core
openList = [];
closeList = [];
closeListNum=[];
openListNum=[];
openList = [openList,point(starNum)];
while length(openList)% opneListcostList = getCost(openList,point(goalNum),n);currentPoint = openList(find(costList==min(costList),1));openList(find(min(costList)==costList,1))=[];closeList = [closeList,currentPoint];neighbourNum = getNeighbour(currentPoint,n);closeListNum = cat(1,closeList.num);openListNum = cat(1,openList.num);for ii = 1:length(neighbourNum)if neighbourNum(ii)==point(goalNum).numpoint(neighbourNum(ii)).father = currentPoint.num;point(goalNum).father = currentPoint.num;disp('ok')routPlot(goalNum,n);return;endlog1=0;trytmp=point(neighbourNum(ii)).Gcost;if tmp ==inflog1 = 1;endcatchlog1=0;endif log1 || ismember(neighbourNum(ii),closeListNum)continue;elseif (ismember(neighbourNum(ii),openListNum))oldGcost = getGcost(point(neighbourNum(ii)),n);father = point(neighbourNum(ii)).father;point(neighbourNum(ii)).father = currentPoint.num;newGcost = getGcost(point(neighbourNum(ii)),n);if newGcost>oldGcostpoint(neighbourNum(ii)).father = father;elsepoint(neighbourNum(ii)).Gcost = newGcost;endcontinue;elseif ~ismember(neighbourNum(ii),closeListNum)point(neighbourNum(ii)).father = currentPoint.num;point(neighbourNum(ii)).Gcost = getGcost(point(neighbourNum(ii)),n);point(neighbourNum(ii)).Hcost = getHcost(point(neighbourNum(ii)),point(goalNum),n);openList = [openList,point(neighbourNum(ii))];endendcloseListNum = cat(1,closeList.num);openListNum = cat(1,openList.num);pause(0.1);mydrawnow(starNum,goalNum,banList,closeListNum,openListNum,n);
end## 四、运行结果
![在这里插入图片描述](https://img-blog.csdnimg.cn/2021011309440483.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L1RJUUNtYXRsYWI=,size_16,color_FFFFFF,t_70)
## 五、matlab版本及参考文献
**1 matlab版本**
2014a**2 参考文献**
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]钱程,许映秋,谈英姿.A Star算法在RoboCup救援仿真中路径规划的应用[J].指挥与控制学报. 2017,3(03)