In this paper, we design a robust, fast, and practical proportional-integral-derivative (PID) controller for the classical double pendulum system. We first derive the equations of motion for the two-link robot manipulator using the Lagrangian approach. These equations are described by nonlinear system of ordinary differential equations. Because closed form solutions of the equations of motion are not available, we use the classical fourth-order Runge-Kutta method to approximate the solution of the initial-value problem. Because of the nonlinear behavior, it is a challenging task to control the motion of the two-link robot manipulator at accurate position defined by the user. For this, we focus mainly on control of the robot manipulator to get the desired position using the computed torque control method. After deriving the equation of motion, control simulation is represented using MATLAB. Several computer simulations are used to verify the performance of the controller. In particular, we present a PID controller to simulate how we would balance the two-links on a moving robot to any specific angle including upside-down.

Two-link robot manipulator, Robotics, PID controller, Dynamic control, Simulation

Robotic manipulators are a major component in the manufacturing industry. They are used for many reasons including speed, accuracy, and repeatability. Increasingly, robotic manipulators are finding their way into our everyday life. In fact in almost every product we encounter, a robotic manipulator has played a part in its production. In robotics, one of the most difficult tasks is to perform a precise and fast movement of a robotic arm. In this paper, one control model is proposed to solve this problem.

The double pendulum system is a common and typical model to investigate nonlinear dynamics due to its various and complex dynamical phenomenon. The double pendulum is an example of a simple dynamical system that exhibits complex behavior, including chaos. It consists of two point masses at the end of light rods. Each mass plus rod is a regular simple pendulum. The two pendula are joined together and the system is free to oscillate in a plane. Two-link manipulators are two-degree-of-freedom robots. They are useful because they function like human arms. We can begin to understand the complex movement of human arms after defining the movement of the two-link manipulator [1]. The total potential and kinetic energies of the two-link system are defined and used to form the Lagrangian. The Euler-Lagrangian equation was used to define the torques applied to each link [2]. A proportional-integral-derivative (PID) controller is implemented to simulate the behavior of the system when torques are applied [3]. The PID controllers are the most common used controllers, due to their easy implementation and versatility. PID controllers remain the standard for industry and research given their flexibility and ease of use in both simulation and hardware systems [4-6].

Due to the nonlinear nature of the two-link manipulator, specialized control systems are often required to maintain a position or perform a task [7]. Researchers have found success using Sliding Mode Control compared to PID [8]. Others have used Artificial Neural Networks and Holographic Neural Networks as part of their PID feedback loop to perform the task of folding paper [9,10]. The proposed solution uses the command ode45 in MATLAB to solve for the angle of each link relative to the positive X-axis. PID control is developed and considered to show how you can force the system to specific angles. We also show graphical results of the system after the PID control is applied.

In this paper, we propose a PID controller for a double pendulum system. We derive the equations of motion for a two-link robot manipulator based on Lagrangian Formulation. The equation of motion for two link robot is a nonlinear differential equation. As the closed form solutions are not available we have to use numerical solution. After deriving the equation of motion, control simulation is represented using MATLAB. We expand upon the standard two-link manipulator problem, to that of an upside-down pendulum with multiple links on top of a moving object. In this case we look to implement the control system into a mobile robot called the Computer and Electronics Engineering Robot (CEEnBot).

The structure of this paper is as follows. In section 4, we derive the equations of motion for a two-link robotic manipulator using the Lagrangian method. We also describe how to obtain the numerical solution using the ode45 command defined in MATLAB to solve the system of ordinary differential equations numerically. We explain the design of the PID controller for the double pendulum system in section 5. In section 6, we present several numerical examples to test the PID controller. Finally, we provide some concluding remarks in section 7.

Double pendula are an example of a simple physical system which can exhibit chaotic behavior. Understanding the two-link manipulator is key to learning robotic manipulators as a whole. We consider the two-link robotic manipulator which is a classic example studied in introductory robotics courses. The physical system is shown in Figure 1.

The system consists of two masses connected by weightless bars. The bars have lengths L1 and L2. The masses are denoted by M1 and M2, respectively. Let θ1 and θ2 denote the angles in which the first bar rotates about the origin and the second bar rotates about the endpoint of the first bar, respectively. This system has two degrees of freedom θ1 and θ2.

