Jimma University

Jimma Institute of Technology

School of Biomedical Engineering
Biomedical Instrumentation (1stYear, 2nd semester M.sc)

Robotics and Control

Assignment- Two

By: Misgana Eticha

ID. No:RM2655/12-0
Submitted to: Dr. Dereje Shiferaw

Date: 11/1/, 2020

Jimma, Ethiopia
1. a) Given the following figure, determine the projections of axes of frame {1}
with respect to frame {0}, i.e. give the projection of x-axis of frame {1} w. r. t. x,
y, z axes of frame {0} and so projection
z0 of y axis of frame {1} w.r.t frame {0}.
y1

z1
=35
y0
x0
x1

Determine the rotational matrix which expresses the rotation of frame {1} with
respect to frame {0}

Answer

1A.
1
√ The projection of x1 on x0,y0,z0 is 0
0

0 0
√ The projection of y1 0n x0,y0,z0 is cos (35) = 0.82
¿ 0.574

0 0
√ The projection of z1 on x0, y0, z0 is −sin ⁡(35) = −0.574
cos ⁡(35) 0.82

projection axes of frame {1} with respect to frame {0} are:

1 0 0

[ 0 0.82 −0.574
0 0.574 0.82 ]
1b) Consider the following diagram. The origin of frame {1} is at (2,10,-5) with
respect to frame {0}. Determine the homogenous transformation matrix T 01 .

z0
p (-2,5.5,4)

y0 x1

x0

y1

z1

Answer
1B.

Since the rotation is on Y axis at 90 degree

cos ⁡∅ 0 sin ∅ cos ⁡( 90) 0 sin ⁡(90) 0 0 1

R=
[ 0 1 0 =
−sin ∅ 0 cos ∅
0
] [ 1 0
−sin ⁡( 90) 0 cos ⁡( 90)
= 0 1 0
] [
−1 0 0 ]
0 0 1 p1 0 0 1 2
R P
[ ] 0
T = 0 1 = −1
0
[ 1
0
0
][0
0
0
p2
p3
1
0 1
= −1 0
0 0
0 10
0 −5
0 1
]
0 0 1 2
0
Therefore: T = −1
0
[ ] 1
0
0
0 10
0 −5
0 1
2. Consider the following robotic manipulator

a) a) List the possible type, number and application of sensors and actuators
that the robot can have
b) Using DH algorithm, determine the link and joint parameters and derive
the forward kinematics of the robot
c) If the position and orientation of the end effector at some time is given by
the matrix below, find the three joint angles using Inverse kinematics
principles.

−0. 6 −0. 5 0. 7 0. 5
T=
[
0.35 0.866 −0.35 −0. 18
0. 7
0
0
0
0. 6
0
0.21
1
]
Answer

2a, Sensors provide analogs to human senses and can monitor other phenomena for
which humans lack explicit sensors.

 Simple Touch: Sensing an object's presence or absence.

 Complex Touch: Sensing an object's size, shape and/or hardness.
 Simple Force: Measuring force along a single axis.
 Complex Force: Measuring force along multiple axes.
 Simple Vision: Detecting edges, holes and corners.
 Complex Vision: Recognizing objects.
 Proximity: Non-contact detection of an object.

Sensors can measure physical properties, such as the distance between objects, the
presence of light and the frequency of sound. They can measure:

 Object Proximity: The presence/absence of an object, bearing, color, and

distance between objects.
 Physical orientation. The co-ordinates of object in space.
 Heat: The wavelength of infrared or ultra violet rays, temperature,
magnitude, direction.
 Chemicals: The presence, identity, and concentration of chemicals or
reactants.
 Light: The presence, color, and intensity of light.
 Sound: The presence, frequency, and intensity of sound.

Motion controllers, potentiometers, taco-generators and encoder are used as joint

sensors, whereas strain-gauge based sensing is used at the end-effector location for
contact force control.

Internal sensor-It is the part of the robot. Internal sensors measure the robot's
internal state. They are used to measure position, velocity and acceleration of the
robot joint or end effectors.

Position sensor-Position sensors measure the position of a joint (the degree to

which the joint is extended). They include:
  Encoder: a digital optical device that converts motion into a sequence of
digital pulses.

 Potentiometer: a variable resistance device that expresses linear or angular

displacements in terms of voltage.
 Linear variable differential transformer: a displacement transducer that
