Initial concepts to develop a semi-autonomous operator support technology for operating a novel forestry machine

Master thesis, 30 hp

Robotics and Control Programme , 120hp Aurumn term 2016

Initial concepts to develop a semi-autonomous

operator support

technology for operating a novel forestry machine

Dong xiaowei

Supervised by:

Pedro La Hera

R&D in SLU and Skogstekniska klustret Shafiq Urréhman

Associated professor in Umea University

Abstract

Forestry machines have the power to lift heavy logs, but they are not so smart at providing information, or help operators perform better work [3].

The main reason to this problem is the low level of technology applied to forestry machines, which has not changed so much since the forestry machines were first introduced in the 1960’s. But starting 2013, ma- chines manufacturers got inspired by developments in the automation and robotics industry, several of new technologies have been developed in the market - computerized hydraulics, feedback controllers for vibration damp- ing, sensor-based motion control systems, improvements in mechanical de- sign, smart suspension controller, etc [11]. Largely, this development is at- tributed to better hardware and software developed during the last decade by researchers of Scandinavian institutes.

In this thesis, we introduce a new type of forestry machine, the harwarder, which can perform the work of two machines (harvester and forwarder) by a single one. The forwarder is a forestry vehicle that carries big felled logs. The harvester is a type of heavy forestry manipulator employed in cut-to-length logging operations for felling, and bucking trees. Both the manipulator and vehicle should work synchronized to get the best out of this design. To benefit out of its design, in the first part of thesis we will an- alyze the kinematics and dynamics of machine, and design a time optimal coordinated motion via virtual holonomic constraints, to solve a particular task of forestry crane. The second part consists on applying optimization to reduce energy consumption during the motion.

Result of thesis work:1) By using coordinated motion, consequently the

energy consumptions are drastically reduced comparing to traditional

motion of the crane. 2) By applying optimization, the energy efficiency

is improved.

Acknowledgments

My deepest gratitude goes to my supervisor Pedro La Hera and his research group (Omar Mendoza Trejo and Daniel Ortiz Morales), and Co-supervisor Pro.Shafiq Ur Rehman, from Umeå University. Their suggestions, guid- ances, patience allow me to finish the thesis successfully.

To Pedro, thanks for navigating the way of exploring robotics ocean, giving me the direction when I was confused.

To Omar, thanks for providing hardware and technical support, and teach- ing me write thesis properly.

To Daniel, thanks for providing guidance of virtual holonomic approach.

To Shafiq, thanks for being my Co-supervisor in university, helping with administrative issues.

To my parents, thanks for encouraging me when I encountered difficulties.

At end, I want to express my deepest gratitude again for those people who

give me huge support during my thesis.

Contents

1 Introduction 8

1.1 Background . . . . 8

1.2 Motivation and objective . . . . 8

1.3 Structure of thesis . . . . 8

2 Kinematics 10 2.1 Robot forward kinematics . . . 10

2.1.1 Denavit-Hartenberg convention . . . 11

2.1.2 Assigning the coordinate frames . . . 11

2.1.3 Forward equation . . . 12

2.2 Robot inverse kinematics . . . 13

2.2.1 Analytical solution . . . 14

2.3 Log container forward kinematics . . . 15

2.4 Coordinate transformation . . . 20

2.4.1 Mapping between x

and q

. . . 20

2.4.2 Mapping between x

and q

. . . 22

2.4.3 Finding maximum joint velocity . . . 25

3 Dynamics 28 3.1 Euler-Lagrange equations . . . 28

3.1.1 Kinetic energy . . . 29

3.1.2 Potential energy . . . 30

3.1.3 Centrifugal and Coriolis forces . . . 30

3.1.4 Manipulator dynamics analysis . . . 30

3.2 Simulation results . . . 33

4 Path planning 37 4.1 Formulation of the path . . . 37

4.1.1 Algebraic polynomial . . . 37

4.1.2 Bézier polynomial . . . 38

4.2 Coordinated motion plan for new forestry machine . . . 39

5 Numerical trajectory optimization 42 5.1 Finding a stable initial configuration for crane . . . 42

5.2 Trajectory optimization with q

. . . 43

6 Virtual Holonomic Constraints 46 6.1 Basic concept of Virtual Holonomic Constraints . . . 46 6.2 Velocity phase plane: trajectory generator . . . 46 6.3 Designing feasible velocity curve in phase plane . . . 48 6.4 Apply virtual holonomic constraints with different paths . . . 49 7 Including a second performance index 54

