手把手教用matlab做无人驾驶（二）-路径规划A*算法

整个程序下载地址如下：https://download.youkuaiyun.com/download/caokaifa/10641075

对于路径规划算法-A*算法在matlab中仿真，首先我们在matlab中构建地图：

先给出matlab主函数程序：

%   editor:  Robert.Cao
%   2018.9.1
clc
clear all
close all
disp('A Star Path Planing start!!')
p.start=[1,1];  %起始点
p.goal=[10,3];  %目标点
p.XYMAX=11; 
obstacle=GetBoundary(p);%得到边界数据
nObstacle=20;
obstacle=GetObstacle(nObstacle,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;

 注释：代码可能直接复制放到matlab中运行可能会出错，代码本身是没错的，可能复制上去格式不对，我运行了一下因为空格的原因，所有你一条一条复制到matlab中肯定不会出错。

现在解释一下上面代码：

p.start=[1,1];  %起始点就是代表机器人的起始点坐标

p.goal=[10,3];  %机器人目标点坐标

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

obstacle=GetBoundary(p);%这里调用了function 函数GetBoundary，这个函数就是通过我们设置 p.XYMAX=11函数来取得地图的边界

nObstacle=20;%这个函数就是我们在地图中随机加入二十个障碍物

obstacle=GetObstacle(nObstacle,obstacle,p);%这个obstacle包含了障碍物和地图边界数据
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; %这段程序没什么解释的，就是画图，包含了机器人坐标的起始和终点位置。

下面，我们给出GetBoundary 函数与GetObstacle函数：

function ：GetBoundary

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 ：GetObstacle

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

现在看看matlab中的效果：

 

图中可以看出边界点，障碍物点，机器人起始坐标，终点坐标，下一篇将在这个地图中做A*算法。