rejudge.m

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

function P=rejudge(P,d,n,O,Xlu,C)
t=0;
sum1=0;
sum2=0;
[n1,n2]=size(P);
[o1,o2]=size(O);
n3=fix(n2/2);
p=zeros(n1,n3);
for i=1:n1
    for j=2:2:n2
        p(i,j/2)=ymadjp2([P(i,j),P(i,j+1)],d);
    end
end
q=p;
for i=1:n1
         for j=2:2:n2-3
             p=q;
        for x=1:n1
             for y=1:n3
             coordinate=ymadjp1(p(x,y),d);%将p中各序号转换为坐标
             P(x,2*y)=coordinate(1);
             P(x,2*y+1)=coordinate(2);
             end
        end
        point0(1)=P(i,j);
        point0(2)=P(i,j+1);
        point1(1)=P(i,j+2);
        point1(2)=P(i,j+3);
       t0=ymadjp2([point0(1),point0(2)],d);
       t1=ymadjp2([point1(1),point1(2)],d);
          if t1==0
           break
          end
        for k=1:o1
          
            if   point0(1)<=O(k,1) & point0(2)<=O(k,3) %point0位于左下方，角为0到90度
                
                    if atan((point1(2)-point0(2))/(point1(1)-point0(1)))<atan((O(k,4)-point0(2))/(O(k,1)-point0(1)))&...
                            atan((point1(2)-point0(2))/(point1(1)-point0(1)))>atan((O(k,3)-point0(2))/(O(k,2)-point0(1)))
                        if point1(1)>=O(k,1)&point1(1)<=O(k,2)&point1(2)>=O(k,4)%point1位于上方
                           for f=j/2+1:n3-1
                           q(i,f+1)=p(i,f);
                           end
                           point=[O(k,1),O(k,4)];
                           q(i,j/2+1)=ymadjp2(point,d)-1;
                           break
                       elseif point1(1)>=O(k,2)&point1(2)>=O(k,3)%point1位于右方和右上方
                           for f=j/2+1:n3-1
                           q(i,f+1)=p(i,f);
                           end
                           point=[O(k,2),O(k,3)];
                           q(i,j/2+1)=ymadjp2(point,d)-d;
                           break
                        elseif point1(1)>O(k,1)&point1(2)<O(k,2)&point1(1)>O(k,3)&point1(2)<O(k,4)
                            q(i,j/2+1)=ymadjp2([O(k,1),O(k,3)],d)-d;
                            break
                           
                       end 
                   end
            elseif point0(1)>=O(k,1)&point0(1)<=O(k,2)&point0(2)<=O(k,3)%point0位于下方
                    
                        if point1(1)<=O(k,1)&point1(2)>=O(k,3)%point1位于左方及左上方
                           if  atan((point1(2)-point0(2))/(point1(1)-point0(1)))<atan((O(k,3)-point0(2))/(O(k,1)-point0(1)))
                               for f=j/2+1:n3-1
                               q(i,f+1)=p(i,f);
                               end
                               point=[O(k,1),O(k,3)];
                               q(i,j/2+1)=ymadjp2(point,d)-1;
                               break
                           end
                         elseif point1(1)>=O(k,2)&point1(2)>=O(k,3) %point1位于右方及右上方
                           if  atan((point1(2)-point0(2))/(point1(1)-point0(1)))>atan((O(k,3)-point0(2))/(O(k,2)-point0(1)))
                             for f=j/2+1:n3-1
                             q(i,f+1)=p(i,f);
                             end
                             point=[O(k,2),O(k,3)];
                             q(i,j/2+1)=ymadjp2(point,d)-d;
                             break
                           end
                           elseif point1(1)>=O(k,1)&point1(1)<=O(k,2)&point1(2)>=O(k,4) %point1位于上方
                           for f=j/2+1:n3-1
                           q(i,f+1)=p(i,f);
                           end
                           [x0,y0]=avail(point1(1),point1(2),O(k,2),O(k,4),point0(1),point0(2),O(k,2),O(k,3));
                          [x1,y1]=avail(point1(1),point1(2),O(k,1),O(k,4),point0(1),point0(2),O(k,1),O(k,3));
                          for ii=1:3
                              a=sqrt((x0-C(ii,1)).^2+(y0-C(ii,2)).^2);
                              sum1=sum1+a;
                              b=sqrt((x1-C(ii,1)).^2+(y1-C(ii,2)).^2);
                              sum2=sum2+b;
                          end
                          if sum1>=sum2
                              x2=x0;y2=y0;
                          else
                              x2=x1;y2=y1;
                          end
                          q(i,j/2+1)=ymadjp2([x2,y2],d)+1;
                          break
                        elseif  point1(1)>O(k,1)&point1(2)<O(k,2)&point1(1)>O(k,3)&point1(2)<O(k,4)
                            q(i,j/2+1)=ymadjp2([O(k,2),O(k,3)],d)-d;
                            break
                         end
                     
            elseif point0(1)>=O(k,2)&point0(2)<=O(k,3)%point0位于右下方
               if  atan((point1(2)-point0(2))/(point1(1)-point0(1)))>atan((O(k,4)-point0(2))/(O(k,2)-point0(1)))&...
                        atan((point1(2)-point0(2))/(point1(1)-point0(1)))<atan((O(k,3)-point0(2))/(O(k,1)-point0(1)))
                    if point1(1)<=O(k,2)&point1(2)>=O(k,4)%point1位于上方及左上方
                      for f=j/2+1:n3-1
                      q(i,f+1)=p(i,f);
                      end
                      point=[O(k,2),O(k,4)];
                      q(i,j/2+1)=ymadjp2(point,d);
                      break
                    elseif point1(1)<=O(k,1)&point1(2)>=O(k,3)&point1(2)<=O(k,4)%point1位于左方
                         for f=j/2+1:n3-1
                         q(i,f+1)=p(i,f);
                         end
                         point=[O(k,1),O(k,3)];
                         q(i,j/2+1)=ymadjp2(point,d)-d; 
                         break
                    elseif  point1(1)>O(k,1)&point1(2)<O(k,2)&point1(1)>O(k,3)&point1(2)<O(k,4)
                        q(i,j/2+1)=ymadjp2([O(k,2),O(k,4)],d);
                        break
                    end
                end
            elseif point0(1)>=O(k,2)&point0(2)>=O(k,3)&point0(2)<=O(k,4)%point0位于右方
                  
                        if point1(1)<=O(k,2)&point1(2)>=O(k,4)%point1位于左上方和上方
                            if atan((point1(2)-point0(2))/(point1(1)-point0(1)))>atan((O(k,4)-point0(2))/(O(k,2)-point0(1)))
                            for f=j/2+1:n3-1
                            q(i,f+1)=p(i,f);
                            end
                            point=[O(k,2),O(k,4)];
                            q(i,j/2+1)=ymadjp2(point,d) ;
                            break
                            end
                   elseif point1(1)<=O(k,2)&point1(2)<=O(k,3)%point1位于左下方和下方
                      if   atan((point1(2)-point0(2))/(point1(1)-point0(1)))<atan((O(k,3)-point0(2))/(O(k,2)-point0(1)))
                           for f=j/2+1:n3-1
                           q(i,f+1)=p(i,f);
                           end
                           point=[O(k,2),O(k,3)];
                           q(i,j/2+1)=ymadjp2(point,d)+1-d;
                           break
                      end
                  elseif point1(1)<=O(k,1)&point1(2)<=O(k,4)&point1(2)>=O(k,3)%point1位于左方
                           for f=j/2+1:n3-1
                           q(i,f+1)=p(i,f);
                           end
                           [x0,y0]=avail(point1(1),point1(2),O(k,1),O(k,4),point0(1),point0(2),O(k,2),O(k,4));
                           [x1,y1]=avail(point1(1),point1(2),O(k,1),O(k,3),point0(1),point0(2),O(k,2),O(k,3));
                           for ii=1:3
                              a=sqrt((x0-C(ii,1)).^2+(y0-C(ii,2)).^2);
                              sum1=sum1+a;
                              b=sqrt((x1-C(ii,1)).^2+(y1-C(ii,2)).^2);
                              sum2=sum2+b;
                          end
                          if sum1>=sum2
                              x2=x0;y2=y0;
                          else
                              x2=x1;y2=y1;
                          end
                         q(i,j/2+1)=ymadjp2([x2,y2],d)+d;   
                         break
                        elseif  point1(1)>O(k,1)&point1(2)<O(k,2)&point1(1)>O(k,3)&point1(2)<O(k,4)
                            q(i,j/2+1)=ymadjp2([O(k,2),O(k,4)],d);
                            break
                  end