8 Discussion 58

9 Conclusions 59

10Appendix A: Data sheet of forest crane 61

List of Tables

1 DH Table. . . 12

2 Joint variable limits. . . 13

3 DH table of end-point 2. . . 17

4 DH table of end-point 1 and 3. . . 18

5 DH table of base-point A and C. . . 18

6 DH convention of end-point 5 . . . 19

7 DH convention of end-point 4 and 6 . . . 19

8 DH convention of base-point D and F . . . 19

9 Hydraulic joint working range. . . 26

10 Comparing energy cost after optimization. . . 45

11 Energy consumptions of three paths at t

=40 s. . . 52

12 Different objective functions optimization for curve path with t

=40 s. . . 56

13 Inertia I

of each link in body frame . . . 61

14 Center of mass of each link in joint initial frame . . . 61

15 Joint angular velocity limits . . . 61

List of Figures

1 Manipulator model. . . 10

2 Assign frames for forward kinematics. . . 12

3 Crane’s manipulator simulation . . . 13

4 The configuration of manipulator for deriving inverse kinematics. 14 5 Container bunk with yaw, roll, pitch angle. . . 16

6 Assign frames for container forward kinematics. . . 17

7 Simulation of container forward kinematics. . . 20

8 Schematics of measured angle q

with essential parts. . . 21

9 Geometry of the angle q

with the essential mechanical com- ponents. . . 22

10 Mapping function validation block diagram. . . 26

11 Maximum linear velocity x

and x

. . . 26

12 Integral of maximum linear velocity of second and third hy- draulic links. . . 27

13 Applying coordinate transformation to get range of q

and q

. 27 14 Differentiating q

and q

to get angular velocities ˙q

, ˙q

. . . 27

15 Joint variable profiles . . . 34

16 Joint velocity profiles . . . 34

17 Joint acceleration profiles . . . 35

18 Dynamics computation testing . . . 36

19 Two polynomials for standard path . . . 39

20 Three different types of motion between container and manip- ulator. . . 41

21 Different initial configurations of manipulator. . . 43

22 ˙θ-θ plane boundary . . . 48

23 The velocity curves in θ- ˙θ plane. . . 50

24 Three types of paths in Cartesian space. . . 51

25 Three types of energy consumptions for standard path. . . 51

26 Three types of energy consumptions for curve path. . . 52

27 Three types of energy consumptions for straight path. . . 52

28 Optimizing q

with objective function 

. . . 55

29 Optimizing q

with objective function 

. . . 55

30 Optimizing q

with objective function 

. . . 56

31 Optimizing q

with objective function 

. . . 56

1 Introduction

1.1 Background

In Sweden, a chain of university research groups and industry companies, are developing the novel idea of semi-autonomous forestry machines [4].

Since forestry machines are difficult to operate, in order to provide opera- tor with smart machines that can cope with difficult tasks, forest industry is trying to incorporate robotics and automation technologies in order to improve the performance of the cranes. Now, from the robotics point of view, many related works to improve systems performance have been done, including feedback controllers for vibration damping [8], sensor-based mo- tion control systems [15], and improvements in mechanical design.

1.2 Motivation and objective

As we have already mentioned, some improvements in forestry machines have been achieved. Nevertheless, the improvements are not significant in terms of trajectory planning, which can be done in many senses, for exam- ple, achieving time-efficient or energy-efficient improvements.

Although the energy and time costs are two important performance criteria for forestry machine, considering both of them together will lead to a com- plex optimization problem, specially when the robot´s model has several degrees of freedom. In this study, a method has been applied to improve the time and energy efficient respect to a particular task of new forestry crane. The main thesis work was broken into four parts : 1) Forestry crane mechanical analysis 2) Path planning for a particular task of crane 3) Design time optimal trajectory by virtual holonomic constraints method [12] 4) Include a second performance index for trajectory optimization.

1.3 Structure of thesis

To ease readability, this thesis has been structured as follows. Chapter 2 in-

troduces the kinematics of forestry crane. The mathematical relations used

for modeling the crane dynamics are presented in Chapter 3. The steps and

procedures used for path planning are described in Chapter 4. Numerical

trajectory optimization is introduced in Chapter 5, and virtual holonomic

constraints method based optimizations are introduced in Chapter 6 and 7

