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

整个程序下载地址如下:https://download.csdn.net/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中的效果:

《手把手教用matlab做无人驾驶(二)-路径规划A*算法》

《手把手教用matlab做无人驾驶(二)-路径规划A*算法》

 

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

 

 

    原文作者:蚁群算法
    原文地址: https://blog.csdn.net/caokaifa/article/details/82314431
    本文转自网络文章,转载此文章仅为分享知识,如有侵权,请联系博主进行删除。
点赞