Trajectory-tracking control of a planar 3-RRRparallel manipulator

Chaman Nasa and Sandipan BandyopadhyayDepartment of Engineering Design

Indian Institute of Technology MadrasChennai, India

Abstract

Parallel manipulators have higher payload capacity, higher me-chanical rigidity and better accuracy than their serial counter parts.In order to utilize all these advantages, an effective control scheme isrequired. But the highly non-linear dynamic model and the presenceof passive links in parallel manipulators make it difficult to develop acontrol scheme which can handle these non-linearities and can be gen-erally applied to any parallel manipulator. This paper presents thetrajectory-tracking control scheme of a planar 3-RRR parallel manip-ulator for pre-determined singularity-free-paths. The control schemepresented in this paper is general and can be applied to any parallelmanipulator.

1 Introduction

The 3-RRR is a planar parallel manipulator with three-degrees-of-freedom.It can be used for many applications, for example, pick and place, profile-cutting etc.

The way the control scheme is developed in order to handle the highlynon-linear terms and coupling in the dynamic model of a parallel manipula-tor is very important. Dynamic model-based control scheme employing thecomputed torque control algorithm is successfully being used (see for e.g. [?],[?], [?]) in trajectory-tracking for parallel manipulators. We have developed asimilar control scheme on the 3-RRR manipulator such that it can be appliedto serial, parallel and hybrid manipulators.

In the following sections we present the kinematics, dynamics and controlsof the manipulator. We reduce the dynamic model in the full configuration

1

space to the actuator space, with which we can treat any parallel manipulatorequivalent to a serial manipulator. With this dynamic model, we commandthe manipulator to follow a particular path. We add disturbances to themodel simulating parametric uncertainties to make it more realistic. Thesame simulation we performed at higher speed with no disturbances added.Numerical results illustrate the formulations presented.

2 Kinematic model of the 3-RRR

In this section, we present the zeroth and first order kinematics of the ma-nipulator.

2.1 Geometry of the manipulator

The 3-RRR planar parallel manipulator is shown in figure (1).

Figure 1: Schematic of the 3-RRR planar parallel manipulator

The “end-effector” of the manipulator is an equilateral triangle, p1p2p3,with each side of length a, which is connected to a fixed base b1b2b3, whichalso forms an equilateral triangle with b as the length of each side. Thevertices pi are connected to the corresponding base points of manipulator biby three identical fingers. Each finger comprises of two links, of length li andri respectively, where i = 1, 2, 3. All joints are revolute joints. The actuatorsare located at bi, and therefore the links l1, l2, l3 are called the active linksand r1, r2, r3 the passive links. The platform has three degrees-of-freedom.These are the two translations in the plane parallel to the fixed base and arotation about the normal to this plane.

2

2.2 Forward kinematics

There are three active (i.e., actuated) variables represented as θ = (θ1, θ2, θ3)T

and four passive (i.e., non-actuated) variables represented as φ = (φ1, φ2, φ3, α)T .The full configuration space can be represented by q, where, q = (θT ,φT ).Note that the orientation of the moving platform, α, is included in the pas-sive variable φ, since like φ1, φ2, φ3, it is also determined indirectly throughthe solution of the forward kinematics of the manipulator.

2.2.1 Loop closure equations

The joint variables are related to the position and orientation of the end-effector by the constraints imposed by the joints. There are three possibleclosed-loops: (1) starting from point b1 to point b2 via points p1, p2, (2)starting from point b1 to point b3 via points p1, p3, (3) starting from pointb2 to point b3 via points p2, p3. The positions for all the points included inthe three loops are as follows:

b1 = (0, 0)T , b2 = (b, 0)T , b3 =

(b

2,

√3

2b

)T

p1 = (l1 cos θ1 + r1 cosφ1, l1 sin θ1 + r1 sinφ1)T

The position of the point p2 defined in terms of l1, r1, a, b1 is given by:

pb12 =(l1 cos θ1 + r1 cosφ1 + a cosα, l1 sin θ1

+ r1 sinφ1 + a sinα)T

