h_da

FB EIT
Work 5: Simulation of RT-robot

Advanced Robotics
Dr. W. Weber, 4
th
of April 2014



1
1
Work 5: Simulation of the joint control
of RT-robot
1. Introduction
In this work we consider the RT-Robot of Fig. 1 as a simple example for a multi-joint robot.
This robot has one revolute joint (joint 1 with
1 1
q u = ) and one prismatic joint (joint 2 with
2 2
q d = ). Even in case of a simple two joint robot we can study various couplings between the
two joints and we can see the advantages of a model based nonlinear control scheme.



Fig. 1: RT robot with one revolute joint and one prismatic joint
2 Model of the plant (servo-system, actuators, gear-train,
robot arm mechanics)
2.1 General description of the controlled system
The model of the robot arm with the generalized joint coordinates condensed in the vector q
and the vector of the torque
1
t acting at joint 1 and the force
2
t acting at joint 2 is given by
( ) ( , ) = + M q q b q q (1)
M(q) is the position dependent mass matrix, vector b describes several forces/torques. The
vector b can be divided in the vector G(q) caused by gravity, the vector ( , ) C q q of
Centripetal- and Coriolis forces/torques, the vector R is a vector of torques respectively forces
due to friction losses. Here we assume ( ) = R q 0.
( ) ( , ) ( ) ( ) ( ) ( ) ( )
C
= + = + + + M q q b q q M q q G q C q f q R q t (2)
h_da
FB EIT
Work 5: Simulation of RT-robot

Advanced Robotics
Dr. W. Weber, 4
th
of April 2014



2
2

But the complete controlled system includes additionally servo-system, actuators and gear-
train. In Fig. 2 the schematic diagram of the controlled system (plant) between the input
vector
S
U and the motion of the joints is given. The vector
S
U comprehends the output
signals of the controller.

Fig. 2: Actuator system and robot arm connected by the gear train.
The equations of motion respectively the inverse model of the whole plant can be described
by

1
* *
( ) ( , )
S

