Iteration is not stopping. Can you please help thanks.

2 次查看（过去 30 天）
JIBIN GEORGE
JIBIN GEORGE 2022-2-2
评论： JIBIN GEORGE 2022-2-2
clf
clc
clear all
close all
%Obstacle #1
Obpf1= 4.5;
Obs1 = [275 0];
%Obstacle #2
Obpf2= 4;
Obs2 = [200 90];
%Obstacle #3
Obpf3= 3;
Obs3 = [200 190];
%Obstacle #4
Obpf4= 2;
Obs4 = [200 290];
%Moving Obstacle
Obpf5 =2;
Obs5 = [125 0];
%Initial Goal 1
Gpf1 =1;
Gi1 = [170 90];
%Initial Goal 2
Gpf2 =1;
Gi2 = [170 190];
%Initial Goal 3
Gpf3 =1;
Gi3 = [170 290];
%Secondary Goal 1
Gsf1 = 1.5;
Gs1 = [0 145];
%Secondary Goal 2
Gsf2 = 1.5;
Gs2 = [0 195];
%Secondary Goal 3
Gsf3 = 1.5;
Gs3 = [0 245];
%Robot 1
R1pf = 1;
R1 = [20 372];
%Robot 2
R2pf = 1;
R2 = [50 372];
%Robot 3
R3pf = 1;
R3 = [80 372];
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Initial Robot Environment
%%%%%%%%%%%%%%%%%%%%%%%%%%%
rectangle('position',[275 0 150 400],'Facecolor','red','edgecolor','black','linewidth',1.5) % sortation area
hold on
rectangle('position',[200 90 75 20],'Facecolor','yellow') %CHUTE NUMBER 1
rectangle('position',[200 190 75 20],'Facecolor','yellow') %CHUTE NUMBER 2
rectangle('position',[200 290 75 20],'Facecolor','yellow') %CHUTE NUMBER 3
hold on
rectangle('position',[168 88 20 20],'curvature',[1,1],'Facecolor','black') %static robot 1
rectangle('position',[170 190 20 20],'curvature',[1,1],'Facecolor','black') %static robot 2
rectangle('position',[170 290 20 20],'curvature',[1,1],'Facecolor','black') %static robot 3
hold on
rectangle('position',[0 145 20 18],'facecolor','b') %truck station 1
rectangle('position',[0 195 20 18],'facecolor','b') %truck station 2
rectangle('position',[0 245 20 18],'facecolor','b') %truck station 3
hold on
rectangle('position',[125 0 5 5],'facecolor','b')
%human_obst(125,0,5,1)
auto_robot(20,372.5,10,1)
auto_robot(50,372.5,10,1)
auto_robot(80,372.5,10,1)
title('Autonomous Warehouse Environment')
axis([0 400 0 400]);%whole environment area
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create Repulsive and attractive potential field forces %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Matrix X x Y of 400 by 400
i=1:400;
j=1:400;
[I,J] = meshgrid(i,j);
Z = []; %empty matrix for z-axis plot
for i=1:400
for j=1:400
pos = [j/10 i/10]; % scaled to fit 400 by 400 matrix
z=0; %accumulator
%Obstacles with exponential curve while goal is by vector
z=z+exp(-norm(pos-Obs1)/Obpf1)+exp(-norm(pos-Obs2)/Obpf2)+ ...
exp(-norm(pos-Obs3)/Obpf3)+exp(-norm(pos-Obs4))/Obpf4+ ...
exp(-norm(pos-Obs5))/Obpf5+ ...
Gpf1*(norm(Gi1-pos))+ Gpf2*(norm(Gi2-pos))+Gpf3*(norm(Gi3-pos)+...
Gsf1*(norm(Gs1-pos))+ Gsf2*(norm(Gs2-pos))+Gsf3*(norm(Gs3-pos)));
% The potential functions chosen are similar to the function
% proposed in the document 'apf.pdf'
Z(i,j) = z; %store in matrix
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Path planning for Robot %
%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot current robot motion
t1=(abs(R1(1)-Gi1(1))+abs(R1(2)-Gi1(2)));
t2=(abs(R2(1)-Gi2(1))+abs(R2(2)-Gi2(2)));
t3=(abs(R3(1)-Gi3(1))+abs(R3(2)-Gi3(2)));
count=1;
while t1>0.5 || t2 >0.5 || t3 > 0.5 %check if the robot had reached the specified target;
%stop when reach target
dx1=[0 0]; %accumulation matrix robot1
dx2=[0 0]; %accumulation matrix robot2
dx3=[0 0]; %accumulation matrix robot3
%Obstacles Accumulate vector sum %Goal Accumulate - Robot 1
dx1= dx1 + (R1-R2)*exp((-norm(R1-R2))/R2pf) + ...
(R1-R3)*exp((-norm(R1-R3))/R3pf) + ...
(R1-Obs1)*exp((-norm(R1-Obs1))/Obpf1)+ ...
(R1-Obs2)*exp((-norm(R1-Obs2))/Obpf2)+ ...
(R1-Obs3)*exp((-norm(R1-Obs3))/Obpf3)+ ...
(R1-Obs4)*exp((-norm(R1-Obs4))/Obpf4)+ ...
(R1-Obs5)*exp((-norm(R1-Obs5))/Obpf5)+ ...
Gpf1*(Gi1-R1)/norm(Gi1-R1);
%Obstacles Accumulate vector sum %Goal Accumulate - Robot 2
dx2= dx2 + (R2-R1)*exp((-norm(R2-R1))/R2pf) + ...
(R2-R3)*exp((-norm(R2-R3))/R3pf) + ...
(R2-Obs1)*exp((-norm(R2-Obs1))/Obpf1)+ ...
(R2-Obs2)*exp((-norm(R2-Obs2))/Obpf2)+ ...
(R2-Obs3)*exp((-norm(R2-Obs3))/Obpf3)+ ...
(R2-Obs4)*exp((-norm(R2-Obs4))/Obpf4)+ ...
(R2-Obs5)*exp((-norm(R2-Obs5))/Obpf5)+ ...
Gpf2*(Gi2-R2)/norm(Gi2-R2);
%Obstacles Accumulate vector sum %Goal Accumulate - Robot 3
dx3= dx3 + (R3-R1)*exp((-norm(R3-R1))/R1pf) + ...
(R3-R2)*exp((-norm(R3-R2))/R2pf) + ...
(R3-Obs1)*exp((-norm(R3-Obs1))/Obpf1)+ ...
(R3-Obs2)*exp((-norm(R3-Obs2))/Obpf2)+ ...
(R3-Obs3)*exp((-norm(R3-Obs3))/Obpf3)+ ...
(R3-Obs4)*exp((-norm(R3-Obs4))/Obpf4)+ ...
(R3-Obs5)*exp((-norm(R3-Obs5))/Obpf5)+ ...
Gpf3*(Gi3-R3)/norm(Gi3-R3);
R1=R1+dx1;
R2=R2+dx2;
R3=R3+dx3;
t1=(abs(R1(1)-Gi1(1))+abs(R1(2)-Gi1(2)));
t2=(abs(R2(1)-Gi2(1))+abs(R2(2)-Gi2(2)));
t3=(abs(R3(1)-Gi3(1))+abs(R3(2)-Gi3(2)));
count=count+1;
plot3(R1(1,1), R1(1,2), Z(round(R1(1,2)), round(R1(1,1))),'K.','MarkerSize',10);
plot3(R2(1,1), R2(1,2), Z(round(R2(1,2)), round(R2(1,1))),'R.','MarkerSize',10);
plot3(R3(1,1), R3(1,2), Z(round(R3(1,2)), round(R3(1,1))),'G.','MarkerSize',10);
title(sprintf('Iteration: %d',count));
refresh;
drawnow;
end
t4=(abs(R1(1)-Gs1(1))+abs(R1(2)-Gs1(2)));
t5=(abs(R2(1)-Gs2(1))+abs(R2(2)-Gs2(2)));
t6=(abs(R3(1)-Gs3(1))+abs(R3(2)-Gs3(2)));
while t4>0.3 || t5 >0.3|| t6 > 0.3 %check if the robot had reached the specified target;
%stop when reach target
dx4=[0 0]; %accumulation matrix robot1
dx5=[0 0]; %accumulation matrix robot2
dx6=[0 0]; %accumulation matrix robot3
%Obstacles Accumulate vector sum %Goal Accumulate - Robot 1
dx4= dx4 + (R1-R2)*exp((-norm(R1-R2))/R2pf) + ...
(R1-R3)*exp((-norm(R1-R3))/R3pf) + ...
(R1-Obs1)*exp((-norm(R1-Obs1))/Obpf1)+ ...
(R1-Obs2)*exp((-norm(R1-Obs2))/Obpf2)+ ...
(R1-Obs3)*exp((-norm(R1-Obs3))/Obpf3)+ ...
(R1-Obs4)*exp((-norm(R1-Obs4))/Obpf4)+ ...
(R1-Obs5)*exp((-norm(R1-Obs5))/Obpf5)+ ...
Gsf1*(Gs1-R1)/norm(Gs1-R1);
%Obstacles Accumulate vector sum %Goal Accumulate - Robot 2
dx5= dx5 + (R2-R1)*exp((-norm(R2-R1))/R2pf) + ...
(R2-R3)*exp((-norm(R2-R3))/R3pf) + ...
(R2-Obs1)*exp((-norm(R2-Obs1))/Obpf1)+ ...
(R2-Obs2)*exp((-norm(R2-Obs2))/Obpf2)+ ...
(R2-Obs3)*exp((-norm(R2-Obs3))/Obpf3)+ ...
(R2-Obs4)*exp((-norm(R2-Obs4))/Obpf4)+ ...
(R2-Obs5)*exp((-norm(R2-Obs5))/Obpf5)+ ...
Gsf2*(Gs2-R2)/norm(Gs2-R2);
%Obstacles Accumulate vector sum %Goal Accumulate - Robot 3
dx6= dx6 + (R3-R1)*exp((-norm(R3-R1))/R1pf) + ...
(R3-R2)*exp((-norm(R3-R2))/R2pf) + ...
(R3-Obs1)*exp((-norm(R3-Obs1))/Obpf1)+ ...
(R3-Obs2)*exp((-norm(R3-Obs2))/Obpf2)+ ...
(R3-Obs3)*exp((-norm(R3-Obs3))/Obpf3)+ ...
(R3-Obs4)*exp((-norm(R3-Obs4))/Obpf4)+ ...
(R3-Obs5)*exp((-norm(R3-Obs5))/Obpf5)+ ...
Gsf3*(Gs3-R3)/norm(Gs3-R3);
R1=R1+dx4;
R2=R2+dx5;
R3=R3+dx6;
t4=(abs(R1(1)-Gs1(1))+abs(R1(2)-Gs1(2)));
t5=(abs(R2(1)-Gs2(1))+abs(R2(2)-Gs2(2)));
t6=(abs(R3(1)-Gs3(1))+abs(R3(2)-Gs3(2)));
count=count+1;
% assuming arrays are 400x400
thisr = min(max(round(R1),1),200);
plot3(R1(1,1), R1(1,2), Z(thisr(2),thisr(1)),'K.','MarkerSize',10);
thisr = min(max(round(R2),1),200);
plot3(R2(1,1), R2(1,2), Z(thisr(2),thisr(1)),'R.','MarkerSize',10);
thisr = min(max(round(R3),1),200);
plot3(R3(1,1), R3(1,2), Z(thisr(2),thisr(1)),'G.','MarkerSize',10);
title(sprintf('Iteration: %d',count));
refresh;
drawnow;
end
the used function
function auto_robot(x,y,r,n)
theta= linspace(0, 2*pi,n);
x1= x+r*cos(theta);
y1= y+r*sin(theta);
plot(x,y,'o','markerfacecolor','c','markersize',6),hold on
plot(x1,y1,'r-')
axis equal
end

请先登录，再回答此问题。

回答（1 个）

Adam
Adam 2022-2-2
Which iteration is not finishing? You have a nested for loop and two while loops in that code. I assume you mean one of the while loops.
I would suggest using the debugger to understand what is happening within the loop.
while t1>0.5 || t2 >0.5 || t3 > 0.5
is inherently an infinite loop, depending entirely on you updating t1, t2 and t3 within the loop to eventually become values for which that evaluates to false. If that never happens then the loop will indeed be infinite.
  1 个评论
JIBIN GEORGE
JIBIN GEORGE 2022-2-2
the first while loop works as I required however the second while loop is not stopping.is there any way I can end the second while loop when its reached its target

请先登录，再进行评论。

类别

在 Help Center 和 File Exchange 中查找有关 MATLAB 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by