The first step in deriving the equations of motion using the Lagrangian approach is to find the kinetic energy KE and the potential energy PE of the system.

The equations for the x-position and the y-position of M1 are given by

x1 = L1cos(θ1), y1 = L1sin(θ1) (4.1)

Similarly, the equations for the x-position and the y-position of M2 are given by

x2 = L1cos(θ1) + L2cos(θ2), y2 = L1sin(θ1) + L2sin(θ2) (4.2)

Next, we calculate the velocities of M1 and M2 using the following formulas

v1 = x˙21 + y˙21−−−−−−−√, v2 = x˙22 + y˙22−−−−−−−√, (4.3)

where,

x˙1 = − L1θ˙1sin(θ1), y˙1 = L1θ˙1cos(θ1),

x˙2 = − L1θ˙1sin(θ1) − L2θ˙2sin(θ2), y˙2 = L1θ˙1cos(θ1) + L2θ˙2cos(θ2)

Here and below the dot ˙ is a derivative with respect to t, i.e., θ˙k = dθkdt,x˙k = dxkdt, for k = 1, 2.

The kinematic energy can be calculated as follows

KE = 12M1v21 + 12M2v22 (4.4)

The equation for the kinetic energy can be written as

KE = 12M1(x˙21 + y˙21) + 12M2(x˙22 + y˙22) (4.5)

Substituting (4.1) - (4.2) into (4.5), we get

KE = 12M1((− L1θ˙1sin(θ1))2 + (L1θ˙1cos(θ1))2) + 12M2((− L1θ˙1sin(θ1) − L2θ˙2sin(θ2))2 + (L1θ˙1cos(θ1) + L2θ˙2cos(θ2))2),

which can be simplified as

KE = 12(M1 + M2)L21θ˙21 + 12M2L22θ˙22 + M2L1L2θ˙1θ˙2cos(θ1 − θ2) (4.6)

In order to calculate the Lagrangian, the potential energy PE has to be calculated. By definition the potential energy of the system due to gravity of the ith pendulum is

PEi(θ) = Mighi(θ), i=1,2, (4.7)

which hi is the height of the center of mass of the ith pendulum, g is the acceleration due to gravity constant, and Mi is the mass of the ith pendulum. Therefore, the total potential energy for both pendulums can be given by

PE = PE(θ) = ∑2i = 1PEi(θ) = ∑2i = 1Mighi(θ) = M1gL1sin(θ1) + M2g(L1sin(θ1) + L2sin(θ2)) (4.8) = (M1 + M2)gL1sin(θ1) + M2gL2sin(θ2)

Next, by Lagrange Dynamics, we form the Lagrangian L which is defined as

L = KE−PE (4.9)

Substituting the expressions for the kinetic energy (4.6) and potential energy (4.8) in for KE and

PE we get

L = 12(M1 + M2)L21θ˙21 + 12M2L22θ˙22 + M2L1L2θ˙1θ˙2cos(θ1 − θ2) − (M1 + M2) (4.10)gL1sin(θ1) − M2gL2sin(θ2).

The Euler-Lagrange equation is given by the equation

ddt[∂L∂θ˙i] − ∂L∂θi = τi, i=1,2, (4.11)

where τi is the torque applied to the ith link.

The derivations for the Lagrangian equation and Euler-Lagrange equation can be found in [1]. We were more concerned with using the formulas in this problem rather than going into too much in detail for these derivations.

From (4.10), we have

∂L∂θ˙i = (M1 + M2)L21θ˙1 + M2L1L2θ˙2cos(θ1 − θ2),

∂L∂θ1 = − M2L1L2θ˙1θ˙2sin(θ1 − θ2) − (M1 + M2)gL1cos(θ1),

and

ddt[∂L∂θ˙i] = (M1 + M2)L21θ¨1 + M2L1L2θ¨2cos(θ1 − θ2) − M2L1L2θ˙2(θ˙1 − θ˙2)sin(θ1 − θ2)

Similarly, we compute

∂L∂θ˙2 = M2L22θ˙2 + M2L1L2θ˙1cos(θ1 − θ2),

∂L∂θ2 = M2L1L2θ˙1θ˙2sin(θ1 − θ2) − M2gL2cos(θ2),