The position of the point p2 defined in terms of l2, r2, b2 is given by:

pb22 =(r2 cosφ2 + l2 cos θ2 + xb2, r2 sinφ2

+ l2 sin θ2 + yb2)T

Similarly, the position of the point p3 starting from point b2 through l2, r2 isgiven by:

pb23 =(xb2 + l2 cos θ2 + r2 cosφ2 + a cos

(2π

3+ α

),

l2 sin θ2 + r2 sinφ2 + a sin

(2π

3+ α

))T

Starting from b3 the position of the point p3 through l3, r3 is given by:

pb33 =(r3 cosφ3 + l3 cos θ3 + xb3, r3 sinφ3

+ l3 sin θ3 + yb3)T

3

Considering the first loop, i.e., b1 to b2 via points p1,p2, the loop closure canbe written as:

pb12 − pb22 = 0 (1)

Writing the loop-closure equations in terms of X and Y coordinates as weget:

η1 =l1 cos θ1 + r1 cosφ1 + a cosα− r2 cosφ2

− l2 cos θ2 − xb2 = 0 (2)

η2 =l1 sin θ1 + r1 sinφ1 + a sinα− r2 sinφ2

− l2 sin θ2 = 0 (3)

Similarly, considering the second loop, i.e., b2 to b3 via points p2,p3, theloop closure can be written as:

pb23 − pb33 = 0 (4)

The corresponding loop-closure equations in terms of X and Y coordinatescan be written as:

η3 =bx2 + l2 cos θ2 + r2 cosφ2 + a cos

(2π

3+ α

)− r3 cosφ3 − l3 cos θ3 − xb3 = 0 (5)

η4 =l2 sin θ2 + r2 sinφ2 + a sin

(2π

3+ α

)− r3 sinφ3 − l3 sin θ3 − yb3 = 0 (6)

Put together, the loop-closure constraints can be written as:

η(q) = η(θ,φ) = 0, (7)

where1,

η = (η1, η2, η3, η4)T

The forward kinematics problem of the manipulator refers to finding thesolutions φ = φ(θ) of equation (7).

1We are not considering the last loop, as the constraint equations resulting from thethird loop are dependent on the other four independent constraint equations resultingfrom the rest of the two loops.

4

2.2.2 Solution of the loop-closure equations

Equations (2, 3) can be solved linearly for sinφ1 and cosφ1 to obtain:

cosφ1 =−l1 cos θ1 − a cosα + r2 cosφ2 + l2 cos θ2 + bx2

r1

sinφ1 =−l1 sin θ1 − a sinα + r2 sinφ2 + l2 sin θ2

r1(8)

From these, φ1 is obtained uniquely:

φ1 = atan2(sinφ1, cosφ1) (9)

where atan2(sin(·), cos(·)) represents the two-argument arc-tangent function.All the three fingers of the manipulator are considered as identical i.e., l1 =l2 = l3 = l and r1 = r2 = r3 = r. Eliminating φ1 by squaring and addingequations (8), we get an equation f1(θ, φ2, α) = 0, which is of the form:

A cosψ +B sinψ + C = 0, (10)

where,

A =− 2br3 + 2ar3 cosα + 2lr3 cos θ1 − 2lr3 cos θ2

B = 2ar3 sinα + 2lr3 sin θ1 − 2lr3 sin θ2

C = 2l2r2 cos(θ1 − θ2) + 2alr2 cos(α− θ2)− 2alr2 cos(α− θ1) + 2abr2 cosα + 2blr2 cos θ1

− 2blr2 cos θ2 − (a2 + b2 + 2l2)r2

Similarly, from equations (5, 6), we obtain φ3 and another equation f2(θ, φ2, α) =0, which is of the same form as equation (10), with A,B and C given as:

A =− br3 + ar3 cosα− 2lr3 cos θ2 + 2lr3 cos θ3

+√

3ar3 sinα

B =√

3br3 −√

3ar3 cosα + ar3 sinα− 2lr3 sin θ2

+ 2lr3 sin θ3

