Sie sind auf Seite 1von 26

# Bogazici University

ME634 Robotics
Assignmet II
2 December 2016

a)

## Figure 1: Two-links manipulator

It is asked us to choose a via point to avoid the obstacle and compute a cubic polynomial trajectory for
joint space and end-effector space. In order to pass to joint variable space, we need to calculate the joint
variables from the given position of the end-effector. The knowns are the location of the inital position
P0

 r10, 55s, position of the final point PF  r55, 50s, and the location of the obstacle PC  r30, 60s.

2
1

 cos1 p P0 p1q

q
2l1 l2

##  atan2pP0 p2q, P0 p1qq  atan2pl2 sinp2 q, l1

(1)
l2 cosp2 qq

For two values of 2 , there are two 1 values. Let us call the joint variables p
polynomial is in the form,

q ptq  a0

a2 t2

a1 t

a 3 t3

(2)

 r1 , 2 s.

A cubic
(3)

## The velocity and acceleration functions are thus,

q9ptq  a1

2a2 t

q:ptq  2a2

3a3 t2

(4)

6a3 t

When the trajectory need to avoid some obstacles, that means the trajectory should not pass through
some points. To make it simple, that means the trajectory should pass through some points that can
avoid hitting the obstacles. The most simple idea is to choose an appropriate velocity so that the
acceleration is continuous in this point. Since there is an obstacle, there must be two paths from inital
position to via point and from there to goal position.

## Since there are 2 paths, that means there is 2 displacement equations.

q1 ptq  a0
q2 ptq  b0

a1 t

a2 t2

a3 t3

b1 t

b2 t2

b3 t 3

t P rt0 , tC s

(5)

t P rtC , tF s

In order to provide the continuos acceleration during the motion of the robotic arm, it is required
to solve 2 equations and equal the acceleration equations. To solve for the unknown coefficients, we
need to have the boundary condition for the first trajectory that is from initial position to via point.
Considering the start time t0 is zero, the boundary conditions:

$, ' q1 pt0 q  A1 / ' / ' & q1 ptC q  B1 / . ' q1 pt0 q  C1 / ' / ' ' / % q1 ptC q  D1 /   A1 a0 0 0     B a t2C t3C 1 1    0 0  C1 a2   9  1  1 0 0 tC 1 0 a0 q91 pt0 q  A1 ,  0  C1 a0 a1 tC 2tC 3t2C a3 t3C  B1 , a2 t2C a3 a1 D1  C1 , a1 2a2 tC 3a3 t2C  D1 is given. Assuming that we estimate the displacement time from initial position to ## obstable, it is obtained three equations regarding the first trajectory.  A1  B1 3a3 t2C  D1 a0 a2 t2C a0 2a2 tC a3 t3C (6) For the second trajectory, lets constitute the equations. The trajectory starts from the obstacle to goal position. The boundary conditions: ,$
/
'
q
p
t
q

A
2
C
2
/
'
'
/
.
& q2 ptF q  B2 /
'
q2 ptC q  C2 /
'
/
'
/
'
% q2 ptF q  D2 /
  
t2C
t3C
b
A2
 0  


t2F
t3F
b
B
1
2




2tC 3t2C
b2  
 C2
9


1

1

0

tC
tF
1

0
q92 ptF q

 0  D2

2tF

3t2F

b3

D2

is given. Assuming that we know the displacement time from obstable to the goal

## position, it is obtained four equations regarding the first trajectory.

b0
b0

b1 t C

b2 t2C

b1 tF

b2 t2F

b1

2b2 tC

b1

2b2 tF

 A2
 B2
3b3 t2C  C2
3b3 t2F  D2
b3 t3C
b3 t3F

(7)

For the continuos acceleration, the two accelerations at the via point must be equal. t0C represents
the time until the end-effector reaches the via point from the inital position. tCF is the time until the
2

## end-effector reaches the goal from the point position selected.