provides high accuracy. It generates an AC signal whose magnitude is a
function of the displacement of a moving core.
 Synchros and Resolvers

Velocity Sensor-A velocity or speed sensor measures consecutive position

measurements at known intervals and computes the time rate of change in the
position values.

2. B

Create a table of link parameters ai , di , αi , θi . ai

i αi ai Di Θi
1 0 0 0 θ1
2 0 L1 0 θ2
3 0 L2 0 θ3

Homogeneous transformation matrices from link 1 to 0:

c 1 −s 1 0 0

[s1 c1
T01 = 0
0
0
0
0
1
0
0
0
1
]
rotate on z axis

Homogeneous transformation matrices from link 1 to 2:

 c 2 −s 2 0 l1

0
[
s2 c2
T12 = 0 0
0
0 0
1 0
0 1
]
rotate on z axis and translate on by l1 x axis

Homogeneous transformation matrices from link 2 to 3:

c 3 −s 3 0 l2

0
[
s3 c3
T23 = 0 0
0
0 0
1 0
0 1
]
rotate on z axis and translate on by l2 x axis

Homogeneous transformation matrices from link 3 to tool:

1 0 0 l3
0
T3tool = 0
0
[ 1
0
0
0 0
1 0
0 1
]
there is no rotation it is only translating

from T0tool = T01 *T12 *T23 *T3tool. This then gives the position and orientation of the
tool frame expressed in base coordinates

c 12 c 3−s 12 s 3 −c 12 s 3−s 12 c 3 0 l 1 c 1+l 2 c 12+l3 c 123

T0tool =
[
s 12c 3+c 12 s 3 −s 12 s 3+c 3 c 13
0
0
0
0
0 l1 s 1+ l2 s 12+l 3 s 123
1
0
0
1
]
x l 1c 1+ l2 c 12+l 3 c 123
The position is y = l 1 s 1+l 2 s 12+l3 s 123
z 0

X =l 1c 1+ l2 c 12+l 3 c 123= l1cos(θ1) + l2cos(θ1+ θ 2) + l3cos(θ1+θ 2+θ 3)

Y = l 1 s 1+l 2 s 12+ l3 s 123= = l1sin(θ1) + l2sin(θ1+ θ2) + l3sin(θ1+θ 2+θ 3)

Z= 0

Ф = θ1 + θ2 + θ3
 2C.

C1 0 S1 0 C 2 −S2 0 L2 C 2 C 3 −S 3 0 L3 C 3
0

[
T 1= S 1
0
0
0 −C1
1 0
0 0
0
0
1
] [ 1
T 2= S 2 C 2
0
0
0
0
0 L2 S2
1
0
0
1
] [ 2
T 3= S 3 C 3
0
0
0
0
0 L3 S 3
1
0
0
1
]
0 0 1 2
T3 = T1 T2 T3

C 1 C 23 −C1 S23 S1 C 1 ( L3 C 23+L2 C 2 ) r 11 r 12 r 13 r 14 −0 .6 −0.5 0. 7 0. 5

0

[
S C
T 3 = 1 23
S23
0
−S 1 S 23 −C 1
C 23
0
0
0
S1 ( L3 C23 + L2 C 2 )
L3 S23 + L2 S2
1
][
r

0
r

0
r
= 21 22 23 24

0
r
r 31 r 32 r 33 r 34
1
][ 0.35 0.866 −0 .35 −0 .18
= 0 .7
0
0
0
0. 6
0
0 .21
1
]
❑ = r 24 = S 1 = −0.18 =−0.36
❑ r 14 C 1 0.5

sin θ 1
=−0.36
cos θ 1

tanθ 1=−0.36

θ 1=tan−1 (−0.36)

θ 1=−19.8 °

For the other two angles

0
T 1 1 T 2 2 T 3 =T
0 −1
T 1 1 T 2=T [ 2 T 3 ]
----------------------------(1)

From equation (1)

 L 2C 1 C 2=−L3 r 11+r 14−−−−−−−−( 2 )

L 2 S 1C 2=−L 3 r 21+ r 24−−−−−−−−(3)

L 2 S 2=−L 3 r 31+ r 34−−−−−−−−−(4 )

Square equation (2) $ (3), add them and use the trigonometric identity

( L 2C 1 C 2=−L3 r 11+r 14)2

( L 2 S 1 C 2=−L 3 r 21+ r 24 )2