( ( =

q M q U b q q (3a)

* *
( ) ( , )
S
= + U M q q b q q (3b)
In Fig. 3 the controlled system is depicted.
1
* *
( ) ( , )
S

( (

M q U b q q
q q U
S
q

Fig. 3: The complete controlled system including robot arm and actuator system.
Equation (4) gives the system matrices in more detail. We assume that all friction losses occur
at the motor shaft and are condensed by the vector ( )
R
M q .

* 1 1 1
* 1 1
( ) [ ( ) ],
( , ) [ ( , ) ( )]
M G A G
M G R


= +
= +
M q K S M q J T
b q q K S b q q M q
(4)
1 ,1 1 1 1
2 ,2 2 2 2
01 1 1 1 1
02 1 2 2 2
/ 0 1/ 0 0 0
, , ,
0 / 0 1/ 0 0
sign( )
( )
sign( )
MI A
G G A M
MI A
R M
R
R M
C K u u J
C K u u J
M q F u q
M q F u q
( ( ( (
= = = =
( ( ( (

+ (
=
(
+

T S J K
M q

(5)
2.2 Model of RT-robot
System matrices of equation (1) for RT-robot:
h_da
FB EIT
Work 5: Simulation of RT-robot

Advanced Robotics
Dr. W. Weber, 4
th
of April 2014



3
3

| | | |
| |
2 2 2
2 2 2 1 1 ,1 ,2 2
2
1 2 2 2 1 1 2 1 2 2 2 2 2
2 2
1 2 2 2 2 1 2 1
( ) 0
( )
0
g sin( ) ( ) 2 ( )
( , )
g cos( ) ( )
z z yy yy L
L
z z L z L
L z L
m q s m s I I m q
m m
q m q s m s m q q q m q s m q
q m m m q s q m q q
( + + + + +
=
(
+

( + + + + + +
=
(
+ +

M q
b q q

(6)

,1 ,2
,1 ,1 ,2 ,2
,1 ,2
0 0 0 0
0 0 ; 0 0 ;
0 0 0 0
xx xx
SP yy SP yy
zz zz
I I
I I
I I
( (
( (
= =
( (
( (

I I
Values of the parameters:
1. Robot mechanics

| | | |
1 2
T T
1 2
,1 ,1 ,1
,2 ,2 ,2
1 1 2 2
20kg, 30kg, g 9.81m/sec
0 0 0.1 , 0 0 0.45
0.06kgm, 0.04kgm, 0.038kgm
6kgm, 6kgm, 0.04kgm

0, 0Nsec/m, 0Nmsec, 0
0 15
xx yy zz
xx yy zz
D D D D
L
m m
I I I
I I I
F F F F
m
s s
= = =
= =
= = =
= = =
= = = =
s s
(7a)
Limits of the joint coordinates:
1 2
/ 2 / 2, 0.25 1.25 q q t t s s s > (7b)
2. Actuator System an gear train:

1 2
1 2
1 2
01 02
1 2
,1 ,2
140, 100,
0.0002, 0.0004
1.8, 1.8
0.04, 0.08
0.0006, 0.0015
3, 1
A A
R R
M M
MI MI
u u
J J
C C
M M
F F
K K
= =
= =
= =
= =
= =
= =
(7c)
3 Design of the controller
3.1 Single axis P-PI controller
To design the controller the couplings between the joints and all nonlinearities are neglected
here.
We assume maximum load and get on the base of equation (3a) the description of the
controlled system:

* *
1 11,max 1 1 1
* *
2 22,max 2 2 2
S D
S D
U M q F q
U M q F q
= +
= +


(8)
In equations (8) the maximum load is considered, because the controller is designed related to
maximum load. The two equations of (8) are linear differential equations and independent of
h_da
FB EIT
Work 5: Simulation of RT-robot

Advanced Robotics
Dr. W. Weber, 4
th
of April 2014



4
4
each other. We take the notation
i i
v q = . The single axis control is done by a cascade control
system (see Fig. 4).

1
s
q
U
S
q=v
.
velocity
controller
position
controller
G
S1
(s)
q
d
v
d
-
-

Fig. 4: Cascaded single axis control with final position controller and secondary
velocity controller.
Here we use a PI controller for secondary velocity control loop and a P-Controller for final
position control loop. Figure 5 shows the velocity control loop with a P-T
1
term for the
controlled system which is derived from equation (8).
( ) ( ) ( ) ( )
1
Sti
i si Si Si
Sti
K
V s G s U s U s
T s
= =
+
. (9)
U
S
v
-
v
d
1
St
St
K
T s +
1
N
P
N
T s
K
T s
+

Fig. 5: Block diagram of the velocity control loop (reference reaction)
In Figures 4 and 5 the index to notate the number of joint is omitted hence the structure for
each joint is the same. With
0 P St
K K K = we get the transfer function of the velocity control
loop:

( )
0
2
0 0
1
( )
( ) ,
( )
1
1 1
N
v P St
d N St
N
T s
V s
G s K K K
V s T T
T s s
K K
+
= = =
| |
+ + +
|
\ .
(10)

We will here use one simple method to design ,
P N
K T . The denominator of ( )
v
G s is
considered as the denominator of a P-T
2
-term:

2 2 2
0 0
1
1 1 1 2
N St
N R R R
T T
T s s d T s T s
K K
| |
+ + + = + +
|
\ .
. (11)
We have to choose suitable values for ,
R R
d T . By comparison of the coefficients in equation
(11) we get:

h_da
FB EIT
Work 5: Simulation of RT-robot

Advanced Robotics
Dr. W. Weber, 4
th
of April 2014



5
5
(2 ) 1
, (2 ) 2
R St R R R
P N R St R R R
St R St St
d T T T T
K T d T T T d
K T T T
| |
= = =
|
\ .
(12)

But notice: In equation (12) a D-Term is part of the numerator and the resulting damping of
( )
v
G s is less than
R
d and the response will be faster than defined by
R
T .
The next step is the approximation of the velocity control loop by a simple transfer
function ( )
v
G s

. Independent of the approach of the design of the PI velocity controller we use

a P-T
2
term with gain 1, time constant
v
T and damping constant
v
d :
2 2
1
( )
1 2
v v v
G s
d T s T s
=
+ +

(13)
On the base of the step response (see Fig. 7 as example) we determine the values of overshoot
and rise-time
An
T and get
v
T and
v
d by
2
max
2
1
1
, ,
arccos( )
1
ln
An v
d
v v
d v
T d
v v
d T
v d

t
t

= = =

| |
+
|
\ .
(14)

Fig. 6: Approximated block diagram for the position control loop with replaced
secondary velocity control loop by an P-T
2
term

t
v
v
d
An
T
max
v

Fig. 7: Approximation of the velocity control loop by a P-T
2
term
h_da
FB EIT
Work 5: Simulation of RT-robot

Advanced Robotics
Dr. W. Weber, 4
th
of April 2014



6
6
The last step is to design the value of the P-Controller of the final position control loop (see
Fig. 6). To determine
L
K we use here the rule of thumb stated in Equation (15).
0.2 0.3
L
v v
K
T T
s s (15)
In Figure 6 the often used velocity precontrol is depicted. The desired velocity of the joint is
weighted by the factor
v
K . It holds:
0 1
v
K s s (16)
3.2 Model-based control
In this chapter the control problem is treated considering the nonlinear couplings respectively
time variant parameters. The characteristics of model-based control schemes are the
evaluation of the inverse model (see Equation 3b) in real time as part of the control algorithm.
3.2.1 Control law to achieve a desired transfer function
We develop the control algorithm in two steps (see Figure 8):

1. Choosing a linear dynamic input-output behaviour as reference model between the desired
motion
d
q and controlled motion q .

2. Derivation of the control law to achieve the desired input-output behaviour.


reference
model
equations
of motion
structure and
parameters of
control law
comparison and
solve for the
output vector of
the controller

Figure 8. Principle of the model based control design
As reference model we define a P-T
2
behaviour model between the vector of desired joint
variables
d
q and the vector of real joint variables q for each of the joints:

1, 0, 0, , i i i i i i d i
q a q a q a q + + = (17)
The parameters a
1,i
, a
0,i
are constant parameters. If we transform to Laplace domain:
0
, , 2
2 1
0 1
0 0
, 0 1 2 2 2
1
( ) ( ) ( )
1
1
2 1 1
( ), ,
1 2
i
i d i d i
i
i i
i i
i
d i i i
i i i i i
a
Q s Q s Q s
a
a a s s
s s
a a
d
Q s a a
d T s T s T T
= = =
+ +
+ +

= =
+ +
(18)
The desired input-output behaviour for all joints can be written in matrix form:

0 1 0 d
+ + = q A q A q A q (19)
The matrices in (19) are constant diagonal matrices. For the reference P-T
2
behaviour it holds
h_da
FB EIT
Work 5: Simulation of RT-robot

Advanced Robotics
Dr. W. Weber, 4
th
of April 2014



7
7
1
2
1 1
0 1
2
2
2 2
2 1
0 0
,
1 2
0 0
d
T T
d
T T
( (
( (
( (
= =
( (
( (

A A . (20)
For further handling (19) is solved for q and the right side is abbreviated by r
0
:
( ) ( )
0 1 0
, ,
d d
= = q A q q A q r q q q (21)
The second step is now to examine, how U
S
, the output vector of the controller, must be
calculated to achieve the reference dynamic of Equation (21). The control law can be derived
in an easy manner, if we compare the equation (21) and the equation of motion (3a). Eq. (3a)
describes the motion of the robot arm under the effect of the vector U
S
. If this motion has to
obey the desired differential equation (21), the right side of the desired behaviour (21) and the
right side of the real behaviour (3a) must be equal:
( )
0
, , , ,
d d d
r q q q q q =
1
* *
( ) ( , )
S

( (

M q U b q q (22)
Now we can solve for U
S
to achieve the control law:

* *
0
( ) ( , )
S
= + U M q r b q q (23)
By Comparing (23) with (3b) we see, that the control law is identical with the inverse model
according to (3b). But in (23) there isnt used the vector q of measured accelerations but the
vector r
0
which is calculated in dependence of the desired and measured values of the joint
variables. Figure 9 shows the block diagram of model-based control.
r
0
q
d
(q
d
, q
d
)
control law
to form r
0
control law
to form U
S
U
S
robot arm
actuators
q,q

Figure 9. General structure of the whole control system with the model based
controller.
3.2.2 Model based cascade control
This method differs from chapter 3.2.1 only in the calculation of the control vector r
0
. First
we don't consider how r
0
can be calculated, but we use (23) as part of the control law in the
same way as in chapter 3.2.1. Now we use (23) for
S
U and apply
S
U in the equation of
motion (3a):

1
* * * *
0
( ) ( ) ( , ) ( , )

( ( = +

q M q M q r b q q b q q . (24)

The result is the virtual model

h_da
FB EIT
Work 5: Simulation of RT-robot

Advanced Robotics
Dr. W. Weber, 4
th
of April 2014



8
8
0
= q r (25)

We have a double I-term as virtual plant and we can apply the cascade control scheme to the
virtual plant accordingly to Figure 4. If we avoid the index for the joint we get the scheme of
Figure 10.

Fig. 10. Structure of model-based cascaded control with virtual plant

For the velocity control loop of each joint (see Fig. 11) we can use a PI-controller:
( )
0 0
0
(1 )
( ) ( )d , ( ) ( ) ( )
t
P N P
P d d d
N N
K T s K
r K v v v v R s V s V s
T T s
t
+
= + =

}
(26)
with v q = . In that case we get the transfer function of the secondary control loop

( )
2
1
( )
( )
( )
1
N
v
N
S
N
P
T s
V s
G s
T
V s
T s s
K
+
= =
+ +
(27)
If we consider the denominator as denominator of a PT
2
-term, it holds:

2 2 2
1 1 2
N
N R R R
P
T
T s s d T s T s
K
+ + = + + (28)
If the user defines
R
d and
R
T the values for
P
K and
N
T can be evaluated. But notice that the
response in reality is faster and less damped compared with a PT
2
-term as a result of the D-
term in the numerator.

Fig. 11. Virtual velocity control loop

As next step we consider the step response accordingly to Figure 7 and evaluate the values of
v
d and
v
T by equation (14). Then we can determine
L
K and
V
K by equation (15)
respectively (16).
h_da
FB EIT
Work 5: Simulation of RT-robot

Advanced Robotics
Dr. W. Weber, 4
th
of April 2014



9
9
4 Task for preparation
4.1 Model of the plant
Calculate the matrix
*
( ) M q and the vector
*
( , ) b q q using the constant parameters of the
controlled system and dependent of the variables
1 2, 1 2,
, , .
L
q q q q m
Establish equation (8) for both joints with maximum load
L
m and give
St
K and
St
T for both
joints on base of equation (8).

4.2 Design of K
P
and T
N
for single axis cascaded control
Design for both joints the values of
P
K and
N
T with equation (12) by using the values for
St
K and
St
T from 4.1. Choose 0.9 and 0.05
R R
d T = = for each joint.

4.3 Design of K
L
single axis cascaded control
Realize with Simulink the simple control loop of Figure 5, plot the step response for each
joint with the values of 4.1 and 4.2, evaluate
v
d and
v
T for each joint with equation (14) and
evaluate
L
K by the thumb rule (eq. 15).
4.4 Model based Control law to achieve a P-T
2
-desired transfer
function
Give the matrices
0 1
and A A for 1 and 0.05
R R
d T = = for each joint.
4.5 Path planning
The robot joints have to move from
0
0.3
St
| |
=
|
\ .
q to
/ 2
1
End
t | |
=
|
\ .
q . Draw both points in the
Cartesian space of
0
K . Give the path for linear path and a possible path if PTP-motion is
applied.

5 Experiments and discussions
5.1 Tool Simulation_RT
A supervisor will explain to you how to work with the tool Simulation_RT.

5.2 Single axis P-PI-Controller
Give a PTP path between point of start
0
0.3
St
| |
=
|
\ .
q and target point
/ 2
1
End
t | |
=
|
\ .
q with
acceleration of 4 rad/sec resp. 4 m/sec a velocity of 1 rad/sec resp. 1m/sec. Take the values
of 4.2/4.3 for the control parameters. Simulate with 0.9
V
K = and maximum load of
15kg
L
m = .
h_da
FB EIT
Work 5: Simulation of RT-robot

Advanced Robotics
Dr. W. Weber, 4
th
of April 2014



10
10
Realize with the same controller and conditions a linear path between and
St End
q q with
acceleration of 4 m/sec and velocity of 1m/sec.

5.3 Model based Control law to achieve a P-T
2
-desired transfer
function
Simulate the motion and same conditions of 5.2 (PTP- and linear motion, 15kg
L
m = ) by
using the Model based Control law to achieve a P-T
2
-desired transfer function
with 1 and 0.05
R R
d T = = for each joint.
Why is there a steady state error?

5.4 Model based cascaded control
Simulate the motion and same conditions of 5.2, 5.3 (PTP- and linear motion, 15kg
L
m = )
by using the Model-based cascaded Control law. Evaluate the values of
P
K and
N
T with
equation (28) with 0.9 and 0.05
R R
d T = = for each joint. Simulate the virtual velocity loop
and evaluate
v
d and
v
T for each joint with equation (14) and evaluate
L
K by the thumb rule
(eq. 15).
Why is there no steady state error?


Appendix: Ramp profile for desired motion

Ramp profile for a single axis motion