2a2
To simplify the equations, we take t0C

6a3 t0C

 2b2

6b3 tCF

(8)

##  tCF . Thus, a2  b2 , and a3  b3 . Moreover, the velocity must

 C2 and a1  b1  0. Lets call the

## q pt0 q, and q ptF q as q0 and qF respectively. From the equation (7),

b2 t2F

b3 t3F
3b3 t2F

2b2 tF

 qF  q0
0

(9)

We know the time of the movement, which is tF , and inital and the goal positions and also tC . Thus, it
can be solved for b2 , and b3 . The specific velocity at the via point equals to,
q9C

 2b2 tC

3b3 t2C

(10)

## ii) End-Effector Cartesian Space

Lets define cosp1 q and sinp1 q as c1 and s1 . We can write the end-effector position associated with the

X
Y

X9

Y:

l2 c12

##  pl1 s1  l2 s12 q1  l2 s12 2

 pl1 c1 l2 c12 q1 l2 c12 2

##  pl1 c1  l2 c12 q1  pl1 s1

 pl1 s1  l2 s12 q1 pl1 c1
9

(11)

l2 s12

Y9

:
X

 l1 c1
 l1 s1

(12)

## l2 s12 q:1  l2 c12 91  2l2 c12 92  l2 s12 :2

l2 c12 q:1  l2 s12 91  2l2 s12 92

l2 c12 :2

(13)

## Q2. Potential Field Path Planning

In this question, potential field functions (attractive and repulsive ) and vectors are used for path
planning. The objective is to move an end effector of two link robot arm from start point to goal point
while avoiding from an obstacle. In order to apply this approach to robot motion planning, the arm
and the obstacle to be avoided are thought as an positive charge and the goal point is considered as a
negative charge. Hence, the manipulator will stay away from the obstacle and tend to move towards
goal.
Considering the approach of Khatib (1986), the conic and quadratic potentials are used. Conic potential
calculates the Euclidean distance between robot and goal point. This potential is used when robot is
far away from the goal, thus the attractive potential becomes large and robot moves faster. Quadratic
potential defined to overcome the discontinuities in the gradient vectors caused by world configuration. A
threshold dgoal is defined to switch between conic and quadratic potentials. The repulsive force, prevents
the collision. While far away from the obstacle, there will be no repulsive force affecting the robot.

## Figure 5: Path planning

Appendices

Q.1.b.Contents

SET UP
inverse kinematics for initial position
inverse kinematics for via point
inverse kinematics for goal position
for theta1
for theta2
for end effector space

## %This code models cubic trajectory for two-link manipulator when an

%obstacle exist in the path.

SET UP
clc
clear
close all
%student name

## inverse kinematics for initial position

Px=-10;Py=55;a1=50;a2=25;
theta2=acos((Px^2+Py^2-a1^2-a2^2)/(2*a1*a2));
theta2_2=2*pi-theta2;
% theta1=atan2(Py,Px)-atan2(a2*sin(theta2),a1+a2*cos(theta2)); %for 1.theta2
theta1_2=atan2(Py,Px)-atan2(a2*sin(theta2_2),a1+a2*cos(theta2_2)); %for 2.theta2

## inverse kinematics for via point

Pxv=30;Pyv=30;
theta2v=acos((Pxv^2+Pyv^2-a1^2-a2^2)/(2*a1*a2));
theta2_2v=2*pi-theta2v;
% theta1v=atan2(Pyv,Pxv)-atan2(a2*sin(theta2v),a1+a2*cos(theta2v)); %for 1.theta2
theta1_2v=atan2(Pyv,Pxv)-atan2(a2*sin(theta2_2v),a1+a2*cos(theta2_2v)); %for 2.theta2

## inverse kinematics for goal position

