from centrex=6, it should move again to 7(as repulsive force becomes zero & only attractive force exists), why does it move to %???????

1 次查看（过去 30 天）
Twinkle
Twinkle 2011-9-21
%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

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

回答（2 个）

Vivek Bhadouria
Vivek Bhadouria 2011-9-22
Yeah, Please mention your specific question.
Twinkle
Twinkle 2011-9-22
Actually, i need to show that in the potential field method, if the obstacles are too near, robot cant go thru the path between the obstacles even when target pulls it in that direction. I suitably chose the values of the constants of the force(repulsive and attractive). So, in this code i have chosen a 'square' range for the robot. Robot assigns a value 1 to those cells/coordinates in which there's an obstacle and zero otherwise.But, attractive force exists all the time. Formula for the force is in the code itself, can check it out. So, at centrex(centre of the robot, which changes as robot moves)=6, only attractive force exists, repulsive force=0 because robot cant sense the obstacles. At centrex=7, both the forces exists, with repulsive being the more dominating one ! So, robot moves away from obstacle to centrex=6.From there it should again move to 7(due to attractive force only), but rather it moves away t0 centrex=5! Note that it should only move to integer coordinates, so i have used the round function.

类别

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

标签

Community Treasure Hunt

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

Start Hunting!

Translated by