Mini Project

Report on PD Controller (Control of End-Effecters position)

Submitted by:

Faiz Muhammad

Submitted to: Prof. Dr. Riaz Akbar Shah

Institute of Mechatronics Engineering

University of Engineering and Technology Peshawar
ACKNOWLEDGMENT

I am grateful, first and foremost, to Allah Almighty by whose grace this project has been
completed. Next I wish to acknowledge with gratefulness the expert guidance and help of Dr.
Riaz Akbar Shah. I personally feel that his able and continuous support during the project work
resulted in the completion of this project.

ABSTRACT
This report focuses on the motion control of the three-degree-of-freedom (3-DOF) planar
manipulator of a Robot. To improve the response performance, the proportional-derivative
controller is introduced to the closed-loop system of a Robot. The simulations show that the end
effecter moves along a circular trajectory. The simulation shows that the response speed is
increased and the maximum overshoot, rise time and settling time are also reduced.

List of Abbreviations

The commonly used symbols in the report are:

DH: Denavit-Hartenberg notation
CL: Closed Loop
Kp: Proportional Gain Constant
Kd: Derivative Gain Constant
Ki: Integral Gain Constant
Kt: Motor Constant
: Motor Torque
ISO: International Standards Organization
PD: Proportional Derivative
PI: Proportional Integral
PID: Proportional Integral Derivative

ACKNOWLEDGMENT.................................................................................................................ii
ABSTRACT...................................................................................................................................iii
List of Abbreviations......................................................................................................................iv
INTRODUCTION.........................................................................................................................vii
1.1 Robot Manipulator...........................................................................................................vii
1.2 Robot..............................................................................................................................viii
1.3 Laws of Robot................................................................................................................viii
1.4 Industrial Robot..............................................................................................................viii
1.5 Robot types.....................................................................................................................viii
CHAPTER 2...................................................................................................................................ix
CONTROLLER..............................................................................................................................ix
2.1 Introduction.....................................................................................................................ix
2.2 Open-loop step response....................................................................................................x
2.3 Proportional control..........................................................................................................xi
2.4 PD control.......................................................................................................................xiii
2.5 PI control............................................................................................................................xiv
2.6 PID control.....................................................................................................................xvi
CHAPTER 3.................................................................................................................................xix
Description of Manipulator..........................................................................................................xix
3.1 Introduction....................................................................................................................xix
3.2 Forward Kinematics of the Manipulator..........................................................................xx
3.3 Inverse Kinematics of the Manipulator.........................................................................xxii
3.4 CLOSED FORM DYNAMIC EQUATIONS...............................................................xxiii
3.5 The outward iterations..................................................................................................xxiv
3.6 Inward iterations..........................................................................................................xxvii
CHAPTER 4............................................................................................................................xxxvii
Dynamic Simulation and Control............................................................................................xxxvii
4.1 Introduction...............................................................................................................xxxvii
4.2 Feedback and closed loop control............................................................................xxxviii
4.3 Simulation runs.........................................................................................................xxxviii
4.4 Matlab Code.....................................................................................................................xl
4.5 PD Control of End Effector...........................................................................................xliv
5

CHAPTER 5................................................................................................................................xlvi
Discussions and conclusions.......................................................................................................xlvi
REFERENCES...........................................................................................................................xlvii

CHAPTER 1

INTRODUCTION
1.1 Robot Manipulator
An industrial robot is comprised of a robot manipulator, power supply, and controllers. The robot
manipulator can be divided into two sections, each with a different function:

Arm and Body - The arm and body of a robot are used to move and position parts or tools
within a work envelope. They are formed from three joints connected by large links.

Wrist The wrist is used to orient the parts or tools at the work location. It consists of two or
three compact joints.

The robot manipulator is created from a sequence of link and joint combinations. The links are the rigid
members connecting the joints, or axes. The axes are the movable components of the robot that cause
relative motion between adjoining links. The mechanical joints used to construct the manipulator consist
of five principal types. Two of the joints are linear, in which the relative motion between adjacent links is
non-rotational, and three are rotary types, in which the relative motion involves rotation between links.
The arm-and-body section of the manipulator is based on one of four configurations. Each of these
anatomies provides a different work envelope and is suited for different applications.

Gantry - These robots have linear joints and are mounted overhead. They are also called
Cartesian and rectilinear robots.

Cylindrical - Named for the shape of its work envelope, cylindrical anatomy robots are fashioned
from linear joints that connect to a rotary base joint.

Polar - The base joint of a polar robot allows for twisting and the joints are a combination of
rotary and linear types. The work space created by this configuration is spherical.

Jointed-Arm - This is the most popular industrial robotic configuration. The arm connects with a
twisting joint, and the links within it are connected with rotary joints. It is also called an
articulated robot.

1.2 Robot
A robot is an automatically guided machine, able to do tasks on its own. Another common characteristic
is that by its appearance or movements, a robot often conveys a sense that it has intent or agency of its
own.

1.3 Laws of Robot

Robots were required to perform according to three principles known as three laws of Robotics, which
are as valid for real robots as they were for Asimovs robots, and they are:
1. A robot should not injure a human being or, through inaction, allow a human to be harmed.
2. A robot must obey orders given by humans except when that conflicts with the First Law.
3. A robot must protect its own existence unless that conflicts with the First or Second Law.

1.4 Industrial Robot

An industrial robot is officially defined by ISO as an automatically controlled, reprogrammable,
multipurpose manipulator programmable in three or more axes. The field of robotics may be more
practically defined as the study, design and use of robot systems for manufacturing (a top-level definition
relying on the prior definition of robot).
Typical

applications

of

robots

include welding,

painting,

assembly, pick and

