如果你对A*多少有过了解,但是不知道如何编程,这篇文章可以帮助你;
如果你对A*毫无了解,想熟悉了解下,这篇文章和参考文章可以帮助你;
如果你对A*了如指掌,并且是算法大师,这篇文章帮不到你,请指教一下。
我接下来基本上所有文章都是针对Atsushi Sakai这个作者在GitHub上发布的,添加注释以及自己的一些思考,希望可以帮助到一些新入门的人。希望大家对机器人方面的算法都大致有个了解,然后选择合适的算法,改进下,用到自己的学习中或者工作中去。
A星算法核心公式就是F值的计算:[1]
F = G + H
F - 方块的总移动代价
G - 开始点到当前方块的移动代价
H - 当前方块到结束点的预估移动代价
A*算法的逻辑:[2]
function [] = AStarSample()
%AStarSample() A*法による最短?路探索プログラム
%
% Author: Atsushi Sakai
%
% Reference:A*による最短?路探索MATLABプログラム - MY ENIGMA
% http://d.hatena.ne.jp/meison_amsl/20140503/1399080847
%
% Copyright (c) 2014, Atsushi Sakai
% All rights reserved.
% License : Modified BSD Software License Agreement
clear all;
close all;
disp('A Star Path Planning start!!');
%参数
p.start=[1,1]; %起始点
p.goal=[10,3]; %目标点
p.XYMAX=11; %地图的最大高度和宽度
%获取边界数据
obstacle=GetBoundary(p);
%获取障碍物数据与边界数据一起获取
nObstacle=20;%障碍物数量
obstacle=GetObstacle(nObstacle,obstacle,p);
%最短路径生成
path=AStar(obstacle,p);
%创建图
figure(1)
if length(obstacle)>=1
plot(obstacle(:,1),obstacle(:,2),'om');hold on;
end
plot(p.start(1),p.start(2),'*r');hold on;
plot(p.goal(1),p.goal(2),'*b');hold on;
if length(path)>=1
plot(path(:,1),path(:,2),'-r');hold on;
end
axis([0-0.5 p.XYMAX+1+0.5 0-0.5 p.XYMAX+1+0.5])
grid on;
end
function path=AStar(obstacle,p)
% 一个通过A *方法找到最短路径的程序
% 返回最短路径的坐标列表
path=[];%路径
%用于在计算过程中存储节点信息[x,y,cost,px(父节点),py(父节点)]存储起始节点
open=[p.start(1) p.start(2) h(p.start,p.goal) p.start(1) p.start(2)];
close=[];%用于存储计算的节点信息
%到相邻节点的运动模型通过更改此参数,可以指定机器人的运动
next=MotionModel();
findFlag=false;%目标发现标志
while ~findFlag
%如果没有open列表中没有点了,而且还没有找到目标点,则找不到路径。
if isempty(open(:,1)) disp('No path to goal!!'); return; end
%选择成本最低的开放节点,类似于优先队列,把cost最低的点放到最前面
[Y,I] = sort(open(:,3));
open=open(I,:);
%目标判断
if isSamePosi(open(1,1:2),p.goal) %判断这个点是不是目标点
disp('Find Goal!!');
%将目标节点移至“关闭”的开头
close=[open(1,:);close];open(1,:)=[];
findFlag=true;
break;
end
for in=1:length(next(:,1))
%计算相邻节点的位置和成本
m=[open(1,1)+next(in,1) open(1,2)+next(in,2) open(1,3)];%open(1,1:2)作为父节点,计算周边点的位置,权重此时是父节点的权重
%公式F= G + H;
%m(3)+next(in,3)相当于G,h(m(1:2),p.goal)相当于H,这里我认为减这一项h(open(1,1:2),p.goal)没有必要,可有可无。
m(3)=m(3)+next(in,3)+h(m(1:2),p.goal)-h(open(1,1:2),p.goal);%计算成本-h(open(1,1:2),p.goal)
%如果相邻节点是障碍物,则查找下一个节点
if isObstacle(m,obstacle) continue; end
%在打开和关闭列表中搜索m
[flag, targetInd]=FindList(m,open,close);
if flag==1 %如果在open列表上,注意是同一个节点的两个不同路径的估价值
if m(3)<open(targetInd,3) %如果新得到的cost小于open列表中的cost值
open(targetInd,3)=m(3);%更新权重,和父节点
open(targetInd,4)=open(1,1);
open(targetInd,5)=open(1,2);
end
elseif flag==2 %如果在close列表中,注意是同一个节点的两个不同路径的估价值
if m(3) < close(targetInd,3)%小于之前计算的
vpa(m(3),10)
vpa(close(targetInd,3),10)
%更新父节点
close(targetInd,4)=open(1,1);
close(targetInd,5)=open(1,2);
open=[open; close(targetInd,:)];
close(targetInd,:)=[];%转到打开列表
end
else %如果两者都不
%添加到带有父节点索引的打开列表
open=[open;[m open(1,1) open(1,2)]];
end
end
%相邻节点计算出的打开节点移动到关闭节点
if findFlag==false
close=[close; open(1,:)];
open(1,:)=[];
end
%路径搜索的步骤动画
% animation(open,close,p,obstacle);
end
%获取最短路径坐标列表
path=GetPath(close,p.start);
end
function result=h(a,b)
%启发式功能
%这里,二维空间中a和b之间的范数距离
result=norm(a-b);
end
function obstacle=GetObstacle(nob,obstacle,p)
%创建一些由随机数指定的障碍,
%一个函数,返回除起始点或目标点之外的任何东西
%用随机数创建障碍
ob=round(rand([nob,2])*p.XYMAX);
%忽略是否在开始和目标处设置障碍
removeInd=[];%用于存储要删除的障碍物索引的列表
for io=1:length(ob(:,1))
if(isSamePosi(ob(io,:),p.start) || isSamePosi(ob(io,:),p.goal))
removeInd=[removeInd;io];
end
end
ob(removeInd,:)=[];%清单
obstacle=[obstacle;ob];
end
function result=isSamePosi(a,b)
%确定2x1向量是否相同的函数
result=false;
if a(1)==b(1) && a(2)==b(2)
result=true;
end
end
function boundary=GetBoundary(p)
% 采集区域边界数据
boundary=[];
for i1=0:(p.XYMAX+1)
boundary=[boundary;[0 i1]];
end
for i2=0:(p.XYMAX+1)
boundary=[boundary;[i2 0]];
end
for i3=0:(p.XYMAX+1)
boundary=[boundary;[p.XYMAX+1 i3]];
end
for i4=0:(p.XYMAX+1)
boundary=[boundary;[i4 p.XYMAX+1]];
end
boundary=[boundary;[11 11]];
boundary=[boundary;[9 1]];
boundary=[boundary;[10 2]];
boundary=[boundary;[11 3]];
boundary=[boundary;[10 1]];
boundary=[boundary;[11 2]];
boundary=[boundary;[11 1]];
end
function animation(open,close,p,obstacle)
% 顺序显示搜索状态的功能
figure(1)
if length(obstacle)>=1
plot(obstacle(:,1),obstacle(:,2),'om');hold on;
end
plot(p.start(1),p.start(2),'*r');hold on;
plot(p.goal(1),p.goal(2),'*b');hold on;
plot(open(:,1),open(:,2),'xr');hold on;
plot(close(:,1),close(:,2),'xk');hold on;
axis([0-0.5 p.XYMAX+1+0.5 0-0.5 p.XYMAX+1+0.5])
grid on;
pause;
end
function flag=isObstacle(m,obstacle)
for io=1:length(obstacle(:,1))
if isSamePosi(obstacle(io,:),m(1:2))
flag=true;return;
end
end
flag=false;%不是障碍
end
function next=MotionModel()
%到相邻节点的运动模型通过更改此参数,可以指定机器人的运动
% [x y cost]
next=[1 1 1
1 0 1
0 1 1
-1 0 1
0 -1 1
-1 -1 1
-1 1 1
1 -1 1];
end
function path=GetPath(close,start)
%获取从开始到目标的坐标列表的功能
ind=1;%目标在关闭列表的顶部
path=[];
while 1
%在列表中注册坐标
path=[path; close(ind,1:2)];
%确定您是否已到达起点
if isSamePosi(close(ind,1:2),start)
break;
end
%在关闭列表中查找父节点
for io=1:length(close(:,1))
if isSamePosi(close(io,1:2),close(ind,4:5))
ind=io;
break;
end
end
end
end
function [flag, targetInd]=FindList(m,open,close)
targetInd=0;
%是否在open数组中
if ~isempty(open)
for io=1:length(open(:,1))
if isSamePosi(open(io,:),m(1:2))
flag=1;
targetInd=io;
return;
end
end
end
%是否在close数组中
if ~isempty(close)
for ic=1:length(close(:,1))
if isSamePosi(close(ic,:),m(1:2))
flag=2;
targetInd=ic;
return;
end
end
end
%都没有
flag=3;return;
end
这是matlab版本的,他后续的还有个python版本,等我注释都写好后,再发上来。我还准备了一个公众号,到时候把二维码贴上来。方便大家接收更新。
大家有不明白的可以留言,重点区域再更新下。
参考文献
[1] https://www.cnblogs.com/leoin2012/p/3899822.html
[2] Youtube视频