respectively. Finally, discussion and conclusions are presented in Chapter

8 and 9 respectively.

2 Kinematics

The manipulator (harvester) used for our project is a 3D-print model, which is made of three revolute joints q

, q

, q

, and one redundant prismatic joint q

[6], the joint variables form the vector of generalized coordinates as (2.1), viewed from Figure 1, the system resembles a serial kinematic chain with an end-effector in the last link. In this section, we will analyze the forward and inverse kinematics of crane (manipulator and container), besides, we derive the mapping relations between cylinder´s piston displacement and joint variables, x

⇔θ

and x

⇔θ

.

q = [q

, q

, q

, q

] (2.1)

z₀ (q₁)

z2 (q

2)

z4 (q

3)

z₆ (q₄)

Figure 1: Manipulator model.

2.1 Robot forward kinematics

By definition, forward kinematics is about determining the position and ori-

entation of the end-effector, when the values of joint variables are provided

[14]. This section is organized in the following manner: we first introduce

the Denavit-Hartenberg (DH) convention method [14], then we present the

procedures of assigning the coordinate frames based on DH convention, the

last step is to derive forward kinematics equations.

2.1.1 Denavit-Hartenberg convention

DH convention can simplify the analysis considerably in robotics applica- tions, each homogeneous transformation A

is represented as a product of four basic transformations [14].

A

=Rot

T rans

T rans

Rot

=











cos(θ

) −sin(θ

)cos(α

) sin(θ

)sin(α

) a

cos(θ

) sin(θ

) cos(θ

)cos(α

) −cos(θ

)sin(α

) a

sin(θ

)

0 sin(α

) cos(α

) 0

0 0 0 1











. (2.2)

Four quantities θ

, d

, a

, α

are given names as, joint angle, link offset, link length, link twist, respectively. Since A

is a single variable function, which means three quantities are constant, and one revolute joint variable θ

or prismatic joint variable d

[14].

The link length a

is distance along x

from o

to the intersection of the axis of x

and the axis of z

.

The link length d

is distance along z

from o

to the intersection of the axis of x

and the axis of z

.

The link twist α

is the angle between z

and z

measured about x

. The Link angle θ

is the angle between x

and x

measured about z

. It is obvious that it is impossible to describe any arbitrary homogeneous transformation using these four quantities. In order to derive unique trans- formation, suppose the two frames satisfy next two additional rules [14]:

1. The axis x

is perpendicular to the axis z

. 2. The axis x

interacts the axis z

.

Following these two features, we claim that there exist unique transforma- tion between two frames.

2.1.2 Assigning the coordinate frames

Even though we have two extra constraints, the choice of assigning coor-

dinate frames is not unique. First of all, we determine the z-axis of each

joint frame, then we combine DH conventions to assign x and y axis of each

joint. Note that sometimes it is necessary to add virtual frame for off-set

point, so as to satisfy DH conventions.

In Figure 2, four red dots are the real joints, the last joint connects the end-effector, besides, there are three virtual frames. We establish the base frame x

y

z

, the direction of z-axis is vertical, the x-axis is arbitrary, but x

y

z

should follow right hand rule. Once the base frame is established, we begin an iterative process to define coming frame under conventions, till the end-effector frame x

y

z

.

Figure 2: Assign frames for forward kinematics.

2.1.3 Forward equation

After assigning the frames, we need to establish the DH table, which con- tains four quantities θ

, d

, a

, α

for each joint i.

Table 1: DH Table.

Link d_i θ_i a_i α_i

1 q1 H 0 0

1 0 0 −l₀ π/2

2 q2+ π/2 0 l1 0

2 −π/2 0 l₂ 0

3 q3+ π/2 0 l3 π/2

3 0 l₄ 0 0

4 0 q4 0 0

According to Table 1, each row of parameters determines a homogeneous

transformation matrix A

. Then the position and orientation of the each

frame respect to base frame are given by (2.2), note that T

, T

, T

, T

rep- resent the transformation from base frame to joints, T

is transformation from base frame to end-effector, T

, T

, T

represent the transformation from base frame to off-set points.

T

= A

(q

)...A

(q

), n = 1...7 (2.3) We can extract the last column of T

, which is the position of each frame respect to the base frame. With these positions, we can plot the manipu- lator configuration when joint variables are provided, as shown in Figure 3, where all joint variables are zero.

-0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8

x (m)

-0.5 0

0.5

y (m) -0.3

