Addis Ababa Science & Technology

University
College of Electrical and Mechanical Engineering

Introduction to Robotics-
EEg5332

Birhanu G.
 Motion equations

College of Electrical & Mechanical Engineering, Department

01/25/2021
2

of Electrical & Computer Engineering, EEEg5332

 01/25/2021
3. Modelling and Control of
Manipulators
Contents:
i. Dynamics

College of Electrical & Mechanical Engineering, Department

a) Euler-Lagrange

of Electrical & Computer Engineering, EEEg5332

b) Newton-Euler

ii. Trajectory Planning

a) Trajectories for Point to Point Motion
 Cubic Polynomial Trajectories
 Linear Segments with Parabolic Blends (LSPB)

iii. Control Algorithm

3
Dynamics

01/25/2021
Dynamics
Robot Dynamics is the study of the relation between the
applied forces/torques and the resulting motion.
The dynamic response of the time rate of change of robot
config- uration to input joint torques can be expressed by a

College of Electrical & Mechanical Engineering, Department

second order nonlinear ordinary differential equations called
equation of motion.

of Electrical & Computer Engineering, EEEg5332

τ = f (q, q˙, q¨)
where τ is force/torque or generalized forces and q is joint
variable
The reasons to study the dynamics of a manipulator is:
 simulation and animation: test desired motions without
resorting to real experimentation
 Analysis and synthesis of suitable control algorithms 4
Dynamics

01/25/2021
There are two approaches to formulate the dynamics of a robot
 Euler-Lagrange
In the Lagrangian formulation we treat the robot as a whole
and perform the analysis using Lagrangian function (the

College of Electrical & Mechanical Engineering, Department

difference between the kinetic energy and the potential
energy)

of Electrical & Computer Engineering, EEEg5332

 Newton-Euler
In Newton-Euler formulation we treat each link of the robot
separetly, and write down the equations describing its linear
and angular motion of each link.
The two techniques are equivalent (provide the same results).

5
Dynamics

01/25/2021
Euler-Lagrange

Euler-Lagrange
Lagrangian of the mechanical system can be defined as:

L=T−U

College of Electrical & Mechanical Engineering, Department

where T and U denote the total kinetic

of Electrical & Computer Engineering, EEEg5332

energy and potential energy of the system, respectively.
Euler-Lagrange equations leads to a second
order nonlinear ordinary differential equations of the form

where n is the number of joints and τi is the generalized

force associated with the generalized coordinate qi 6
Dynamics

01/25/2021
Euler-Lagrange

The generalized forces are the sum of the nonconservative

forces, which include the joint actuator torques, joint friction

College of Electrical & Mechanical Engineering, Department

torques, and joint torques induced by end-effector forces at the
contact with the environment

of Electrical & Computer Engineering, EEEg5332

Therefore, The above equations establish the relations existing
between the generalized forces applied to the robot and the joint
positions, velocities and accelerations.

7
Dynamics
 

01/25/2021
Euler-Lagrange
Kinetic Energy
The kinetic energy of a rigid object is the sum of two terms:
 Translational kinetic energy: obtained by concentrating the
entire mass of the object at the center of mass
 Rotational kinetic energy of the body about the center of
mass.

College of Electrical & Mechanical Engineering, Department

For example, for a point mass the kinetic energy is given as

of Electrical & Computer Engineering, EEEg5332

T=

where v = r ω and r is the distance between the center of rotation

and point mass
Therefore the total kinetic energy is dependent on the angular mo-
tion of the system
T = Iω 2
8
Dynamics

01/25/2021
Euler-Lagrange

 
Inertia Tensor
Moment of inertia, usually denoted by I, measures the extent
to which an object resists rotational acceleration about a
particular axis
Let

College of Electrical & Mechanical Engineering, Department

of Electrical & Computer Engineering, EEEg5332
be the coordinates in frame i of the infinitesimal mass dm.
The Inertial tensor with respect to the i reference frame is
th

