日志
无人机寻找污染源matlab代码
热度 1
P=5.5;
a=80;%无人机横坐标
b=80;%无人机纵坐标
L=1;%无人机起始搜索步长
Y=[0,0];%污染源坐标
Q=25;%气体源强
k=0.08;%气体扩散系数
distance=0;%初始化总路程为0
xa=-100:0.1:100;
ya=xa;
[x,y] = meshgrid(xa,ya);
d=sqrt(x.^2+y.^2);
z=4*pi*k*d*(1/Q);
contour(x,y,z,20,'LineWidth',3) %创建原始空气污染物状态情况
hold on
for h=1:1:20
A=[a,b];
plot(a,b,'rs');
B=[a+L*cos(pi/6),b-L*sin(pi/6)];
plot([a;a+L*cos(pi/6)],[b;b-L*sin(pi/6)],'r');%直线AB
C=[a+L*cos(pi/6),b-L*sin(pi/6)-L];
plot([a+L*cos(pi/6);a+L*cos(pi/6)],[b-L*sin(pi/6);b-L*sin(pi/6)-L],'r');%直线BC
D=[a,b-2*L*sin(pi/6)-L];
plot([a+L*cos(pi/6);a],[b-L*sin(pi/6)-L;b-2*L*sin(pi/6)-L],'r');%直线CD
E=[a-L*cos(pi/6),b-L*sin(pi/6)-L];
plot([a;a-L*cos(pi/6)],[b-2*L*sin(pi/6)-L;b-L*sin(pi/6)-L],'r');%直线DE
F=[a-L*cos(pi/6),b-L*sin(pi/6)];
plot([a-L*cos(pi/6);a-L*cos(pi/6)],[b-L*sin(pi/6)-L;b-L*sin(pi/6)],'r');%直线EF
plot([a-L*cos(pi/6);a],[b-L*sin(pi/6);b],'r');%直线FA
%每个点的空气清洁指数
AirA=4*pi*k*norm(Y-A)*(1/Q);
AirB=4*pi*k*norm(Y-B)*(1/Q);
AirC=4*pi*k*norm(Y-C)*(1/Q);
AirD=4*pi*k*norm(Y-D)*(1/Q);
AirE=4*pi*k*norm(Y-E)*(1/Q);
AirF=4*pi*k*norm(Y-F)*(1/Q);
%寻找最小值作为新的起始点
Air=[AirA,AirB,AirC,AirD,AirE,AirF];
[m,n]=min(Air);
switch n
case 1
A=[a,b];
S=A;
case 2
A=[a+L*cos(pi/6),b-L*sin(pi/6)];
S=B;
case 3
A=[a+L*cos(pi/6),b-L*sin(pi/6)-L];
S=C;
case 4
A=[a,b-2*L*sin(pi/6)-L];
S=D;
case 5
A=[a-L*cos(pi/6),b-L*sin(pi/6)-L];
S=E;
case 6
A=[a-L*cos(pi/6),b-L*sin(pi/6)];
S=F;
end
a=A(1);
b=A(2);
distance=distance+6*norm(Y-A)+norm(A-S);
if AirA==m;
fprintf('迭代次数为%d次\n',h)
fprintf('总路程为：%d\n',distance)
fprintf('到污染源的距离为：%d\n',norm(Y-AirA))
if norm(Y-AirA)>=1
fprintf('到污染源的距离过远！\n')
end
break
else
L=P*(1/(AirA-m));
end
end
hold off
format long g
distance %#ok<NOPTS>