-0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5

z (m)

T4 0 T5

0 T

6

0 T

7 0

T0 0 T2

0 T3

0

T1 0

Figure 3: Crane’s manipulator simulation

2.2 Robot inverse kinematics

By definition, inverse kinematics is concerned with finding the joint vari- ables in terms of the end-effector’s position and orientation [14]. Actually, we could find different configurations reaching the same goal point. To avoid that, we provide q

for inverse kinematic calculation, and Table 2 redefine the working range of each joint variable, in this way, the unique inverse kinematics solution can be obtained.

Table 2: Joint variable limits.

q₁(rad) q₂(rad) q₃(rad) q₄(m) [0, 2π] (0,^π₂) (−π, 0) [0, 0.1]

2.2.1 Analytical solution

In Figure 4, the end-effector position (X

, Y

, Z

) and links length l

, l

, l

, l

, l

are given, assuming corresponding joint variables are q

, q

, q

, q

.

Figure 4: The configuration of manipulator for deriving inverse kinematics.

First, we project end-effector onto the x

-y

plane as shown in Figure 4.

q

:

q

= Atan2(y

, x

), (2.4) in which Atan2(x, y) represents the two arguments arctangent function.

According to geometry relation (2.5), in order to calculate q

, we need to calculate three angles α

, α

, α

respectively:

q

= α

+ α

+ α

− π

2 (2.5)

α

= Atan2(l

, l

) (2.6)

α

= arccos( a

+ c

− b

2ac ), (2.7)

where a = pl

+ l

, b = pl

+ (l

+ q

)

, c = √

r

+ s

.

α

= Atan2(s, r), (2.8)

where r

= px

+ y

, r = r

+ l

, s = z

− H . The geometry relation of q

is indicated as (2.9).

q

= π

2 + α

(2.9)

In order to compute α

, we need to calculate angles α

, α

, α

respectively:

α

= π

2 − α

(2.10)

α

= arccos( a

+ b

− c

2ab ), (2.11)

where a = pl

+ l

, b = pl

+ (l

+ q

)

, c = √

r

+ s

.

α

= Atan2((l

+ q

), l

) (2.12) α

= π − (α

+ α

+ α

) (2.13)

2.3 Log container forward kinematics

Next we consider the container vehicle, which cooperates with manipulator

to store logs. The motion of container can be described by Euler angles [5],

in later section, we will discuss how to design the coordinated motion be-

tween manipulator and container. In Figure 5, there are six fixed cylinders

installed on the platform plane, to avoid collision during the coordinated

motion with manipulator, we should be able to control cylinder’s position

and orientation. For each cylinder in Cartesian space, we can describe it

as a straight line which is connected between base-point and end-point.

Figure 5: Container bunk with yaw, roll, pitch angle.

Initially, we can use forward kinematics to calculate base-points A, B, C

and end-points 1, 2, 3. Due to geometric symmetry, then we can derive

the base-points D, E, F and end-points 4, 5, 6 position and orientation. As

shown in Figure 6, we assign frames for A, B, C, 1, 2, 3 respectively.

Figure 6: Assign frames for container forward kinematics.

DH table of end-point 2 is shown in Table 3, the homogeneous transforma- tion matrix T

and T

are given by (2.15) and (2.16).

Table 3: DH table of end-point 2.

Link di θi ai αi

1 π/2 + yaw H 0 π/2

2 π/2 + roll 0 0 π/2

3 pitch H₂ H₃ 0

A

= Rot

T rans

T rans

Rot

(2.14) T

=

3

Y

i=1

A

(2.15)

T

=

2

Y

i=1

A

(2.16)

After we know the T

respect to base frame, the homogeneous matrix T

, T

can be obtained by adding one more homogeneous transformation based on T

, corresponding DH table is shown in Table 4. Similarly, T

, T

can be obtained by adding one more homogeneous transformation based on T

, corresponding DH table is shown in Table 5.

Table 4: DH table of end-point 1 and 3.

Homogeneous matrix di θi ai αi

Ae1 π/2 0 −d1 π/2

Ae3 π/2 0 d2 π/2

T

= T

∗ A

(2.17)

T

= T

∗ A

(2.18)

Table 5: DH table of base-point A and C.

Homogeneous matrix di θi ai αi

Aa π/2 0 −d1 π

Ac π/2 0 d1 π

T

= T

∗ A

(2.19)

T

= T

∗ A

