using the transformation matrices not only to derive the ... · the whole sys-tem has, therefore,...

Using the Transformation Matrices Not Only to Derive the MotionEquations

JIRI ZATOPEKTomas Bata University in Zlin

Department of Automatic and Control EngineeringNad Stranemi 4511, 760 05 Zlin

CZECH [email protected]

Abstract: This article introduces different ways of transformation matrices use. The first utilization is to deter-mine the motion equations of the complex mechanical structure. Analysed model is designed in 3D CAD systemSolidWorks. It consists of seven main physical objects. The corresponding inertia/pseudo-inertia matrix is derivedfor each of them with the aid of transformation matrices. These matrices are further used to derive the motionequations. The ball position scanning by camera system provides another possibility of the transformation matrixuse; the camera local coordinate system is different against the plate local coordinate system and it is necessary torecalculate the real ball position. The final mentioned use of transformation matrices is in the determination of ballfalling direction - it is valid only in case of its zero initial velocity.

Key–Words: Motion Control, Transformation Matrix, Motion Equations, Gradient, Simulation, Matlab/Simulink

1 IntroductionDerivation of systems motion equations (deductiveidentification) is a crucial part of designing the mo-tion control law of any mechanical structure. Thesestructures are composed of mass objects and acts bynot inconsiderable effect on other components of thesystem.

Each mass object has inertia in the motion.These inertias are reflected in the form of centrifu-gal/centripetal and Coriolis generalized forces withthe combination of rotational movements - non-linearity into the system is introduced. The whole sys-tem has, therefore, highly non-linear behaviour duringfast movements - non-linearities are bounded to therate of change of state variables largely.

The system which is the object of our interest iscalled ”Ball and plate”, shown in Fig. 1. To derive theequations of motion of the system the transformationmatrices individual parts are advantageous to know.These transformation matrices can be further used forderivation of the inertia matrix, the position determin-ing of the individual parts of the assembly in a 3Dspace, and non-measurable state variables obtaining -during the whole process procedure. The matrix formof Lagrange equations of the second type there willbe used, which involves the use of the transformationmatrices directly.

The Denavit-Hartenberg (DH) notation/methodof placement for coordinate systems is used to deter-

mine the transformation matrices. The position mea-surement of the ball on the plate is realized using thecamera system. As shown in Fig. 10, with respect tothe fixed location of the camera is necessary to trans-form the camera measured position of the ball to theactual position of the ball on the plate. Even in thiscase, the transformation matrix method is advanta-geously uses.

2 Ball and plate2.1 3D model

Figure 1: Total 3D model

WSEAS TRANSACTIONS on APPLIED and THEORETICAL MECHANICS Jiri Zatopek

E-ISSN: 2224-3429 169 Volume 11, 2016

Fig. 1 shows a complete 3D assembly model.This model is created in SolidWorks and has 4 gen-eralized degrees of freedom. Tilts of the plate are twoof them. They are realized by two rotary kinematicpairs in perpendicular relation to their common nor-mal. The ball position on the plate are other two gen-eralized degree of freedom.

For purposes of determining the system dynamiccharacteristics is preferably to simplify the system tothe extent not too complicated equations of motionand at the same time the dynamic characteristics ofthe simplified model not be much different from thebehaviour of the original system.

Figure 2: Simplified 3D model

The simplified model shown in Fig. 2 containsonly parts that have a major influence on its dynam-ics or serves as a global coordinate system base.

There are generally shown the basic dimensionsthat will be used in the coordinate system transforma-tion - will be possible to change them in the resultingequation.

The stand as a base and the first motor need notbe taken into account because they do not perform anymovement (they are fixed) - as well as the connectingparts with negligible mass. The motor that moves theplate is replaced by cuboid.

2.2 Transformation of coordinate systems

Table 1: DH parameters general tableLink ai αi θi di

1 a1 α1 θ1 d12 a2 α2 θ2 d2. . . . . . . . . . . . . . .

n an αn θn dn

The model is divided into seven parts for deter-mining the homogeneous coordinates of the system.

The part which is the transformation relates is alwaysshown in the accompanying figures.

Used Denavit-Hartenberg (DH) notation/methodof placement for coordinate systems is probably themost popular method used in robotics kinematics.

The calculation of the required transformationsfrom the Table 1 is performed using the following re-lationships:

i−1Ti =

cos θi − cosαi sin θi sinαi sin θi ai cos θisin θi cosαi cos θi − sinαi cos θi ai sin θi

