3o SiPGEM – 3o Simpósio do Programa de Pós-Graduação em Engenharia Mecânica

Escola de Engenharia de São Carlos – Universidade de São Paulo

3 e 4 de setembro de 2018, São Carlos - SP

INVERSE DYNAMICS-BASED LINEAR QUADRATIC CONTROL

APPLIED TO A 3-LINK PLANAR ROBOT

Denis Mosconi, Dinâmica e Mecatrônica, denis.mosconi@usp.br

Adriano Almeida Gonçalves Siqueira, Dinâmica e Mecatrônica, siqueira@sc.usp.br

Abstract. The objective of this work was to apply the Linear Quadratic design to determinate the feedback gains in
Inverse Dynamics Control with PD feedback. Six tests was performed: four using the Inverse Dynamics Control with
PD feedback, one using only PD loop whose the gains was empirically determined and the last using only PD control
with the gains determined by the LQ method. The use of Linear Quadratic design has been shown to be a good manner
to determine the gains in controllers with error-based feedback loop. The systems based on Linear Quadratic Inverse
Dynamics design presented the best response, with the lowest tracking errors power consumption.

Keywords: Linear Quadratic Design, Robot Control, Feedforward Control

1. INTRODUCTION

A robot manipulator is a general purpose machine programmable to perform a variety of tasks, such as: pick up and
put, paint spraying, welding, parts assembling and package. The tasks realization is based on motions of the end-effector
or forces between the end-effector and the environment where the robot is working (Lammerts, 1993).
Motion control of manipulators is a challenging work: To do this its necessary to known the motion of each link.
Because this motion is generated by a motor that drives the corresponding joint, the tracking control problem is to
determine the actuator torques and forces that make the manipulator follow the desired trajectory as well as possible,
with the desired performance.
The better performance is defined by the process, for example: high path accuracy in laser cutting, high speed accuracy
in painting, high speed and acceleration in material handling or small overshoots in spot welding (Moberg, 2010).
Using the desired end-effector trajectory and the inversion of the dynamics model of the manipulator, is possible to
calculate the torques necessaries to perform the task, this method is called computed-torque control (Moolam, 2013).
However, although model equations are well known, only imprecise knowledge of the parameters is available. This so-
called parametric uncertainty may be caused by an unknown load at the end-effector, poorly known inertias, uncertain and
slowly time-varying friction parameters. Thus, to realize the desired tasks with the necessary performance, a controller
with feedback loop is required (Lammerts, 1993).
The purpose of this work was to study a control system with a feedforward loop based on inverse dynamics and
a feedback PD loop whose the gains Kp and Kv was determined using the Linear Quadratic design. The controllers
obtained was compared with the traditional PD controllers. This work also evaluated the effect of the change of the
weights of the matrix Q on the kinematic and dynamics variables of the manipulator.
The importance of the study is justified by the fact that it is necessary to find ways to determine the gains that provide
robustness and optimized control of manipulator robots.

2. LINEAR QUADRATIC DESIGN

The Linear Quadratic Optimal Control was introduced by Kalman in 1960 (Kalman, 1960) with the aim to providing
a solution for a classical problem in theory control: the design of linear control system in order to minimize the integral
of the square of tracking error, evaluated among motions of the system. This problem was first approached by Wiener and
Hall around 1943 ((Wienerm 1949), (Hall, 1943)), but was not rigorously formulated from a mathematical point of view.
Studying a plant represented by the following system in state-space form
dx
= A(t)x(t) + B(t)u(t) (1)
dt
where x(t) ∈ Rm is the state vector and u(t) ∈ Rn is the input control function. The behavior of the model is described
by the solution of the Eq. (1), whose general solution has the form (Grant, 2007):
Z t
x(t) = φ(t, t0 )x(t0 ) + φ(t, τ )B(τ )u(τ )dτ (2)
0