given by

9
Dynamics

01/25/2021
Euler-Lagrange

where I is the moment of inertia of a 3x 3 matrix and given as

College of Electrical & Mechanical Engineering, Department

 
The vector is first moments which is the product of mass and

of Electrical & Computer Engineering, EEEg5332

the position of center of mass, and is the mass of the link.

Ji is a constant matrix that is evaluated once for each link.

It depends on the geometry and mass distribution of link i .

10
Dynamics

01/25/2021
Euler-Lagrange

If the axes of Link i frame coincide with the central axes of

inertia, then the inertia products are null and the inertia tensor
relative to the center of mass is a diagonal matrix.
The Inertia Tensor matrix with respect to frame-0 is given by

College of Electrical & Mechanical Engineering, Department

of Electrical & Computer Engineering, EEEg5332
The kinetic energy of link i is written as

The trace of an n-by-n square matrix is defined to be the sum of the

elements on the main diagonal.
Therefore the total Kinetic energy of the an n-link robot is given as

11
Dynamics

01/25/2021
Euler-Lagrange

Moment of Inertia of Rod

The moment of inertia of a point mass is given by I = mr 2, but
the rod would have to be considered as an infinite number of
point masses, and each must be multiplied by the square of its
distance from the axis.The general form for the moment of

College of Electrical & Mechanical Engineering, Department

inertia is:

of Electrical & Computer Engineering, EEEg5332

12
Dynamics

01/25/2021
Euler-Lagrange

 Asshown from the figure above, a thin uniform rod of mass M

and length L is mounted on an axis passing through the end of the
rod, and perpendicular to the plane of the rod.
Choose the origin at the end of the rod and the x-axis oriented

College of Electrical & Mechanical Engineering, Department

along the rod, positive to the right in the figure. Denote the
length of a small element of the rod by

of Electrical & Computer Engineering, EEEg5332

dl = dx
Since the rod is uniform, the mass per unit length is a constant

Therefore the mass in the infinitesimal length is given by

13
Dynamics

01/25/2021
Euler-Lagrange

 Inorder to calculate the moment of inertia through an axis passing

through the end of the rod

College of Electrical & Mechanical Engineering, Department

of Electrical & Computer Engineering, EEEg5332
Therefore the moment of inertia of the rod at the end point is

14
Dynamics

01/25/2021
Euler-Lagrange
 
Potential Energy
In the case of rigid dynamics, the only source of potential
energy is gravity.
The potential energy of the i-th link can be computed by
assuming that the mass of the entire object is concentrated at

College of Electrical & Mechanical Engineering, Department

its center of mass and is given by

of Electrical & Computer Engineering, EEEg5332

)
Where the matrix gives the coordinates of the center of mass of
link i with respect to inertial frame. is a 4x4 matrix , which gives
the direction of gravity in the inertial frame.

15
 Dynamics

01/25/2021
Euler-Lagrange
 
The total potential energy applied by the actuator to resist the grav-
itational potential energy of the n-link robot is given by

College of Electrical & Mechanical Engineering, Department

of Electrical & Computer Engineering, EEEg5332
In the case that the robot contains elasticity, for example, flexible
joints, then the potential energy will include terms containing the
energy stored in the elastic elements.

Note that the potential energy is a function only of the general-

ized coordinates and not their derivatives, i.e. the potential energy
depends on the configuration of the robot but not on its velocity.
16
Dynamics

01/25/2021
Euler-Lagrange

Example of Two Link Robot

College of Electrical & Mechanical Engineering, Department

of Electrical & Computer Engineering, EEEg5332
For Link-1 the Inertia tensor matrix with respect to frame-1 is
given by

17
Dynamics

01/25/2021
Euler-Lagrange

 For lumped mass located at center of gravity, the Inertia is given

by

The Inertia tensor with respect to frame-0 is given by

College of Electrical & Mechanical Engineering, Department