place, packaging and palletizing, product inspection, and testing, all accomplished with high endurance,
speed, and precision.

1.5 Robot types

The most commonly used robot configurations are articulated robots, SCARA robots and Cartesian
coordinate robots, (aka gantry robots or x-y-z robots). In the context of general robotics, most types of
robots would fall into the category of robotic arms (inherent in the use of the word manipulator in the
above-mentioned ISO standard). Robots exhibit varying degrees of autonomy:
Some robots are programmed to faithfully carry out specific actions over and over again (repetitive
actions) without variation and with a high degree of accuracy. These actions are determined by

programmed routines that specify the direction, acceleration, velocity, deceleration, and distance of a
series of coordinated motions.

CHAPTER 2
CONTROLLER
2.1 Introduction
The three-term controller
The transfer function of the PID controller looks like the following:

Figure 2: The three-term controller

(2)

Where Kp is the proportional gain, Ki is the integral gain, and Kd is the derivative gain. First, let's take a
look at the effect of a PID controller on the closed-loop system using the schematic above. To begin, the
variable e is the tracking error or the difference between the desired reference value (r) and the actual
output (y). The controller takes this error signal and computes both its derivative and its integral. The
signal which is sent to the actuator (u) is now equal to the proportional gain (Kp) times the magnitude of
the error plus the integral gain (Ki) times the integral of the error plus the derivative gain (Kd) times the
derivative of the error.
Generally speaking, for an open-loop transfer function which has the canonical second-order form of?

1
y +2 W R +W 2R
2

10

a large Kp will have the effect of reducing the rise time and will reduce (but never eliminate) the steadystate error. Integral control (Ki) will have the effect of eliminating the steady-state error, but it will make
the transient response worse. If integral control is to be used, a small Ki should always be tried first.
Derivative control will have the effect of increasing the stability of the system, reducing the overshoot,
and improving the transient response. The effects on the closed-loop response of adding to the controller
terms Kp, Ki and Kd are listed in table form below.

CL RESPONSE

RISE TIME

OVERSHOOT

SETTLING TIME

S-S ERROR

Kp
Decreases
Increases
No Change
Decreases
Ki
Decreases
Increases
Increases
Eliminates
Kd
No Change
Decreases
Decreases
No Change
Note that these correlations are not exactly accurate, because Kp, Ki, Kd are related to each other.
Changing one of these variables can change the effect of the other two. For this reason, the table should
only be used as a reference when you are determining the values for Ki, Kp and Kd by trial & error.

2.2 Open-loop step response

Many PID controllers are designed by the trial & error selection of the variables Kp, Ki, and Kd. There
are some rules of thumb that you can consult to determine good values to start from; see your controls
book for some explanations of these recommendations.
Suppose we have a second-order plant transfer function:

Let's first view the open-loop step response. To model this system into Matlab, create a new m-file and
add in the following code:
num=1;
den=[1 10 20];
step(num,den)

11

Figure:
The DC gain of the plant transfer function is 1/20, so 0.05 is the final value of the output for a unit step
input. This corresponds to a steady-state error of 0.95, quite large indeed. Furthermore, the rise time is
about one second, and the settling time is about 1.5 seconds. Most likely, this response will not be
adequate. Therefore, we need to add some control.

2.3 Proportional control

From the chart above it is clear that Kp will reduce the steady-state error. Let's first add a proportional
controller into the system, by changing the m-file to look like the following:
num=1;
den=[1 10 20];
Kp=10;
[numCL,denCL]=cloop(Kp*num,den, -1);
t=0:0.01:2;
step(numCL, denCL,t)
The cloop command in Matlab is used to convert the open loop transfer function into a closed-loop one.
Since the cloop command only accepts one transfer function, the plant and controller transfer functions
have to be multiplied together before the loop is closed. It should also be noted that it is not a good idea to
use proportional control to reduce the steady-state error, because you will never be able to eliminate the
error completely. This fact will become evident below. If m-file is rerun, the following plot will be shown:

12

Now, the rise time has been reduced and the steady-state error is smaller, if greater Kp is used a, the rise
time and steady-state error will become even smaller. Change the Kp value in the m-file:
Kp=500;
Rerun the m-file and we should get the following plot:

It is clear that the rise time is now about 0.1 second and the steady-state error is much smaller. But the
overshoot has gotten very large. From this example shows that a large proportional gain will reduce the
steady-state error but at the same time, worsen the transient response. For a small overshoot and a small
steady-state error, a proportional gain alone is not enough.

13

2.4 PD control
The rise time is now probably satisfactory (rise time is about 0.1 second). Now let's add a derivative
controller to the system to see if the overshoot can be reduced. Add another variable, Kd, to the m-file, set
it equal to 10 and rerun the m-file:
Kp=500;
Kd=10;
numc=[Kd Kp];
[numCL, denCL]=cloop(conv(num,numc),den);
step(numCL, denCL,t)

The overshoot is much less than before. It is now only twenty percent instead of almost forty-five percent.
It can be improved further more. By increasing Kd to 100, the overshoot will be eliminated completely.

14

The above system is with a fast rise time and no overshoot. Unfortunately, there is still about a 5 percent
steady-state error. It would seem that a PD controller is not satisfactory for this system. Let's try a PI
controller instead.

2.5 PI control
As proportional control will reduce the steady-state error, but at the cost of a larger overshoot.
Furthermore, proportional gain will never completely eliminate the steady-state error. For that integral
control is required. Let's implement a PI controller and start with a small Ki. Lets go back to the m-file
and change it so it looks like the following (note that t input is removed from the step command so more
of the response can be seen):
Kp=500;
Ki=1;
Kd=0;
numc=[Kd Kp Ki];
denc=[1 0];
[numCL, denCL]=cloop(conv(num,numc),conv(den,denc));
step(numCL, denCL)