being φ(t, τ ), defined for all t, the solution of the free system represented by Eq. (1), and
φ(t, t) = I (3)
 3o SiPGEM – 3o Simpósio do Programa de Pós-Graduação em Engenharia Mecânica
Escola de Engenharia de São Carlos – Universidade de São Paulo
3 e 4 de setembro de 2018, São Carlos - SP

The solution Eq. (2) is referred to as

x(t) = φ(t; x, t0 ) (4)

The purpose of a control system is to produce a control action u(t) for any state x given in any time t, and depending
on x, t0 which transfers x to the equilibrium state 0, that is, u(t) must to ensure

lim φ(t; x, t0 ) = 0 (5)

t→inf

If x(t) is known and considering the feedback principle (Lewis, 1993), the control law is given as

u(t) = −Kx(t) (6)

where K = [Kp Kv ] is the feedback gain matrix.

The determination of K is achieved solving the Riccati equation

AT P + P A − P BR−1 B T P + Q = 0 (7)

where P is a symmetric m×m solution matrix, Q is a symmetric positive semidefinite m×m matrix (Q ≥ 0) and R is a
symmetric positive definite n×n matrix (R > 0).
Solving the following equation, the feedback gain matrix is defined:

K = R−1 B T P (8)

The matrices Q and R must be chosen in order to minimize the quadratic performance index (Lewis, 1993):

1 ∞ T
Z
J= (x Qx + uT Ru)dt (9)
2 0

The time function xT Qx + uT Ru must goes to zero as t becomes large.

3. INVERSE DYNAMICS CONTROL

The robot dynamics model is given by

M (q)q̈ + V (q, q̇) + G(q) = τ (10)

where M (q) is the inertia matrix n×n and function of the joint-position, V (q, q̇) is the centripetal/Coriolis n×1 vector and
G(q) the gravity vector n×1 (Lewis, 1993). The torques necessary to produce the acceleration is given by the n×1 vector
τ (n is the number of joints).
Sometimes the dynamics equation is write as

M (q)q̈ + N (q, q̇) = τ (11)

where

N (q, q̇) = V (q, q̇) + G(q) (12)

represents nonlinear terms.

Having a set of desired accelerations, velocities, positions and a dynamic model of the robot, the torque required to
produce the desired trajectory can be computed using Eq. (10). If the mathematical model were well known, the torque
could be computed off-line and used to implement a feedforward tracker controller (Craig, 2012). This ideal model is
presented in Figure 1.
..
qd
.
qd Dynamic Model τ Robot q
qd
Figure 1. Computed-torque control for ideal model
 An exact dynamic model is impossible to be achieved: some variables are very difficult to be modeled (friction),
others are neglected during modeling (backlash). Thus, tracking errors arise, and the model presented in Figure 1 becomes
inapplicable, and it is necessary to create a feedback loop to correct the errors.
Considering the trajectory, velocity and acceleration errors, respectively, as

e = q − qd (13)

ė = q̇ − q˙d (14)
ë = q̈ − q¨d (15)
where the index d indicates the desired variable.
Solving Eq. (11) for q̈

q̈ = M −1 τ − M −1 N (16)

The Inverse Dynamics Control Law is formulated as

τ = M̂ q¨d + N̂ + u (17)

where M̂ and N̂ are the estimated inertia and nonlinear terms matrices, and u the feedback control function.
Replacing Eq. (17) in Eq. (16) and making M̂ = M and N̂ = N , the acceleration error becomes

ë = M −1 u (18)

The inverse dynamics control scheme has a nonlinear feedforward loop and a feedback control loop, as shown in the
Figure 2.

.. . .
q d , qd , q d Feedforward τ q, q
+ Robot
Loop +

u(t)
Feedback
Loop
.
Figure 2. Inverse dynamics control with feedback loop

The function u(t) must be chosen in order to stabilize Eq. (14) and Eq. (15) so that Eq. (13) goes to zero.

4. MATERIAL AND METHODS