Pxf=55;Pyf=50;
theta2f=acos((Pxf^2+Pyf^2-a1^2-a2^2)/(2*a1*a2));
theta2_2f=2*pi-theta2f;
% theta1v=atan2(Pyf,Pxf)-atan2(a2*sin(theta2f),a1+a2*cos(theta2f)); %for 1.theta2
7

## theta1_2f=atan2(Pyf,Pxf)-atan2(a2*sin(theta2_2f),a1+a2*cos(theta2_2f)); %for 2.theta2

for theta1
tv=2;Vv=-0.518325 ; tf=4;Vf=0;
Cv=[1 0 0 0
1 tv tv^2 tv^3
0 1 0 0
0 1 2*tv 3*tv^2];
Rv=[theta1_2;theta1_2v;0;Vv];
Av=inv(Cv)*Rv;
t=0:0.02:tv;

%time interval

## qv=Av(1)+Av(2)*t+Av(3)*t.^2+Av(4)*t.^3; %path for via

qv_dot=Av(2)+2*Av(3)*t+3*Av(4)*t.^2;

## %velocity for theta1 until via point

qv_ddot=2*Av(3)+6*Av(4)*t;

## % from via point to final

Cf=[1 tv tv^2 tv^3
1 tf tf^2 tf^3
0 1 2*tv 3*tv^2
0 1 2*tf 3*tf^2];
Rf=[theta1_2v; theta1_2f;Vv;Vf];
Af=inv(Cf)*Rf;
tf=tv:0.02:4;
qf=Af(1)+Af(2)*tf+Af(3)*tf.^2+Af(4)*tf.^3; %path for final
qf_dot=Af(2)+2*Af(3)*tf+3*Af(4)*tf.^2;
qf_ddot=2*Af(3)+6*Af(4)*tf;
%plotting

## %velocity for theta1 until via point

subplot(2,2,1);
plot(t,qv*180/pi,LineWidth,2);
hold on
plot(tf,qf*180/pi,LineWidth,2);
hl = legend(${\theta_1}$);
set(hl, Interpreter, latex);
subplot(2,2,2);
plot(t,qv_dot,LineWidth,2);
hold on
plot(tf,qf_dot,LineWidth,2);
hl = legend($\dot{\theta_1}$);
set(hl, Interpreter, latex);
subplot(2,2,[3,4]);
plot(t,qv_ddot,LineWidth,2);
hold on
plot(tf,qf_ddot,LineWidth,2);
hl = legend($\ddot{\theta_1}$);
set(hl, Interpreter, latex);

figure;
150

0.5
31

31

100

50

-0.5

-1
0

0.5
31

-0.5

-1
0

0.5

1.5

2.5

3.5

for theta2
tv=2; Vv2=0.482625; tf=4;Vf=0;
Cv_2=[1 0 0 0
1 tv tv^2 tv^3
0 1 0 0
0 1 2*tv 3*tv^2];
Rv_2=[theta2_2; theta2_2v;0;Vv2];
Av_2=inv(Cv_2)*Rv_2;
t=0:0.02:tv;
qv2=Av_2(1)+Av_2(2)*t+Av_2(3)*t.^2+Av_2(4)*t.^3; %path for via theta2
qv2_dot=Av_2(2)+2*Av_2(3)*t+3*Av_2(4)*t.^2;

## %velocity for theta2 until via point

qv2_ddot=2*Av_2(3)+6*Av_2(4)*t;

## % from via point to final

Cf_2=[1 tv tv^2 tv^3
1 tf tf^2 tf^3
0 1 2*tv 3*tv^2
0 1 2*tf 3*tf^2];
Rf_2=[theta2_2v; theta2_2f;Vv2;Vf];
Af_2=inv(Cf_2)*Rf_2;
tf=tv:0.02:4;
qf2=Af_2(1)+Af_2(2)*tf+Af_2(3)*tf.^2+Af_2(4)*tf.^3;

## %path for final

qf2_dot=Af_2(2)+2*Af_2(3)*tf+3*Af_2(4)*tf.^2;

