1. 初始化参数

主要参数:

地图大小
起始点和目标点坐标

clc
clear all
m = 30;n = 30;
Spoint = [3 3];      %起始点坐标
Epoint = [29 22];  %目标点坐标

2. 构建地图

-inf表示不可到达的障碍物点

%%构建地图
for i = 1:m+2if i == 1for j = 1:n+2Matrix(i,j) = -inf;endelseif i == m+2for j = 1:n+2Matrix(i,j) = -inf;endelsefor j = 1:n+2if ((j == 1)|(j == n+2))Matrix(i,j) = -inf;elseMatrix(i,j) = inf;endendend
end
%%障碍
for j=2:10Matrix(5,j)=-inf;
for j=2:15Matrix(24,j)=-inf;
for j=9:24
%for j=6:24Matrix(10,j)=-inf;
for j=20:31Matrix(15,j)=-inf;
for j=5:20Matrix(20,j)=-inf;
for j=18:27Matrix(28,j)=-inf;
for i=2:6  Matrix(i,18)=-inf;
for i=17:20Matrix(i,5)=-inf;
for i=23:25 Matrix(i,20)=-inf;
for i=13:17Matrix(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+2for j = 1:m+2k = 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');endhold onend
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+2for q=1:n+2if(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];endendendendcloselist(Nextpoint(1),Nextpoint(2))=1;for i = 1:3for j = 1:3k = 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);endendif(((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;endendif(((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;endendif(((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
endP=[];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:12for j=2:12if(G(i,j)~=inf&&G(i,j)~=-inf)count=count+1;endend
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航迹规划仿真——A*算法相关推荐

动态分区分配的“首次适应算法_无人机集群——航迹规划你不知道的各种算法优缺点...
我们都知道无人机(UAV )因具有可探测性低.造价低廉.不惧伤亡.起降简单.操作灵活.系统配置多样化.自动控制智能化等特点,因而在未来一体化联合作战中扮演越来越重要的角色.然而早期的无人机都是按照地面 ...
python无人机路径规划算法_无人机集群——航迹规划你不知道的各种算法优缺点...
我们都知道无人机(UAV )因具有可探测性低.造价低廉.不惧伤亡.起降简单.操作灵活.系统配置多样化.自动控制智能化等特点,因而在未来一体化联合作战中扮演越来越重要的角色.然而早期的无人机都是按照地面 ...
【Matlab路径规划】A_star算法智能仓储机器人移动避碰路径规划【含源码 1180期】
一.代码运行视频(哔哩哔哩) [Matlab路径规划]A_star算法智能仓储机器人移动避碰路径规划[含源码 1180期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文 ...
【Matlab路径规划】A_star算法机器人栅格地图路径规划【含源码 116期】
一.代码运行视频(哔哩哔哩) [Matlab路径规划]A_star算法机器人栅格地图路径规划[含源码 116期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1] ...
【SSA三维路径规划】基于matlab麻雀算法无人机三维航迹规划【含Matlab源码 301期】
⛄一.麻雀算法求解无人机三维路径规划简介 1 引言随着无人机(Unmanned Aerial Vehicle,UAV)技术的不断发展,国内外学者对于无人机作战应用的研究日益增多.无人机航迹规划是任务 ...
【WOA三维路径规划】基于matlab灰狼算法无人机三维航迹规划【含Matlab源码 2445期】
⛄一.灰狼优化算法的无人机三维航迹规划 1 无人机航迹规划建模 1.1 地形模型首先, 本研究须对三维地理环境 (规划空间) 进行离散化处理.通过对规划空间进行立方体网格划分, 将空间划分为大小相等 ...
【MVO三维路径规划】基于matlab多元宇宙算法多无人机避障三维航迹规划【含Matlab源码 2579期】
⛄一.无人机简介 0 引言随着现代技术的发展,飞行器种类不断变多,应用也日趋专一化.完善化,如专门用作植保的大疆PS-X625无人机,用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机,以及用作水 ...
【WOA三维路径规划】灰狼算法无人机三维航迹规划【含Matlab源码 2445期】
⛄一.灰狼优化算法的无人机三维航迹规划 1 无人机航迹规划建模 1.1 地形模型首先, 本研究须对三维地理环境 (规划空间) 进行离散化处理.通过对规划空间进行立方体网格划分, 将空间划分为大小相等 ...
单目标应用：蜣螂优化算法求解无人机三维航迹规划，含四种对比算法（提供Matlab代码）
一.无人机三维航迹规划三维航迹规划是无人机在执行任务过程中的非常关键的环节,三维航迹规划的主要目的是在满足任务需求和自主飞行约束的基础上,计算出发点和目标点之间的最佳航路. 1.1路径最短约束无人 ...
单目标应用：蜣螂优化算法DBO与麻雀搜索算法SSA求解无人机三维航迹规划（提供Matlab代码）
一.无人机三维航迹规划三维航迹规划是无人机在执行任务过程中的非常关键的环节,三维航迹规划的主要目的是在满足任务需求和自主飞行约束的基础上,计算出发点和目标点之间的最佳航路. 1.1路径最短约束无人 ...

Matlab航迹规划仿真——A*算法

文章目录

1. 初始化参数

2. 构建地图

3. A*算法搜索路径

4. 路径优化

5. 效果图

6. 下载链接

Matlab航迹规划仿真——A*算法相关推荐

最新文章

热门文章