0 sinαi cosαi di0 0 0 1

(1)

0Tn =0T1 . . .n−2Tn−1

n−1Tn (2)

Gr =GTB ·Br (3)

The ball transformation in a global coordinatesystem is listed here as a pattern, other transforma-tions have similar characteristics. The global coordi-nate system (X0, Y0, Z0) is positioned at the rotationcenter axis of the first motor, as shown in Fig. 3.

Figure 3: Coordinate system placement - part 7

Table 2: DH parameters - part 7Link ai αi θi di

1 e1 + r1 + c1 + e2 + r2π2

π2

π2 + α

2 r2 + e3 + c3 +R π2 p β

3 0 0 q 0

The gradual transformations from the Fig. 3 canbe written in Table 2. Utilising the relationship inthe Equation 1 is found the transformation matrix be-tween systems.

Equation 2 is used for finding the final transfor-mation matrix from the local body coordinate system- chosen at the center of gravity, into the global coor-dinate system.


E-ISSN: 2224-3429 170 Volume 11, 2016

It is possible to determine the position vector ballin the global coordinate system (ball position in 3Dspace) by using the Equation 3.

The matrix 0T3 is listed in Equation 4. It is la-belled as T7 according to the selected assembly partsnumbering.

T7 =

− cosβ sinα cosα − sinα sinβ T 1

7

cosα cosβ sinα cosα sinβ T 27

sinβ 0 − cosβ T 37

0 0 0 1

(4)

in which:

T 17 =p cosα− sinα (c1 + e1 + e2 + r1 + r2)−− q sinα sinβ − cosβ sinα (R+ c3 + e3 + r2)

T 27 = cosα (c1 + e1 + e2 + r1 + r2) + p sinα+

+ q cosα sinβ + cosα cosβ (R+ c3 + e3 + r2)

T 37 = sinβ (R+ c3 + e3 + r2)− q cosβ

Transformation of the remaining parts of the assemblyare given in Equations 5 - 10, Tables 3 - 8 and comesfrom Fig. 4 - 9.



1 e1 + r1 + c12

π2 0 π

2 + α

2 0 0 a1−b12 0

T1 =

− sinα 0 cosα T 1

1

cosα 0 sinα T 21

0 1 0 00 0 0 1

(5)

in which:

T 11 = cosα

a1 − b12

− sinα(c1

2+ e1 + r1

)T 21 = sinα

a1 − b12

+ cosα(c1

2+ e1 + r1

)



1 e1 + r1 + c12

π2 0 π

2 + α

2 c1+c22 0 a2

2 − b1 0

T2 =


2

cosα 0 sinα T 22

0 1 0 00 0 0 1

(6)

in which:

T 12 = cosα

(a12− b1

)− sinα

(c1 +

c22

+ e1 + r1

)T 22 = sinα

(a12− b1

)+ cosα

(c1 +

c22

+ e1 + r1

)



1 e1 + r1 + c12

π2 0 π

2 + α

2 c1+c22 0 a1 − a2

2 0

T3 =


3

cosα 0 sinα T 23

0 1 0 00 0 0 1

(7)


E-ISSN: 2224-3429 171 Volume 11, 2016

in which:

T 13 = cosα

(a1 −

a22

)− sinα

(c1 +

c22

+ e1 + r1

)T 23 = sinα

(a1 −

a22

)+ cosα

(c1 +

c22

+ e1 + r1

)



1 e1 + r1 + c1 + e2 + r2π2 0 π

2 + α

2 0 0 −2b1+a42 0

T4 =


4

cosα 0 sinα T 24

0 1 0 00 0 0 1

(8)

in which:

T 14 =− sinα (c1 + e1 + r1 + e2 + r2)− cosα

(b1 +

a42

)T 24 = cosα (c1 + e1 + r1 + e2 + r2)− sinα

(b1 +

a42

)



1 e1 + r1 + c1 + e2 + r2π2 0 π

2 + α

2 0 0 a1−b12 β

T5 =

− cosβ sinα sinα sinβ cosα T 1

5

cosα cosβ − cosα sinβ sinα T 25

sinβ cosβ 0 00 0 0 1

(9)

in which:

T 15 = cosα

a1 − b12

− sinα (c1 + e1 + r1 + e2 + r2)

T 25 = sinα

a1 − b12

+ cosα (c1 + e1 + r1 + e2 + r2)



1 e1 + r1 + c12