## %velocity for theta1 from via to final

qf2_ddot=2*Af_2(3)+6*Af_2(4)*tf;

## %acc. for theta1 until via point

subplot(2,2,1);
plot(t,qv2*180/pi,LineWidth,2);
hold on
plot(tf,qf2*180/pi,LineWidth,2);
hl = legend(${\theta_2}$);
set(hl, Interpreter, latex);
subplot(2,2,2);
plot(t,qv2_dot,LineWidth,2);
hold on
plot(tf,qf2_dot,LineWidth,2);
hl = legend($\dot{\theta_2}$);
set(hl, Interpreter, latex);
subplot(2,2,[3,4]);
plot(t,qv2_ddot,LineWidth,2);
hold on
plot(tf,qf2_ddot,LineWidth,2);
hl = legend($\ddot{\theta_2}$);

10

figure;
350

2
32

32

300

250

200

-1
0

2
32

-2

-4
0

0.5

1.5

2.5

11

3.5

## for end effector space

Xv=a1*cos(qv)+a2*cos(qv+qv2);
Yv=a1*sin(qv)+a2*sin(qv+qv2);
Xf=a1*cos(qf)+a2*cos(qf+qf2);
Yf=a1*sin(qf)+a2*sin(qf+qf2);
subplot(2,1,1);
plot(t,Xv,LineWidth,2);
ylabel(X-position(mm));
hold on
grid on
plot(tf,Xf,LineWidth,2);
subplot(2,1,2);
plot(t,Yv,LineWidth,2);
hold on
plot(tf,Yf,LineWidth,2);
grid on
xlabel(time(s));
ylabel(Y-position(mm));
figure;
%X_dot until the via point
for i=1:length(qv)
Xv_dot(i)=(-a1*sin(qv(i))-a2*sin(qv(i)+qv2(i)))*qv_dot(i)-(a2*sin(qv(i)+qv2(i)))*qv2_dot(i);
Yv_dot(i)=(a1*cos(qv(i))+a2*cos(qv(i)+qv2(i)))*qv_dot(i)+(a2*sin(qv(i)+qv2(i)))*qv2_dot(i);
end
%X_dot until the goal from the via point
for i=1:length(qf)
Xf_dot(i)=(-a1*sin(qf(i))-a2*sin(qf(i)+qf2(i)))*qf_dot(i)-(a2*sin(qf(i)+qf2(i)))*qf2_dot(i);
Yf_dot(i)=(a1*cos(qf(i))+a2*cos(qf(i)+qf2(i)))*qf_dot(i)+(a2*sin(qf(i)+qf2(i)))*qf2_dot(i);
XYf_dot(i)=sqrt(Xf_dot(i)^2+Yf_dot(i)^2);
end
%plotting X_dot and Y_dot
subplot(2,1,1);
plot(t,Xv_dot,LineWidth,2);
hold on
plot(tf,Xf_dot,LineWidth,2);
hl = legend($\dot{X}$);
set(hl, Interpreter, latex);
subplot(2,1,2);
plot(t,Yv_dot,LineWidth,2);
hold on
plot(tf,Yf_dot,LineWidth,2);
hl = legend($\dot{Y}$);
set(hl, Interpreter, latex);
xlabel(time(s));
ylabel(velocity(mm/s));

12

figure;
%X_ddot and Y_ddot in the end-effector space
for i=1:length(qv)

Xv_ddot(i)=(-a1*cos(qv(i))-a2*cos(qv(i)+qv2(i)))*qv_dot(i)-(a1*sin(qv(i))+a2*sin(qv(i)+qv2(i)))*qv_dd
-2*a2*cos(qv(i)+qv2(i))*qv2_dot(i)-a2*cos(qv(i)+qv2(i))*qv_dot(i)-a2*sin(qv(i)+qv2(i))*qv2_ddot(i);