the kinetic energy of link-1 is given by

of Electrical & Computer Engineering, EEEg5332

()T)

T1
For lumped mass the kinetic energy is

18
Dynamics

01/25/2021
Euler-Lagrange

  The potential energy of link-1 is

)
Where the Hg1 is given by

College of Electrical & Mechanical Engineering, Department

of Electrical & Computer Engineering, EEEg5332
 Therefore, the potential energy applied by the actuator to resist
the gravitational potential energy of link-1 is given as

19
Dynamics

01/25/2021
Euler-Lagrange

Similarly, for link-2 the Inertia tensor with respect to frame-2 is given
by

College of Electrical & Mechanical Engineering, Department

 For lumped mass located at center of gravity, the Inertia is given

of Electrical & Computer Engineering, EEEg5332

by

The Inertia tensor with respect to frame-0 is given by

The kinetic energy of link-2 is given by

20
Dynamics

01/25/2021
Euler-Lagrange

 The
potential energy of link-2 is given by
)
where the Hg2 is given by

College of Electrical & Mechanical Engineering, Department

of Electrical & Computer Engineering, EEEg5332
 Therefore, the potential energy applied by the actuator to resist
the gravitational potential energy of link-2 is given as

21
Dynamics

01/25/2021
Euler-Lagrange
 The Lagrangian equation of the two link robot is given by

where the number of joints are n = 2

The Euler-Lagrange equations are

For link-1 the torque equation is given by

College of Electrical & Mechanical Engineering, Department

of Electrical & Computer Engineering, EEEg5332
similarly for link-2 the torque equation is given by

22
 Dynamics
Newton-Euler

Reading Assignment

College of Electrical & Mechanical Engineering, Department

01/25/2021
of Electrical & Computer Engineering, EEEg5332
23
 01/25/2021
Trajectory Planning

Trajectory Planning

Path and trajectory planning means the way that a robot is

moved from one location to another in a controlled manner.

College of Electrical & Mechanical Engineering, Department

A trajectory is a function of time q(t) such that q(to) = qinit and
q(tf ) = qfinal . In this case, tf − t0 represents the amount of time

of Electrical & Computer Engineering, EEEg5332

taken to execute the trajectory.
The task of point to point motion is to plan a trajectory from q(t0)
to q(tf ), i.e., the path is specified by its initial and final
configurations.
In some cases, there may be velocity and acceleration constraints
on the trajectory.
24
Trajectory planning

01/25/2021
Trajectories for point to point Motion
Trajectories for Point to Point Motion
The objective is to find a joint trajectory q(t) that connects an
initial to a final configuration while satisfying other specified
constraints at the endpoints (e.g., velocity and/or acceleration
constraints).

College of Electrical & Mechanical Engineering, Department

Suppose at time t0 the joint variable satisfies

of Electrical & Computer Engineering, EEEg5332

q(t0) = q 0
q˙(t0) = v0
and we want to attain the values at tf
q(tf ) = qf
q˙(tf ) = vf
In addition, The initial and final acceleration constraints are.
q¨(t0) = α0 25
Trajectory planning

01/25/2021
Trajectories for point to point Motion

Cubic Polynomial Trajectories

The objective is to generate a cubic polynomial function
between two configurations.
If we have four constraints to satisfy, we require a polynomial

College of Electrical & Mechanical Engineering, Department

with four independent coefficients that can be chosen to satisfy

of Electrical & Computer Engineering, EEEg5332

these constraints.

Thus we consider a cubic trajectory of the form

q(t) = a 0 + a 1 t + a 2 t2 + a 3 t3
Then the desired velocity is given as
q˙(t) = a1 + 2a2t + 3a3t2
26
Trajectory planning

01/25/2021
Trajectories for point to point Motion