The Ki controller really slows down the response. The settling time becomes more than 500 seconds. To
reduce the settling time, we can increase Ki, but by doing this, the transient response will get worse (e.g.
large overshoot). Try Ki=10, by changing the Ki variable. The plot can be seeing better if
an axis command is added after the step response. The m-file should now look like the following:

15

Kp=500;
Ki=10;
Kd=0;
numc=[Kd Kp Ki];
denc=[1 0];
[numCL, denCL]=cloop(conv(num,numc),conv(den,denc));
step(numCL, denCL)
axis([0 100 0 1.5])

Now it has a large overshoot again, while the settling time is still long. To reduce settling time and
overshoot, a PI controller by itself is not enough.

2.6 PID control

From the two controllers above, for a fast response, small overshoot, and no steady-state error, neither a
PI nor a PD controller will suffice. Let's implement both controllers and design a PID controller to see if
combining the two controllers will yield the desired response. Recalling that the PD controller gives a
pretty good response, except for a little steady-state error. Let's start from there, and add a small Ki (1).
Change the m-file to the following to implement the PID controller and plot the closed-loop response:
KP=500;
KI=1;
KD=100;

16

numc=[KD KP KI];
denc=[1 0];
[numCL, denCL]=cloop(conv(num,numc),conv(den,denc));
Step (numCL, denCL)

The settling time is still very long. Increase Ki to 100.

The settling time is much shorter, but still not small enough. Increase Ki to 500 and change
the step command to step(numCL, denCL,t):

17

Now the settling time reduces to only 1.5 seconds. This is probably an acceptable response for this
system. To design a PID controller, the general rule is to add proportional control to get the desired rise
time, add derivative control to get the desired overshoot, and then add integral control (if needed) to
eliminate the steady-state error.

PID Examples
Cruise control, DC Motor, Bus Suspension, Inverted pendulum

18

CHAPTER 3
Description of Manipulator
3.1 Introduction
This is a two link planar manipulator as shown in the figure. All mass exists as a point mass at the distal
end of each link. A separate motor through a gearbox powers each link. The base is fixed to the ground.
Various symbols used are defined as follows.

M 1 MASS OF LINK 1
M 2 MASS OF LINK 2
M 3 MASS OF LINK 3
LI LENGTH OF LINK 1
L 2 LENGTH OF LINK 2
L3 LENGTH OF LINK 3
DH TABLE
I
1
2
3
4

i-1
0
0
0
0

i
1
2
3
0

di
0
0
0
0

ai-1
0
L1
L2
L3

Figure: Schematic of the three link manipulator

With link frames attached.
1

3.2 Forward Kinematics of the Manipulator

19

C1 S1
S1 C1
0
0
0
0

0
0
1
0

C2 S2
S 2 C2
0
0
0
0

0 L1
0 0
1 0
0 1

0
0
0
1

T1 =

T2 =

T3 =

c3 -s3 0 l2
S3 c3 0 0
0 0

1 0

0 0 0

T3 = 1T2. 2T3 =

c23 -s23 0 c2 l2+l1

S23 c23 0 s2l2

T3 = 0T1. 1T3 =

c123 -s123 0 l2c12+l1c1

S123 c123 0 l2s12+l1s1

T4 =

1 0 0 l3
0 1 0 0
0 0 1 0
0 0 0 1

20

T4 = 1T3. 3T4 =

c23 -s23 0 c23 l3+c2l2+l1

S23 c23 0 s23l3+l2s2

T4 = 0T1. 1T4 =

c123 -s123 0 Px
S123 c123 0 Py
0

Where
r11=c1c2-s1s23=+c123
r12=-c1s23-s1c23=-s123
r13=0
PX=r14=c1 (c23L3+c2L2+L1)-s1 (L3s23+s2L2)
r21=s1c23+c1s23=s123
r22=-s1s23+c1c23=c123
r23=0
Py=r24=s1 (c23L3+c2L2+L1) +c1 (L3s23+s2L2)
r31=0, r32, r33=1, r34=0
r41=0, r42=0, r43=0, r44=1
Let
PX=X
Py=Y
and from 0T3
X=l2c12+l1c1
Y=l2s12+l1s1

21

3.3 Inverse Kinematics of the Manipulator

Squaring and adding the X and Y

X 2 Y 2 L12 L 2 2 2.L1.L 2.C 2

C2

( X 2 Y 2 L12 L 2 2 )
2.L1.L 2

S2 1 C22

2 ATAN 2( S 2, C 2)
1 ATAN 2(Y , X ) ATAN 2( K 2, K1)
Where
K1 L1 L 2.C 2
K 2 L 2.S 2
C=C123
S=S123
=1 + 2+ 3
=atan2(S, C)

22

3.4 CLOSED FORM DYNAMIC EQUATIONS

The locating vectors for center of masses of each link are
1

PC1 L1 X 1

PC2 L 2. X 2

Because of po int masses

C1

I1 0

C2

I2 0

There are no forces at end

f3 0
n3 0
As base is stationary
0 0
.

0 0
including gravity
0 .

v 0 g .Y 0

3Pc3=L3X
f i force exerted on link i by link i 1
n i torque exerted on link i by link i 1

23

3.5 The outward iterations

The outward iterations for link 1 are as follows:

0

w1 0
.
1

0

w1 0
..
1

1 .

gS1

v1 gC1
0

1 .

0

w1 0
.
1

1 .

v C1

F1

L1. 12 g .S1
..

L1. 1 gC1
.

m1.L1. 12 m1.g .S1

..

m2.L1. 1 m2.gC1

0

N1 0
0

24