π2 0 π

2 + α

2 0 0 a1−b12 β

3 0 π2 0 0

4 r2 + e3 + c32 0 b3−a3

2 0

T6 =


6


sinβ 0 − cosβ T 36

0 0 0 1

(10)

in which:

T 16 = cosα

a1 − b12

− sinα (c1 + e1 + r1 + e2 + r2) +

+ sinα sinβa3 − b3

2− cosβ sinα

(r2 + e3 +

c32

)T 26 = sinα

a1 − b12

+ cosα (c1 + e1 + r1 + e2 + r2)−

− cosα sinβa3 − b3

2+ cosα cosβ

(r2 + e3 +

c32

)T 36 = cosβ

a3 − b32

+ sinβ(r2 + e3 +

c32

)


E-ISSN: 2224-3429 172 Volume 11, 2016

2.3 Motion EquationsMotion equations matrix form of serial manipulatorhas the general form:

D(q) · ¨q + H(q, ˙q

)+ G (q) = Q (11)

in which:D(q) . . . symmetric inertia matrixH(q, ˙q

). . . bounded velocity vector

G (q) . . . gravitational force vectorQ . . . non-potential (non-conservative) generalizedforces causing change of q

Individual parts of the Equation 11 can be decom-posed to the form:

Di,j =n∑

r=max{i,j}

tr

[∂ 0Tr∂qj

·r Ir(∂ 0Tr∂qi

)T](12)

Hi =n∑

k=1

n∑m=1

Hikm qk ˙qm (13)

Hikm =n∑

r=max{i,k,m}

tr

[∂2 0Tr∂qk ∂qm

·r Ir(∂ 0Tr∂qi

)T](14)

Gi = −n∑

r= i

mr

(0g)T · ∂ 0Tr

∂qi·rrr (15)

in which:n . . . number of linksi, j . . . state variablesI . . . pseudo-inertia matrix0g . . . gravitational vector in global coordinate systemrrr . . . the center of mass position vector of link in thelocal coordinate system

The gravitational vector 0g is in the direction ofthe global axis Y , therefore:

0g =

0−g01

(16)

The center of mass position vector of link is zeroin his local coordinate system - all the local coordinatesystems are placed in the body center of mass:

rrr =

rxryrz1

=

0001

(17)

Only the last unknown parameter is missing inEquation 11 - the pseudo-inertia matrix I .

2.3.1 Inertia matrixThe inertia matrix of individual assembly parts is alsoneed to know to derive the pseudo-inertia matrices intheir local coordinate system. Because the local co-ordinate system are always placed in the object centerof mass the deviance moments (i.e. off-diagonal ele-ments) of inertia matrices will be zero.

The numbering is similar as for the transforma-tion matrices.

I1 =m1

12

(a1+b1)2+d21 0 0

0 (a1+b1)2+c21 0

0 0 c21+d21

(18)

I2 = I3 =m2

12

a22+d21 0 00 a22+c22 00 0 c22+d21

(19)

I4 =m4

12

a24+b24 0 00 a24+c24 00 0 b24+c24

(20)

I5 =m5

12

3r22+(a1+b1−2a2)2 0 0

0 3r22+(a1+b1−2a2)2 0

0 0 r22

(21)

I6 =m6

12

(a3+b3)2+d23 0 0

0 (a3+b3)2+c23 0

0 0 c23+d23

(22)

I7 =2

5m7R

2

1 0 00 1 00 0 1

(23)

2.3.2 Pseudo-inertia matrix

I =

−Ixx+Iyy+Izz2

Ixy Ixz mrx

IyxIxx−Iyy+Izz

2Iyz mry

Izx IzyIxx+Iyy−Izz

2mrz

mrx mry mrz m

(24)

The general definition of pseudo-inertia matrix islisted in Equation 24. Now there is all you need toderive the resulting motion equations - Equation 11.

I1P =

m1c21

120 0 0

0m1d

21

120 0

0 0m1 (a1 + b1)

2

120

0 0 0 m1

(25)

I2P = I3P =

m2c22

120 0 0

0m2d

21

120 0

0 0m2a

22

120

0 0 0 m2

(26)


E-ISSN: 2224-3429 173 Volume 11, 2016

I4P =

m4c24

120 0 0

0m4b

24

120 0

0 0m4a

24

120

0 0 0 m4

(27)

I5P =

m5r22

40 0 0

0m5r

22

40 0

0 0m5 (a1 − 2a2 + b1)