Combining the above two equations with the four constraints yields four
equations and four unknowns at the initial and final configurations
q 0 = a 0 + a 1 t0 + a 2 t0 2 + a 3t03
v 0 = a 1 + 2a 2 t0 + 3a 3 t0 2
q f = a 0 + a 1 tf + a 2 tf 2 + a 3 tf3

College of Electrical & Mechanical Engineering, Department

v f =equations
These four a 1 + 2a 2 tcan
f + 3a tf2
be 3combined into a single matrix equation

of Electrical & Computer Engineering, EEEg5332

The determinant of the coefficient matrix is (tf-t0)4 and the above equation
always has a unique solution if a nonzero time interval is allowed for the
execution of the trajectory.

27
Trajectory planning

01/25/2021
Trajectories for point to point Motion

Quintic Polynomial Trajectories

A cubic trajectory gives continuous positions and velocities at
the start and final points but discontinuities in the acceleration.
A discontinuity in acceleration leads to an impulsive jerk,

College of Electrical & Mechanical Engineering, Department

which may excite vibrational modes in the manipulator and

of Electrical & Computer Engineering, EEEg5332

reduce tracking accuracy.
For this reason, we have to specify constraints on acceleration
in addition to position and velocity.
Therefore to include the six constraints, we need a fifth order
polynomial of the form
q(t) = a 0 + a 1 t + a 2 t2 + a 3 t3 + a 4 t4 + a 5 t 5

28
Trajectory planning

01/25/2021
Trajectories for point to point Motion
From the above equation we obtain the following equations
q 0 = a 0 + a 1 t0 + a 2 t 0 2 + a 3 t0 3 + a 4 t0 4 + a 5 t 05
v0 = a1+ 2a2t0 + 3a3t02 + 4a4t03 + 5a5t04
a0 = 2a2+ 6a3t0 + 12a4t02 + 20a5t03

College of Electrical & Mechanical Engineering, Department

q f = a 0 + a 1 t f + a 2 t f2 + a 3 tf3 + a 4 tf 4 + a 5 tf5

of Electrical & Computer Engineering, EEEg5332

vf = a1+ 2a2tf + 3a3tf2 + 4a4tf3 + 5a5tf4
a f = 2a 2 +
Which can be write as
6a 3 t f + 12a 4 t f
2
+ 20a 5 t f
3

29
Trajectory planning

01/25/2021
Trajectories for point to point Motion

Linear Segments with Parabolic Blends (LSPB)

This type of trajectory is appropriate when a constant velocity
is desired along a portion of the path.
The LSPB trajectory is such that the velocity is initially ramped
up to its desired value and then ramped down when it

College of Electrical & Mechanical Engineering, Department

approaches the goal position.

of Electrical & Computer Engineering, EEEg5332

To achieve this we specify the desired trajectory in three parts.
The first part from time t0 to time tb is a quadratic
polynomial. This results in a linear ramp velocity.
At time tb , called the blend time, the trajectory switches to a
linear function. This corresponds to a constant velocity.

30
Trajectory planning

01/25/2021
Trajectories for point to point Motion

Finally, at time tf − tb the trajectory switches to a quadratic poly-

nomial so that the velocity is linear.
We choose the blend time tb so that the position curve is
symmetric as shown in the figure below. For convenience
suppose that t0 = 0 and q˙(tf ) = 0 = q˙(0). Then between times 0

College of Electrical & Mechanical Engineering, Department

and tb we have

of Electrical & Computer Engineering, EEEg5332

q(t) = a 0 + a 1 t + a 2 t2

31
Trajectory planning

01/25/2021
Trajectories for point to point Motion

 So that the velocity is

=a1+2a2t
The constraints and imply
a0=q0
a1=0

College of Electrical & Mechanical Engineering, Department

At time tb the velocity starts to be constant, say v. thus we have

of Electrical & Computer Engineering, EEEg5332

Therefore the required trajectory between 0 and tb is given as

Where a denotes the acceleration.

32
Trajectory planning

