from centrex=6, it should move again to 7(as repulsive force becomes zero & only attractive force exists), why does it move to %???????
3 Ansichten (letzte 30 Tage)
Ältere Kommentare anzeigen
%It is an example of occurence of a situation in 'potential field method' in
%which the robot is unable to pass through 2 closely spaced obstacles
%the robot is assumed to be square in shape with a width(perpendicular distance between centre and any edge)
%the obstacle is assumed to be rectangular and long SO THAT FORCE EXISTS ALONG THE LATERAL X-DIRECTION ONLY
close all;
clear all;
clc;
%creating the CONFIGURATION SPACE as x-y coordinates
AXIS([-20 20 -20 20]);
%specifications of the constants present in the force equation
Fct=3;
fcr=1;
n=1;
k=1; %specifying the steering angle
v=1;
%ROBOT SPECIFICATION
%start point of the robot which gives the initial centre position
robotix=1;
robotiy=0;
%size and orientation of the robot with respect to x axis
centrex=[];
centrey=[];
centrex(1)=robotix;
centrey(1)=robotiy;
theta=[];
theta(1)=0;
width=1;
robv1x(1)=centrex(1)-(sqrt(2)*width)*cos(theta(1)-pi/4);
robv1y(1)=centrey(1)-(sqrt(2)*width)*sin(theta(1)-pi/4);
%perpendicular distance from centre to any edge of the robot
range=3; %square region in whiuch obstacles are sensed i.e. ACTIVE REGION
%specifying the target location
targetx=20;
targety=0;
%OBSTACLE SPECIFICATION
ob1=[10,3];
widthOB1=2;
heightOB1=3;
ob2=[10,-5];
widthOB2=3;
heightOB2=2;
for i=1:(2*range+1)
for j=1:(2*range+1)
C(i,j)={0};
end
end
%robx and roby correspond to the active cells of the robot, C{i,j}
%represents their active values depending on the presence of obstacles
i=1;
delta=[];
while(1)
for roby=centrey(i)-range:centrey(i)+range
for robx=centrex(i)-range:centrex(i)+range
for kk=3:6
for k=10:12
if ((robx==k) && (roby==kk))
C(robx-centrex(i)+range+1,roby-centrey(i)+range+1)={1};
end
end
end
for kk=-5:-3
for k=10:13
if ((robx==k) && (roby==kk))
C(robx-centrex(i)+range+1,roby-centrey(i)+range+1)={1};
end
end
end
D(robx-centrex(i)+range+1,roby-centrey(i)+range+1)= {sqrt(((centrex(i)-robx)^2)+((centrey(i)-roby)^2))};
D(range+1,range+1)={1}; %for code completion, else at this point distance is alwayz zero and it renders further calulation undefined!
Frepx(robx-centrex(i)+range+1,roby-centrey(i)+range+1)={(fcr*width*(C{robx-centrex(i)+range+1,roby-centrey(i)+range+1})*(centrex(i)-robx))/((D{robx-centrex(i)+range+1,roby-centrey(i)+range+1})^2)}
Frepy(robx-centrex(i)+range+1,roby-centrey(i)+range+1)={(fcr*width*(C{robx-centrex(i)+range+1,roby-centrey(i)+range+1})*(centrey(i)-roby))/((D{robx-centrex(i)+range+1,roby-centrey(i)+range+1})^2)}
end
end
Tfrepx=0;
for k=1:(2*range+1)
for kk=1:(2*range+1)
Tfrepx=Tfrepx+Frepx{k,kk};
end
end
Tfrepy=0;%Actually it is nearly equal to zero in matlab calculations! But mathematically, this is correct!
Fattx=Fct*(targetx-centrex(i))/(((centrex(i)-targetx)^2)+((centrey(i)-targety)^2))
Fatty=Fct*(targety-centrey(i))/(((centrex(i)-targetx)^2)+((centrey(i)-targety)^2))
Tfrepx
Rx=Tfrepx + Fattx;
Ry=Tfrepy + Fatty;
R=sqrt((Rx^2)+(Ry^2));
delta(i)=atan2(Ry,Rx)
if ((delta(i)<0) && (delta(i)>=pi))
delta(i)=2*pi+delta(i);
end
robv1x(1)=centrex(i)-(sqrt(2)*width)*cos(theta(i)-pi/4);
robv1y(1)=centrey(i)-(sqrt(2)*width)*sin(theta(i)-pi/4);
robv1x(2)=centrex(i)+(sqrt(2)*width)*cos(theta(i)-pi/4);
robv1y(2)=centrey(i)-(sqrt(2)*width)*sin(theta(i)-pi/4);
robv1x(3)=centrex(i)+(sqrt(2)*width)*cos(theta(i)-pi/4);
robv1y(3)=centrey(i)+(sqrt(2)*width)*sin(theta(i)-pi/4);
robv1x(4)=centrex(i)-(sqrt(2)*width)*cos(theta(i)-pi/4);
robv1y(4)=centrey(i)+(sqrt(2)*width)*sin(theta(i)-pi/4);
patch(robv1x,robv1y,'g');
hold on
plot(centrex(i),centrey(i),'bo');
hold on
theta(i+1)=delta(i)
if (theta(i+1)>=2*pi)
theta(i+1)=theta(i+1)-2*pi;
end
if (theta(i+1)<0)
theta(i+1)=theta(i+1)+2*pi;
end
centrex(i+1) = round(centrex(i) + v*(cos(theta(i+1))))
centrey(i+1) = 0;
if (((Rx==0) && (Ry==0))||(i==8))%In this particular iteration ROBOT comes back to its previous position !!!
display('ROBOT ACTUALLY STOPS HERE !!!!!!!');
break;
end
z=rectangle('Position',[ob1(1),ob1(2),widthOB1,heightOB1],'Facecolor','c');
z=rectangle('Position',[ob2(1),ob2(2),widthOB2,heightOB2],'Facecolor','y');
plot(targetx,targety,'ro');
i=i+1;
end
1 Kommentar
Walter Roberson
am 21 Sep. 2011
Please add a (clear) question at the top of the body of your message.
Antworten (2)
Siehe auch
Kategorien
Mehr zu Robotics finden Sie in Help Center und File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!