The outward iterations for link 2 are as follows:

w2

.
.

0
0

1 2

0
0

2 .

w2

..

..

1 2

2 .

..

2
L1. 1.S 2 L1 1 .C 2 g .S12

..
..

v 2 L1. 1.C 2 L1. 12 .S 2 gC12

.

2
2
L 2( 1 2) L1. 1.S 2 L1 1 .C 2 g .S12

..
..
..
..

v C2 L 2( 1 2) L1. 1.C 2 L1. 12 .S 2 gC12

.

2 .

..

..

2
2
M 2.L 2( 1 2) M 2. L1. 1.S 2 M 2.L1 1 .C 2 M 2.g .S12

..
..
..
..

F 2 M 2.L 2( 1 2) M 2.L1. 1.C 2 M 2.L1. 12 .S 2 M 2.gC12

.

25

0

N 2 0
.
0

The outward iterations for link 3 are as follows:

0
3

3=

0
(

2+ 3+ 1

0
3

3dot =

0
(

1+ 2+ 3

(l1C3S2 +l1c2 s3+l2s3) 1 - (l1c3+l1s2s3)

-l2c3(

2+ 1

)2

+l2s3 2 +c3gs12 +s3gc12

(-l1s3S2 +l1c2 c3+l2c3) 1 - (l1s3+l1s2c3)

+l2s2(

2+ 1 )2 +l2c3 2 -s3gs12 +c3gc12

3

V3dot=

VC3dot =

-l2c3(

-l3 (

2+ 3 1

)2 + (l1C3S2 +l1c2 s3+l2s3) 1 - (l1c3-l1s2s3)

2+ 1 )2 +l2s3 2 +c3gs12 +s3gc12

26

L3 (

1+ 2+ 3
(

) +(-l1s3S2 +l1c2 c3+l2c3) 1 - (l1s3+l1s2c3)

2+ 1 )2

+l2s2

+l2c3 2 -s3gs12 +c3gc12

F3=m3 3VC3dot =m3

1

-l3 (

L3 (

2+ 3 1
-l2c3(

)2 + (l1C3S2 +l1c2 s3+l2s3) 1 - (l1c3-l1s2s3)

2+ 1 )2 +l2s3 2 +c3gs12 +s3gc12

1+ 2+ 3

) +(-l1s3S2 +l1c2 c3+l2c3) 1 - (l1s3+l1s2c3)

2+ 1 )2 +l2c3 2 -s3gs12 +c3gc12

+l2s2(

N3 =

0
0
0

3.6 Inward iterations

Inward iterations for link 3 are as follows:

f3=3R44f4+3F3

As 3R44f4=0

27

f 3 3F3
=

-l3 ( 2 + 3 + 1 )2 + (l1C3S2 +l1c2 s3+l2s3) 1- (l1c3-l1s2s3) 1

1 -l2c3( 2 +

m3L3(

1+ 2 + 3

+l2s2( 2 +

1 )2 +l2s3 2 +c3gs12 +s3gc12

) +(-l1s3S2 +l1c2 c3+l2c3) 1- (l1s3+l1s2c3) 1 1

1 )2 +l2c3 2 -s3gs12 +c3gc12

n3=3N3+3R44n4+ 3PC3 x 3F3+3P4 x3R44f4

l3
0
3

n3=

F3

0
0
3

n3=

m3*l3^2+m3*l2*l3*c3+m3*l1*l3*c32+m3*l3^2+m3*l2*l3*c3+m3*l3^
2+(m3*l2*l3*s2)(thetadot1+thetadot2)^2+
(m3*l1*l3*c3*s2+m3*l2*l3*s3)(thetadot1^2)+m3*l3*c3*g*cos(theta1+theta2)m3*l3*s3*g*s(theta1+theta2)

3
=

n3t

Z3=

m3*l3^2+m3*l2*l3*c3+m3*l1*l3*c32+m3*l3^2+m3*l2*l3*c3+m3*l3^2+

(m3*l2*l3*s2)(thetadot1+thetadot2)^2+
28

(m3*l1*l3*c3*s2+m3*l2*l3*s3)(thetadot1^2)+
m3*l3*c3*g*cos(theta1+theta2)-m3*l3*s3*g*s(theta1+theta2)

Inward iterations for link 2 are as follows:

2

f2=2 R3 3f3 + 2F2

C3

-S3

S3

C3

f2=
0

f3
2

+
0

{-m3 c3 l3 (

F2

2+ 3+ 1 )2 +m l c32 s
3 1
2

1 m l c32 1
3 1

m3 l2 c32 (

2+ 1 )2 +

m3 c32 gs12 + m3 l1c2 c3 s3 1 + m3 l1 c3 s2 s3 1

+m3 l2 c3 s3 (

1+ 2 )+ m
3

s32 gs12 m3 l1 c2 c3 s3 1
29

m3 l1 s2 s32 1 -m3l1s32 1

2+ 3
l2 s3( 1+

f2= {-m3 l3 s3 (

m3 l2 s2 s3 (

2+ 1 )2 m c s gc 3 3 3
12

m3

2+ 3+ 1 )2 + m l c s s 1 m l c s 1
3 1 3 2 3
3 1 3 3

2+ 1 )2 +

m3 l2 c3 s3 (

+2F2

m3 c3 s3 gs12 + m3 l1 c2 s32 1

2+ 3
s3 gc12} + m3 l3 c3 ( 1+
2

m3 c3 s3 s2 l1 1

gs12 + m3 l1 c2 c32 1

+ m3 l2 c32 (

+m3 l1 s2 s32 1

+ m3 l2 s32 (

1+ 2 )+ m
3

+ m3 l1 s3 1

+ m3 l1 c32s2 1

c3 + m3 l2 c3 s2 (

2+ 1 )2 m c s
3 3 3

1+ 2 )+ m c3 gc
3
12

0
A

30

a11
2

f2 =

a21
+ 2F2

0
A3*1

a11 +
2

..

M 2.L 2( 1 2) 2 M 2. L1. 1.S 2 M 2.L1 12 .C 2 M 2.g .S12

f2=
..

a21 +

..

..

..

M 2.L 2( 1 2) M 2.L1. 1.C 2 M 2.L1. 12 .S 2 M 2.gC12

0

n2=2N2+2R33n3+ 2PC2 x 2F2+2P3 x2R33f3

C3

n2 =

-S3

l2

l2

C3

-S3

0
0
0

S3

C3

n3 +

x 2F2 + 0

S3

C3

f3

After multiplying and adding these matrices we get

0
2

n2=

31

m3*l3^2+m3*l2*l3*c3+m3*l2*l3*c3+m2*l2^2+m3*l2^2*s3^2+m3*l2^2*c2^2m3*l1*l3*s2*s3+m3*l1*l3*c2*3+
m2*l1*l2*c2+m3*l1*l2*c2*s3^2+m3*l1*l2*c2*c3^2
,m3*l3^2+m3*l2*l3*c3+m3*l2*l3*c3+m2*l2^2+m3*l2^2*s3^2
+m3*l2^2*c3^2 + m3*l3^2+m3*l2*l3*c3 (-m3*l2*l3*s3)
(thetadot1+thetadot2+thetadot3)^2+(m3*l2*l3*s2-m3*l2^2*c3*s3+
m3*l2^2*c3*s2)(thetadot1+thetadot2)^2+
(m3*l3*L2*s3+m3*l3*L1*c3*s2+m2*l1*L2*s2+m3*l1*L2*s2*s3^2+
m3*l1*L2*c3^2*s2)(thetadot1^2)+

-m3*l3*s3*g*s12+

(m3*l3*c3+m2*l2+m3*l2*s3^2+m3*l2*c3^2)*(g*c(theta1+theta2)

3x1

2
=

n2t 2Z2= m3*l3^2+m3*l2*l3*c3+m3*l2*l3*c3+m2*l2^2+m3*l2^2*s3^2+m3*l2^2*c2^2-

m3*l1*l3*s2*s3+m3*l1*l3*c2*3+
m2*l1*l2*c2+m3*l1*l2*c2*s3^2+m3*l1*l2*c2*c3^2
,m3*l3^2+m3*l2*l3*c3+m3*l2*l3*c3+m2*l2^2+m3*l2^2*s3^2
+m3*l2^2*c3^2 + m3*l3^2+m3*l2*l3*c3 (-m3*l2*l3*s3)
(thetadot1+thetadot2+thetadot3)^2+(m3*l2*l3*s2m3*l2^2*c3*s3+m3*l2^2*c3*s2)(thetadot1+thetadot2)^2+
(m3*l3*L2*s3+m3*l3*L1*c3*s2+m2*l1*L2*s2+
m3*l1*L2*s2*s3^2+m3*l1*L2*c3^2*s2)(thetadot1^2)+ -m3*l3*s3*g*s12+
(m3*l3*c3+m2*l2+m3*l2*s3^2+
m3*l2*c3^2)*(g*c(theta1+theta2)

f1=1 R2 2f2 + 1F1 , No need 1f1 for torque equation of link 1.

n1=1N1+1R22n2+ 1PC1 x 1F1+1P2 x1R22f2

C2

n1 =

-S2

l1

l1

C2

-S2

0
S2
0

C2

n2 +

x 1F1 + 0

S2

C2

f2

32

0
0

After multiplying and adding these matrices we get

0
0
1

n1 =

m3*l3^2+2(m3*l2*l3*c3)+m2*l2^2+m3*l2^2*s3^2+m3*l2^2*c3^22(m3*s2*s3*l1*l3)+m3*l1*l3*c2*c3+
m2*l1*l2*c2+m3*l1*l2*c2*s3^2+m3*c3*c2*c3*l1*l2+m1*l1^2+m3*l1^2*c3^2*s2*s2
+m2*l1^2*s2^2+
m3*l1^2*s2^2*s3^2+m3*l1^2*s3*s3*c2*c2+m3*l1*l2*c2*s3*s3+m3*l1*l3*c3*c2+m3
*l1^2*c2^2*c3^2+
m3*l1*l2*c3^2*c2+m2*l1^2*c2^2+m2*l1*l2*c2
,m3*l3^2+m3*l2*l3*c3+m3*l2*l3*c3+m2*l2^2+
m3*l2^2*s3^2+m3*l2^2*c3^2m3*l1*l3*s2*s3+m3*l1*l2*c2*s3^2+m3*l1*l3*c2*c3+m3*l1*l2*c2*c3^2+
m2*l1*l2*c2 ,m3*l3^2+m3*l2*l3*c3-m3*l1*l3*s2*s3+m3*l1*l3*c2*c3(m3*l2*l3*s3- m3*l1*l3*c2*s3)
(thetadot1+thetadot2+thetadot3)^2+(m3*l2*l3*s2-m3*l2^2*c3*s3+m3*l2^2*c3*s2m3*l1*l2*s2*c3^2
-m2*l1*l2*s2-m3*l1*l2*s2^2*s3+m3*l1*l2*c2*c3*s3+m3*l1*l2*c2*c3*s2)
(thetadot1+thetadot2)^2
+(m3*l2*l3*s3+m3*l1*l3*c3*s2+m2*l1*l2*s2m3*l1*l2*c2*s3+m3*l1*l2*s2*s3^2+m3*l1*l2*c3^2*s2+
m3*l1*l2*s3*c3-m3*l1^2*s2*c3^2+m3*l1^2*s2^2*c3*s3-m3*l1^2*c2*s3^2m3*l1^2*s2^2*c3*s3m3*l1^2*c2*c3*s3+m3*l1^2*c2*s2*s3^2+m3*l1^2*c2*c3*s3+m3*l1^2*c2*c3^2*s2+m2*l1^2
*c2*s2)
(thetadot1^2)+ (-m3*l3*s3+m3*l1*s2*c3^2+m2*l1*s2+m3*l1*s2*s3^2)*(g*s12)+
(m3*l3*c3+m2*l2
+m3*l2*s3^2+m3*l2*c3^2+m3*l1*c2*s3^2+m3*l1*c2*c3^2+m2*l1*c2)*(g*c(theta1+theta2)+
m1*l1*g*c1
3x1

33

1
=

n1t

Z1=

m3*l3^2+2(m3*l2*l3*c3)+m2*l2^2+m3*l2^2*s3^2+m3*l2^2*c3^2-

2(m3*s2*s3*l1*l3)+m3*l1*l3*c2*c3+
m2*l1*l2*c2+m3*l1*l2*c2*s3^2+m3*c3*c2*c3*l1*l2+m1*l1^2+m3*l1^2*c3^2*s
2*s2+m2*l1^2*s2^2+
m3*l1^2*s2^2*s3^2+m3*l1^2*s3*s3*c2*c2+m3*l1*l2*c2*s3*s3+m3*l1*l3*c3*c2
+m3*l1^2*c2^2*c3^2+
m3*l1*l2*c3^2*c2+m2*l1^2*c2^2+m2*l1*l2*c2
,m3*l3^2+m3*l2*l3*c3+m3*l2*l3*c3+m2*l2^2+
m3*l2^2*s3^2+m3*l2^2*c3^2m3*l1*l3*s2*s3+m3*l1*l2*c2*s3^2+m3*l1*l3*c2*c3+m3*l1*l2*c2*c3^2+
m2*l1*l2*c2 ,m3*l3^2+m3*l2*l3*c3-m3*l1*l3*s2*s3+m3*l1*l3*c2*c3(m3*l2*l3*s3- m3*l1*l3*c2*s3)
(thetadot1+thetadot2+thetadot3)^2+(m3*l2*l3*s2-m3*l2^2*c3*s3+m3*l2^2*c3*s2m3*l1*l2*s2*c3^2
-m2*l1*l2*s2-m3*l1*l2*s2^2*s3+m3*l1*l2*c2*c3*s3+m3*l1*l2*c2*c3*s2)
(thetadot1+thetadot2)^2
+(m3*l2*l3*s3+m3*l1*l3*c3*s2+m2*l1*l2*s2m3*l1*l2*c2*s3+m3*l1*l2*s2*s3^2+m3*l1*l2*c3^2*s2+
m3*l1*l2*s3*c3-m3*l1^2*s2*c3^2+m3*l1^2*s2^2*c3*s3-m3*l1^2*c2*s3^2m3*l1^2*s2^2*c3*s3m3*l1^2*c2*c3*s3+m3*l1^2*c2*s2*s3^2+m3*l1^2*c2*c3*s3+m3*l1^2*c2*c3^2
*s2+m2*l1^2*c2*s2)
(thetadot1^2)+ (-m3*l3*s3+m3*l1*s2*c3^2+m2*l1*s2+m3*l1*s2*s3^2)*(g*s12)+
(m3*l3*c3+m2*l2
+m3*l2*s3^2+m3*l2*c3^2+m3*l1*c2*s3^2+m3*l1*c2*c3^2+m2*l1*c2)*(g*c(thet
a1+theta2)+
m1*l1*g*c1
Extracting the Z component we find the joint torques :
The Dynamic equations can be written in the form
..

M ( ) V ( , ) G ( ) F ( , )

34

where
M ( ) n n mass matrix
.

V ( , ) n 1 vector of centrifugal and coriol is terms

G ( ) n 1 vector of gravity terms
.

F ( , ) friction terms

The friction term consists of both the viscous friction and coulomb friction.
We have

M( )=[m3*l3^2+2(m3*l2*l3*c3)+m2*l2^2+m3*l2^2*s3^2+m3*l2^2*c3^22(m3*s2*s3*l1*l3)+m3*l1*l3*c2*c3+m2*l1*l2*c2+
m3*l1*l2*c2*s3^2+m3*c3*c2*c3*l1*l2+m1*l1^2+m3*l1^2*c3^2*s2*s2+m2*l1^2*s2^2+m3*l
1^2*s2^2*s3^2+
m3*l1^2*s3*s3*c2*c2+m3*l1*l2*c2*s3*s3+m3*l1*l3*c3*c2+m3*l1^2*c2^2*c3^2+m3*l1*l2*
c3^2*c2+m2*l1^2*c2^2+
m2*l1*l2*c2
,m3*l3^2+m3*l2*l3*c3+m3*l2*l3*c3+m2*l2^2+m3*l2^2*s3^2+m3*l2^2*c3^2m3*l1*l3*s2*s3+
m3*l1*l2*c2*s3^2+m3*l1*l3*c2*c3+m3*l1*l2*c2*c3^2+m2*l1*l2*c2
,m3*l3^2+m3*l2*l3*c3-m3*l1*l3*s2*s3+
m3*l1*l3*c2*c3;m3*l3^2+m3*l2*l3*c3+m3*l2*l3*c3+m2*l2^2+m3*l2^2*s3^2+m3*l2^2*c2^
2-m3*l1*l3*s2*s3+
m3*l1*l3*c2*3+m2*l1*l2*c2+m3*l1*l2*c2*s3^2+m3*l1*l2*c2*c3^2
,m3*l3^2+m3*l2*l3*c3+m3*l2*l3*c3+
m2*l2^2+m3*l2^2*s3^2+m3*l2^2*c3^2 ,
m3*l3^2+m3*l2*l3*c3;m3*l3^2+m3*l2*l3*c3+m3*l1*l3*c32 ,
m3*l3^2+m3*l2*l3*c3 , m3*l3^2];

V(, )=[(-m3*l2*l3*s3-m3*l1*l3*c2*s3)(thetadot1+thetadot2+thetadot3)^2+
(m3*l2*l3*s2-m3*l2^2*c3*s3+m3*l2^2*c3*s2m3*l1*l2*s2*c3^2-m2*l1*l2*s2m3*l1*l2*s2^2*s3+m3*l1*l2*c2*c3*s3+m3*l1*l2*c2*c3*s2)(thetadot1+thetadot2)^2
+(m3*l2*l3*s3+m3*l1*l3*c3*s2+m2*l1*l2*s2-m3*l1*l2*c2*s3+
m3*l1*l2*s2*s3^2+m3*l1*l2*c3^2*s2+m3*l1*l2*s3*c335

m3*l1^2*s2*c3^2+m3*l1^2*s2^2*c3*s3-m3*l1^2*c2*s3^2-m3*l1^2*s2^2*c3*s3m3*l1^2*c2*c3*s3+
m3*l1^2*c2*s2*s3^2+m3*l1^2*c2*c3*s3+m3*l1^2*c2*c3^2*s2+m2*l1^2*c2*s2)
(thetadot1^2); (-m3*l2*l3*s3)
(thetadot1+thetadot2+thetadot3)^2+ (m3*l2*l3*s2m3*l2^2*c3*s3+m3*l2^2*c3*s2)(thetadot1+thetadot2)^2
+(m3*l3*L2*s3+m3*l3*L1*c3*s2+m2*l1*L2*s2+m3*l1*L2*s2*s3^2+
m3*l1*L2*c3^2*s2)(thetadot1^2);
(m3*l2*l3*s2)(thetadot1+thetadot2)^2+(m3*l1*l3*c3*s2+m3*l2*l3*s3)
(thetadot1^2)];
G()=[(-m3*l3*s3+m3*l1*s2*c3^2+m2*l1*s2+m3*l1*s2*s3^2)*(g*s12)+
(m3*l3*c3+m2*l2+m3*l2*s3^2+m3*l2*c3^2+m3*l1*c2*s3^2+
m3*l1*c2*c3^2+m2*l1*c2)*(g*c(theta1+theta2)+ m1*l1*g*c1; -m3*l3*s3*g*s12+
(m3*l3*c3+m2*l2+m3*l2*s3^2+m3*l2*c3^2)
(g*c(theta1+theta2); m3*l3*c3*g*cos(theta1+theta2)-m3*l3*s3*g*s(theta1+theta2)];
Motor torque or theoretical Torque for a link pivoted at the end is given by:

t (J m

.
..
M . g .L
ML2 ..

Sin

m Cm m
m
2
2
N
N
N

For large value of N:

..

N t N 2 J m N 2 C m

Where

36

 Jo int torque
..

Jo int acceleration
.

Jo int velocity
Jo int angle
J m Inertia cons tan t for motor
C m Friction cons tan t for motor
N Gear ratio
g gravitational cons tan t

37

CHAPTER 4
Dynamic Simulation and Control
4.1 Introduction
To simulate the motion of the manipulator we make use of the dynamics model just developed.
Simulation requires solving the dynamic equation for acceleration. We then use Euler
Integration to integrate the acceleration to compute future positions and velocities. Putting all
things together from previous equations, The final system Dynamics model is as follows.

N12 J m1
..1 M ( )
0
2

..

2
N 2 J m2

.
N 12
2

V
(

1
,

1 G ( 1) N1 C m1
1

N 2 2 2 V ( 2, .2 G ( 2)
0

2
N 2 Cm2

-1
1
2
3

(( N 1)2 Jm 1 )
0
0

= M() +

V ( 3 , 3)

G( 3)

0
0
(N 2) 2 Jm2
0
0
(N 3)2 Jm 3

(N 1) 2 1
(N 2) 2 2
(N 3) 2 3

(N 1) 2 cm1
0
0
0
(N 2) 2 cm2
0
0
0
( N 3)2 cm3

1
2
3

Given initial conditions on the motion of manipulator i.e.

(0) 0
.

(0) 0

We then numerically integrate forward in time by step t.

38

.1
2

..

(t t ) (t ) (t )t
.
1 ..
(t t ) (t ) (t )t (t )t 2
2

4.2 Feedback and closed loop control

The manipulator is modeled as a mechanism with sensor at each joint to measure the joint angle, and an
actuator at each joint to apply torque on the next link. For the manipulator to follow the desired trajectory,
we use the closed loop Proportional Derivative (PD) controller to compute appropriate motor currents,
which provide the required torque to drive the joint. The complete closed loop control system is shown on
the next page. The feedback is used to compute servo error by finding the difference between the desired
position and the actual position, and likewise between desired and actual velocity.

d
.

The control system computes how much current to send to the motor as a function of the servo error
.

i k p k d

The motor takes in this current and produces a motor torque

t i.k t
where k t is the motor cons tan t and depends on the size and watage of motor

4.3 Simulation runs

Dynamic simulation of the manipulator was performed in matlab. The trajectory followed was a circle of
radius 1-m. as shown in the figure. The path is divided into 21 equal points with a time increment of t =
0.001 sec. The following values were used for various parameters in the simulation.

39

Cm1 = 0.1
Cm2 = 0.1
Cm3 = 0.1

Jm1 = 0.01
Jm2 = 0.01
Jm3 = 0.01

M1 = 1 kg
M2 = 0.5 kg
M3=0.5kg
g = 9.8 m/s2
Kt = 10
L1 = 1 m
L2 = 2 m
L3=1m
KP = 100

L3

Kd =0.5

L2

L1
Initial position of the manipulator

40

And for 2nd time

Kd=1.05

4.4 Matlab Code

% Date 25-08-2015
% PD control of three link planar manipulator
% To move the end effecter in circular path.
%function project (kp, kd, t)
cm1=0.1;
cm2=0.1;
jm1=0.01;
jm2=0.01;
m1=1;
m2=0.500;
g=9.8;
kt=10;
l1=1;
l2=2;
thetac1=0.7854;
thetac2=0;
thetaep1=0;
thetaep2=0;
% setting initial conditions equal to zero
thetadotdot1=0;
thetadotdot2=0;
41

thetadot1=0;
thetadot2=0;
figure
for index=2:22
x=[2.121 2.002 1.723 1.414 1.105 .826 .605 .463 .414 .463 .605 .826 1.105 1.414 1.723
2.002 2.223 2.365 2.441 2.365 2.223 2.121 2.002 ];
y=[2.121 2.223 2.365 2.414 2.365 2.223 2.002 1.723 1.414 1.105 0.826 0.605 0.463 0.414
0.463 0.605 0.826 1.105 1.414 1.723 2.002 2.121 2.233 ];

c2=(x(index)^2+y(index)^2-l1^2-l2^2)/(2*l1*l2);
s2=sqrt(1-c2^2);
theta2=atan2(s2,c2);
theta1=atan2(y(index),x(index))- atan2(l2*s2,l1+l2*c2);
locationx(index-1)=l1*cos(theta1)+l2*cos(theta1+theta2);
locationy(index-1)=l1*sin(theta1)+l2*sin(theta1+theta2);
%savingx(index-1)=theta1*57.3
%savingy(index-1)=theta2*57.3
M=[l2^2*m2+2*l1*l2*m2*c2+l1^2*(m1+m2) , l2^2*m2+l1*l2*m2*c2 ;
l2^2*m2+l1*l2*m2*c2 ,l2^2*m2];

42

V=[ -m2*l1*l2*s2*theta2^2-2*m2*l1*l2*s2*thetadot1*thetadot2;
m2*l1*l2*s2*thetadot1 ];
G=[m2*l2*g*cos(theta1+theta2)+(m1+m2)*l1*g*cos(theta1);
m2*l2*3*cos(theta1+theta2)];
thetae1=theta1-thetac1;
thetae2=theta2-thetac2;
thetae1r(index-1)=thetae1;
thetae2r(index-1)=thetae2;

thetadote1=(thetaep1-thetae1)/t;
thetadote2=(thetaep2-thetae2)/t;

current1=kp*thetae1+kd*thetadote1;
current2=kp*thetae2+kd*thetadote2;
torque1=current1*kt;
torque2=current2*kt;

thetadotdot=inv(M+[1 0;0 1])*([10*torque1;10*torque2]-V-G-([10 0;0

10]*[thetadot1;thetadot2]));

43

thetadotdot1=thetadotdot(1);
thetadotdot2=thetadotdot(2);
thetadot1=thetadot1+thetadotdot1*t;
thetadot2=thetadot2+thetadotdot2*t;

theta1=theta1+thetadot1*t+0.5*thetadotdot1*t^2;
theta2=theta2+thetadot2*t+0.5*thetadotdot2*t^2;
savingx(index-1)=l1*cos(theta1)+l2*cos(theta1+theta2);
savingy(index-1)=l1*sin(theta1)+l2*sin(theta1+theta2);

thetac1=theta1;
thetac2=theta2;
thetaep1=thetae1;
thetaep2=thetae2;

end
%plot(thetae1r);
%hold on
%plot(thetae2r,'r');
plot(savingx,savingy);
hold on
plot(x,y,'r');
%hold on
%plot(differencex,differencey,'r');
44

%location=[(locationx)',(locationy)']
4.5 PD Control of End Effector
The rise time is about 0.001 second. Now let's add a derivative controller to the system ,the overshoot
can be reduced. Add variable, Kd, kp to the m-file, set it equal to 0.5 and 100 respectively and rerun the
m-file:

Inner circle Desired trajectory

Outer circle ...Actual trajectory

The rise time

is still 0.001
second. Now
let's add a
derivative
controller to
the system
,the overshoot
can be
reduced. Add
variable, Kd,
to the m-file,
set it equal to
1.05 and
rerun the mfile:

45

Inner circle Desired trajectory

Outer circle ...Actual trajectory

46

CHAPTER 5
Discussions and conclusions
In developing the model i made many assumptions, which in real life will require more
considerations. Some of these assumptions are
a) Stiffness is not considered.
b) It is assumed that friction and inertia terms will remain constant.
c) Noise in the control system is not considered
d) Environmental and surrounding external effects are not considered

In order to eliminate the steady-state error a modification should be made to the controller. This
involves the addition of an integral term to the controller so that the system will have no steady state
error.

The value of Kd is selected so that the error is minimal. Plot of Error Vs Kd is shows that for a value
of Kd=1.05 the error is minimum, corresponding to Kp = 100.

47

REFERENCES
1. R K Mittal and I J Nagrath, Robotics and Control.
2. Benjamin KUO , Automatic Control
3. John J. Craig, Introduction to Robotics mechanics and control
4. Prof. Hamid D. Taghirad, Robotics: Evolution, technology and applications

48