2 部分代码
%% 清空环境
clc;
clear all;
close all;
tic
%% 障碍物数据
position = load('barrier.txt');
plot([0,200],[0,200],'.');
hold on
B = load('barrier.txt');
xlabel('km','fontsize',12)
ylabel('km','fontsize',12)
title('二维规划空间','fontsize',12)
%% 描述起点和终点
Start = [20,180];
Target = [160,90];
plot([Start(1),Target(1)],[Start(2),Target(2)],'.');
% 图形标注
text(Start(1),Start(2)+7,'S');
text(Target(1)+3,Target(2)-4,'T');
%% 描绘障碍物图形
fill(position(1:4,1),position(1:4,2),[0,0,0]);
fill(position(5:8,1),position(5:8,2),[0,0,0]);
fill(position(9:12,1),position(9:12,2),[0,0,0]);
fill(position(13:15,1),position(13:15,2),[0,0,0]);
% 下载链路端点数据
L = load('lines.txt');
%% 描绘线及中点
v = zeros(size(L));
for i=1:20
plot([position(L(i,1),1),position(L(i,2),1)],[position(L(i,1),2)...
,position(L(i,2),2)],'color','black','LineStyle','--');
v(i,:) = (position(L(i,1),:)+position(L(i,2),:))/2;
plot(v(i,1),v(i,2),'*');
text(v(i,1)+2,v(i,2),strcat('v',num2str(i)));
end
%% 描绘可行路径
sign = load('matrix.txt');
[n,m]=size(sign);
for i=1:n
if i == 1
for k=1:m-1
if sign(i,k) == 1
plot([Start(1),v(k-1,1)],[Start(2),v(k-1,2)],'color',...
'black','Linewidth',2,'LineStyle','-');
end
end
continue;
end
for j=2:i
if i == m
if sign(i,j) == 1
plot([Target(1),v(j-1,1)],[Target(2),v(j-1,2)],'color',...
'black','Linewidth',2,'LineStyle','-');
end
else
if sign(i,j) == 1
plot([v(i-1,1),v(j-1,1)],[v(i-1,2),v(j-1,2)],...
'color','black','Linewidth',2,'LineStyle','-');
end
end
end
end %到此能画出MAKLINK链路图
v1=zeros(22,2); %v1存放包括S和T的总共22个结点，作为DijkstraPlan
的参数
v1(1,:)=[20,180];
v1(22,:)=[160,90];
for i=2:21
v1(i,:)=v(i-1,:);
end
path = DijkstraPlan(v1,sign);
j = path(22);
plot([Target(1),v(j-1,1)],[Target(2),v(j-
1,2)],'color','yellow','LineWidth',3,'LineStyle','-.');
i = path(22);
j = path(i);
count = 0;
while true
plot([v(i-1,1),v(j-1,1)],[v(i-1,2),v(j-
1,2)],'color','yellow','LineWidth',3,'LineStyle','-.');
count = count + 1;
i = j;
j = path(i);
if i == 1 || j==1
break;
end
end
plot([Start(1),v(i-1,1)],[Start(2),v(i-
1,2)],'color','yellow','LineWidth',3,'LineStyle','-.');
count = count+3;
pathtemp(count) = 22;
j = 22;
for i=2:count
pathtemp(count-i+1) = path(j);
评论0