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

这里，我们更新主程序如下：

%
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);
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
grid on;

其实，这里与前面对比，主要增加了path=AStar(obstacle,p)这调语句，这个function 函数AStar就是完成A*算法的程序，我首先给出它的程序，然后来分析它

function path=AStar(obstacle,p)
path=[];
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
if isempty(open(:,1)) disp('No path to goal!!');
return;
end
[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)];
%m(3)=m(3)+next(in,3)+h(m(1:2),p.goal)-h(open(1,1:2),p.goal);
m(3)=next(in,3)+h(m(1:2),p.goal);
if isObstacle(m,obstacle) continue; end
[flag, targetInd]=FindList(m,open,close)
if flag==1
disp("cao")
continue;
elseif flag==2
continue;
else
open=[open;[m open(1,1) open(1,2)]]
end
end
if findFlag==false
close=[close; open(1,:)]
open(1,:)=[];
end
end
path=GetPath(close,p.start);

这里，通过next=MotionModel()去更新状态，整个状态完成findFlag在while完成，

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

这个就是判断有没有可能到达目标，因为障碍物是随机产生的，所有是有这个可能的，

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)];
%m(3)=m(3)+next(in,3)+h(m(1:2),p.goal)-h(open(1,1:2),p.goal);
m(3)=next(in,3)+h(m(1:2),p.goal);
if isObstacle(m,obstacle) continue; end
[flag, targetInd]=FindList(m,open,close)
if flag==1
disp("cao")
continue;
elseif flag==2
continue;
else
open=[open;[m open(1,1) open(1,2)]]
end
end
if findFlag==false
close=[close; open(1,:)]
open(1,:)=[];
end

这个是循环找到最优的轨迹放到close里面保存下来。

最后，看看matlab仿真效果：

这个整个程序我会上传上去，地址如下：

https://download.csdn.net/download/caokaifa/10641075

(22条消息) 手把手教用matlab做无人驾驶（三）-路径规划A*算法_caokaifa的博客-CSDN博客

最后

以上就是辛勤嚓茶为你收集整理的手把手教用matlab做无人驾驶（三）-路径规划A*算法的全部内容，希望文章能够帮你解决手把手教用matlab做无人驾驶（三）-路径规划A*算法所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。