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

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

文中所使用的代码出自手把手教用matlab做无人驾驶（三）-路径规划A*算法

一、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