PID control dynamics of a robotic arm manipulator with two degrees of freedom.

10,550 views

Published on

This paper presents a basic example of PID control applied to a robotic manipulator arm with two DOF (degrees of freedom), as well as the design of the dynamic model, explaining in detail each step so that in future work, we can increase the diculty level regardless of DOF of the robot. Also shown how to introduce the PID controller parameters to the equation of the dynamics of the robot, left as future work the implementation of a real-time control of these variables.

Published in: Technology, Business

PID control dynamics of a robotic arm manipulator with two degrees of freedom.

  1. 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, 2012Abstract 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, its can be used tonamic 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. Actuallygardless 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-derivativetion of the dynamics of the robot, left as future work controllers (PID) in closed-loop systems in order tothe implementation of a real-time control of these reach the desired congurations.variables. Keywords: PID control, Dynamic model, Robotic The proportional-integral-derivative (PID) controlmanipulator 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 robot1 Introduction manipulators employed in industrial operations areThe robot arm manipulators in recent years had controlled by PID algorithms independently at eacha slight growth in the industry. Because in stan- joint [3].dard industrial the controllers dont include thenon-linearities between the joints of the robot and In practice, the tuning of PID controllers is easierthe 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 usecomputing the inertia tensor of a moving rigid of these reductions eectively increases the torque orbody. The purpose of robot arm control is to force produced by the actuators, and therefore, thesemaintain the dynamic response of a computer-based are able to drive links of considerably large masses.manipulator in accordance with some pre speciedsystem performance and goals. Most of the robot Many control algorithm such as computer torquemanipulators are driven by electric, hydraulic, or method, optimal control, variable structure controlpneumatic actuators, which apply torques (or forces, (VSC), neural networks (NNs) and fuzzy system havein the case of linear actuators) at the joints of the been proposed by many researchers to deal with thisrobot. problem. Our proposal is to use an approach of an 1
  2. 2. David I. Robles G. Control de Procesos y Robótica 2adaptive 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 asThe dynamic model of a robot is the relationship a function of the forces and torques involvedbetween the movement and the forces involved θ(t) = f (τ (t)).in the robot. This establishes the mathematical • The inverse dynamic model determines therelationship between the location of the robot joint forces and torques required to achieve a jointvariables, 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 thedimensional 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 thereforeance of forces in Newtons 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 asAugust 17, 2012 Maestría en Ciencias Área Cibertrónica
  3. 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 torqueswe obtain the following dynamic model and forces). However its 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 , Kiwe 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 thegeneralized 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-     enamic 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 Tjoint number or degrees of freedom, M (q) is the in- the form ξ T eT e˙T = ξ ∗T 0T 0T T whereertia matrix, C(q, (q)) = {ckj } ∈ Rnxn represents ˙centrifugate force, g(q) is vector of gravity torquesand 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. 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 dont know the velocity but we know thatis 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 linkfor which ξ ∗ is constant, is by solving the dierential using the variables we already know, in other words,equations we can put x1 = L1 sinθ1d 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 1For this experiment, what we want to control with the + M2 L2 θ˙2 + M2 L1 L2 cosθ2 (θ˙1 θ˙2 + θ˙1 ) 2 2 2PID 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 isthe actual and the desired coordinate. To developthe dynamic model we will consider the robot armmanipulator 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) comeAugust 17, 2012 Maestría en Ciencias Área Cibertrónica
  5. 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 bythe 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) f2August 17, 2012 Maestría en Ciencias Área Cibertrónica
  6. 6. David I. Robles G. Control de Procesos y Robótica 63 Results and ConclusionsOur rst approach to control the robotic manipulator armis by trial and error, the two parameters of the controllerswill be tuned manually to obtain the best performance.The best performance of the controller parameters valuesare 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 errorreaches 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. 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

×