To realize the tests was used a 3-link planar robot type Uarm (Figure 3), in the Robotic Lab of the University of São
Paulo - São Carlos/Brazil.

θ3

L3

θ2

L2
L1
θ1

Figure 3. 3-link planar robot scheme

 3o SiPGEM – 3o Simpósio do Programa de Pós-Graduação em Engenharia Mecânica
Escola de Engenharia de São Carlos – Universidade de São Paulo
3 e 4 de setembro de 2018, São Carlos - SP

Writing the errors in state-space form (Eq. (1)), we have:

      
d e 0 I e 0
= + u (19)
dt ė 0 0 ė M −1

where
 
  e
u = −Kx = − Kp Kv (20)
ė

The feedback gain K was determined using the Linear Quadratic design presented in section 2. The matrices A and
B were obtained from Eq. (19), and the matrices Q and R had their values assigned empirically.
Six tests were performed:in the first four tests, was applied a Inverse Dynamics Control with PD feedback whose the
gains Kp and Kv were determined as described above. In each of these tests the values of the Q and R matrices were
changed, as indicated below:

Test 1:  
  2.1078 0.8004 0.1690
10I3 03×3
Q= R = 0.1I3 Kp = 10I3 Kv = 0.8004 0.9121 0.1827
03×3 0.01I3
0.1690 0.1827 0.4817

Test 2:  
  2.0737 0.8328 0.1675
10I3 03×3
Q= R = 0.1I3 Kp = 10I3 Kv = 0.8328 0.8233 0.2113
03×3 0.001I3
0.1675 0.2113 0.3624

Test 3:  
  2.0700 0.8367 0.1669
10I3 03×3
Q= R = 0.1I3 Kp = 10I3 Kv = 0.8367 0.8126 0.2159
03×3 0.0001I3
0.1669 0.2159 0.3472

Test 4:  
  2.0697 0.8371 0.1668
10I3 03×3
Q= R = 0.1I3 Kp = 10I3 Kv = 0.8371 0.8115 0.2164
03×3 0.00001I3
0.1668 0.2164 0.3456

A PD control was used in the last tests. In the fifth test, the controller gains were empirically determined and in the
sixth test, the controller gains were obtained from the Linear Quadratic design. The gains are presented below:

Test 5:
 
5 0 0
Kp = 0 5 0 Kv = 0.1I3
0 0 2

Test 6:  
  2.0697 0.8371 0.1668
10I3 03×3
Q= R = 0.1I3 Kp = 10I3 Kv = 0.8371
 0.8115 0.2164
03×3 0.00001I3
0.1668 0.2164 0.3456

The inertia matrix is time-variable so that, to calculate constant feedback gain, it was considerate as a constant value
matrix, being determined with the robot in the position with maximum moment of inertia:
 
0.2506 0.1224 0.0292
M = 0.1224 0.0703 0.0195
0.0292 0.0195 0.0097
5. RESULTS

In this section the results obtained with the tests are analyzed. Each subsection deals with a variable of importance.

5.1 Position
0.4 0.4

0.3 0.3
Position

Position
0.2 0.2
1 1

2 2
0.1 0.1
3 3

d d
0 0
0 1 2 3 4 5 0 1 2 3 4 5
Time Time

(a) Test 1 (b) Test 2

0.4 0.4

0.3 0.3
Position

Position

0.2 0.2
1 1

2 2
0.1 0.1
3 3

d d
0 0
0 1 2 3 4 5 0 1 2 3 4 5
Time Time

(c) Test 3 (d) Test 4

0.4 0.4

0.3 0.3
Position

Position

0.2 0.2
1 1

2 2
0.1 0.1
3 3

d d
0 0
0 1 2 3 4 5 0 1 2 3 4 5
Time Time

(e) Test 5 (f) Test 6

Figure 4. Positions (rad) x Time (s)

