rejudge.m
来自「规则障碍物环境下的机器人路径规划matlab源码！」· M 代码 · 共 338 行 · 第 1/2 页
M
338 行
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);
break
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 ;
break
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;
break
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);
break
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
[x0,y0]=avail(point1(1),point1(2),O(k,2),O(k,3),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,1),O(k,4));
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(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);
break
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;
break
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);
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,4)],d);
break
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;
break
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 ;
break
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
[x0,y0]=avail(point1(1),point1(2),O(k,2),O(k,4),point0(1),point0(2),O(k,1),O(k,4));
[x1,y1]=avail(point1(1),point1(2),O(k,2),O(k,3),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)+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,4)],d);
break
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
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?