This summary provides the key details about the document in 3 sentences or less:
The document describes developing a dynamic model and PID controller for a robotic arm with two degrees of freedom. It outlines deriving the Lagrangian equation to model arm dynamics and introduces a PID controller to apply torque to joints based on position error. The goal is to use PID control to move the arm to a desired position by minimizing error between actual and desired joint angles.
"I see eyes in my soup": How Delivery Hero implemented the safety system for ...
PID control dynamics of a robotic arm manipulator with two degrees of freedom.
1. PID control dynamics of a robotic arm manipulator with two
degrees of freedom.
David I. Robles G.
david.robles@lasallistas.org.mx
August 17, 2012
Abstract
Conventional robot control methods depend heav-
This paper presents a basic example of PID control ily upon accurate mathematical modelling, analysis,
applied to a robotic manipulator arm with two DOF and synthesis. Dynamic modelling of manipulators
(degrees of freedom), as well as the design of the dy- is a very active eld of research, it's can be used to
namic model, explaining in detail each step so that investigate the system responses and system proper-
in future work, we can increase the diculty level re- ties, like nding the stability of the system. Actually
gardless of DOF of the robot. Also shown how to the manipulators use proportional-derivative con-
introduce the PID controller parameters to the equa- trollers (PD) or proportional-integral-derivative
tion of the dynamics of the robot, left as future work controllers (PID) in closed-loop systems in order to
the implementation of a real-time control of these reach the desired congurations.
variables.
Keywords: PID control, Dynamic model, Robotic The proportional-integral-derivative (PID) control
manipulator arm. has simple structure and clear physical meanings for
its three gains. The control performances are accept-
able in the most of industrial processes. Most robot
1 Introduction manipulators employed in industrial operations are
The robot arm manipulators in recent years had controlled by PID algorithms independently at each
a slight growth in the industry. Because in stan- joint [3].
dard industrial the controllers don't include the
non-linearities between the joints of the robot and In practice, the tuning of PID controllers is easier
the problem of modelling the dynamics and motion for robots whose transmission system includes re-
control arise the complications of the system when duction mechanisms such as gears or bands. The use
computing the inertia tensor of a moving rigid of these reductions eectively increases the torque or
body. The purpose of robot arm control is to force produced by the actuators, and therefore, these
maintain the dynamic response of a computer-based are able to drive links of considerably large masses.
manipulator in accordance with some pre specied
system performance and goals. Most of the robot Many control algorithm such as computer torque
manipulators are driven by electric, hydraulic, or method, optimal control, variable structure control
pneumatic actuators, which apply torques (or forces, (VSC), neural networks (NNs) and fuzzy system have
in the case of linear actuators) at the joints of the been proposed by many researchers to deal with this
robot. problem. Our proposal is to use an approach of an
1
2. David I. Robles G. Control de Procesos y Robótica 2
adaptive tuning methods: These are for automated There are two approaches to dynamic robot model,
on-line tuning based on real-time identication. the direct and inverse dynamic model.
• The direct dynamic model expresses the tempo-
1.1 Dynamic model ral evolution of the robot joint coordinates as
The dynamic model of a robot is the relationship a function of the forces and torques involved
between the movement and the forces involved θ(t) = f (τ (t)).
in the robot. This establishes the mathematical • The inverse dynamic model determines the
relationship between the location of the robot joint forces and torques required to achieve a joint
variables, dened by its coordinates or location of evolution of the coordinates determined τ (t) =
its end, its derivatives (velocity and acceleration), g(θ(t)).
the forces or torques applied to the joints and the
dimensional parameters of the robot. There is also another alternative approach for ob-
taining the model using the Lagrangian formulation
This model relies heavily on the established bal- based on energetic considerations, that is therefore
ance of forces in Newton's second law (equation 1) or the dierence between kinetic energy (KE ) and po-
its equivalent for rotational movements, the so-called tential energy (PE ), this formulation greatly facili-
law of Euler (equation 2). tates the obtaining of a more complex model of a
robot. This equation is dened as
d
F = (mv) (1)
dt
L(q(t), q(t)) = KE (q(t), q(t)) − PE (q(t))
˙ ˙ (3)
d
T = Iω = I ω + ω × (Iω)
˙ (2)
dt With the Lagrangian L, we can solve the Euler-
Thus, in the simplest case of a robot with one link Lagrange equation which relies on the partial deriva-
shown in Figure (1), the balance of forces-torques tive of kinetic and potential energy properties of me-
would result in the following equation: chanical systems to compute the equations of motion
and is dened as follow
d2 θ d ∂L ∂L
τ − M gLcosθ = I ⇒ F = − (4)
dt2 dt ∂θ ∂θ
˙
¨
τ = M L2 θ + M gLcosθ
where F is the external force acting on the θ
generalized coordinate, represents the torque applied
to the robot and L is the Lagrangian equation of the
motion.
The kinetic energy equation is dened as
1
KE = mv 2 (5)
2
where m is the mass of link and v is the the
angular velocity of the joint. For simplicity, the mass
of each link is supposed to be punctual and located
at the end of the link.
Figure 1: Model of the link with concentrated mass.
The potential energy equation is dened as
August 17, 2012 Maestría en Ciencias Área Cibertrónica
3. David I. Robles G. Control de Procesos y Robótica 3
1.2 PID control
PE = mgl (6)
Considering the nonlinear equation that are derive
Where g is the gravity and l is the projection of from Euler-Lagrange equation (12), the control in-
the position vector of the link. put variable F which represents the torque applied
to the robot, is unknown, (we assumed that the con-
In the simplest case of a robot shown in gure (1) sidered robot actuators were ideal sources of torques
we obtain the following dynamic model and forces). However it's requires a control in the
torque applied of the joints to reach a nal position.
1 ˙
KE = I θ2 (7) Therefore we use the classical linear PID law as shown
2 below;
where I is the inertia tensor I = M L2 . Also
t
PE = M gh = M gLsenθ (8) F = Kp e + Ki e(τ )dτ + Kd e
˙ (13)
0
and substituting with the Lagrangian equation (3) where e = q d − q , q d is desired joint angle, Kp , Ki
we obtained the following expression. and Kd are proportional, integral and derivative gains
of the PID controller, respectively. This PID control
L(q(t), q(t)) = KE (q(t), q(t)) − PE (q(t))
˙ ˙ law can be expressed via the following equations.
1 (9)
˙ ˙
L(q(t), q(t)) = M L2 θ2 − M gLsenθ F = Kp e + Kd e + ξ
˙
2
˙
(14)
ξ = Ki e, ξ(0) = ξ0
Now we can calculate the force applied to the
generalized coordinate using the equation Euler- The integral action of the PID control law (14) in-
Lagrange troduces an additional state variable that is denoted
here by ξ and whose time derivative is ξ˙ = Ki e. The
∂L
= −M gLcosθ closed-loop equation is obtained by substituting the
∂θ control action F from (14) in the robot model (12).
∂L
˙
= ML θ2˙
(10)
∂θ
d ∂L ¨ M (q)¨ + C(q, q) + g(q) = Kp e + Kd e + ξ
q ˙ ˙ (15)
= M L2 θ
˙
dt ∂ θ
which may be written in terms of the state vector
and nally we get the torque applied to the joint. T
ξ T eT e˙
T, as
¨
F = M L2 θ + M gLcosθ (11)
Whatever the method is chosen to obtain the dy-
e
namic model of the robot, it has the following form ξ
d e
˙
of a nonlinear equation e = d
q − M (q)−1 [Kp e + Kd e + Ki ξ
dt ˙
e
˙
−C(q, q)q − g(q)
˙ ˙
F = M (q)¨ + C(q, q) + g(q)
q ˙ (12) (16)
where q ∈ R represents the link positions. n is
n The equilibrium of the equation above, if any, have
T
joint number or degrees of freedom, M (q) is the in- the form ξ T eT e˙T = ξ ∗T 0T 0T T where
ertia matrix, C(q, (q)) = {ckj } ∈ Rnxn represents
˙
centrifugate force, g(q) is vector of gravity torques
and F ∈ Rn is control input. −1
ξ ∗ = Ki M (q d )q d + C(q d , q˙d )q˙d + g(q d )
¨ (17)
August 17, 2012 Maestría en Ciencias Área Cibertrónica
4. David I. Robles G. Control de Procesos y Robótica 4
must be a constant vector. Certainly, for ξ ∗ to calculating the kinetic and potential energy. How-
be a constant vector, if the desired joint position q d ever, we don't know the velocity but we know that
is time-varying, it may not be arbitrary but should the derivative of an position respect to time is the ve-
have a very particular form. One way to obtain a q d locity. So we dene the position in the end of the link
for which ξ ∗ is constant, is by solving the dierential using the variables we already know, in other words,
equations we can put
x1 = L1 sinθ1
d qd q˙d y1 = L1 cosθ1
= (19)
dt q˙d M (q d )−1 F0 − C(q d , q˙d )q˙d − g(q d ) x2 = L1 sinθ1 + L2 sin(θ1 + θ2 )
y2 = L1 cosθ1 + L2 cos(θ1 + θ2 )
(18)
Then substituting in the equation of kinetic energy
qd 0 2n we obtain the following expression
∈
q˙d 0
where F0 ∈ n
is a constant vector. This way KE =
1 1 1 1
M1 x2 + M1 y1 + M2 x2 + M2 y2 (20)
˙1 ˙2 ˙2 ˙2
ξ = K i F0 .
∗ −1 2 2 2 2
and by simplication.
2 PID control dynamics of a
1 1
robotic arm manipulator KE =
2
(M1 + M2 )L2 θ˙1 + M2 L2 θ˙1 + M2 L2 θ˙1 θ˙2
1
2
2 2
2
2
1
For this experiment, what we want to control with the + M2 L2 θ˙2 + M2 L1 L2 cosθ2 (θ˙1 θ˙2 + θ˙1 )
2
2 2
PID is the force applied to the robot to take it to a 2
(21)
desired position using as parameter the error between And Potential energy is
the actual and the desired coordinate. To develop
the dynamic model we will consider the robot arm
manipulator with two degrees of freedom as shown in PE = M1 gL1 cosθ1 + M2 g(L1 cosθ1 + L2 cos(θ1 + θ2 ))
Figure 2, where M 1 = M 2 = 1 and L1 = L2 = 1. (22)
So we can now form the Lagrangian equation (3).
1 1
L= (M1 + M2 )L2 θ˙1 + M2 L2 θ˙1 + M2 L2 θ˙1 θ˙2
1
2
2
2
2
2 2
1
+ M2 L2 θ˙2 + M2 L1 L2 cosθ2 (θ˙1 θ˙2 + θ˙1 )
2
2 2
2
−M1 gL1 cosθ1 + M2 g(L1 cosθ1 + L2 cos(θ1 + θ2 ))
(23)
The next step is to form the Lagrange-Euler equa-
tion (4) with the Lagrangian (L) we already know, in
order to calculate the force applied to the robot.
d ∂L ∂L
Figure 2: Two-link planar manipulator. Fθ1,2 = − (24)
dt ∂θ1,2
˙ ∂θ1,2
So, the dynamic equations after simplications be-
First we need to solve the Lagrangian equation (3) come
August 17, 2012 Maestría en Ciencias Área Cibertrónica
5. David I. Robles G. Control de Procesos y Robótica 5
however, the physical torque inputs to the system are
Fθ1 = ((M1 + M2 )L2 + M2 L2 + 2M2 L1 L2 cosθ2 )θ1 ¨
1 2 f θ1 f1
¨ ˙ ˙ ˙ = M (q) (30)
+(M2 L2 + M2 L1 L2 cosθ2 )θ2 − M2 L1 L2 sinθ2 (2θ1 θ2 + θ2 )
2 2
f θ2 f2
−(M1 + M2 )gL1 sinθ1 − M2 gL2 sin(θ1 + θ2 ) The error signals of the system are
(25)
e(θ1 ) = θ1f − θ1
(31)
¨
Fθ2 = (M2 L2 + M2 L1 L2 cosθ2 )θ1 + M2 L2 θ2¨ e(θ2 ) = θ2f − θ2
2 2
(26)
˙ ˙
−M2 L1 L2 sin(θ2 )θ1 θ2 − M2 gL2 sin(θ1 + θ2 ) where θf is the nal positions. For the experiment, the
nal position is given by
So, we can describe the motion of the system by
the equation (12), θ1f
=
π
2 (32)
θ2f −π2
θ1
q=
θ2 and for the initial position is given by
D1 D2 −π
M (q) = θ0 = π
2 (33)
D3 D4 2
where with this information, we can design the general struc-
ture of PID controller for any input using the classical
linear law (13). So, the complete system equations with
D1 = ((M1 + M2 )L2 + M2 L2 + 2M2 L1 L2 cosθ2 )
1 2 control would be
D2 = (M2 L2 + M2 L1 L2 cosθ2 )
2
D3 = M2 L2 + M2 L1 L2 cosθ2
2 q = M (q)−1 [−C(q, q) − g(q)] + F
¨ ˙ ˆ (34)
D4 = M2 L22
with
˙ ˙ ˙2
−M2 L1 L2 sinθ2 (2θ1 θ2 + θ2 )
C(q, q) =
˙ ˙ ˙
−M2 L1 L2 sinθ2 θ1 θ2 f1 ˙
Kp1 (θ1f − θ1 ) − Kd1 θ1 + Ki1 e(θ1 )dt
ˆ
F = =
f2 ˙2 + K21
Kp2 (θ2f − θ2 ) − Kd2 θ e(θ2 )dt
(35)
−(M1 + M2 )gL1 sinθ1 − M2 gL2 sin(θ1 + θ2 )
g(q) =
−M2 gL2 sin(θ1 + θ2 ) Recalling the physical actual torques(30). However
in order to apply all controls of Proportional-Derivative-
Fθ1
Integral actions, a extra state is added for each angle to
F =
Fθ2
resemble the integration inside the computer:
Having the system equation, we can have x1 = e(θ1 )dt ⇒ x = θ1f − θ1
˙
(36)
x2 = e(θ2 )dt ⇒ x = θ2f − θ2
˙
q = M (q)
¨ −1 ˆ
[−C(q, q) − g(q)] + F
˙ (27)
with So, the system equations are
F = M (q)−1 F ⇔ F = M (q)F
ˆ ˆ (28)
x1 = θ1f − θf
˙
So, we decoupled the system to have the new (non-
x2 = θ2f − θf
˙
physical) input ¨
θ1
= M (q)−1 [−C(q, q) − g(q)] +
˙ ˆ
F
f1
¨
θ2
ˆ
F = (29) (37)
f2
August 17, 2012 Maestría en Ciencias Área Cibertrónica
6. David I. Robles G. Control de Procesos y Robótica 6
3 Results and Conclusions
Our rst approach to control the robotic manipulator arm
is by trial and error, the two parameters of the controllers
will be tuned manually to obtain the best performance.
The best performance of the controller parameters values
are shown below
Kp1 = 15
Kd1 = 7
Ki1 = 10
Kp2 = 15 Figure 5: Torque of Theta 1.
Kd2 = 10
Ki2 = 10
As shown in the gures (3) and (4), the position error
reaches zero at a considerably fast time,
Figure 6: Torque of Theta 2.
Figure 3: Theta 1 error.
By the experiments, we can notice how Kp is related
to direct error and to speed of evolution, Kd is related
to speed of interaction with change in states and Ki is
related to overall error cancellation. However the slight-
est change in the parameters of the controller produces
more overshoots and oscillations and are highly sensitive
to initial and nal positions.
As future work is proposed to use other techniques,
such as fuzzy controller systems that works only with the
position of the angle and torque response as well as an
online controller with auto tuning of the parameters Kp ,
Kd and Ki
Figure 4: Theta 2 error. References
[1] Reyes, Cortés, F. (2012) MATLAB Aplicado a
the force or torque applied can be seen in Figures (5) Robótica y Mecatrónica (1
st
Edition). México City.
and (6), slight overshoot or peaks but stabilize quickly Alfaomega.
August 17, 2012 Maestría en Ciencias Área Cibertrónica
7. David I. Robles G. Control de Procesos y Robótica 7
[2] Barrientos, A. Peñin, Luis, F. Balaguer, C. Aracil,
R. (2007) Fundamentos de Robótica (2nd Edition).
México City. McGrawHill.
[3] M.W.Spong and M.Vidyasagar (1989) Robot Dy-
namics and Control Canada. John Wiley Sons Inc.
[4] R. Kelly, V. Santibáñez and A. Loría (2005) Control
of Robot Manipulators in Joint Space (2 Edition).
nd
Springer.
August 17, 2012 Maestría en Ciencias Área Cibertrónica