and

ddt[∂L∂θ˙2] = M2L22θ¨2 + M2L1L2θ¨1cos(θ1 − θ2) − M2L1L2θ˙1(θ˙1 − θ˙2)sin(θ1 − θ2)

Therefore, (4.11) gives the following two nonlinear equations of motion which are second-order system of ordinary differential equations

(M1 + M2)L22θ¨1 + M2L1L2θ¨2cos(θ1 − θ2) + (4.12a)M2L1L2θ˙22sin(θ1 − θ2) + (M1 + M2)gL1cos(θ1) = τ1,

M2L22θ¨2 + M2L1L2θ¨2cos(θ1 − θ2) − M2L1L2θ˙21sin(θ1 − θ2) + M2gL2cos(θ2) = τ2, (4.12b)

or equivalently,

L1θ¨1 + δL1L2θ¨2cos(θ1 − θ2) = δτ1M2L1 − δL2θ˙22sin(θ1 − θ2) − gcos(θ1), (4.13a)

L2θ¨2 + L1θ¨1cos(θ1 − θ2) = τ2M2L2 + L1θ˙21sin(θ1 − θ2) − gcos(θ2), (4.13b)

where, δ = M2M1 + M2.

Solving for θ¨1 and θ¨2, we get the normal form of the dynamics equations

θ¨1 = g1(t,θ1,θ2,θ˙1,θ˙2), θ¨2 = g2(t,θ1,θ2,θ˙1,θ˙2), (4.14a)

where,

g1 = δτ1M2L1 − δL2θ˙22sin(θ1 − θ2) − gcos(θ1) − δcos(θ1 − θ2)(τ2M2L2 + L1θ˙21sin(θ1 − θ2) − gcos(θ2))L1(1 − δcos2(θ1 − θ2)), (4.14b)

g2 = τ2M2L2 + L1θ˙21sin(θ1 − θ2) − gcos(θ2) − cos(θ1 − θ2)(δτ1M2L1 − δL2θ˙22sin(θ1 − θ2) − gcos(θ1))L2(1 − δcos2(θ1 − θ2)). (4.14c)

In order to solve for the angles θ1 and θ2, we need to solve the above second-order system of ordinary differential equations. To do this, we first reduce the system into an equivalent system of first-order ordinary differential equations.

Let us introduce four new variables

u1 = θ1,u2 = θ2,u3 = θ˙1,u4 = θ˙2. (4.15)

After differentiating, we have

u˙1 = θ˙1 = u3, u˙2 = θ˙2 = u4, u˙3 = θ¨1 = g1(t,u1,u2,u3,u4), u˙4 = θ¨2 = g2(t,u1,u2,u3,u4)

Thus, we obtain a system of first-order nonlinear differential equations of the form

dUdt = S(t,U), U(0) = U0, (4.16a)

Where U = [u1,u2,u3,u4]t and S = [s1,s2,s3,s4]t with

s1 = u3, s2 = u4, s3 = g1(t,u1,u2,u3,u4), s4 = g2(t,u1,u2,u3,u4)

The initial conditions are given by

U0 = [u1(0), u2(0),u3(0),u4(0),]t, (4.16b)

Where,

u1(0) = θ1(0), u2(0) = θ2(0), u3(0) = θ˙1(0), u4(0) = θ˙2(0)

The system (4.16a) subject to the initial condition (4.16b) can be solved for the unknown vector U, using, for example, the ode45 command defined in MATLAB to solve the system of ordinary differential equations numerically. This command is based on the fourth-order Runge-Kutta method. For more details consult [2,11].

Robotic manipulators are generally difficult to control. In particular, it is a challenging task to stabilize a robot manipulator at a fixed, accurate position. In this section, we focus mainly on control of the robot manipulator to get the desired position using computed torque control method. After deriving the equation of motion, control simulation is represented using MATLAB.