All systems tested had a settling time of approximately four seconds. Controllers with feedforward plus loop feedback
loop demonstrated lower RMS error compared to controllers with only loop feedback. Analyzing the RMS error values
presented in Table 1, it is possible to verify that, by reducing the weight related to the velocity error, in the Q matrix, the
position tracking error also decreases, with the controller of the test 4 having the smallest tracking error.

Table 1. RMS position error of joints (×10−3 )

Joint Test 1 Test 2 Test 3 Test 4 Test 5 Test 6

q1 2.2 2.2 2.0 1.5 14.3 3.6
q2 1.5 1.2 1.1 1.2 6.9 2.1
q3 2.1 2.5 2.4 1.7 10.9 1.9
 3o SiPGEM – 3o Simpósio do Programa de Pós-Graduação em Engenharia Mecânica
Escola de Engenharia de São Carlos – Universidade de São Paulo
3 e 4 de setembro de 2018, São Carlos - SP

5.2 Velocity

0.3 0.3
1 1
0.25 0.25
2 2

0.2 3 0.2 3
Velocity

Velocity
d d
0.15 0.15

0.1 0.1

0.05 0.05

0 0
0 1 2 3 4 5 0 1 2 3 4 5
Time Time

(a) Test 1 (b) Test 2

0.3 0.3
1 1
0.25 0.25
2 2

3 3
0.2 0.2
Velocity

Velocity

d d
0.15 0.15

0.1 0.1

0.05 0.05

0 0
0 1 2 3 4 5 0 1 2 3 4 5
Time Time

(c) Test 3 (d) Test 4

0.3 0.3
1 1
0.25 0.25 2
2

3 3
0.2 0.2
Velocity

Velocity

d d

0.15 0.15

0.1 0.1

0.05 0.05

0 0
0 1 2 3 4 5 0 1 2 3 4 5
Time Time

(e) Test 5 (f) Test 6

Figure 5. Velocities (rad/s) x Time (s)

The tests showed that the LQ-based controllers have a better velocity profile when reducing the velocities weights of
the Q matrix. Among these controllers, the one with the best velocity profile (low velocity error throughout the entire
movement) was the pure PD (test 6). The controllers used in tests 1 to 4, despite producing a better result after the
reduction of Q weights, still present increased noise during deceleration. The PD controller with empirical gains presents
errors with low noise, but great amplitude, in relation to the other ones.

Table 2. RMS velocity error of joints (×10−3 )

Joint Test 1 Test 2 Test 3 Test 4 Test 5 Test 6

q1 17.7 10.6 7.0 8.8 42.7 10.1
q2 36.6 20.7 12.8 17.2 21.1 12.4
q3 15.4 12.1 9.7 11.5 23.8 12.8
5.3 Torque
Computed-Torque Feedback Torque Total Torque
0.2 0.2 0.2
1 1 1

2 2 2
Computed-Torque

0.1 0.1 0.1

Feedback Torque
3 3 3

Total Torque
0 0 0

-0.1 -0.1 -0.1

-0.2 -0.2 -0.2

0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5
Time Time Time

(a) Total Torque (b) Computed-Torque (c) Feedback Torque

Figure 6. Torque (N.m) x Time (s) - Test 1

Computed-Torque Feedback Torque Total Torque

0.2 0.2 0.2
1 1 1

2 2 2
Computed-Torque

0.1 0.1 0.1

Feedback Torque

3 3 3

Total Torque
0 0 0

-0.1 -0.1 -0.1

-0.2 -0.2 -0.2

0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5
Time Time Time

(a) Total Torque (b) Computed-Torque (c) Feedback Torque

Figure 7. Torque (N.m) x Time (s) - Test 2

Computed-Torque Feedback Torque Total Torque

0.2 0.2 0.2
1 1 1

2 2 2
0.1 0.1
Computed-Torque

0.1
Feedback Torque

3 3 3
Total Torque

0 0 0

-0.1 -0.1 -0.1

