a star 算法 matlab,路径规划A*算法matlab代码注释

路径规划A*算法matlab代码注释

一、Aplanning.m

disp('A Star Path Planing start!!')

p.start=[1,1]; %起始点

p.goal=[3,5]; %目标点

p.XYMAX=11; %%代表我们要画一个地图的长和宽

obstacle=GetBoundary(p);%得到边界数据

nObstacle=20;%在地图中随机加入20个障碍物

obstacle=GetObstacle(nObstacle,obstacle,p);%障碍物和边界

path=AStar(obstacle,p);%A*算法

%绘制地图障碍物起点终点

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

grid on;

主要实现功能如下

1、确定起始点、目标点；

2、定义一个地图，并生成一些随机障碍物；

3、调用A算法函数，A函数返回值为路径；

4、绘制地图、地图障碍物、起点终点、路径

二、Astar.m

function path=AStar(obstacle,p)

path=[];%用于存储路径

%OpenList

open=[p.start(1) p.start(2) h(p.start,p.goal) p.start(1) p.start(2)];

%CloseList

close=[];

next=MotionModel();%更新状态--下一步的八个点

findFlag=false;%目标标志

while ~findFlag

if isempty(open(:,1)) disp('No path to goal!!');

return;

end

[Y,I] = sort(open(:,3))%对OpenList中第三列排序

open=open(I,:);

%判断是否达到目标

if isSamePosi(open(1,1:2),p.goal)

disp('Find Goal!!');

close=[open(1,:);close]

open(1,:)=[];

findFlag=true;

break;

end

%找到最优轨迹放到close list保存

for in=1:length(next(:,1))

m=[open(1,1)+next(in,1) open(1,2)+next(in,2) open(1,3)];

m(3)=next(in,3)+h(m(1:2),p.goal);%m--F值

if isObstacle(m,obstacle)

continue;

end

%判断周围的点在不在openlist中。如果不在则加入到open list中

[flag, targetInd]=FindList(m,open,close)

if flag==1 %在Openlist中

disp('已存在OpenList中')

continue;

elseif flag==2

disp('已存在CloseList中')

continue;

else

open=[open;[m open(1,1) open(1,2)]]

end

end

if findFlag==false

%将OpenList中的第一行移动到CloseList中

close=[close; open(1,:)]

open(1,:)=[];

end

end

path=GetPath(close,p.start);

实现不揍为：

1、将起始点存放进OpenList中

2、对OpenList中的第三列(代价函数值)进行排序

3、计算当前点周围八个点的F值，并存入OpenList中

4、将OpenList中的第一行移动到CloseList中

三、其他函数

1.GetBoundary( p )

获得地图边界函数

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

end

2.GetObstacle(nob,obstacle,p)

获得地图中的障碍物

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];

3.isObstacle(m,obstacle)

判断当前点是否与障碍物重合

for io=1:length(obstacle(:,1))

if isSamePosi(obstacle(io,:),m(1:2))

flag=true;return;

end

end

flag=false;

end

4.GetPath(close,start)

获取路径

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