We define control as the ability to hold the system of two links in a particular position on the xy-plane. Having control gives us the ability to hold each link at a particular angle θi with respect to the positive x-axis. The proportional-integral-derivative (PID) controller is a common control algorithm. The "P" in PID stands for Proportional control, the "I" stands for Integral control, and the "D" stands for Derivative control. This algorithm works by defining an error variable Verror = Vset - Vsensor that takes the position we want to go (Vset) minus the position we are actually at (Vsensor). We get the proportional part of the PID control by taking a constant defined as KP and multiplying it by the error. The I comes from taking a constant KI and multiplying it by the integral of the error with respect to time. Derivative control is defined as a constant KD multiplied by the derivative of the error with respect to time. Many industrial processes are controlled using PID controllers. Below is a table describing PID control Table 1.

The equations of motion (4.12) can be written compactly as

M(θ)θ¨+c(θ,θ˙)+G(θ)=F, (5.1)

where,

θ = [θ1θ2],M(θ)=[(M1+M2)L21M2L1L2cos(θ1 - θ2)M2L1L2cos(θ1 - θ2)M2L22],

c(θ,θ˙)=[M2L1L2θ˙22sin(θ1 - θ2) - M2L1L2θ˙21sin(θ1 - θ2)], G(θ)=[(M1+M2)gL1cos(θ1)M2gL2cos(θ2)],F = [τ1τ2]

We can solve for some theoretical values of forces given certain initial inputs. Solving for θ¨ we get

θ¨=- M− 1(θ)[c(θ,θ˙)+G(θ)]+Fˆ, (5.2)

Where

Fˆ=M− 1(θ)F

Thus, we decoupled the system to have the new input

Fˆ=[f1f2]

However, the physical torque inputs to the system arė

F=[τ1τ2]=M(θ)[f1f2] (5.3)

Let us denote the error signals by

e(θ1)=θ1f-θ1, e(θ2)=θ2f-θ2,

Where the target positions of M1 and M2 are given by the angles θ1f and θ2f, respectively.

We assume that the system has initial positions

θ0=[θ1(0)θ2(0)]

A common technique for controlling a system with input is to use the following general structure of PID controller

f=KPe + KDe˙ + K1∫edt. (5.4)

In our situation, the technique for controlling the double pendulum system with inputs f1 and f2

is to employ two independent controllers, one for each link, as follows

f1=KP1e1(θ1) + KD1e˙1(θ1) + KI1∫e(θ1)dt=KP1(θ1f − θ1) − KD1θ˙1 + KI1∫(θ1f − θ1)dt,

f2=KP2e2(θ2) + KD2e˙2(θ2) + KI2∫e(θ21)dt=KP2(θ2f − θ2) − KD2θ˙2 + KI2∫(θ2f − θ2)dt,

Where θ1f and θ2f are given constants.

The complete system of equations with control is then

θ¨=− M− 1(θ)[c(θ,θ˙)+G(θ)]+Fˆ, (5.5)

Where

Fˆ=[f1f2]=⎡⎣⎢⎢⎢KP1(θ1f - θ1)− KD1θ˙1 + KI1∫(θ1f - θ1)dtKP2(θ2f - θ2)− KD2θ˙2 + KI2∫(θ2f - θ2)dt⎤⎦⎥⎥⎥

We would like to emphasize that the actual physical torques are

[τ1τ2]=M(θ)[f1f2]

To implement the PID controller, we introduce the following new states

x1=∫e(θ1)dt, x2=∫e(θ2)dt

Differentiating with respect to t gives

x˙1=e(θ1)=θ1f − θ1, x˙2=e(θ2)=θ2f − θ2

The complete equations are

x˙1=θ1f − θ1, (5.6)

x˙2=θ2f − θ2, (5.7)

[θ¨1θ¨2] = - M− 1(θ)[c(θ,θ˙) + G(θ)] + [KP1(θ1f - θ1) - KD1θ˙1 + KI1x1KP2(θ2f - θ2) - KD2θ˙2 + KI2x2], (5.8)

[τ1τ2] = M(θ)[KP1(θ1f - θ1) - KD1θ˙1 + KI1x1KP2(θ2f - θ2) - KD2θ˙2 + KI2x2] (5.9)

To discretize the above system of differential equations in time, we transform them into a system of first-order ordinary differential equations. To do this, we define six new variables as follows

u1 = x1, u2 = x2, u3 = θ1, u4 = θ2, u5 = θ˙1, u6 = θ˙2, (5.10)

After differentiating, we have

u˙1 = x˙1 = θ1f − u3, u˙2 = x˙2 = θ2f − u4,