(2.20)

After deriving half side of base-points and end-points, the other half points

can be found by following same process before. According to Table 6, we

determine end-point 5 and base-point E, then based on Table 7 and Table

8, we can obtain end-points 4, 6 and base-points D, F respectively.

Table 6: DH convention of end-point 5

Link d_i θ_i a_i α_i

1 π/2 + yaw H 0 π/2

2 π/2 + roll 0 0 π/2

3 pitch −H2 H3 0

T

=

3

Y

i=1

A

(2.21)

T

=

2

Y

i=1

A

(2.22)

Table 7: DH convention of end-point 4 and 6

Homogeneous matrix d θ a α

A_e4 π/2 0 −d₁ π/2

Ae6 π/2 0 d2 π/2

T

= T

∗ A

(2.23)

T

= T

∗ A

(2.24)

Table 8: DH convention of base-point D and F

Homogeneous matrix d_i θ_i a_i α_i

Ad π/2 0 −d1 π

A_f π/2 0 d₂ π

T

= T

∗ A

(2.25)

T

= T

∗ A

(2.26)

After we obtain all base-points and end-points position and orientation, we

simulate essential part of container in software, as shown in Figure 7, 7(a)

depicts the initial state of container, 7(b) shows the container configuration

rotating with yaw = π/2.

0 0.5 0.2

y (m) 0.4

0

z (m)

0.6

x (m) 0.4 0.6

0 0.2 -0.5 -0.2

(a) yaw = 0; roll = 0; pitch = 0

0 0.5 0.2 0.4

z (m)

y (m)

0.6

0 0.4

x (m) 0.6

0 0.2 -0.5 -0.2

(b) yaw =^π₂; roll = 0; pitch = 0 Figure 7: Simulation of container forward kinematics.

2.4 Coordinate transformation

In hydraulic robotics, electrical driven motor cannot provide enough power to lift such heavy links, for our manipulator, the rotation of joints 2, 3 are powered by cylinder´s piston displacement, so it is necessary to establish the relation between x

⇔θ

and x

⇔θ

.

The trigonometric mapping between linear cylinder’s piston displacement and corresponding joint angle can be found based on the geometry of the manipulator. Those mapping q

(x

) and q

(x

) functions are used for de- riving the joint torques and computing joint angular velocities when linear velocities of cylinder are given.

2.4.1 Mapping between x2 and q2

In order to calculate the cylinder´s piston displacement x

as a function

of the joint angle q

, we draw geometry of essential mechanical parts, the

link length parameters r

, r

, r

, r

, r

, l

are known, as shown in Figure 8.

Figure 8: Schematics of measured angle q2 with essential parts.

In order to calculate q

, we need to derive β

, β

by law of cos.

β

= arccos( (r

+ r

− r

)

(2r

r

) ) (2.27)

β

= arccos( (r

+ r

− x

)

(2r

r

) ) (2.28)

β

= arcsin( r

r

) (2.29)

β

= β

− β

(2.30)

q

= −(π/2 − β

− β

) (2.31) Inverse calculation, given q

to calculate x

:

β

= arcsin( r

r

) (2.32)

β

= arccos( (r

+ r

− r

)

(2r

r

) ) (2.33)

β

= β

+ π/2 − β

+ q

(2.34) x

=

q

(r

+ r

− 2r

r

cos(β

)) (2.35) The relation (2.31) and (2.35) establish the mapping between x

and q

.

2.4.2 Mapping between x3 and q3

In Figure 9, we draw the simplified manipulator, the essential part of sec- ond hydraulic cylinder is a four-bar linkage system (polygon C-O-D-E) [9], which contains the important geometry relations for deriving mapping functions between x

and q

.

Figure 9: Geometry of the angle q3with the essential mechanical components.

Mapping relation q

(x

):

In triangle AA

C , we represent x

by x

.

∠AA C = ∠AA B + ∠BA C, (2.36)

where ∠BA

C = arccos(

9x₃

) , ∠AA

B = arccos(

9d₁₀

) . x

=

q

x

+ d

− 2x

d

cos(∠AA

C) (2.37) In triangle AOC, we calculate m by law of cos.

∠OAC = ∠BAC − ∠BAO, (2.38)

where ∠BAC = arccos(

) , ∠BAO = arccos(

) . m =

q

(x

+ d

− 2x

d

cos(∠OAC)) (2.39) Next, in triangle COD and DOE, we can calculate ∠COD and ∠DOE respectively.

