judge.m

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

            elseif point0(1)>=O(k,2)&point0(2)>=O(k,4)%point0位于右上方
                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(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); 
                        elseif point1(1)>=O(k,1)&point0(1)<=O(k,2)&point0(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)+1 ;
                      end
                 end
            elseif point0(1)>=O(k,1)&point0(1)<=O(k,2)&point0(2)>=O(k,4) %point0位于上方
                   
                       
                         if point1(1)<=O(k,1)&point1(2)<=O(k,4)%point1位于左下方及左方
                             if atan((point1(2)-point0(2))/(point1(1)-point0(1)))>atan((O(k,4)-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,4)];
                            q(i,j/2+1)=ymadjp2(point,d)-1; 
                             end
                     elseif 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);
                          end
                     elseif point1(1)<=O(k,2)&point1(1)>=O(k,1)&point1(2)<=O(k,3)%point1位于下方
                            for f=j/2+1:n3-1
                            q(i,f+1)=p(i,f);
                            end
%                             eq1=vpa('y-point1(2)-((O(k,3)-point1(2))/(O(k,2)-point1(1)))*(x-point1(1))=0');
%                             eq2=vpa('y-point0(2)-((O(k,4)-point0(2))/(O(k,2)-point0(1)))*(x-point0(1))=0');
                            [x,y]=avail(point1(1),point1(2),O(k,2),O(k,3),point0(1),point0(2),O(k,2),O(k,4));
                            if (double(x)<0|double(y)<0)||(double(x)>10|double(y)>10)
%                             eq1=vpa('y-point1(2)-((O(k,3)-point1(2))/(O(k,1)-point1(1)))*(x-point1(1))=0');
%                             eq2=vpa('y-point0(2)-((O(k,4)-point0(2))/(O(k,1)-point0(1)))*(x-point0(1))=0');
                            [x,y]=avail(point1(1),point1(2),O(k,1),O(k,3),point0(1),point0(2),O(k,1),O(k,4));
                            end
                            q(i,j/2+1)=ymadjp2([x,y],d)+1;
                            
                      elseif point1(1)>O(k,1)&point1(1)<O(k,2)&point1(2)>O(k,3)&point1(2)<O(k,4)%障碍物里
                           q(i,j/2+1)=ymadjp2([O(k,2),O(k,4)],d);
                           
                           end
                         
                   
            elseif point0(1)<=O(k,1)&point0(2)>=O(k,4)%point0位于左上方
                     if atan((point1(2)-point0(2))/(point1(1)-point0(1)))>atan((O(k,3)-point0(2))/(O(k,1)-point0(1)))&...
                            atan((point1(2)-point0(2))/(point1(1)-point0(1)))<atan((O(k,4)-point0(2))/(O(k,2)-point0(1)))
                          if point1(1)>=O(k,1)&point1(2)<=O(k,3)%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)-1;
                      elseif point1(1)>=O(k,2)&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,2),O(k,4)];
                             q(i,j/2+1)=ymadjp2(point,d);
                          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,4)],d);
                       end
                     end
            elseif point0(1)<=O(k,1)&point0(2)<=O(k,4)&point0(2)>=O(k,3)%point0位于左方
                   if point1(1)>=O(k,1)&point1(2)>=O(k,4)%point1位于上方和右上方
                             if atan((point1(2)-point0(2))/(point1(1)-point0(1)))<atan((O(k,4)-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,4)];
                                q(i,j/2+1)=ymadjp2(point,d)-1;
                             end
                    elseif 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)-d ;
                       end
                  elseif point1(1)>=O(k,2)&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
%                             eq1=vpa('y-point1(2)-((O(k,4)-point1(2))/(O(k,2)-point1(1)))*(x-point1(1))=0');
%                             eq2=vpa('y-point0(2)-((O(k,4)-point0(2))/(O(k,1)-point0(1)))*(x-point0(1))=0');
                            [x,y]=avail(point1(1),point1(2),O(k,2),O(k,4),point0(1),point0(2),O(k,1),O(k,4));
                             if(double(x)<0|double(y)<0)||(double(x)>10|double(y)>10)
%                             eq1=vpa('y-point1(2)-((O(k,3)-point1(2))/(O(k,2)-point1(1)))*(x-point1(1))=0');
%                             eq2=vpa('y-point0(2)-((O(k,3)-point0(2))/(O(k,1)-point0(1)))*(x-point0(1))=0');
                            [x,y]=avail(point1(1),point1(2),O(k,2),O(k,3),point0(1),point0(2),O(k,1),O(k,3));
                             end
                            q(i,j/2+1)=ymadjp2([x,y],d)+d;
                   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,4)],d);
                  end
            end           
          end
       end
    end 
 
    p=q;
      for i=1:n1
    for j=1:n3
         coordinate=ymadjp1(p(i,j),d);%将p中各序号转换为坐标
         P(i,2*j)=coordinate(1);
         P(i,2*j+1)=coordinate(2);
    end
end

%检测变异产生的个体是否超出变量范围
for i=1:n1
    for j=1:2*n3
        xl=Xlu(j,1);
        xu=Xlu(j,2);
        if P(i,j+1)<xl
            P(i,j+1)=xl;
        end
        if P(i,j+1)>xu
            P(i,j+1)=xu;
        end
    end
end