2

120

0 0 0 m5

(28)

I6P =

m6c23

120 0 0

0m6d

23

120 0

0 0m6 (a3 + b3)

2

120

0 0 0 m6

(29)

I7P =

m7R2

50 0 0

0m7R

2

50 0

0 0m7R

2

50

0 0 0 m7

(30)

2.4 The ball position on the plateObviously, the transformation matrix using is not onlyrefer to the motion equations. The system has fourgeneralized degree of freedom as Section 2.1 shows.This state variable is need to know during all the reg-ulation process.

Tilts of the plate are given by encoders on the mo-tors which controls them. However, the ball positionon the plate is unknown - it rolls independently in thedirection of the fastest decrease in potential energy - anegative gradient. It is given by the tilt of the plate.

Figure 10: 3D Model with camera evaluation

The ball position on the plate is measured by us-ing a camera, which is placed as shown in Fig. 10 -the optical axis of the camera is identical to the globalaxis Y0 and the plate edges are parallel to the camerapixel array edges. The ball coordinates on axes X0

and Z0 are measured by this placement, not the sizeof the p and q as shown in Fig. 3.

The best way would be moving the camera andthe plate together, but it is ineffective from the con-struction and control viewpoint. However, the p andq parameters is possible to calculate with the transfor-mation matrices help.

The ball position in the global coordinate systemis expressed from Equation 3.

X =p cosα− sinα (c1 + e1 + e2 + r1 + r2)−− q sinα sinβ − cosβ sinα (R+ c3 + e3 + r2)

Y = cosα (c1 + e1 + e2 + r1 + r2) + p sinα+

+ q cosα sinβ + cosα cosβ (R+ c3 + e3 + r2)

Z = sinβ (R+ c3 + e3 + r2)− q cosβ

(31)

Parameters p and q are expressed from Equa-tion 31, X and Z are measured by the camera system.

p =

X − sinα sinβ [Z − sinβ (R+ c3 + e3 + r2)]

cosβ+

+ sinα (c1 + e1 + e2 + r1 + r2) ++ cosβ sinα (R+ c3 + e3 + r2)

cosα

q =sinβ (R+ c3 + e3 + r2)− Z

cosβ

(32)

All the state variables are already fully defined. Ifthe vertical ball position is necessary to know in theglobal coordinate system, state variables p and q justput into Y in Equation 31.

Figure 11: Difference between measured ball positionby camera and real (computed) ball position


E-ISSN: 2224-3429 174 Volume 11, 2016

In Fig. 11 is shown correct (computed) ball posi-tion (red point) and measured ball position by camera(blue points) in dependence on the tilt of the beam (αand β). The tilt of the beam was chosen randomly inan interval (−30◦, 30◦). Parameters p and q are invari-able in this case (ball is firmly connected with plate).The ball position in axis Z is approximately in therange (−0.273m,−0.24m). The ball position in axisX is approximately in the range (0.075m,0.285m) -more than 6x larger range than Z range. This is causedby a perpendicular placing of the first rotation axis -the rotation axis of tilt α, which affects the ball posi-tion in the X axis. It is placed below the plate axis andtherefore do not tilted the plate only, but it also moveswith the plate in the X axis.

2.5 Ball falling directionThe ball falling direction determining is listed as a fi-nal example of use the transformation matrix. Thisneeds to know the orientation of the inclined plate in3D space, which can be determined by the transfor-mation matrix T7 - Equation 4. All constants can bechosen zero, the tilts of the plate are known. The localcoordinate system of the plate will be moved to any3 points of this plate. General transformation matrix(TG), which will be further work with, is shown inEquation 33.

T =



sinβ 0 − cosβ −q cosβ0 0 0 1

(33)

in which:T 1 =p cosα− q sinα sinβ

T 2 =p sinα+ q cosα sinβ

Searched 3 points are determined by changing the pand q parametres (unitary length is chosen). The nor-mal vector w is determined with their help:

~w = (cosβ sinα,− cosβ cosα,− sinβ) (34)

This vector defines the general form of the equationof a plane. It can be written as Equation 34 after edit-ing. Its local coordinate system is oriented the sameas the global coordinate system (because of the trans-formation matrix definition - the transformation fromthe local coordinate system to the global coordinatesystem):

y =sinα

cosαx− sinβ

cosβ cosαz (35)

The components of the gradient in coordinates are thepartial derivative of the scalar field function - Equa-tion 35:

∇y =

