finder.m

规则障碍物环境下的机器人路径规划matlab源码！

%找出公共点
function pt=finder(BesT,d)
pt=zeros(1,2);
[n1,n2]=size(BesT);
%  b=0.3;
%  [BesT,BesT1]=updown(BesT);
%  BesT=BesT(1:b*n11,:);
%  [n1,n2]=size(BesT);
  [amin,aa]=min(BesT(n1,:));
  A1=zeros((aa-1)/2-1,3);
  
  
  for j=2:2:n2-4
      t1=ymadjp2([BesT(n1,j),BesT(n1,j+1)],d);
      t2=ymadjp2([BesT(n1,j+2),BesT(n1,j+3)],d);
  if t2==0
      break
  end
  A1(j/2,1:2)=[t1,t2];
  end
  
  
  for j=2:2:n2-4
       t1=ymadjp2([BesT(n1,j),BesT(n1,j+1)],d);
       t2=ymadjp2([BesT(n1,j+2),BesT(n1,j+3)],d);
      if t1==0
      break
      end
    for i=2:n1
      for k=2:2:n2-4
          t3=ymadjp2([BesT(i,k),BesT(i,k+1)],d);
          t4=ymadjp2([BesT(i,k+2),BesT(i,k+3)],d);
          if t4==0
              break
          end
          if t1==t3&t2==t4
              A1(j/2,3)=A1(j/2,3)+1;
          end
      end
    end
  end
  
  [amax,a]=max(A1(:,3));
%   B=A;
%   for i=a:(aa-1)/2-1-1%hang
%       B(i,:)=A(i+1,:);
%   end
%   [bmax,b]=max(B(:,3))
  pt(1:2)=A1(a,1:2);
%   pt(3:4)=B(b,1:2);