d

= q

d

+ l

(2.40)

∠COD = arccos( m

+ d

− d

2md

) (2.41)

∠DOE = Atan2(d

, l

) (2.42)

∠COE = ∠COD + ∠DOE (2.43)

Final step, the angle relation of q

is described as (2.46), before that we need to calculate ∠EOF by (2.45).

∠COA = arccos( m

+ d

− x

2md

) (2.44)

∠EOF = π − ∠COA − ∠COE (2.45)

q

= π/2 − ∠EOF (2.46)

Mapping relation x

(q

) :

Inversely, the essential step is to derive length m. Now we have already

known the link length CO and CD in triangle COD, in order to apply law

of cos, we need to derive ∠CDO as follow:

∠EOF = π/2 − q

(2.47)

In triangle AOB, AB = d

, AO = d

, BO = d

, according to law of cos:

∠AOB = arccos( d

+ d

− d

2d

d

) (2.48)

d

= −d

cos(∠AOB) (2.49)

d

= −d

sin(∠AOB) (2.50)

∠AOE = π − ∠EOF (2.51)

∠BOE = ∠AOE + ∠AOB = π/2 + q

+ ∠AOB (2.52) In triangle DOE, DE = d

, OE = l

, ∠OED =

, so we can get

∠DOE = Atan2(d

, l

) (2.53)

∠BOD = ∠BOE − ∠DOE (2.54)

In triangle BOD, BO = d

, OD = d

= pl

+ d

, according to law of cos:

BD = q

d

+ d

− 2d

d

cos(∠BOD) (2.55) In triangle CBD, CB = d

, CD = d

, BD = d

, according to law of cos:

∠CDB = arccos( d

+ d

− d

2d

d

) (2.56)

OD

= l

sin(q

) − d

cos(q

) (2.57) OD

= l

cos(q

) + d

sin(q

) (2.58)

∠3 = Atan2(OD

, OD

) (2.59)

BD

= l

sin(q

) − d

cos(q

) − d

(2.60) BD

= l

cos(q

) + d

sin(q

) − d

(2.61)

∠1 = Atan2(BD

, BD

) (2.62)

∠BDO = ∠3 − ∠1 (2.63)

∠CDO = ∠CDB + ∠BDO (2.64)

m = q

d

+ d

− 2d

d

cos(∠CDO) (2.65) In triangle BOC, BO = d

, BC = d

, CO = m, according to law of cos:

∠BOC = arccos( d

+ m

− d

2d

m ) (2.66)

∠AOC = ∠BOC − ∠AOB (2.67)

In triangle AOC, AO = d

, AC = x

, CO = m, according to law of cos:

x

= q

m

+ d

− 2md

cos(∠AOC) (2.68)

∠A

AC = ∠A

AB − ∠CAB, (2.69)

where ∠CAB = arccos(

) , ∠A

AB = arccos(

10d₄

) . x

=

q

d

+ x

− 2d

x

cos(∠A

AC) (2.70) The relation (2.46) and (2.70) establish the mapping between x

and q

.

2.4.3 Finding maximum joint velocity

From previous section, we obtained mapping functions q

(x

) and q

(x

) ,

which are used for computing joint angular velocity given linear velocity of

cylinder. According to q

, q

range in Table 2 and mapping functions, the

range of x

and x

are shown in Table 9.

Table 9: Hydraulic joint working range.

units x2 x3

m [0.04947, 0.1447] [0.103, 0.1561]

In Figure 10, the block chain describes how to verify the result of our map- ping function. For example, we can choose θ

as sin wave, and check the output error signal.

q q → x x → q − scope

Figure 10: Mapping function validation block diagram.

The hydraulic links of our 3D model have the same maximum linear veloc- ity 0.01 m/s. Based on mapping functions, we can determine the maximum angular velocity value of ˙q

, ˙q

, the corresponding sketch diagram is shown in Figure 11:

X˙_max

Z x → q d

dt scope

Figure 11: Maximum linear velocity x2 and x3.

First we integrate ˙x

from x

to x

, representing the motion that hydraulic cylinders move from fully-closed to fully-open. The hydraulic cylinder’s piston displacement x

and x

during the motion are shown in Figure 12 (a) and Figure 12 (b) respectively.

Afterwards, we apply mapping functions q

(x

) and q

(x

) , the results

indicate that q