2 2 2 2
( L 2C 1 C 2) +( L 2 S 1 C 2) =(−L 3 r 11 +r 14) +(−L3 r 21+r 24)

( L 2C 2 )2 (C 1 ¿ ¿ 2+ S 12)=(−L 3 r 11+r 14)2 +(−L 3r 21+r 24)2 ¿

L 2C 2=± √(−L3 r 11+r 14)2 +(−L3 r 21+r 24)2-----------------------(5)

Dividing equation (4) by equation (5)

L2 S 2 −L 3 r 31+ r 34
= =
L2 C 2 ± √ (−L3 r 11+r 14)2 +(−L 3 r 21+ r 24 )2

−L3∗0.7+ 0.21
± √ (−L3∗−0.6+ 0.5)2 +(−L3∗0.35−0.18)2

0.21−0.7 L 3
¿
± √( 0.6 L 3+0.5)2 +(−0.35 L 3−0.18)2

0.21−0.7 L3
θ 2=tan−1 ( )=¿ ¿
± √ (0.6 L3+ 0.5)2 +(−0.35 L3−0.18)2

To obtain θ 2 first we have ¿ calculate L3 from equation (2) and (3)

L2 S 1C 2 −L 3 r 21+ r 24
=
L2 C 1 C 2 −L3 r 11+r 14

S 1 −L 3 r 21+ r 24
=
C 1 −L3 r 11+r 14
 −L 3 r 21+ r 24 −l3∗0.35−0.18
tanθ 1= =
−L3 r 11+r 14 −l3∗−0.6+ 0.5

−l3∗0.35−0.18
−0.36=
−l 3∗−0.6+ 0.5

−0.36 ( 0.6 l 3+0.5 )=−0.35 l3−0.18

−0.21 l3+ 0.35l 3=−0.18+0.18

l 3=0

0.21−0.7 L3 0.21
θ 2=tan−1 ( )=tan−1 =tan−1 (± 0.4¿)¿ 49.41° ∨−49.41 ° ¿
2
± √ (0.6 L3+ 0.5) +(−0.35 L3−0.18)
2 ± 0.53

θ 2=21.8 °∨−21.8°

The third angle is obtained by dividing (r22) by (r21) and equating to left side

−S 1 S 23 rr 22 0.866
= =
S 1 C 23 r 21 0.35

tan ( θ 2+θ 3 ) =−2.474

θ 2+θ 3=tan −1−2.474

θ 2+θ 3=−67.99°

θ 3=−67.99−θ 2= -67.99+21.8 or -67.99 – 21.8

θ 3=−46.19 °∨−89.79 °

3) . A spherical robotic manipulator having 3 robotic joints shown below is

required to move an object from location A=[-0.6m -0.8m 0m] to location
B=[0.8m 0.6m 0.0m ].
 a) Design a third order polynomial joint space trajectory for the three joints
assuming equal time motion of the three joints. T=10sec. Assume L1=1.5m,
L2=1m and L3=0.5m all having shape of rectangular prism

b) In the joint trajectory given above, if the mass of the three joints is
M1=1.0Kg, M2=1Kg and M3=1.0Kg,. Which of the three motors, need large
torque.

c) With the details given in b above and considering all valid assumptions,
implement the robotic manipulator using SIMMECHANICS. Implement a PID
controller and show the motion of the manipulator for the trajectory designed in a
above.

Answer
3A

1 0 0 −0.6 1 0 0 0.8
0
Position matrix A= 0
0
[ 1 0 −0.8
0 −1
0 0
0
1
]
Position matrix B=
[ 0
0
0
1 0 0.6
0 −1 0
0 0 1
]
Performing inverse kinematics to get the initial and final angle values for
polynomial trajectory calculation:

C 1 C 23 −C1 S23 S1 C 1 ( L3 C 23+L2 C 2 ) r 11 r 12 r 13 r 14

0
T 3=
[ S 1 C23
S23
0
−S 1 S 23 −C 1
C 23
0
0
0
S1 ( L3 C23 + L2 C 2 )
L3 S23 + L2 S2
1
][
r r r
= 21 22 23 24
r
r 31 r 32 r 33 r 34
0 0 0 1
]
To calculate the angle for position A

C 1 ( l3 C 23+ L 2 C 2 )=r 14=−0.6 ----------------(1)

S 1 ( l 3 C 23+l 2C 2 ) =r 24=−0.8------------------(2)

From above equations

 −0.8