C =alr2 cos(α− θ2)− alr2 cos(α− θ3)+ 2l2r2 cos(θ2 − θ3) +

√3alr2 sin(α− θ2)

+√

3alr2 sin(α + θ3) +√

3blr2 sin θ2 −√

3bl2 sin θ3

+ blr2 cos θ3 + 2abr2 cosα− blr2 cos θ2

− (a2 + b2 + 2l2)r2

5

Incidentally, f1, f2 are linear in cosφ2, sinφ2 and therefore applying a sim-ilar procedure as above, we solve for φ2, and obtain the following equationcontaining only one unknown, α:

f(θ, α) = 0 (11)

This trigonometric equation is converted into an algebraic equation by sub-stituting the trignometric terms cosα and sinα as follows (see. e.g., [?]):

cosα =1− t2α1 + t2α

, sinα =2tα

1 + t2α,

where,

tα = tan(α

2

)Using the above relations, after rationalization and some symbolic manipu-lations, we get six degree polynomial in tα:

a0t6α + a1t

5α + a2t

4α + a3t

3α + a4t

2α + a5tα + a6 = 0 (12)

The coefficients, ai, i = 0, . . . , 6, are obtained as closed-form expressions ofthe architecture parameters (a, b, l, r) and the motors input, θ. For a givenset of numerical values of the inputs, equation (12) can be solved numericallyfor all the six values of tα. Therefore all the corresponding values of α and φican be obtained uniquely, leading to the 6 sets of solutions to the forwardkinematics problem.

2.3 Velocity kinematics

Referring to figure (1), coordinates of the center point p, of the movingplatform are function of the all the joint variables and can be expressed as:

p = p(q) = p(θ,φ) (13)

Differentiating equation (13) with respect to time to get:

p =∂p

∂θθ +

∂p

∂φφ

Or,

p = Jpθθ + Jpφφ (14)

6

where,

Jpθ =∂p

∂θ, Jpφ =

∂p

∂φ

From equation (14), to calculate p, we need to find out passive joint rates.In order to do so we differentiate equation (7) with respect to time to get:

∂η

∂θθ +

∂η

∂φφ = 0

⇒ φ =− ∂η

∂φ

−1∂η

∂θθ, det

(∂η

∂φ

)6= 0

⇒ φ =− J−1ηφJηθθ

where,

Jηθ =∂η

∂θ,Jηφ =

∂η

∂φ

Therefore, we can write:

φ = Jφθθ, Jφθ = −J−1ηφJηθ, det (Jηφ) 6= 0 (15)

Substituting φ in equation (14), we get:

p =Jpθθ + JpφJφθθ

p =Jvθ

where,

Jv = Jpθ + JpφJφθ (16)

As we can calculate φ using equation (15), we can always write:

p =∂p

∂qq = Jpqq, where Jpq =

∂p

∂q(17)

All the Jacobian matrices appearing above can be computed at any config-uration after the forward kinematics problem has been solved. Therefore,the linear velocity of any point of interest, can be obtained by the aboveprocess, if the joint velocity vector, θ, is known. With the knowledge of theposition and velocities, we now proceed to formulate the dynamic model ofthe manipulator.

7

2.4 Singularity condition

Due to its closed-loop structure, the 3-RRR has three types of singularities [?]i.e., (a) inverse or loss-type singularity, (b) forward or gain-type singularity,(c) combined singularity with loss-type and gain-type singularity together.

Among these three types, gain type singularity requires special attentionas it restricts the motion of the manipulator within the workspace. The con-dition for gain type singularity for 3-RRR manipulator is given in a simplifiedform in [?]:

det(Jηφ) =ar3{sin(φ1 − α) sin(φ2 − φ3)

+ sin(φ1 − φ2) sin

(α +

2π

3− φ3

)} = 0, (18)

where, Jηφ = ∂η∂φ

. We define another singularity measure, S(φ) which isindependent of the scale of the manipulator:

S(φ) =det(Jηφ)

ar3(19)

3 Dynamic model of the 3-RRR