(sinα

cosα,− sinβ

cosβ cosα

)(36)

Gradient in Equation 36 determines the greatest rateof increase of the function. The vector of gravity issituated in the negative direction of the global axis Y0,therefore, the negative gradient direction determinesthe fastest decrease in potential energy, and thereforethe direction where the ball will move by influence ofgravity.

Figure 12: Ball falling direction in 2D view - MAT-LAB

In Fig. 12 is displayed the ”Ball and plate” modelwith the ball falling direction vector for α = −10◦,β = 30◦. Set of the ball falling direction vectors isshown in Fig. 13. The tilt waveforms of the plate areshown in the upper graph. These inclines and gravitythen determine the ball falling direction vectors - thebottom left graph. The vectors are normalized andstretched into a time line for better clarity, how followin order - the bottom right graph.

3 ConclusionThe article contains derivations of the transformation,inertia matrices and all other required formalities nec-essary for the determine motion equations using thematrix form shown in Equation 11. The substitutioninto the Equation 11 and the resultant form is notlisted here due to the high dimensionality of the re-sulting expression. This substitutions and mathemat-ics adjustments are implemented in MATLAB.

The model is divided into 7 parts for ease identi-fying and acceptable inertia matrices form - each partis one of the basic shapes. These parts connection oc-curs during substituting into the Equation 11. Trans-formation matrices are designed to transform the localcoordinate system position (connected with the objectcenter of mass) to the global coordinate system. Thisconnection ensures zero inertia matrix deviance mo-ments and therefore zero pseudo-inertia matrices off-diagonal elements.

Transformation matrices are also used in the reg-ulated value determining, which is different from thecorrect values, with respect to the fixed camera loca-tion to the moving plate. It is used for conversion


E-ISSN: 2224-3429 175 Volume 11, 2016

Figure 13: Set of ball falling direction vectors depending on the tilt of the plate

between the measured and the real output/regulatedvalue.

Another usage of transformation matrices is in thedetermination of the ball falling direction. In that casewe will be able to say what size and direction the ballwill be falling in case of its zero initial velocity.

Suitable coordinate system placement and thenthe transformation matrix form is a key considerationas follows from the foregoing. It is important from theviewpoint of complexity of other calculations, fromthe perspective of finding potential errors or their fur-ther use - correctness of the transformation matrix canbe easily geometrically verified, which cannot be saidabout the system dynamics describing structures.

References:

[1] S. Crandall, D. C, Karnopp and E. F. Kurz,Dynamics of mechanical and electromechanicalsystems, McGraw–Hill, NewYork, 1968.

[2] H. K. Khali, Nonlinear Systems, Upper SaddleRiver, N. J: Pearson, 2001.

[3] J. Denavit and R. S. Hartenberg, A kinematic no-tation for lower-pair mechanisms based on ma-trices, ASME J. Ap. Rob. 77, 1955, pp. 215-221.

[4] R. N. Jazar, Theory of Applied Robotics,Springer US, Boston, 2010.

[5] Y. Shaoqiang, L. Zhong and L. Xingshan, Mod-elling and simulation of robot based on Mat-

lab/SimMechanics, Chin. Contr. Conf. 27, 2008,pp. 161-165.

[6] Z. Urednicek, Robotika, Tomas Bata universityin Zlin, Zlin, 2012.

[7] Z. Urednicek, Elektromechanicke akcni cleny,Tomas Bata university in Zlin, Zlin, 2009.

[8] R. C. Rosenberg, Multiport Models in MechanicJ. Dyn. Sys., Meas., Con. 94, pp. 206–212, 1972

[9] D. C. Karnopp and R. C. Rosenberg, Systemdynamics, A Unifies Approach. Wiley & Sons,New York, 1975.

[10] M.–T. Ho, Y. Rizal and L.–M. Chu, Visual Ser-voing Tracking Control of a Ball and Plate Sys-tem: Design, Implementation and Experimen-tal Validation Int. J. Adv. Robot. Syst. 10, 2013,pp. 287–302.

[11] X. Dong, Y. Zhao, Y. Xu, Z. Zhang andP. Shi, Design of PSO Fuzzy Neural Net-work Control for Ball and Plate SystemInt. J. Inn. Comp. Inf. Contr. 7, 2011, pp. 7091–7103.


E-ISSN: 2224-3429 176 Volume 11, 2016

using the transformation matrices not only to derive the ... · the whole sys-tem has, therefore,...

Documents