∈ (0,π/2), q

∈ (0,-π) during that motion, as shown in Figure

13. Simultaneously, by differentiating q

and q

, we get angular velocity

profiles, as shown in Figure 14, the maximum magnitude of ˙q

, ˙q

, are

0.2671 rad/s and 1.011 rad/s respectively.

(a) x2 (b) x3

Figure 12: Integral of maximum linear velocity of second and third hydraulic links.

(a) q2 (b) q3

Figure 13: Applying coordinate transformation to get range of q2and q3.

(a) ˙q2 (b) ˙q3

Figure 14: Differentiating q2and q3 to get angular velocities ˙q2, ˙q3.

3 Dynamics

In this section we deal with 4-link robot manipulators dynamics. The dy- namics of robot describes the relation between torque and motion, which is essential equation for trajectory optimization and control design. There- fore, we introduce the so-called Euler-Lagrange equations, which describe the motion of a mechanical system in terms of Holonomic constraints.

3.1 Euler-Lagrange equations

The Lagrangian L can describe the motion of a manipulator which repre- sents its dynamic behavior [14]:

d dt ( ∂L

∂ ˙q ) − ∂L

∂q = τ, L = K − U (3.1)

The torques which are applied to the actuator of each joint via the La- grangian involving the kinetic energy K and potential energy U. Since the potential energy always depends on the height of center of mass, we can write the potential energy as a function of the joint variables q, U = U(q).

Using the fact that the potential energy is independent of the time we can separate it from the kinetic energy:

d

dt ( ∂K

∂ ˙q ) − ∂K

∂q + ∂U

∂q = τ (3.2)

Now we calculate each element in equation (3.2) K = 1

2 ˙q

M (q) ˙q (3.3)

∂K

∂ ˙q = M (q) ˙q (3.4)

d

dt ( ∂K

∂ ˙q ) = M ¨ q + ˙ M ˙q (3.5) G(q) = ∂U

∂q (3.6)

∂K

∂q = 1

2 ˙q

∂M

∂q ˙q (3.7)

By replacing equations (3.3) to (3.7) in equation (3.2), we get the following expression:

M q + ˙ ¨ M ˙q − 1

2 ˙q

∂M

∂q ˙q = τ − G(q) (3.8)

The left hand side of equation (3.8) represents the inertial forces of the ma- nipulator. Furthermore, the dynamics of a robotic manipulator ignoring the friction forces usually can be represented as [14]:

M

(q)¨ q

+ V

(q, ˙q) + G

(q) = τ

(3.9) We can see from the above derivations that an important step to compute the mass matrix M is to calculate the kinetic energy from our analysis.

From the kinetic energy, we should be able to directly find the mass matrix M . If we have the mass matrix M it is easy to compute the V (q, ˙q) which represents the centrifugal and Coriolis forces.

3.1.1 Kinetic energy

By definition, kinetic energy is the work done by external forces in such a way to bring the system from rest to its current state [1].

Consider a manipulator which can perform linear or rotational motions:

K

= 1

2 m

v

v

(3.10)

K

= 1

2 w

I

w

(3.11)

where m

is the mass of the link i, v

is the linear velocity of the link i, w

is the angular velocity of the link i and I

is the inertia tensor of the link i. Please note that each parameter is calculated from the base to center of mass (COM) of each link. Thus the kinetic energy for the link i is:

K

= 1

2 w

I

w

+ 1

2 m

v

v

(3.12) Then, according to the Jacobian matrix for each link:

v

= J

˙q (3.13)

w

= J

˙q (3.14)

By substituting the above equations (3.13) to (3.14) into (3.12), we got:

1

2 ˙q

M ˙q = 1 2 ˙q

4

X

i=1

(w

I

w

+ m

v

v

) ˙q (3.15)

3.1.2 Potential energy

By definition, potential energy is possessed by an object because of its position in a gravitational field [1].

The total potential energy of a manipulator depends on its configuration:

U =

4

X

1

U

=

4

X

1

m

(−g

P

), (3.16) where m

is mass of each link, P

is the position of each link’s COM, g is gravity vector.

3.1.3 Centrifugal and Coriolis forces

In the general dynamic equation (3.9), V (q, ˙q) represents the Centrifugal and Coriolis forces. By definition, the Coriolis force is an inertial force that acts on objects that are in motion relative to a rotating reference frame. The centrifugal force is an inertial force directed away from the axis of rotation that appears to act on all objects when viewed in a rotating reference frame [1].

