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);
A*算法 路径规划MATLAB代码
1星 需积分: 42 84 浏览量
2019-03-08
09:46:02
上传
评论 21
收藏 3KB RAR 举报
森林不缺木
- 粉丝: 6
- 资源: 2
最新资源
- 基于CarNet实现裂缝检测python源码+文档说明+数据+图片(课程设计)
- 课程设计-基于耐火材料裂缝剥落检测python源码+课件
- 基于OpenCV的视频道路车道检测python源码+文档说明+实验演示+图片+使用方法(高分毕业设计)
- 基于OpenCV的案例:图像边缘、角点和轮廓检测,图像分割,图像增强;图片拼接;运动目标检测,颜色直方图比较,三帧帧差法,抠图
- SmartPlug-html大一笔记
- SmartPlug-proteusdemo
- Preliminary Findings on Handmade Rattan Baby Crib andBassinet Designs Regarding.zip
- aveebfq_v1.2.83_downyi.com.apk
- 基于有机发光二极管(OLED)的建模优化算法的matlab仿真源码+数据+文档说明+项目说明(高分课程设计)
- hash01-test.c 本人哈希表(一)的示例代码,仅供参考!
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