Robot manipulator modeling in MatlabSimMechanics with PD control and online gravity compensation
ABSTRACT The objective of this article is to present a method to model the mechanics of robot manipulators. A complete description of the procedure to model and control a twolink planar robot arm is detailed and simulated using Matlab/Simulink from the generation of a mechanical model in SimMechanics Toolbox. One of the simplest position controllers for robot manipulators is the PD control with desired gravity compensation. The implementation of this method was verified on this model. The results of simulation show that SimMechanics can be used to model the mechanics of robot and the better performance can be obtained with the online gravity compensation method as compared to the case of constant gravity compensation method.

Conference Paper: Explicit kinematic model of the Mitsubishi RVM1 robot arm
[Show abstract] [Hide abstract]
ABSTRACT: A complete kinematic model of the 5 DOFMitsubishi RVM1 manipulator is presented in this paper. The forward kinematic model is based on the Modified DenavitHartenberg notation, and the inverse one is derived in closed form by fixing the orientation of the tool. A graphical interface is developed using MATHEMATICA software to illustrate the forward and inverse kinematics, allowing student or researcher to have handson of virtual graphical model that fully describe both the robot's geometry and the robot's motion in its workspace before to tackle any real task.Control, Automation and Information Sciences (ICCAIS), 2012 International Conference on; 01/2012
Page 1
Robot manipulator modeling in Matlab
Simmechanics with PD control and online Gravity
compensation
Le Tien Dung
Graduate School of Electrical Engineering
University of Ulsan
Ulsan, Korea
dung.letien@gmail.com
HeeJun Kang, YoungShick Ro
School of Electrical Engineering
University of Ulsan
Ulsan, Korea
hjkang@ulsan.ac.kr, ysro@uslan.ac.kr
Abstract — The objective of this article is to present a method to
model the mechanics of robot manipulators. A complete
description of the procedure to model and control a twolink
planar robot arm is detailed
Matlab/Simulink from the generation of a mechanical model in
SimMechanics Toolbox. One of the simplest position controllers
for robot manipulators is the PD control with desired gravity
compensation. The implementation of this method was verified
on this model. The results of simulation show that SimMechanics
can be used to model the mechanics of robot and the better
performance can be obtained with the online gravity
compensation method as compared to the case of constant gravity
compensation method.
and simulated using
Keywords: Robotic manipulator; Simulation; Modelling; PD
control; Gravity compensation.
I.
INTRODUCTION
The position control of robot manipulators is the simplest
aim in robot control, at the same times, it is one of the most
relevant issues in practical manipulators. Many approaches
have been introduced to treat this control problem. It is well
known that a rigid robot arm can be globally asymptotically
stabilized around a given joint configuration via a PD
controller on the joint errors, provided that gravity is cancelled
by feedback. Under a mild condition on the proportional gain,
this scheme can be simplified by performing only constant
gravity compensation at the desired configuration [1].
This paper presents a case study which has been done by
Sciavicco and Siciliano [2], where a nonlinear gravity
compensation term evaluated online at
configuration. But here we model the mechanics of robot
manipulator by using SimMechanics Toolbox. The use of
SimMechanics as a tool to model the mechanics of the robot
allows the possibility to verify modelbased control algorithm.
The precise mechanical model, as in a real application, is
unknown for the designer so that the ability of the control
algorithm to match the real model can be easily proven. That
would allow the testing of techniques like the dynamic model
based control but also learning techniques that extract the
model of the plant under consideration.
the current
The paper is organized as follow. Section 2 describes the
tasks of building mechanical model of twolink arm using
SimMechanics Toolbox. The Dynamic model of general
industrial robot is recalled, and detailed for twolink planar
robot manipulator. PD control and gravity compensation
method is also reviewed in this section. Section 3 shows the
results of simulation. Finally, section 4 concludes the paper.
II.
DESCRIPTION
A. Mechanical Model
The first step is to create a model of a simple twolink
planar arm for testing the control methods. Figure 1 shows the
mechanical model of the twolink planar arm described with
SimMechanics. The model is composed by two revolute joints
and two bodies. The module receives torques as input and
outputs are joint angles, angular velocities, joint torques. The
masses are considered to be concentrated at the end of each
link, to simply the modeling tasks. In SimMechanics this is
realized setting the inertias as ‘zeros(3,3)’, a 3x3 matrix that
defines point masses and defining the Centre of Gravity (CG)
on the edge of each link.
B. Dynamical Model
The dynamic model of a njoint robot manipulator can be
written in the Lagrangian form as:
τ=++
)(),()(
qGq
?
qCq
? ?
qM
(1)
where q is the joint variable nvector and τ is the vector of
generalized forces acting on the robot manipulator. M(q) is the
inertia matrix, are the Coriolis/centripetal forces, and
G(q) is the gravity vector. In equation (1) we are not taking into
account the friction torques that are always to be found in a real
robot manipulator.
),(
q
?
qC
The dynamic model of the twolink planar arm following
(1) is:
⎥⎦
⎤
⎢⎣
⎡
=
2
1
q
q
q
; ; ;
⎥⎦
⎤
⎢⎣
⎡
=
2
1
τ
τ
τ
⎥⎦
⎤
⎢⎣
⎡
=
)()(
)()(
)(
2221
1211
qMqM
qMqM
qM
Page 2
⎥⎦
⎤
⎢⎣
⎡
=
),(
),(
)(
2
1
q
?
qC
q
?
qC
qC
; G
(2)
⎥⎦
⎤
⎢⎣
⎡
=
)(
)(
)(
2
1
qG
qG
q
Where:
2212
l l
2
22
2
12111
cos2)()(
qmlmlmmqM
+++=
(3)
2212
l l
2
2221 12
cos)()(
qmlmqMqM
+==
(4)
2
22 22
)(
lmqM
=
(5)
2
2
221212
l l
1
sin)2 (),(
qq
?
q
?
q
?
mq
?
qC
+−=
(6)
2
2
1212
l l
2
sin),(
qq
?
mq
? =
qC
(7)
)cos(cos)()(
212211211
qq glmqglmmqG
+++=
(8)
) cos()(
21222
qqglmqG
+=
(9)
The term l and
respectively and m1 and m2 are their masses. In our example,
and
m
m1
ll
5 . 0
21
==
kg
20
=
are the length of links 1 and 2,
, .
12l
kgm
10
1=
C. PD control with online gravity compensation
Let a constant equilibrium posture be assigned for the
system as the vector of desired joint variables
to find the structure of the controller which ensures global
asymptotic stability of the above posture.
Take the vector [
as the system state, where:
q
?
~
. It is desired
d q
]
T
TTq
~
qqqd−=
(10)
The PD control with constant gravity compensation in
(Tomei, 1991) is express as
)(
~
q
dDP
qGq
?
KK
+−=τ
(11)
Where
typically diagonal matrices.
and are both symmetric and
0
>
P
K
0
>
D
K
Sciavicco and Siciliano [2] proposed the PD control with
online gravity compensation method as:
~
qKqK
DP
−=
?
τ
)(
qG
+
(12)
The different between the control laws (11) and (12) is that
the term computed offline in (11) and online in (12).
)(
G
d q
For proving the globally asymptotically stable, choose the
following positive definite quadratic form as Lyapunov
function candidate
0
~
q
~
q
2
1
)(
2
1
)
~
q
,(
>+=
Kq
?
qMq
?
q
?
V
P
TT
0
~
,
≠∀ qq ?
(13)
An energybased interpretation of (13) reveals a first term
expressing the system kinetic energy and a second term
expressing the potential energy stored in the system of
equivalent stiffness KP provided by the n position feedback
loops.
Differentiating (13) with respect to time, and recalling that
qd is constant, yields:
qKq
?
q
?
qM
?
q
?
q
? ?
qMq
?
V
P
TTT
~
)(
2
1
)(
?
−+=
(14)
Solving (1) for and substituting it in (14) gives:
qqM
? ? ) (
)
~
q
)(()) ,(2)((
2
1
KqGq
?
q
?
q
?
qCqM
?
q
?
V
P
TT
−−+−=τ
?
(15)
The first term on the righthand side is null because of the
skewsymmetry of matrix
controller (12) gives:
. Then, the choice of
CMN
2
−=?
(16)
q
?
Kq
?
V
D
T
?
−=
According to the above, the function candidate V decreases
as long as
that the system reaches an equilibrium posture when
and
0
≡−=
qqq
d
. The above rigorously shows that any
manipulator equilibrium posture is globally asymptotically
0
≠
q ?
for all system trajectories. It can be shown
0
≡
q ?
~
Figure 1. Mechanical model of robot in SimMechanics
Page 3
stable under a controller with a PD linear action and a
nonlinear gravity compensation action. Stability is ensured for
any choice of KP and KD, as long as these are positive definite
matrices. matrices.
stable under a controller with a PD linear action and a
nonlinear gravity compensation action. Stability is ensured for
any choice of KP and KD, as long as these are positive definite
0123456789 10
200
100
0
100
200
300
400
500
600
Link 1 Torque
Time [s]
Torque [Nm]
012345678910
50
0
50
100
150
200
Link 2 Torque
Time [s]
Torque [Nm]
III. RESULTS III. RESULTS
0123456789 10
0
0.5
1
1.5
2
Link 1 position
Time [s]
Angle [rad]
0
Figure 3. Link positions in the test 1 with online (solid) and
constant (dashed) gravity compensation
12345678910
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
Link 2 position
Angle [rad]
Time (s)
Figure 2 shows the complete Matlab model of the control
system. It includes the mechanical model and the PD controller
with gravity compensation. with gravity compensation.
Figure 2 shows the complete Matlab model of the control
system. It includes the mechanical model and the PD controller
Simulation tests have been carried out in order to measure
dynamic performance of the PD controller with online vs.
constant gravity compensation.
constant gravity compensation.
Simulation tests have been carried out in order to measure
dynamic performance of the PD controller with online vs.
The test 1 was done with a step change of the link positions
from the initial configuration q
configuration q
rad. The robot is initially in an
equilibrium state, under the action of the control torque equilibrium state, under the action of the control torque
)(
G
=
)(
G
=τ
. The gains of both controllers in (11) and (12) are
set to:
to the desired
to the desired
The test 1 was done with a step change of the link positions
from the initial configuration
configuration rad. The robot is initially in an
d
] 6/2/[
ππ=
] 6/2/[
ππ=
TT
i
] 0 ] 00 [ 0 [
==
iq
TT
d q
iq
iq
}260,260{
diagKP=
, } 80,80 {
diagKD=
And the results obtained are shown in figures 3 and 4.
Although both controllers achieve the correct steadystate
position, the transient with the online gravity compensation is
faster than with the constant gravity compensation. This is
achieved at the cost of just slightly higher motor torques at the
start of motion.
The test 2 was done with the inverse requested motion,
from rad to q
behavior are less evident. After extensive simulations, it has
been observed in general that, for a given set of gains, transient
performance with the online gravity compensation scheme is
usually better than with constant gravity compensation.
, differences in
T
iq
] 6/2/[
ππ=
T
d
] 0 0 [
=
Figure 2. Complete control system modelled with
Matlab/Simulink
Figure 4. Motor torques in the test 1 with online (solid) and
constant (dashed) gravity compensation
IV. CONCLUSIONS
This paper describes the process of modeling a simple robot
manipulator using PD control
compensation method. The use of SimMechanics as a tool to
model the mechanics of the robot allows the possibility to
verify model, as in real application, is unknown for the
designer so that the ability of the control method to match the
real model can be easily proven. That would allow the testing
of techniques like the dynamic modelbased control but also
learning techniques that extract the model of the plant under
consideration.
with online gravity
V. ACKNOWLEDGMENT
The authors would like to express financial supports from
Ministry of Knowledge Economy under Human Resources
Development Program for Convergence Robot Specialists.
Page 4
REFERENCES
[1] P. Tomei: “Adaptive PD controller for robot manipulators”,
IEEE Trans. Robotics Automat., Vol. 7, pp. 565570,1991.
[2] L. Sciavicco, B. Siciliano, Modelling and control of robot
manipulators (2nd ed.), London: Springer.
[3] Rafael Kelly: “PD Control with Desired Gravity Compensation of
Robotic Manipulator: A Review”, The International Journal of
Robotics Research 1997; 16; 660.
[4] John J. Craig, Introduction to Robotics Mechanics and control,
third edition, Pearson Prentice Hall, 2005.
[5] Krzysztof Kozlowski, Modelling and Identification in robotics,
Springer – London, 1998.