V (q, ˙q) = C(q)

[ ˙q

]

+ B(q)

n×ⁿ⁽ⁿ⁻¹⁾₂

[ ˙q

˙q

]

2 ×1

(3.17)

3.1.4 Manipulator dynamics analysis

Guidelines for deriving the dynamic equations:

1. Calculate the center of mass (COM) for each rigid link respect to base frame.

We already know the each link’s center of mass represented in each initial joint frame (without rotation) c

, as shown in Appendix A, Table 14. When joint rotating around z axis, the center of mass (COM) respect to each initial joint frame (without rotation) will change according to:

c

= R

(θ

) × c

(n = 0, 2, 4), (3.18)

where R

(θ

) is the rotation matrix.

If joint is prismatic, for instance the crane telescope, the center of mass c

is represented in initial joint 4 frame (without translation), when telescope extending with q

:

c

= c

+ q

(3.19)

According to the crane forward kinematics, we have already derived each joint’s initial joint frame respect to base frame, represented by homoge- neous matrix T

, T

, T

, T

. So the COM of each link’s homogeneous matrix can be expressed respect to base frame:

COM

1



= T

c

1



(n = 0, 2, 4, 6). (3.20) 2. Calculate velocity Jacobian J

and angular velocity J

from the base frame to the COM of each link i (i=1, ..., n).

J

= [ ∂COM

∂q

, 0

, 0

, 0

], (3.21) J

= [ ∂COM

∂q

, ∂COM

∂q

, 0

, 0

], (3.22) J

= [ ∂COM

∂q

, ∂COM

∂q

, ∂COM

∂q

, 0

], (3.23) J

= [ ∂COM

∂q

, ∂COM

∂q

, ∂COM

∂q

, ∂COM

∂q

], (3.24) where 0

represents 3x1 matrix whose elements are all 0.

From homogeneous transformation T

, T

, T

, T

, we extract the columns of z

component by ρ

T

(1 : 3, 3) . ρ

z

will be components of the full Jacobian matrix. ρ

= 1 for prismatic joint, otherwise is 0.

J

= [ρ

T

(1 : 3, 3), 0

, 0

, 0

], (3.25)

J

= [ρ

T

(1 : 3, 3), ρ

T

(1 : 3, 3), 0

, 0

], (3.26)

J

= [ρ

T

(1 : 3, 3), ρ

T

(1 : 3, 3), ρ

T

(1 : 3, 3), 0

], (3.27)

J

= [ρ

T

(1 : 3, 3), ρ

T

(1 : 3, 3), ρ

T

(1 : 3, 3), ρ

T

(1 : 3, 3)], (3.28) where 0

represents 3x1 matrix whose elements are all 0.

3. With Jacobian matrix, mass m

and moment of inertia I

, we can state the mass matrix M using following equation:

M =

4

X

i=1

(m

J

i

J

+ J

i

I

J

)(i = 1, ..., 4), (3.29) where

I

=





I

I

I

I

I

I

I

I

I



 .

Note that inertia I

in equation is relative to link i body frame.

4. Calculate the centrifugal C(q)

and Coriolis B(q)

2

matrix. For simplification, we introduce Christoffel’s symbols (3.30) and (3.31) [14], then we formulate elements in C(q)

and B(q)

2

with Christoffel’s symbols.

C(q)

=











b

b

b

b

b

b

b

b

b

b

b

b

b

b

b

b









 .

B(q)

=











2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b

2b









 .

b

= 1/2(m

+ m

− m

) (3.30) m

= ∂m

∂θ

; m

= ∂m

∂θ

; m

= ∂m

∂θ

, (3.31)

where m

is the elements from mass matrix.

5. Calculate the potential energy with each rigid link’s COM:

U =

n

X

1

m

(−g

COM

), g = [0, 0, 9.81], (3.32) where COM

is COM position of each link respect o base frame, g is grav- ity vector. m

is mass of each link.

G

(q) = ∂U

∂q

(i = 1, ..., 4), (3.33) Alternative method is:

G

(q) =

4

X

1

m

J

g(i = 1, ..., 4) (3.34)

3.2 Simulation results

In order to verify the dynamics model we got, we make a comparison be-

tween data obtained from the real machine and the simulation result. In

Figure 15, 16, 17, same position, velocity, acceleration profiles are shared

by real machine and our simulation.