The dynamic model is an essential part of the control scheme as it is used forfeedback linearisation of the equation of motion in the control scheme. Weuse the Lagrangian framework to develop the equation of motion.

3.1 Constrained Lagrangian formulation in the full con-figuration space

Let Ti be the kinetic energy of the ith link. Then the total kinetic energy Tof the manipulator can be written as:

T =7∑i=1

Ti, Ti =1

2vTpci

mivpci +1

2ωi

TIciωi, (20)

where, mi is the mass, vpci is the velocity of the mass center pci , ωi is theangular velocity and Ici is the moment of inertia of the ith link about itscenter with reference to the body-fixed frame. Using equation (17), we canwrite

vpci = Jpqiq, (21)

8

and similarly, ωi = Jωiq. Therefore, we rewrite Ti as:

Ti =1

2qTJTpqimiJpqiq +

1

2qTJTωiIciJωiq (22)

From equation (22), the mass matrix of the system can be obtained as:

M =7∑i=1

Mi (23)

where,M i = JTpqimiJpqi + JTωiIciJωi

Further, the matrix of centripetal and Corioli’s force, C, can be computedfrom the mass matrix (see, for e.g., [?]):

Cij =1

2

n∑k=1

(∂M ij

∂qk+∂M ik

∂qj− ∂M kj

∂qi

)qk (24)

With these elements, we can now put together the Lagrangian equation ofmotion for the manipulator as:

M (q)q +C(q, q)q = τ + τ c, (25)

where, τ represents the applied torques and τ c represents the constraintforces and moments associated with the loop-closure constraints. Note thatthe potential term G(q) representing the forces due to gravity is not takeninto account in equation (25) as the mechanism is considered in horizontalXY plane for all the simulations.

It is well-known that τ c can be written as JTηqλ (see, e.g., [?]), where:

Jηq =∂η

∂q

Therefore, the equation of motion of the manipulator can be written as:

M (q)q +C(q, q)q = τ + JTηqλ, (26)

where, λ is a vector of Lagrange multipliers. It is possible to explicitlyevaluate λ from equation (26) by obtaining q from it and substituting it inthe second derivative of the constraint equation η(q) = 0 (see, e.g., [?])

λ = −(JηqM−1JTηq)

−1{Jηqq + JηqM−1(τ −Cq)},

where,

det(JηqM−1JTηq) 6= 0.

However, it is also possible to eliminate λ without explicit evaluation (seee.g., [?]).

9

3.2 Reduction of the model to the actuator space

In order to eliminate λ from equation (26), first we rewrite it as:

M (q)q +C(q, q)q − τ − JTηqλ = 0 (27)

Pre-multiplying equation (27) by qT , we get:

qTM (q)q + qTC(q, q)q − qTτ − qTJTηqλ = 0 (28)

Note that:

dη

dt= 0⇒ Jηqq = 0, (29)

From equation (29),

qTJTηq = 0

Therefore, in equation (28), qTJTηqλ = 0 for any finite λ. This step, there-fore, effectively eliminates the constraint force term, and creates an equivalentunconstrained dynamic system as follows:

qTM(q)q + qTC(q, q)q − qTτ = 0, (30)

Using equation (15), we can write q as:

q =

(θ

φ

)=

(I3Jφθ

)θ = Jqθθ, (31)

where, I3 is the 3×3 identity matrix.Differentiating equation (31) with respect to time, we get:

q = Jqθθ + Jqθθ (32)

Substituting q, q from equation (31, 32), into equation (30), we get:

θT

(JTqθMJqθθ + JTqθ

(MJqθ +CJqθ

)θ

− JTqθτ ) = 0 (33)

Equation (33), is satisfied by any arbitrary θ and therefore,

JTqθMJqθθ + JTqθ

(MJqθ +CJqθ

)θ − JTqθτ = 0 (34)

10

Equation (34) can be rewritten more compactly as:

Mθθ +Cθθ = τ θ (35)

where Mθ = JTqθMJqθ is the mass matrix of the manipulator as reflected