Yv_ddot(i)=(-a1*sin(qv(i))-a2*sin(qv(i)+qv2(i)))*qv_dot(i)+(a1*cos(qv(i))+a2*cos(qv(i)+qv2(i)))*qv_dd
-2*a2*sin(qv(i)+qv2(i))*qv2_dot(i)-a2*sin(qv(i)+qv2(i))*qv_dot(i)+a2*cos(qv(i)+qv2(i))*qv2_ddot(i);
end
for i=1:length(qf)

Xf_ddot(i)=(-a1*cos(qf(i))-a2*cos(qf(i)+qf2(i)))*qf_dot(i)-(a1*sin(qf(i))+a2*sin(qf(i)+qf2(i)))*qf_dd
-2*a2*cos(qf(i)+qf2(i))*qf2_dot(i)-a2*cos(qf(i)+qf2(i))*qf_dot(i)-a2*sin(qf(i)+qf2(i))*qf2_ddot(i);

Yf_ddot(i)=(-a1*sin(qf(i))-a2*sin(qf(i)+qf2(i)))*qf_dot(i)+(a1*cos(qf(i))+a2*cos(qf(i)+qf2(i)))*qf_dd
-2*a2*sin(qf(i)+qf2(i))*qf2_dot(i)-a2*sin(qf(i)+qf2(i))*qf_dot(i)+a2*cos(qf(i)+qf2(i))*qf2_ddot(i);
end
%plotting X_ddot and Y_ddot
subplot(2,1,1);
plot(t,Xv_ddot,LineWidth,2);
hold on
plot(tf,Xf_ddot,LineWidth,2);
hl = legend($\ddot{X}$);
set(hl, Interpreter, latex);
subplot(2,1,2);
plot(t,Yv_ddot,LineWidth,2);
hold on
plot(tf,Yf_ddot,LineWidth,2);
hl = legend($\ddot{Y}$);
set(hl, Interpreter, latex);
xlabel(time(s));
ylabel(accelaration(mm/s^2));

X-position(mm)

60
40
20
0
-20
0

0.5

1.5

0.5

1.5

2.5

3.5

2.5

3.5

Y-position(mm)

60
50
40
30
20

time(s)

13

30
X

20
10
0
-10
0

0.5

1.5

2.5

3.5

20

velocity(mm/s)

-20

-40
0

0.5

1.5

2.5

3.5

time(s)
100
X

50
0
-50
-100

accelaration(mm/s 2 )

0.5

1.5

2.5

3.5

100
Y

50

-50
0

0.5

1.5

2.5

3.5

time(s)

Contents

SET UP
Q2.a creat a 2-DOF RR robot name of Kadir
joint space
cartesian space

## %This code models Stanford robotic manipulator using the @Robotics

%Toolbox. It is for the question 2.a,b,c.

SET UP
clc
close all
%student name
14

%seting up the toolbox
startup_rvc;

## Robotics, Vision & Control: (c) Peter Corke 1992-2011 http://www.petercorke.com

- Robotics Toolbox for Matlab (release 9.10)
- pHRIWARE (release 1.1): pHRIWARE is Copyrighted by Bryan Moutrie (2013-2016) (c)
Run rtbdemo to explore the toolbox

## Q2.a creat a 2-DOF RR robot name of Kadir

L(1) = Link([0, 0, 50, 0,0]); %revolute 1
L(2) = Link([0, 0, 25, 0,0]);

%revolute 2

## %linking the joints

%displaying robot
robo.display;