tanθ 1=
−0.6

θ 1=tan−1 1.333=53 ° which is initial angle

∴ θinitial=53 °

To calculate the angle for position B

C 1 ( l3 C 23+ L 2 C 2 )=r 14=0.8 ----------------(3)

S 1 ( l 3 C 23+l 2C 2 ) =r 24=0.6------------------(4)

From above equations

0.6
tanθ 1=
0.8

θ 1=tan−1 0.75=53.13 °=37 ° which is final

angel

∴ θfinal=37°

The standard form of third polynomial trajectory is given by

θ ( t )=a0 + a1 t +a2 t 2 + a3 t 3

θ̇ ( t )=a1 +2 a2 t 1 +3 a3 t 2

We have four constraints:

θ ( 0 ) =θ0−−−−−−−−−−−(1)

θ ( tf )=θf −−−−−−−−−−−(2)

θ̇ ( 0 ) =0−−−−−−−−−−−−(3)
θ̇ ( tf )=0−−−−−−−−−−−( 4)

From above equations

θ ( 0 ) =θ0=a0=¿53

a 0=¿53

θ̇ ( 0 ) =a1 +2 a2 0+3 a3 0=0

a 1=0

θ ( tf )=θf =a 0+ a1 t +a 2 t 2+ a3 t 3=37

a 1 t+ a2 t 2 +a3 t 3=37−53

a 2 t 2+ a3 t 3=−16−−−−−−−(5)

θ̇ ( tf )=0=a1 +2 a2 t 1 +3 a3 t 2

2 a2 t 1 +3 a3 t 2=0

2 a2 +3 a3 t=0−−−−−−−−−−(6)

From equation (5) and (6) compute a 2 and a 3

3
a 2= 2 (−16)
t

3
a 2= (−16 )=−0.48
100

a 2=−0.48

2
a 3= 3 (−16)
t

−2
a 3= (−16 )=0.032
1000
a 3=0.032

∴ θ (t )=53−0.48 t 2+ 0.032t 3

θ̇ ( t )=−0.96 t+ 0.064 t 2

4 a) Explain the basic idea of MPC system

b) For the system given by

Am= 0 1
[ ] Bm= 0[] Cm=[1 0]
−2 −3 1

Find the augmented MPC model of the system. If the initial states of the system is
given by

x ( 0 )= [ 20] and if the control signal is given by ∆ u ( 0 )=2.5 ,calculate the next state
x(1/0) and y(1/0) of the MPC

c) Explain the terms prediction horizon and receding horizon in MPC

Answer
4a, Model Predictive Control (MPC) is an optimal control strategy based on
numerical optimization. A model predictive control law contains the basic
components of prediction, optimization and receding horizon implementation.
Model Predictive Control (MPC) is a multivariable control algorithm that uses:

 an internal dynamic model of the process

 a cost function J over the receding horizon
 an optimization algorithm minimizing the cost function J using the control
input u

MPC provides a systematic method of dealing with constraints on inputs and

states. Such constraints are present in all control engineering applications and
represent limitations on actuators and plant states arising from physical, economic,
or safety constraints. In MPC these constraints are accounted for explicitly by
solving a constrained optimization problem in real-time to deter-mine the optimal
predicted inputs.

How far ahead the model predicts the future. When the prediction horizon is well
matched to the lag between input and output, the user learns how to control the
system more rapidly, and achieves better performance.

4B

Solution

A common discrete state space model is given as

x k+1= A x k + B uk −−−−−−−−(1)

y k =C x k + d k −−−−−−−−(2)

Where, x k+1 is state, uk is input or control signal, d k is disturbance and y k is

output

One step ahead prediction models is given at sample ‘k’ , one can determine data at
sample ‘k+1’
y k +1=C x k+1 +d k +1−−−−−−−(3)

From equation (1) and (3)

y k +1=C ( A x k +B u k ) +d k+1 , assume d k +1=d k

y k +1=C ( A x k +B u k ) +d k −−−−−−−(4)

The one-step ahead prediction can be used recursively to find an n-step ahead
prediction

x k+2 =A x k +1+ B uk +1= A ( A x k + B u k )+ B uk +1= AA x k + AB uk + Bu k+1

x k+3 =A x k +2+ B u k+2= A ( AA x k + AB uk + B uk+1 ) + B uk +1=A 3 x k + A 2 B uk + AB uk +1+ B u k+1