on the active coordinates alone, Cθ = JTqθ

(MJqθ +CJqθ

)is the corre-

sponding centripetal and Corioli’s term, and τ θ = JTqθτ is the vector of jointtorques.

Expressed in this way, the dynamic model of the parallel manipulatoris no different from that of an equivalent serial manipulator with the sameactuators. It also has a compact form, i.e., it has only 3 ordinary differentialequations as opposed to 7 in the original system (26). However, there is a veryserious distinction, which is apparently buried in the matrix manipulationsabove. The model (35) is only valid when the mapping q = Jqθθ is well-defined, i.e., when Jφθ exists, i.e., when Jηφ is non-singular. Physically, thismeans that the model is valid only when the manipulator is not at a gain-type singularity. Further, integration of this model only updates the activevariable, θ, while the matrices Mθ,Cθ etc. are dependent on θ as well as φ.Therefore, to complete the update of the model, every integration step hasto be followed by a step of forward kinematics, so as to update the passivevariables φ. From an implementation point of view, this also introducesanother complexity: while computing the passive variables at any time step,one has to ensure that the branch of solution chosen (among the possible 6)is the same as the original, i.e., at the initial time. This branch-following inforward kinematics is essential in successfully implementing the equivalentmodel.

4 Formulation of the control scheme

In this section, we follow a model-based control approach for achieving trajec-tory tracking along a pre-computed non-singular-path. TheMθ,Cθ matricesin equation of motion of 3-RRR manipulator are highly non-linear. Applyinga linear control scheme is not expected to produce accurate results. There-fore, we have implemented a control scheme in which we use the estimate ofthe plant to cancel out the non-linear terms, through feedback linearisation.

11

4.1 Model-based trajectory-tracking control of the 3-RRR manipulator

4.1.1 Feedback linearisation

Given a trajectory we can calculate the desired set of active joints θd andtheir derivatives and θd (where subscript ‘d’ represents the desired value)from inverse kinematics.

Referring to figure (2). The desired and the initial value of θ and θis compared with the desired one to get error e(t) and e(t), where e(t) =(θ(t)− θd(t)). We apply a simple linear control strategy to get the value oftorque required to be provided to the motors to compensate this error andhence follow the desired trajectory.

4.1.2 Assumptions

• The model is deterministic with no parametric uncertainties.

• There are no unmodeled dynamics e.g. friction.

• Full-state feedback is available.

We implement the control law partitioning (see for e.g. [?]), by assumingthat the output of the controller can be written as:

τθ = ατ ′θ + β, (36)

where,

α = Mθ, β = Cθ (37)

Substituting τθ, α, β in equation (35), we get a linear equation:

τ ′θ = θ (38)

Linear control strategy can now be applied to the system. If we choose theτ ′θ as:

τ ′θ = θd(t)−Kpe(t)−Kve(t), (39)

the final error dynamic equation can be written as:

e(t) +Kpe(t) +Kve(t) = 0, (40)

where, Kp represents the proportional gain and Kv represents the derivativeor velocity gain.

12

4.1.3 Linear feedback

We assume Kp and Kv as two positive definite diagonal matrices with thespecific case of critical damping:

Kp = kpI3, Kv = kvI3, (41)

where, kv = 2√kp. The values of kp is chosen to be scalar.

4.2 Feedback loop

Figure 2: Model-based-dynamic control of 3-RRR manipulator

Referring to figure (2), α that we get by branch following is used toperform the forward kinematics to get the rest of the passive joint angles. Wehave the full set of configuration variables now with θ coming directly fromthe feedback and the passive variables φ coming from the branch-followingand forward kinematics.

5 Results of numerical simulations

The theoretical developments described above illustrated through a series ofnumerical simulations, as detailed in this section. We present the results ofthree different cases. The coordinates of the base points of the manipulatorare:

xb1 = yb1 = 0, xb2 = b, yb2 = 0, xb3 = b/2, yb3 = b/√

3,

where, b = 0.52. All the links are taken in proportion with the base linklength i.e., l = b/2, r = b/3 and a = b/4. The details of the geometric pa-

