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即可使用,或者也可以点击下载。
审核编辑:汤梓红
声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。
举报投诉
-
matlab
+关注
关注
185文章
2974浏览量
230437 -
算法
+关注
关注
23文章
4610浏览量
92860
原文标题:Matlab航迹规划仿真—A*算法
文章出处:【微信号:3D视觉工坊,微信公众号:3D视觉工坊】欢迎添加关注!文章转载请注明出处。
发布评论请先 登录
相关推荐
怎么使用Matlab进行电机控制算法仿真?
板,毕竟不是自己的东西,总感觉不得要领,达不到对方波驱动算法那种游刃有余的状态,看网上很多高手都会用matlab 对电机建模进行Foc 算法仿真,现在疑惑的是
发表于 07-13 18:43
典型雷达航迹的仿真与实现
O 引言在各种雷达训练和信号模拟器中,都需要进行航迹模拟及航迹显示,以便于为仿真平台提供信号源。对于便携式雷达模拟器来说,无法使用PC,需用嵌入式系统来实现人机交互及信号处理。但当前绝大部分的
发表于 07-10 08:11
基于遗传算法的PID 控制及其MATLAB 仿真
本 文介绍了遗传算法和基于遗传算法的PID 控制设计, 并对设计MATLAB/SIMULINK 下进行了仿真,取得了良好的控制效果。关键词:遗传算法
发表于 06-11 09:06
•101次下载
多传感器异步航迹融合算法与仿真
针对分布式多传感器数据融合系统,提出了一种多传感器异步航迹融合算法。由于不同传感器的采样时间各不相同,融合算法首先利用最小二乘法将局部航迹统一到融合中心的融合
发表于 08-07 09:47
•12次下载
基于半实物仿真系统的多假目标航迹欺骗研究
根据半实物仿真的特点和优点,本文提出了基于半实物仿真系统的多假目标航迹欺骗研究的优势和价值。然后从实现多假目标航迹欺骗的必要条件、航迹欺骗产
发表于 07-06 16:31
•25次下载
基于改进RRT算法的无人机航迹规划
为了提高无人机的作战效率,航迹规划系统必须为无人机设计出安全系数高,能量消耗少,处理时间短,同时还必须满足飞行器自身物理特性的威胁回避轨迹。基于上述研究目的,本文
发表于 07-25 11:54
•100次下载
基于模糊聚类的多雷达航迹关联算法
了相似计算与矩阵迭代的次数,最终达到了减小运算量的目的。仿真结果表明:所提算法在保证关联正确率的前提下,耗时减小了54%,有效地提高了多雷达航迹关联算法的效率。
发表于 12-18 17:07
•0次下载
一种用于内层规划的改进粒子群算法
内层规划的改进粒子群算法,在粒子群算法中引入变异因子,设计了特定的扰动算子,提高了航迹寻优能力。仿真实验表明,在相同约束的
发表于 12-19 16:01
•0次下载
模糊航迹关联算法的分析及改进
效果变差。为此,研究并改进模糊综合决策航迹关联算法,通过引入模糊数学综合评判中的双层结构,提出双层结构模糊综合决策航迹关联算法。仿真结果表明
发表于 03-16 10:58
•6次下载
评论