u˙3 = θ˙1 = u5, u˙4 = θ˙2 = u6,

u¨5 = θ¨1 = ϕ(t,u1,u2,u3,u4,u5,u6), u¨6 = θ¨2 = ψ(t,u1,u2,u3,u4,u5,u6),

were ϕ and ψ are expressed in terms of uk, k = 1-6, as

[ϕψ] = - M− 1(θ)[c(θ,θ˙) + G(θ)] + [KP1(θ1f - u3) - KD1u5 + KI1u1KP2(θ2f - u4) - KD2u6 + KI2u2],

and θ = [u3u4].

A simple calculation shows that

ϕ = − M2L2u26sin(u3 − u4) − (M1 + M2)gcos(u3) − M2cos(u3 − u4)(L1u25sin(u3 − u4) − gcos(u4))L1(M1 + M2 − M2cos2(u3 − u4)) + KP1(θ1f − u3) − KD1u5 + KI1u1,

ψ = cos(u3 − u4)(M2L2u26sin(u3 − u4) + (M1 + M2)gcos(u3)) + (M1 + M2)(L1u25sin(u3 − u4) − gcos(u4))L2(M1 + M2 − M2cos2(u3 − u4))+ KP2(θ2f − u4) − KD2u6 + KI2u2

Thus, we obtain a system of first-order nonlinear differential equations of the form

dUdt = H(t,U), U(0) = U0, (5.10)

Where U = [u1,u2,u3,u4,u5,u6]t and H = [h1,h2,h3,h4,h5,h6]t with

h1 = θ1f − u3, h2 = θ2f − u4, h3 = u5, h4 = u6,

h5 = ϕ(t,u1,u2,u3,u4,u5,u6), h6 = ψ(t,u1,u2,u3,u4,u5,u6)

The initial conditions are given by

U0 = [u1(0), u2(0), u3(0), u4(0), u5(0), u6(0)]t,

Where,

u1(0) = x1(0), x2(0) = θ2(0), u3(0) = θ1(0), u4(0) = θ2(0), u5(0) = θ˙1(0), u6(0) = θ˙2(0)

The system (3.10) can be solved for the unknown vector U, using, for example, the ode45 command defined in MATLAB to solve the system of ordinary differential equations numerically. This command is based on the fourth-order Runge-Kutta method. For more details consult [2,11].

Once we solve for, we obtain to torques using U = (u1,u2,u3,u4,u5,u6)t,

[ τ1 τ2 ] = M(θ)[ KP1(θ1f - u3) - KD1u5 + KI1u1 KP2(θ2f - u4) - KD2u6 + KI2u2 ], (5.11)

or equivalently,

τ1 = (M1 + M2)L21(KP1(θ1f − u3) − KD1u5 + KI1u1) + M2L1L2cos(u3 − u4)(KP2(θ2f − u4) − KD2u6 + KI2u2),

τ2 = M2L1L2cos(u3 − u4)(KP1(θ1f − u3) − KD1u5 + KI1u1) + M2L22(KP2(θ2f − u4) − KD2u6 + KI2zu2)

The PID controller helps get the outputs, which are the final positions of M1 and M2 determined by the angles θ1 and θ2, where we want it, in a short time, with minimal overshoot, and with little error. This will be demonstrated using the following examples.

Example 1. We consider the simplified model of a two-link manipulator shown in Figure 1. In this experiment, we take the following parameters

M1 = M2 = 1, L2 = 2, L2 = 1 (6.1)

The target positions (final positions) are [θ1fθ2f] = [π/20].

The initial positions, initial angle velocities, and initial states are, respectively, taken as

[θ1(0)θ2(0)] = [π/2π/2], [θ˙1(0)θ˙2(0)] = [00], [x1(0)x2(0)] = [00]

The PID parameters for θ1 and θ2, are taken as

KP1 = 30, KD1 = 15, KI1 = 20,

KP2 = 30, KD2 = 10, KI2 = 20.

In Figure 2, we show the initial and target positions of the two-link manipulator. Next, we plot the positions θ1 and θ2 of M1 and M2 versus time over the interval [0,30] in Figure 3.

In Figure 4 we present the difference between where we want to go and where we are actually at i.e.,

e(θ1) = θ1f − θ1, e(θ2) = θ2f − θ2,