-0.2 -0.2 -0.2

0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5
Time Time Time

(a) Total Torque (b) Computed-Torque (c) Feedback Torque

Figure 8. Torque (N.m) x Time (s) - Test 3

Computed-Torque Feedback Torque Total Torque

0.2 0.2 0.2
1 1 1

2 2 2
0.1 0.1 0.1
Computed-Torque

Feedback Torque

3 3 3
Total Torque

0 0 0

-0.1 -0.1 -0.1

-0.2 -0.2 -0.2

0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5
Time Time Time

(a) Total Torque (b) Computed-Torque (c) Feedback Torque

Figure 9. Torque (N.m) x Time (s) - Test 4

 3o SiPGEM – 3o Simpósio do Programa de Pós-Graduação em Engenharia Mecânica
Escola de Engenharia de São Carlos – Universidade de São Paulo
3 e 4 de setembro de 2018, São Carlos - SP

Total Torque Total Torque

0.2 0.2
1 1

2 2
0.1 3
0.1 3

Total Torque
Total Torque

0 0

-0.1 -0.1

-0.2 -0.2
0 1 2 3 4 5 0 1 2 3 4 5
Time Time

Figure 10. Torque (N.m) x Time(s) - Test 5 Figure 11. Torque (N.m) x Time(s) - Test 6

The inverse dynamics based controllers have two source of torque: the computed and the feedback that is based on
errors. Is possible to notice that the computed-torque remains unchanged between one test an another, this is because this
control is based on the characteristics of the robot and the desired trajectory, which do not change over time. However,
the feedback torque changes from a test to another due to change in gains Kp and Kv.
According to Table 3, it is possible to verify that reducing the velocities weights of the Q matrix the energy consumption
of the controllers is reduced, so that in test 4 there was the lower consumption. The controllers with only feedback loop
have higher RMS torque values than controllers with feedforward loop.

Table 3. RMS total torque of joints (×10−3 )

Joint Test 1 Test 2 Test 3 Test 4 Test 5 Test 6

q1 40.4 43.8 39.7 38.4 72.4 49.7
q2 36.1 25.9 21.9 24.2 34.9 27.5
q3 23.5 26.7 25.6 19.1 22.2 20.4

6. CONCLUSIONS

The Linear Quadratic design demonstrate to be a good way to determine the feedback gains in a inverse dynamics
controller with PD feedback. Reducing the velocities weights of the matrix Q was possible to obtain low tracking errors
and power consumption. The controls with feedforward loop based on inverse dynamics proved to be better than the ones
with PD feedback loop only.

7. REFERENCES

CRAIG, J.J.Introduction to Robotics: Mechanics and Control. 3. ed. Pearson Prentice Hall, 2012.
GRANT, C. P.Theory of Ordinary Differential Equations. Brigham Young University, 2007.
HALL, A. C. The Analysis and Synthesis of Linear Servomechanisms. Phd Thesis. Cambridge: Massachusetts Institute
of Technology, 1943.
KALMAN, R. E. Contributions to the Theory of Optimal Control, Bol. Soc. Mexicana., p. 102-119, 1960.
LAMMERTS, I.M.M. Adaptive computed reference computed torque control of flexible manipulators, Phd Thesis
Eindhoven University of Technology, Eindhoven. 1993.
LEWIS, F.L. ET AL. Control of robot manipulators Macmillan Publishing Company, 1993.
MOBERG, S. Modeling and Control of Flexible Manipulators. Phd Thesis. Linkoping University, Linkoping, 2010.
MOOLAM, R. K. Dynamic Modeling and Control of Flexible Manipulators. Phd Thesis. Politecnico di Milano, Milan,
2013.
WIENER, N. Extrapolation, Interpolation and Smoothing of Stationary Time Series. New York: Wiley, 1949.

8. RESPONSIBILITY NOTICE

The authors are the only responsible for the printed material included in this paper.