robo =
Kadir (2 axis, RR, stdDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j |

theta |

d |

a |

alpha |

offset |

+---+-----------+-----------+-----------+-----------+-----------+
|

1|

q1|

0|

50|

0|

0|

2|

q2|

0|

25|

0|

0|

+---+-----------+-----------+-----------+-----------+-----------+
grav =

base = 1

9.81
0

tool =
0

0
0

0
0

joint space
q_0=[2.2143 4.7124]; %initial point
q_p=[1.3086 4.1538]; %via point
q_f=[0.8321 5.9994]; %final point
%%pats
tempo=0:0.1:4;

%time
15

qa=jtraj(q_0,q_p,tempo);
qb=jtraj(q_p,q_f,tempo);
W = [-60 60 0 100 0 60];

## %workspace for the robot

C=[30,60];
circle(C,10);

%obstacle

hold on
robo.plot(qa,workspace, W);
robo.plot(qb,workspace, W);
title(Joint space);
figure;

16

cartesian space
T0=robo.fkine(q_0);

## % forward kinemetics for the inital position

Tp=robo.fkine(q_p);

## % forward kinemetics for via point

Tf=robo.fkine(q_f);

## % forward kinemetics for goal position

q1=ctraj(T0,Tp,50);
q2=ctraj(Tp,Tf,50);

%N=50 samples

circle(C,10);
hold on
grid on
tranimate(q1);
tranimate(q2);
title(End-effector space);

17

End-effector space

70
65
60
55

YZ X

50
45
40
35
Y
ZX

30
25
20

25

30

35

40

45

50

55

Q1.c.Contents

SET UP
Q2.a creat a 2-DOF RR robot name of Kadir
joint space
cartesian space

SET UP
clc
close all
%student name
%seting up the toolbox
startup_rvc;

## Q2.a creat a 2-DOF RR robot name of Kadir

L(1) = Link([0, 0, 50, 0,0]); %revolute 1
L(2) = Link([0, 0, 25, 0,0]);

%revolute 2

## %linking the joints

18

60

%displaying robot
robo.display;

robo =
Kadir (2 axis, RR, stdDH, fastRNE)
+---+-----------+-----------+-----------+-----------+-----------+
| j |

theta |

d |

a |

alpha |

offset |

+---+-----------+-----------+-----------+-----------+-----------+
|

1|

q1|

0|

50|

0|

0|

2|

q2|

0|

25|

0|

0|

+---+-----------+-----------+-----------+-----------+-----------+
grav =

base = 1

9.81
0

tool =
0

0
0

joint space
q_0=[2.2143 4.7124]; %initial point
q_p=[1.3086 4.1538]; %via point
q_f=[0.8321 5.9994]; %final point
%%pats
tempo=0:0.1:4;

%time

qa=jtraj(q_0,q_p,tempo);
qb=jtraj(q_p,q_f,tempo);
W = [-60 60 0 100 0 60];

## %workspace for the robot

C=[30,60];
circle(C,10);

%obstacle

hold on
robo.plot(qa,workspace, W);
robo.plot(qb,workspace, W);
title(Joint space);
figure;

19

cartesian space
T0=robo.fkine(q_0);

## % forward kinemetics for the inital position

Tp=robo.fkine(q_p);

## % forward kinemetics for via point

Tf=robo.fkine(q_f);

## % forward kinemetics for goal position

q1=ctraj(T0,Tp,50);
q2=ctraj(Tp,Tf,50);

%N=50 samples

circle(C,10);
hold on
grid on
tranimate(q1);
tranimate(q2);
title(End-effector space);

20

End-effector space

70
65
60
55

YZ X

50
45
40
35
Y
ZX

30
25
20

25

30

35

40

45

50

55

Q2.a.Contents
SET UP
map

SET UP
clc
clear
close all
%student name

map
pos = [-60 0 120 100];
%initial and the final position of the end-efefctor
x0=-10;
xf=55;
y0=55;
yf=50;
rectangle(Position,pos);
hold on
plot(x0,y0,O,LineWidth,2);
hold on
plot(xf,yf,X,LineWidth,2);
hold on
%obstacle
Obs_pos=[20,50,20,20];
r=10;
21

60

Obs_x=30;
Obs_y=60;
rectangle(Position,Obs_pos,Curvature,[1 1],Facecolor,r)
%attractive function
d=10;
p0=2;
[X,Y]=meshgrid(-100:1:100);
ksi=1;
eta=12;
for i=1:length(X)
for j=1:length(Y)
dist=sqrt((X(i,j)-xf)^2+(Y(i,j)-yf)^2);
if dist<d
U_att(i,j)=0.5*ksi*((X(i,j)-xf)^2+(Y(i,j)-yf)^2);
else
U_att(i,j)=d*ksi*sqrt((X(i,j)-xf)^2+(Y(i,j)-yf)^2)-0.5*ksi*d^2;
end
end
end
% repulsive function
for i=1:length(X)
for j=1:length(Y)
if (sqrt((X(i,j)-Obs_x)^2+(Y(i,j)-Obs_y)^2)-r) <= p0
U_rep(i,j)=0.5*eta*(1/(sqrt((X(i,j)-Obs_x)^2+(Y(i,j)-Obs_y)^2)-r)-1/p0)^2;
else
U_rep(i,j)=0;
end
end
end
for i=1:length(X)
for j=1:length(Y)
if sqrt((X(i,j)-Obs_x)^2+(Y(i,j)-Obs_y)^2)>=1.5*r
U_rep(i,j)=U_rep(i,j);
elseif sqrt((X(i,j)-Obs_x)^2+(Y(i,j)-Obs_y)^2)<1.5*r && sqrt((X(i,j)-Obs_x)^2+(Y(i,j)-Obs_y)^2)>r
U_rep(i,j)=U_rep(i,j);
else
U_rep(i,j)=2500;
end
if U_rep(i,j)~=2500 && U_rep(i,j)~=0
U_rep(i,j)=500;
U(i,j)=U_rep(i,j)+U_att(i,j);
else
U(i,j)=U_rep(i,j)+U_att(i,j);
22

end
end
end
meshc(X,Y,U)
title(Contour lines of potential surfaces (goal + obstacle));

Q2.b.Contents
fields
clc
clear
close all
pos = [-60 0 120 100];
%initial position and goal
x_0=-10;
x_f=55;
y_0=55;
y_f=50;
rectangle(Position,pos,Curvature,[1 1]);
hold on
plot(x_0,y_0,O,LineWidth,2);
hold on
plot(x_f,y_f,X,LineWidth,2);
%obstacle
r=10;
x_Obs=30;
y_Obs=60;
Obs=[x_Obs,y_Obs];

23

hold on
rectangle(Position,[x_Obs-r,y_Obs-r,2*r,2*r],Curvature,[1,1], FaceColor,r);
hold on
100
90
80
70
60
50
40
30
20
10
0
-60

-40

-20

20

40

60

fields
q0=[x_0;y_0];
qf=[x_f;y_f];
q_0=q0;
safetydistance=2;
d_star=sqrt((q_0(1)-qf(1))^2+(q_0(2)-qf(2))^2)*safetydistance;
speed=5;
time=0;
time_inc=0.001;
ksi=1; eta=12; %parameters for the potential functions
dis_inc=0.5;
while (q_0(1)>qf(1)+dis_inc || q_0(1)<qf(1)-dis_inc || q_0(2)>qf(2)+dis_inc || q_0(2)<qf(2)-dis_inc)
%attraction field
temp=q_0(end,:);
d=sqrt((temp(1)-qf(1))^2+(temp(2)-qf(2))^2);
if d<=d_star
else
end
%repulsion field
d_obst=sqrt((temp(1)-x_Obs)^2+(temp(2)-y_Obs)^2)-r;

24

qstar=r*2;
if d_obst<qstar
end
q_0=[q_0;temp];
time=time+time_inc;
end
plot(q_0(:,1),q_0(:,2),b,LineWidth,2);
100
90
80
70
60
50
40
30
20
10
0
-60

-40

-20

20

40

25

60