2All the linear entities are in meters and all the angles are in radians unless specifiedotherwise.

13

rameters and inertia of the manipulator are as follows:

link length mass (Kg) moment of inertia×10−3(Kg-m2)l 0.2500 0.4680 2.4380r 0.1667 0.3120 0.7220a 0.1250 0.2340 0.3050

The numerical simulations pertain to the tracking of a circle in the XYplane.

5.1 Tracing the non-singular circle

In the first case we trace a non-singular circle of radius rc = 0.05 and center at(xc, yc)

T = (0.25, 0.21)T with a average speed v = 0.2 m/sec. The desired ori-entation of the platform αd(t) is set to zero. The values of control gains, Kp

and Kv, are selected through numerical simulations and for this particularpath kp was taken as 1000. Initial configuration of the manipulator in termsof x, y, α is 0.20, 0.21, 0 respectively. The translational motion along thetrajectory is described by the third-order polynomial with respect to time.

Figure (3) shows that the circle traced is non-singular as it is not cut by

Figure 3: Desired circle superposed on the contours of singularity functionS(φ)

the zero-valued contour line corresponding to α = 0. Figure (5) which givesa clear measure of singularity along the trajectory. We can observe that the

14

Figure 4: Initial configuration of 3-RRR: x = 0.20, y = 0.21, α = 0

value of S(φ) stays away from zero along the trajectory indicating that thepath traced is singularity-free. Figure (7) shows the error in path-tracking.Figure (8) shows the performance of control over α along the trajectory. Thevalues of Kp and Kv are so chosen that along with a good control we alsoshould get feasible values of torques at all the motors. Figure (9) shows thevariation of torques at three motors installed at three base points of the ma-nipulator.

Figure 5: Variation of S(φ) along the trajectory

15

Figure 6: The path tracked vs. the desired path

Figure 7: Error in path-tracking

16

Figure 8: Variation of α along the trajectory

Figure 9: Variation of the joint torques

17

5.2 Tracing the non-singular circle with disturbancesadded

In the second case we trace the same circle as above with v = 0.2m/sec butnow with disturbances added into the plant. We incremented the masses andthe length of the links by 15% to introduce the errors into the model andrepeated the same simulation with Kp,Kv values kept same. Figure (10)and (11) show that the error in path-tracking has increased in comparison tothe first case, where there are no disturbances. Figure (12) shows the increasein torques values to compensate for these errors. We have kept the controlgains same as were in the first case, so as to present a clear comparison ofthe two cases. However, again tuning the control gains may result in lesserpath-tracking errors.

Figure 10: The path tracked vs. the desired path with 15% error in massand length of all the links

5.3 Tracing the non-singular circle with increased speed

In the third case we trace the same circle but with increased speed. Thistime we trace the circle with double the speed i.e., v = 0.4m/sec with nodisturbances. We can observe from figure (13) and (14) that the error inpath-tracking is much larger in comparison to the first case, where the track-ing speed was 0.2m/sec. Figure (15) shows that the torques are almost four

18

Figure 11: Error in path-tracking with 15% error in mass and length of allthe links

times of the previous value of torques at v = 0.2m/sec (see in figure (9)).

6 Conclusion

In this paper we have presented a trajectory-tracking control scheme for 3-RRR planar parallel manipulator. It is shown that the linear control schemeworks effectively for 3-RRR manipulator even with 15 percent error addedin lengths and masses. However, with increase in speed of the end-effectorthere are large amount of deviations from the desired path and hence thesame control gains do not work for the same trajectory. This requires thetuning of control gains, Kp,Kv. It is proposed that the same formulationcan be applied to any parallel manipulator to trace singularity-free-paths.

19

Figure 12: Variation of the joint torques with 15% error in mass and lengthof all the links

Figure 13: The path tracked vs. the desired path, with v = 0.4m/s

20

Figure 14: Error in path-tracking with v = 0.4m/s

Figure 15: Variation of the joint torques with v = 0.4m/s

21