Matlab航迹规划仿真——A*算法
2021/12/15 17:20:33
本文主要是介绍Matlab航迹规划仿真——A*算法,对大家解决编程问题具有一定的参考价值,需要的程序猿们随着小编来一起学习吧!
文章目录
- 1. 初始化参数
- 2. 构建地图
- 3. A*算法搜索路径
- 4. 路径优化
- 5. 效果图
- 6. 下载链接
基本的A*算法在这里不再讲述,想要了解的朋友可以自己在CSDN搜索,在此主要解释下代码。
1. 初始化参数
主要参数:
- 地图大小
- 起始点和目标点坐标
clc clear all m = 30;n = 30; Spoint = [3 3]; %起始点坐标 Epoint = [29 22]; %目标点坐标
2. 构建地图
-inf
表示不可到达的障碍物点
%%构建地图 for i = 1:m+2 if i == 1 for j = 1:n+2 Matrix(i,j) = -inf; end elseif i == m+2 for j = 1:n+2 Matrix(i,j) = -inf; end else for j = 1:n+2 if ((j == 1)|(j == n+2)) Matrix(i,j) = -inf; else Matrix(i,j) = inf; end end end end %%障碍 for j=2:10 Matrix(5,j)=-inf; for j=2:15 Matrix(24,j)=-inf; for j=9:24 %for j=6:24 Matrix(10,j)=-inf; for j=20:31 Matrix(15,j)=-inf; for j=5:20 Matrix(20,j)=-inf; for j=18:27 Matrix(28,j)=-inf; for i=2:6 Matrix(i,18)=-inf; for i=17:20 Matrix(i,5)=-inf; for i=23:25 Matrix(i,20)=-inf; for i=13:17 Matrix(i,13)=-inf; end end end end end end end end end end %end % 显示地图 %subplot(2,2,1); h1 = plot(Spoint(1),Spoint(2),'gO'); hold on h2 = plot(Epoint(1),Epoint(2),'rO');
3. A*算法搜索路径
%%寻路 Matrix(Spoint(1),Spoint(2))=0; Matrix(Epoint(1),Epoint(2))=inf; G=Matrix; F=Matrix; openlist=Matrix; closelist=Matrix; parentx=Matrix; parenty=Matrix; openlist(Spoint(1),Spoint(2)) =0; %closelist(Epoint(1),Epoint(2))=inf; for i = 1:n+2 for j = 1:m+2 k = Matrix(i,j); if(k == -inf) %subplot(2,2,1); h3 = plot(i,j,'k.'); % elseif(k == inf) % show green feasible point % %subplot(2,2,1); % plot(i,j,'gh'); % else % %subplot(2,2,1); % plot(i,j,'gh'); end hold on end end axis([0 m+3 0 n+3]); %subplot(2,2,1); plot(Epoint(1),Epoint(2),'b+'); %subplot(2,2,1); plot(Spoint(1),Spoint(2),'b+'); while(1) num=inf; for p=1:m+2 for q=1:n+2 if(openlist(p,q)==0&&closelist(p,q)~=1) Outpoint=[p,q]; if(F(p,q)>=0&&num>F(p,q)) num=F(p,q); Nextpoint=[p,q]; end end end end closelist(Nextpoint(1),Nextpoint(2))=1; for i = 1:3 for j = 1:3 k = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j); if(i==2&&j==2|closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)==1) continue; elseif (k == -inf) G(Nextpoint(1)-2+i,Nextpoint(2)-2+j) = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j); closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=1; elseif (k == inf) distance=((i-2)^2+(j-2)^2)^0.5; G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1),Nextpoint(2))+distance; openlist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=0; % H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5;%欧几里德距离启发函数 H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数 H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2)); H=sqrt(2)*H_diagonal+(H_straight-2*H_diagonal); % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数 F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)+H; parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1); parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2); else distance=((i-2)^2+(j-2)^2)^0.5; if(k>(distance+G(Nextpoint(1),Nextpoint(2)))) k=distance+G(Nextpoint(1),Nextpoint(2)); % H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5; %欧几里德距离启发函数 H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数 H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2)); H=sqrt(2)*10*H_diagonal+10*(H_straight-2*H_diagonal); % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数 F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=k+H; parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1); parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2); end end if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf) parentx(Epoint(1),Epoint(2))=Nextpoint(1); parenty(Epoint(1),Epoint(2))=Nextpoint(2); break; end end if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf) parentx(Epoint(1),Epoint(2))=Nextpoint(1); parenty(Epoint(1),Epoint(2))=Nextpoint(2); break; end end if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf) parentx(Epoint(1),Epoint(2))=Nextpoint(1); parenty(Epoint(1),Epoint(2))=Nextpoint(2); break; end end P=[]; s=1; while(1) if(num==inf) break; end %subplot(2,2,1); h4 = plot(Epoint(1),Epoint(2),'b+'); P(s,:)=Epoint; s=s+1; % pause(1); xx=Epoint(1); Epoint(1)=parentx(Epoint(1),Epoint(2)); Epoint(2)=parenty(xx,Epoint(2)); if(parentx(Epoint(1),Epoint(2))==Spoint(1)&&parenty(Epoint(1),Epoint(2))==Spoint(2)) %subplot(2,2,1); plot(Epoint(1),Epoint(2),'b+'); P(s,:)=Epoint; break; end end P(s+1,:)=Spoint; legend([h1,h2,h3,h4],'起始点','目标点','障碍物','航迹点'); count=0; for i=2:12 for j=2:12 if(G(i,j)~=inf&&G(i,j)~=-inf) count=count+1; end end end count
4. 路径优化
%将得到的折现曲线拟合成光滑的曲线 P=P'; a=[]; b=[]; a=P(1,:); b=P(2,:); figure %subplot(2,2,3); plot(a,b); axis([0,n+3,0,n+3]); values = spcrv([[a(1) a a(end)];[b(1) b b(end)]],3); figure %subplot(2,2,4); plot(values(1,:),values(2,:),'r'); axis([0,m+3,0,m+3]);
5. 效果图
A*路径
优化后路径
6. 下载链接
直接复制到matlab即可使用,或者也可以点击下载
(68条消息) Matlab航迹规划仿真——A*算法_漫长IT路-CSDN博客_matlab 航迹规划
这篇关于Matlab航迹规划仿真——A*算法的文章就介绍到这儿,希望我们推荐的文章对大家有所帮助,也希望大家多多支持为之网!
- 2024-12-17机器学习资料入门指南
- 2024-12-06如何用OpenShift流水线打造高效的机器学习运营体系(MLOps)
- 2024-12-06基于无监督机器学习算法的预测性维护讲解
- 2024-12-03【机器学习(六)】分类和回归任务-LightGBM算法-Sentosa_DSML社区版
- 2024-12-0210个必须使用的机器学习API,为高级分析助力
- 2024-12-01【机器学习(五)】分类和回归任务-AdaBoost算法-Sentosa_DSML社区版
- 2024-11-28【机器学习(四)】分类和回归任务-梯度提升决策树(GBDT)算法-Sentosa_DSML社区版
- 2024-11-26【机器学习(三)】分类和回归任务-随机森林(Random Forest,RF)算法-Sentosa_DSML社区版
- 2024-11-18机器学习与数据分析的区别
- 2024-10-28机器学习资料入门指南