.
.

x k+n =A x k+n−1 +B u k+n−1= A n x k + A n−1 B uk + A n−2 Bu k+1 +…+ AB uk+n −2 + B uk+ n−1

Similarly for y k +n

y k +n=CA n x k + C( A n−1 Bu k + A n−2 B uk +1+ …+ ABu k+n−2 +B u k+n−1)+d k

Prediction notation is double subscript, the 1 st term determines as the sample of the
prediction (how many steps ahead) and the 2 nd denotes the sample at which the
prediction was made.

As general the augmented matrix of MPC is:

x k+1 /k Ax k Buk /k

[ ] [ ][ ]
2
x k+2 /k A xk ABuk/ k +Bu k+1/ k
. = . + .
. . .
. . .
n n −1
x k+n /k A x k A Bu k/ k + …+ A Bu k+n−2 k + Buk+n−1 /k
 x k+1 /k A B 0 . . . 0 u k/ k

[ ][ ] [ ][ ]
2
x k+2 /k A AB B . . . 0 uk+1 / k
. = . x + . . . . . . .
k
. . . . . . . . .
. . . . . . . . .
n n −1 n−2
x k+n /k A A B A B . . . B u k+n−1/ k

Similarly

y k+1 /k CA CB 0 . . . 0 uk /k dk

[ ][ ] [ ][ ] [ ]
2
y k+2 /k CA CAB CB . . . 0 uk +1/ k dk
. = . . . . . . . .
xk + + .
. . . . . . . . . .
. . . . . . . . . .
n n−1 n−2
y k+n / k CA CA B CA B . . . CB u k+n−1 /k d k

Now for a give problem we can calculate the next state x 1/ 0 and y 1 /0of the MPC

x 1/ 0= A x 0+ B u0 / 0

x 1/ 0= [−20 −31 ][02] + [ 01]2.5

x 1/ 0= 0 + 0
−4 [ ] [ ]
2.5

0
x 1/ 0= [−1.5 ]
If there is no disturbance, d 0=0

y 0=C x0

y 0= [ 1 0 ] 2
0[]
y 0=2

y 1 /0=CA x 0 +CBu 0/ 0
y 1 /0=C ( A x0 + B u0 ) … , since x 1/ 0= A x 0 +B u 0

y 1 /0=C x1 /0

0
y 1 /0= [ 1 0 ] [−1.5 ]
y 1 /0=0

4c, Prediction horizon is the number of future control intervals the MPC controller
must evaluate by prediction when optimizing its Manipulated Variable at control
interval. The prediction horizon keeps being shifted forward and for this reason
MPC is also called receding horizon control.

Receding horizon is a general-purpose control scheme that involves repeatedly

solving a constrained optimization problem, using predictions of future costs,
disturbances, and constraints over a moving time horizon to choose the control
action. RHC handles constraints, such as limits on control variables, in a direct and
natural way, and generates sophisticated feed-forward actions.

5) The dynamic equation of a robotic manipulator is given below. Design feedback

linearizing controller for the system
2 2
1 0 ẍ 1 + 0.7 x 2 ẋ 1 ẋ 2 + 0 = u1
[ ][ ] [
0 0.5 ẍ 2 0.15 ẋ 21 ][ ][ ]
1.5 x 2 u2

Answer

Solution
τ =M ẍ +C (x ẋ) ẋ +G( x)

ẍ=M −1 ( τ−C ( x ẋ ) ẋ−G ( x ) )

 −1
1 0
M =
−1
[ 0 0.5 ] =[ 10 02 ]
ẍ1 u1 0.7 x 2 ẋ 21 ẋ 22 u1 0.7 x 2 ẋ21 ẋ 22
[ ] [ ] ([ ] [
ẍ2
=1 0
0 2 u2
−
0.15 ẋ 1 2
] [ ]) [ ] [
− 0
1.5 x2
=
2u 2
−
0.3 ẋ 12
][ ]− 0
3 x2

0.7 x 2 ẋ 21 ẋ 22
let v=
[][ ][
ẍ 1 u
= 1 −
ẍ 2 2 u2 0.3 ẋ 1 2
][ ]
− 0
3 x2

2 2
1 0 v 1 + 0.7 x2 ẋ 1 ẋ 2 + 0 = u 1
[ 0 0.5 v 2 ][ ] [
0.15 ẋ 21 ][ ] [ ]
1.5 x 2 u 2