Where θ1f and θ2f are the target positions and θ1(t) and θ2(t) are the numerical approximations obtained by solving the system of ODEs. These results indicate that the PID controller gets the final positions of M1 and M2 determined by the angles θ1 and θ2, in a short time (almost 7 seconds) with very small error.

Finally, in Figure 5, we show the torques f1 and f2 versus time

Note that the actual joints torques are

τ 1 τ 2 = M θ f 1 f 2

Example 2. We repeat the previous example with all parameters kept unchanged except for the final positions where we use

[θ1fθ2f] = [π/4− π/4] (6.2)

The initial and target positions of M1 and M2 are shown in Figure 6. We present the positions θ1 and θ2 of M1 and M2 versus time in Figure 7. In Figure 8 we present the errors between the estimate positions and target positions. Finally, in Figure 9, we show the torques versus time. Once again, these results demonstrate that the PID controller gets the final positions of M1 and M2 in a short time (approximately 5 seconds).

Example 3. In this example, we consider the two-link manipulator shown in Figure 1 with the following parameters

M1 = M2 = 1, L2 = 2, L2 = 1 (6.3)

The initial positions, initial angle velocities, and initial states are, respectively, taken as

[θ1(0)θ2(0)] = [−π/2 π/2] , [θ˙1(0)θ˙2(0)] = [00] , [x1(0)x2(0)] = [00].

The target positions (final positions) are [θ1fθ2f] = [00].

The PID parameters for θ1 and θ2, are taken as

KP1 = 30, KD1 = 15, KI1 = 20,

KP2 = 30, KD2 = 20, KI2 = 20.

The initial and target positions of the two-link manipulator are shown in Figure 10. In Figure 11 we present the positions θ1 and θ2 of M1 and M2 versus time over the interval [0,30]. In Figure 12 we present the errors e(θ1) and e(θ2) between the target position and the estimate position found numerically. Again it takes only few seconds to get the target position. We would like to mention that the PID controller gets the final positions of M1 and M2 in a short time (approximately in seconds) with very small error. Finally, we show the torques f1 and f2 versus time in Figure 13.

Example 4. Finally, we consider the two-link manipulator shown in Figure 1 with the following parameters

M1 = M2 = 1, L2 = 2, L2 = 1 (6.4)

The initial positions, initial angle velocities, and initial states are, respectively, taken as

[θ1(0)θ2(0)] = [− π/2− π/2] [θ1(0)θ2(0)] = [π/2π/2][x1(0)x2(0)] = [00]

The target positions (final positions) are [θ1fθ2f] = [00].

The PID parameters for θ1 and θ2, are taken as

KP1 = 30, KD1 = 15, KI1 = 20,

KP2 = 30, KD2 = 20, KI2 = 20.

The initial and target positions of the two-link manipulator are shown in Figure 14. The positions θ1 and θ2 of M1 and M2 versus time over the interval [0, 30] are shown in Figure 15. In Figure 16 we plot the errors e(θ1) and e(θ2) between the target position and the approximate position found numerically. As in the previous examples, it takes only few seconds to get the target position. Finally, we present the torques f1 and f2 versus time in Figure 17.

In this paper, a two-link robotic manipulator was studied and its dynamics were modeled using Lagrange mechanics. First, we derived the dynamical equation of the two-link robot manipulators using Euler-Lagrange method and then a simple and efficient control scheme was developed. More specifically, a robust Proportional-Integral-Derivative (PID) controller was introduced to control the motion of the robot at a specific position. In particular, we described how a PID controller can be used to keep the links in a desired position. The efficiency of the PID controller is verified by the simulation results. The proposed research can be used to design and code control algorithms for a mobile robot. These algorithms allow us to balance upside-down pendulums on a mobile robot. There could be many solutions for coding this PID control depending on what sensors are used to find the angles of the links. In this study, we considered a planar robot having rotational joints. However, the PID controller presented in this paper can be extended for robots having prismatic joints. In the future, we are planning to consider complex joints such as a ball and socket joint.

This project was funded by the Kerrigan Research Minigrant Program. The authors would like to thank the anonymous reviewer for the valuable comments and suggestions which improved the quality of the paper.

The Authors declare that there is no conflict of interest.