避碰路径规划算法.rar_切线图算法、Dijkstra、路径规划_建模 最短_路径规划_避碰_障碍环境建模

function[]=TangentGraph() %根据起始点和终止点做障碍物的切线，并用dijistra算法求最短路 point_start=[20;20]; point_end=[180;160]; % % point_start=[20;180]; % % point_end=[100;20]; % % % point_start=[20;180]; % % % point_end=[180;30]; text(point_start(1),point_start(2),'o','color','k') text(point_end(1),point_end(2),'o','color','k') text(point_start(1)+3,point_start(2)+3,'Origin') text(point_end(1)+3,point_end(2)+3,'Destination') x_1=[40 60 100 60]; y_1=[140 160 140 120]; patch(x_1,y_1,[0.60,0.95,0.85]); x_2=[120 160 180 180]; y_2=[160 180 170 100]; patch(x_2,y_2,[0.60,0.95,0.85]); x_3=[120 140 170]; y_3=[40 80 40]; patch(x_3,y_3,[0.60,0.95,0.85]); x_4=[30 80 100 50]; y_4=[40 80 40 30]; patch(x_4,y_4,[0.60,0.95,0.85]); x_5=[45 50 45]; y_5=[30 20 10]; patch(x_5,y_5,[0.60,0.95,0.85]); x_6=[80 85 85]; y_6=[85 85 80]; patch(x_6,y_6,[0.60,0.95,0.85]); x_7=[20 35 35]; y_7=[80 80 46]; patch(x_7,y_7,[0.60,0.95,0.85]); Region{1}=[x_1;y_1]; Region{2}=[x_2;y_2]; Region{3}=[x_3;y_3]; Region{4}=[x_4;y_4]; Region{5}=[x_5;y_5]; Region{6}=[x_6;y_6]; Region{7}=[x_7;y_7]; original_region=Region; for i=1:length(Region) for j=1:length(Region{i}(1,:)) text(Region{i}(1,j),Region{i}(2,j),['(',num2str(Region{i}(1,j)),',',num2str(Region{i}(2,j))]); end end hold on; axis equal; %切线算法 n=cell(4,1);%探测线的起始点终止点坐标及其相交障碍物 %第一二个元组为起始点和终点坐标，第三个元组为形成该探测线的原探测线的相交区域，第四个元组为该探测线的相交区域 R=cell(2,1);%最终的切线集 MM=zeros(1,length(original_region));%记录所有的探测线的相交障碍物 count_R=0; count00=0;%用来记录while循环次数 n{1,1}=point_start; n{2,1}=point_end; m=1; while (length(n(1,:))>=1) count00=count00+1; start_point=n{1,m}; end_point=n{2,m}; %相交障碍物 Region_flag=SegmentCrossObstacle_Judge(start_point,end_point,original_region); Record_flag=Region_flag;%记录最初相交障碍物的所有相交情况，包括与障碍物相切 n{4,m}=Region_flag; %如果探测线不与任何障碍物相交，则探测线即为最短路。 if count00==1&&sum(Region_flag)==0 % count_R=count_R+1; % R{1,count_R}=point_start; % R{2,count_R}=point_end; xx=[point_start(1,1) point_end(1,1)]; yy=[point_start(2,1) point_end(2,1)]; plot(xx,yy,'r-'); return; end %将相交障碍物中的相切的情况删除 if count00>1 for p=1:length(Region_flag) if Region_flag(p)==1&&n{3,m}(1,p)==1 Region_flag(p)=0;%将生成切线的探测线的相交障碍物去掉 end end n{4,m}=Region_flag; end count_crossregion=0;%用来记录探测线段的相交障碍物的个数 for i=1:length(original_region) if Region_flag(1,i)==1 MM(1,i)=1; count_crossregion=count_crossregion+1; end end M_region=cell(1,count_crossregion);%记录探测线的相交障碍物 flag_region=0; for i=1:length(original_region) if Region_flag(1,i)==1 flag_region=flag_region+1; M_region{1,flag_region}=original_region{i}; end end %对起始点和终止点分别求相交障碍物的切线段 count_tangenline=0;%用于计切线数 N=cell(2,1);%相交障碍物的总的点数 for ii=1:count_crossregion pointss=length(M_region{1,ii}(1,:)); pointss_flag=zeros(1,pointss); %求起始点对第i个相交障碍物的切线 for j=1:pointss start_point=n{1,m}; end_point_x=M_region{1,ii}(1,j); end_point_y=M_region{1,ii}(2,j); end_point=[end_point_x;end_point_y]; pointss_flag(1,j)=TangentLine_Judge(start_point,end_point,M_region(1,ii));%注意元胞数组的引用 if pointss_flag(1,j)==1; count_tangenline=count_tangenline+1; N{1,count_tangenline}=start_point; N{2,count_tangenline}=end_point; end end %求终止点对第i个相交障碍物的切线 for j=1:pointss start_point_x=M_region{1,ii}(1,j); start_point_y=M_region{1,ii}(2,j); start_point=[start_point_x;start_point_y]; end_point=n{2,m}; pointss_flag(1,j)=TangentLine_Judge(start_point,end_point,M_region(1,ii)); if pointss_flag(1,j)==1; count_tangenline=count_tangenline+1; N{1,count_tangenline}=start_point; N{2,count_tangenline}=end_point; end end end %判断切线是否与其他障碍物相交，是则删除该切线，并以该切线段的起终点对该障碍物做切线,并放入N中，否则将该.... %切线放入R集合中，切点放入P集合中。 count_addline=0;%用来记录切线段需要添加为探测线的情况（切线段与其他障碍物相交） for iii=length(N(1,:)):-1:1%探测线对应的切线数 % flag1=1;%判断切线与障碍物的相交情况 % flag11=1;%用于判断探测线与切线的相交障碍物的一致情况 start_point=N{1,iii}; end_point=N{2,iii}; Region=original_region; Region_flag1=SegmentCrossObstacle_Judge(start_point,end_point,Region); N{3,iii}=Region_flag1;%用来存储该切线的相交障碍物区域 %%判断切线是否有一个点为探测线的点 if (N{1,iii}(1,1)==start_point(1,1)&&N{1,iii}(2,1)==start_point(2,1))||.... (N{1,iii}(1,1)==end_point(1,1)&&N{1,iii}(2,1)==end_point(2,1))||.... (N{2,iii}(1,1)==start_point(1,1)&&N{2,iii}(2,1)==start_point(2,1))||.... (N{2,iii}(1,1)==end_point(1,1)&&N{2,iii}(2,1)==end_point(2,1)) flag_pointontangent=1;%切线有一个点为探测线的点 else flag_pointontangent=0; end if sum(N{3,iii})==1%切线段不与构成切线外的其他障碍物相交 count_R=count_R+1; R(1,count_R)=N(1,iii); R(2,count_R)=N(2,iii); N(:,iii)=[]; else%切线段与障碍物相交 if flag_pointontangent==1%切线的起点或终点有一个是探测线的起点或终点 count_tangent1=0; flag_flag=zeros(1,sum(N{3,iii})); for j=1:length(N{3,iii}) if N{3,iii}(1,j)==1 tangentorcross_flag=TangentLine_Judge(start_point,end_point,original_region(1,j)); if tangentorcross_flag==0%说明切线跟该障碍物是相交的关系，而不是相切的关系 if MM(1,j)~=1&&Record_flag(1,j)~=1%说明跟未曾相交过的障碍物j相交，将该切线添加为探测线。 count_addline=count_addline+1; n{1,length(n(1,:))+1}=N{1,iii}; n{2,length(n(1,:))}=N{2,iii}; n{3,length(n(1,:))}=Record_flag; break; else%说明跟已经相交过的障碍物j相交 if j==length(N{3,iii}) break; end%用于判断所有的相交关系都是跟已经相交过的障碍物相交 end else%说明切线与该障碍物时相切的关系 count_tangent1=count_tangent1+1;