01/25/2021
Trajectories for point to point Motion

 Now, between time tb and tf -tb, the trajectory is a linear segment

(corresponding to a constant velocity v)

Since, by symmetry,

College of Electrical & Mechanical Engineering, Department

We have

of Electrical & Computer Engineering, EEEg5332

Which implies that

Since the two segments must blend at time tb

b

33
Trajectory planning

01/25/2021
Trajectories for point to point Motion

 Solving for the blend time tb

Note that we have the constraint 0<tb This leads to the inequality

College of Electrical & Mechanical Engineering, Department

To put it another way we have the inequality

of Electrical & Computer Engineering, EEEg5332

Thus the specified velocity must be between these limits or the
motion is not possible.

34
Trajectory planning

01/25/2021
Trajectories for point to point Motion

 The portion of the trajectory between tf-tb and tf is now found

By symmetry considerations. The complete LSPB trajectory is
given by
q(t)=

College of Electrical & Mechanical Engineering, Department

of Electrical & Computer Engineering, EEEg5332
35
Trajectory planning

01/25/2021
Trajectories for point to point Motion
 
Minimum Time (BangBang) Trajectories
The objective is seeking the fastest trajectory between q0 and qf
with a given constant acceleration α.

We can achieve minimum time trajectory with maximum

College of Electrical & Mechanical Engineering, Department

acceleration value +α until switching time ts and with minimum

of Electrical & Computer Engineering, EEEg5332

acceleration value −α from ts to tf
Assume that the trajectory begins and ends at rest (zero initial
and final velocities), and symmetry considerations would
suggest that the switching time ts is just .

If we let vs denote the velocity at time ts then we have

v s = αt s 36
Trajectory planning

01/25/2021
Trajectories for point to point Motion
 and also

The symmetry condition implies that

Combining these two we have the conditions

s

College of Electrical & Mechanical Engineering, Department

Which implies that

of Electrical & Computer Engineering, EEEg5332

The complete BangBang trajectory is given by
q(t)=

37
Control Algorithm

01/25/2021
Control Algorithm
The dynamic equations of a serial rigid manipulator is given as:
M(q)¨q + C (q, q˙) + G (q) = τ
where, M(q) is an nxn inertia matrix of a
manipulator, C (q, q˙) is a nx 1 matrix related to the centrifugal

College of Electrical & Mechanical Engineering, Department

and Coriolis terms,
G (q) is an nx 1 vector related to gravitational force,

of Electrical & Computer Engineering, EEEg5332

τ is the joint generalized force and n is the number of joint.
The purpose of this section is to figure out a strategy
of calculating the torque τ that each motor should produce in
order to follow a desired trajectory (qd ).
In order to ensure trajectory qd tracking by the joint
variables, we define the tracking error as follows:
e = qd − q 38
 01/25/2021
Control Algorithm

 Thevelocity and acceleration error is given as

and
From the dynamic equation the actual acceleration is
computed as

College of Electrical & Mechanical Engineering, Department

Substituting acceleration error into the above equation gives as
)

of Electrical & Computer Engineering, EEEg5332

The control input function
)

The following tracking error dynamics equation is derived as

39
 01/25/2021
Control Algorithm

 Theabove equation shows the tracking error evolves

over time with respect to the control input u.
The equation is linear and time-Invariant system.
The state space representation is

College of Electrical & Mechanical Engineering, Department

where are the states

of Electrical & Computer Engineering, EEEg5332

To stabilize the system we have to choose a control input

This means that choosing a control input is much

easier than choosing a stabilizing as a control input.
For each value of u the corresponding torque/force
can be calculated by

40
 Control Algorithm

This is known as the computed-torque control law.

College of Electrical & Mechanical Engineering, Department

01/25/2021
of Electrical & Computer Engineering, EEEg5332
41
 Thank you!

College of Electrical & Mechanical Engineering, Department

01/25/2021
of Electrical & Computer Engineering, EEEg5332
42