平行式油壓平台力量控制的推導與實驗

國 立 交 通 大 學

機械工程學系

碩士論文

平行式油壓平台力量控制的推導與實驗

Modelling and Experiments on the Computed Force Control of

Hydraulic Parallel Machine

研 究 生：孫晏晞

指導教授：秦繼華 博士

平行式油壓平台力量控制的推導與實驗

Modelling and Experiments on the Computed Force Control of

Hydraulic Parallel Machine

研究生 ：孫晏晞 Student：Yen-His Sun

指導教授：秦繼華 Advisor：Dr. Jih-Hua Chin

國 立 交 通 大 學

機械工程學系

碩士論文

A Thesis Submitted to Department of Mechanical Engineering College of Engineering, National Chiao Tung University

In Partial Fulfillment of the Requirements for the degree of Master of Science in Mechanical Engineering

June 2005

Hsinchu, Taiwan, Republic of China

平行式油壓平台力量控制的推導與實驗

研究生：孫晏晞 指導教授：秦繼華

國立交通大學機械工程學系

論文摘要

平行式油壓平台(Parallel hydraulic manipulator)能夠提供巨大的能量搭載重 物，並可做出許多的姿態，故傳統一般皆用於飛行模擬器上。而隨著工業的進步， 對於大型工件的加工需求也越來越多，若能將平行油壓平台引進成為搭載大型工 件的工具機平台，必能提升加工效率和品質；因此，如何提升油壓平台的效能和 空間軌跡的追踨能力，已為目前發展油壓平台的最大課題。本論文目的主要在於 提出油壓平台的動態控制方法，並結合多軸交叉偶合預補償控制器(Multi-axis cross-coupled pre-compensation method ,MCCPM)，來求提升油壓平台的控制及軌跡 追縱能力。在論文中會推導油壓平台的動態公式，找出各缸所承受力量，來做為 油壓缸計算力 (Computed force)控制的依據，並以 MCCPM 來計算求得空間中軌 跡誤差所需的補償量。於後面章節，提出以簡單的適應控制法並結結合切削力等 外力模組，來做為往後更進一步加工機發展的控制基礎。本論文最後會經由實 驗，來證明油壓平台及新控制器的工作效能及軌跡追踨的成果。

Modelling and Experiments on the Computed Force

Control of Hydraulic Parallel Machine

Student: Yen-His Sun Advisor: Dr. Jih-Hua Chin

Department of Mechanical Engineering

National Chiao-Tung University

Abstract

Parallel hydraulic manipulator can provide large power to take heavy loading. It can do arbitrary position in workspace, so that it is generally used for flight simulation. As industrial progress, demand for cutting large and heavy work-piece is gradually increased. If hydraulic manipulator is utilized for large-scale cutting machine, then the efficiency and quality could be improved. Therefore, the key of hydraulic manipulator development is to improve the control efficacy and trajectory tracking.

In this paper, computed force control of hydraulic manipulator is presented. In addition, MCCPM, Multi-axis cross-coupled pre-compensation method, is introduced to improve the tracking ability. Dynamic formulation of hydraulic manipulator is developed for deriving the acting force on links, and computed control force; MCCPM can obtain tracking velocity to compensate trajectory error. A simplified adaptive control strategy, with external force model, is advised for the future implementation and development. Further, experimental results are presented, and also achievement of this new controller is presented.

致謝

從這篇論文的開始到完成，遇到了相當多的困難和問題，而多虧

身旁的指導老師、好友、同學、以及我敬愛的家人，能夠支持陪伴著

我，讓我能夠面對這挑戰而不屈服。要感謝的人真的很多，首先、也

是最重要要感謝的是我的指導教授，秦繼華教授，感謝教授不厭其煩

的為我指導，無論是邏輯的構思、paper 的閱讀、論文的寫作、口試

的攻防等，皆讓我的論文更加完整，也讓我更了解自己的缺陷並進而

修改。我的父母總是不斷的鼓勵支持我，我的姊姊也總是能在最我最

需要的時候，伸出援手給我一個擁抱；指導老師的教導讓我成長，家

人的關懷讓我不至於迷失而無望，對他們的感恩之心，難以一句謝謝

足以形容。此外，我的同學以及好友也是支持我走下去的關鍵，感謝

實驗室的國瑋、偉倫同學，雄雄、小孔學弟，你們給了我一個很充實

且充滿回憶的實驗室生活。小孩、阿海、強哥、鳥毛、鐘响等許多好

友們，感謝你們始終陪著我歡笑和努力。

當初進研究所，我選擇了提前畢業和提前入學，就為了想縮短拿

碩士的時間；但這條卻是如此難辛難走，必須犠牲許多與同學相處的

時間、休息的日子等。在寫論文的過程中，我曾因不斷思考及後悔自

己的行為，而感到迷失。但所幸我終究是渡過了，也許過程及結果並

非如此完美，但很高興的我還是得到、學到很多，也經歷了永遠值得

回味的日子。再一次的感謝所有的人，指導老師、家人、同學好友們，

謝謝你們！。

Contents

CHAPTER 1 INTRODUCTION--- 12

1.0 Introduction 12

1.1 Background 13

CHAPTER 2 KINEMATICS AND DYNAMICS ANALYSES OF HYDRAULIC

MANIPULATOR --- 15

2.0 Introduction 15

2.1 Coordinate system 16

2.2 Inverse kinematics 19

2.2.1. Inverse position kinematics 19

2.2.2. Inverse velocity kinematics 20

2.2.3. Inverse acceleration kinematics 20

2.3 Inverse dynamics 21

2.3.1 Dynamics equation 21

2.3.2 Dynamics programming 25

2.4 Forward kinematics 30

2.4.1 Iterative step of Newton-Raphson method 31

2.5 Forward dynamics 32

2.6 Dynamics of hydraulic piston and computed force control 34

2.7 Analysis of workspace 39

CHAPTER 3 MCCPM AND INTERPOLATION --- 42

3.0 Introduction 42

3.1 MCCPM controller 42

3.1.1 Trajectory contour error 43

3.2 Pre-compensation velocity 45

3.3 Interpolator 46

3.3.1 Transformation of link trajectory 47

CHAPTER 4 EXPERIMENT OF HYDRAULIC MANIPULATOR --- 50

4.1 Set-up of hydraulic manipulator 50

4.2 Controller design 51

4.4 Set-up of experiment 55

4.5 Experimental result 58

4.6 Result analysis and discussion 66

CHAPTER 5 CONCLUSION --- 67 CHAPTER 6 FUTURE WORK: EXTERNAL FORCE MODEL AND CONCEPT

OF ADAPTIVE CONTROLLER --- 68

6.0 Introduction 68

6.1 Models of external force 69

6.1.1 Frictional force 70

6.2 Cutting force 71

6.3 Parameterized model 74

6.4 Stability and adaptation algorithm 76

List of figures

Fig. 2.1. (a) Hydraulic manipulator --- 16

Fig. 2.1. (b) Hydraulic motion platform --- 17

Fig. 2.2. Coordinates transformation--- 18

Fig. 2.3. Forces components and length expressions on link i--- 21

Fig. 2.4. (a) Displacement of cylinder from equation (2.12)--- 27

Fig. 2.4. (b) Computed force from equation (2.41) --- 27

Fig. 2.5. (a) Displacement of cylinder from equation (2.12)--- 28

Fig. 2.5. (b) Computed force from equation (2.41) --- 28

Fig. 2.6. Simulation block diagram --- 33

Fig. 2.7. Servo-valve and cylinder system --- 34

Fig. 2.8 Computed force control strategy --- 36

Fig. 2.9. Computed force control for manipulator--- 37

Fig. 2.10. Computed force control with MCCPM for manipulator --- 37

Fig. 2.11. Computer simulator of hydraulic manipulator with hydraulic actuator ---- 38

Fig. 2.12. Flow chart of workspace determination programming --- 40

Fig. 2.13. Spatial workspace of Hydraulic Manipulator--- 41

Fig. 3.1. Spatial path contour error --- 43

Fig. 3.2. Surface function--- 46

Fig. 3.3. Isoparameter of function S--- 48

Fig. 4.1. Diagram of hydraulic manipulator control with PC based --- 51

Fig. 4.2. Controller for hydraulic manipulator--- 51

Fig. 4.3. Relation between spool displacement and cylinder velocity --- 53

Fig. 4.4. Relation between spool displacement and cylinder velocity with loading -- 53

Fig. 4.5. (a) Experiment set-up of hydraulic manipulator (Velocity controller) --- 56

Fig. 4.5. (b) Experiment set-up of hydraulic manipulator (Velocity with computed force controller) --- 56

Fig. 4.5. (c) Experiment set-up of hydraulic manipulator

(Velocity with MCCPM controller)--- 57

Fig. 4.5. (d) Experiment set-up of hydraulic manipulator (Velocity with computed force and MCCPM controller) --- 57

Fig. 4.6. (a) Cylinder tracking without computed force (Trajectory 1) --- 58

Fig. 4.6. (a) Cylinder tracking with computed force (Trajectory 1) --- 58

Fig. 4.6. (b) Cylinder tracking without computed force (Trajectory 2) --- 59

Fig. 4.6. (b) Cylinder tracking with computed force (Trajectory 2) --- 59

Fig. 4.6. (c) Cylinder tracking without computed force (Trajectory 3) --- 60

Fig. 4.6. (c) Cylinder tracking with computed force (Trajectory 3) --- 60

Fig. 4.6. (d) Cylinder tracking without computed force (Trajectory 4) --- 61

Fig. 4.6. (d) Cylinder tracking with computed force (Trajectory 4) --- 61

Fig. 4.7. (a)

( )

650, cos 0.4

( )

(

) ( )

, sin 0.4

(

)

15 15 Z t = α t = π πt β t = π πt (Trajectory 1) --- 62

Fig. 4.7. (b)

( )

650, cos 1.2

( )

(

) ( )

, sin 1.2

(

)

15 15 Z t = α t = π πt β t = π πt (Trajectory 2) --- 62 Fig. 4.7. (c)

( )

600 10 , 0,

( )

( )

sin 0.4

(

)

18 Z t = + t α t = β t = π πt (Trajectory 3) --- 63

Fig. 4.7. (d)

( )

650 5 , cos 1.2

( )

(

) ( )

, sin 1.2

(

)

16 16 Z t = + t α t = π πt β t = π πt (Trajectory 4) --- 63

Fig. 6.1. Elastic bristle model --- 70

Fig. 6.2. Milling cutter --- 71

Fig. 6.3. Control system with external force consideration modified from Fig. 2.9 -- 73

List of Tables

Table 2.1. Designate data --- 25

Table 2.2. Platform and Base layouts --- 26

Table 4.3. Estimated physical parameters --- 54

Table 4.4. Trajectory test in experiments --- 55

Table 4.5. (a) IAE results for trajectory tracking in Fig. 4.6 and 4.7--- 64

Nomenclature

q: coordinate vector with position and orientation variables

z

, a,b

l: link vector

P and W: platform and world frame which vector reference to W_{Rp: rotational matrix}

ai and bi: joint vector respects to center of moving and fixed platform Li , li, and ni: link vector, length of link, unit vector of link i

ω and α: angular velocity and acceleration

J: Jacobian matrix

Fi and Mi: vector of external force and moment, and on link i

fi and mi: scalar of external force and moment, and on link i mu and mp: mass of link and platform

G: gravitational acceleration

W_c

i: unit vector normal to two revolute axes of universal joint

C: initial force matrix

F: vector of link reacted force

PL: pressure of load oil QL: load flow

Kvl and Kpt: valve and oil pressure coefficients xp : piston displacement

xv: spool displacement Ap : area of piston

V : total volume of cylinder chamber β: effective bulk modulus of oil Cl: leakage coefficient

Mt, Bp, Kc : coefficients of mass, damping, and spring

ζ

and

ω

h: hydraulic damping ratio, hydraulic natural frequency, k1 and k2: hydraulic constants

u1 and u2: force and velocity command for servo valve Kv : constant

Fc: computed force Kp : P gain of velocity Pa: actual position

Pe: trajectory point most close to Pa

E: position tracking error vector Er: path contour error vector Kv and Kiv: PI gains of MCCPM u and v: parameters of interpolation

s0 : stiffness in friction model

s1 : damping coefficient in friction model

s2: can be treated as Coulomb coefficient in friction model Fco : Coulomb friction force

Fs : Stribeck force vs : Stribeck velocity Ff: friction force tc: chip thickness Kt , Kr , Ka : tool parameters Y : regressor matrix q: parameter vector

V: Lyapunov candidate function

CHAPTER 1

INTRODUCTION

1.0 Introduction

In general, parallel manipulator with physically closed form has advantages of high stiffness, low inertia, and large loading capacity. Parallel manipulators have been widely found in many applications, such as aircraft simulators, anti-vibrated payloads, and high-accuracy telescope. Since parallel manipulators possess higher degrees of freedom with compact mechanism, it is very suitable to be as milling or cutting machine tool. In the other side, as known, hydraulic cylinder has large power and stable dynamic performance. Therefore, parallel manipulator driven by hydraulic cylinder can provide larger loading application and milling motion, and its development becomes more and more important.

In hydraulic cylinder system, the loading force may cause effects on piston motion and make the piston unable to track trajectory accurately. A control strategy of computed force to reject force effect is proposed. With velocity control, computed force controller derives desired actuating force and compensates the valve command. Therefore, dynamic characteristics of parallel manipulator are required for obtaining the reacting forces and loads on cylinder, when platform moving.

In addition, the MCCPM is introduced to make an attempt on tracking trajectory accurately. Since, by MCCPM, trajectory error is compensated in link space, then it is unnecessary to compensate trajectory on Cartesian space. The forward kinematics is avoided, and the control efficiency is increased. Therefore, the milling ability and precision of hydraulic manipulator could be improved.

Besides, a novel control strategy is advised for such complicated system as parallel manipulator. For cutting machine, external force, like cutting force and friction, will make machine deteriorating performance. It is difficult to model these uncertain reacting forces for computed force control. Therefore, for more accurate control ability, an adaptive control, which can eliminate the problem of uncertainty of system, is developed for parallel machine tool.

The dynamic formulation of hydraulic manipulator is developed by Newton method in Chapter 2. Also, dynamic characteristic of hydraulic cylinder and computed force will be observed and discussed. In Chapter 3, the MCCPM method will be introduced; the interpolator will be used to obtain trajectory function. In Chapter 4, experimental results of trajectory tracking are exhibited and performance of

controllers will be compared and discussed. Further, the conclusion and summary are drawn in Chapter 5. In Chapter 6, for future development, a simple method is advised to accomplish adaptive control algorithm, and include external reaction force, such as friction and cutting force, which will be also introduced [1][2].

1.1 Background

Hydraulic manipulator, with high load-capacity, is most used as flight simulator, which needs large power. However, it’s inadequate to be as a milling machine for its poor precision. Generally, hydraulic machine has poor dynamic characteristics and unable to accurately track working trajectory. Many hydraulic researches focus on how to improve the accuracy and control performance. Concepts for computed force on hydraulic cylinder had been proposed; Lischinsky et al [3] computed load force with friction model and compensated it, but no dynamic is considered; Kwon et al [4] compensated the load and friction effect for tracking control, however they didn’t apply it on parallel manipulator and system dynamic wasn’t concerned. Zhow [5] developed force compensation controller for hydraulic robot, yet not for parallel manipulator. Kosuge et al [6] used feedback force compensation to achieve velocity control, whereas, it’s very passive and unable to offer actual force information.

On the other hand, in the cutting machine, precisely tracking spatial trajectory is required; therefore, MCCPM (Multi-axis cross-couple pre-compensation) [7][8][9] algorithm is introduced to analyze the contour error and compute desired

compensation for spatial trajectory. As known, in contrast to serial manipulator, link of parallel manipulator is not orthogonal, so that, the contour error should be

transformed to link error and it’s very time-consuming. MCCPM can directly derive compensating velocity of link and help platform track its trajectory rapidly. Here, the MCCPM algorithm is redefined and redeveloped to apply to our three-axis hydraulic manipulator [10].

Adaptive control [11]-[15] of manipulator has been very interested in many

researches for recent decade, and even Dasgupta and Mruthyunjaya [16]had gathered

the most control scheme and brought out ideas of parallel control strategy. It can provide robust control for coupled and nonlinear system and guarantee tracking stability. With powerful computational ability of modern computer, dynamical modeled mechanism can be calculated in time. Then, the control algorithm computes how to compensate the effects of mechanical system, including inertial, Coriolis, gravity, friction and other force. Adaptive control with cross-coupled

pre-compensation has been discussed in Chin and Tsai’s [9], yet he implemented it on robotic manipulator (PUMA 560). In the future, adaptive control scheme can be utilized on hydraulic manipulator, and consider reacted friction and milling force on machine tool to realize its practicability.

CHAPTER 2

KINEMATICS AND DYNAMICS ANALYSES

OF HYDRAULIC MANIPULATOR

2.0 Introduction

Our hydraulic parallel machine which has three degrees-of-freedom is composed of three hydraulic cylinders and one moving platform. Each cylinder is connected with universal joints in their both ends, and the platform is bounded on the up-end of piston. The position and orientation of the platform is determined by lengths of cylinders. Generally, inverse kinematics is used to derive how long the cylinders should lengthen and make cylinders reach desired position and orientation of platform. Our main purpose is to control the three cylinders to track desired trajectories. For controlling the three hydraulic cylinders of machine more efficiently, dynamic model must be concerned. Inverse dynamics is utilized to derive demand forces of cylinders when trajectory proceeding. So, the computed force can provide the command of control cylinder. Since dynamic formulations of Parallel Platform are quite numerically complicated, many mathematical methods, for, Lagrange [17], and principal virtual work method [18][19], have been formulated for solving this

dynamics problem. In this paper Newton-Euler formulation [20][21][22] is introduced to derive dynamics, and the dynamic of hydraulic will be discussed, too.

2.1 Coordinate system

The moving platform of hydraulic manipulator possesses three

degrees-of-freedom which can be written a coordinate vector q with position and orientation variables [20]

( , , )z α β T =

q (2.1)

z

are Cartesian vector along to Z-axis, and a,b are Euler angle representation. The link space is consisted of 3-variables

[

1 2 3

]

T l l l = l (2.2)

Frame P

Platform

z

y

x

Frame W

W

_Y

W

_X

W

_Z

2

a

1

a

3

a

1 b 3

b

Fig. 2.1. (b) Hydraulic motion platform

Hydraulic manipulator is as shown in Fig. 2.1 (a) and (b).A triangular support is connected to the center of platform with a pair of revolute joints; therefore it can eliminates residual degrees of freedom when singularity of equivalent lengths occurring. Whereas, the triangular support would constrain mobility of manipulator and reduce its workspace, and it drives the platform respect to X axis. As parallel manipulator, three hydraulic cylinders’ lengths specify the pose of platform. And operator reaches the desired pose by adjusting lengths of cylinders. In Fig. 2.2, the frame W is world fixed frame; the frames P are reference frames which are attached to the moving platform, as seen in Fig. 2. The origin of coordinates P and W is assumed to be located on the mass center of platform and base. At initial position, coordinate frame P reference to frame W are represented

(

0 0 z

)

T

= W_P

(2.3) The other coordinate frame P with orientation rotation reference to frame W can be

written as [20]

(

0 0 z_P

)

T WR_P

= +

Fig. 2.2. Coordinates transformation

W_R

p is rotational matrix which is consisted of rotation about

x,y

-axes: Rotation a about the X-axis of the moving coordinate P

1 0 0 ( , ) 0 0 R x c s s c α α α α α ⎡ ⎤ ⎢ ⎥ =_⎢ − _⎥ ⎢ ⎥ ⎣ ⎦ (2.5)

Rotation b about the Y-axis of the moving coordinate P

0 ( , ) 0 1 0 0 c s R y s c β β β β β ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢− ⎥ ⎣ ⎦ (2.6)

Since the angular rotation about the Z-axis of the moving coordinate P is locked, then the transformation matrix is as element matrix

1 0 0 ( , ) 0 1 0 0 0 1 R z γ ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ (2.7)

The rotational matrix WRp is obtained by multiplying the three rotation matrix

( , ) ( , ) ( , ) 0 W P c s s s c R R z R y R x c s s c s c c β β α β α γ β α α α β β α β α ⎡ ⎤ ⎢ ⎥ = =_⎢ − _⎥ ⎢− ⎥ ⎣ ⎦ (2.8)

2.2 Inverse kinematics

2.2.1. Inverse position kinematics

In Figure 2.1 (a), the points ai i=1,2,3 on the moving platform are joint locations. The ai vector reference to frame P can be written as [20]

W P R = + W W P i p i a x a , Wx_p =

[

0, 0,z

]

T (2.9) p_a

i is a vector ai with reference to frame P. Once the ai is obtained, the limb vector Li can be expressed as

= −

W W W

i i i

L a b (2.10)

The limb length li which is the distance vector Li can be computed as

i

l = L L (2.11) _i⋅ _i

Thus, the link space can be written as

[

1 2 3

]

T T l l l = = ⎡_{⎣ 1 2 3} ⎤_⎦ l L L L (2.12)

and unit vector can also be obtained [20]

i l = W W i i L n (2.13)

2.2.2. Inverse velocity kinematics

The velocity of ai is determined by taking time differentiating of equation (2.9)

W P R = + ×   W W P i p P i a x ω a , Wx_p =

[

0, 0,z

]

Tandω_P = ⎣⎡α β  0⎤_⎦T(2.14) Then, the limb velocity is projection of velocity vector ai on the limb vector ni

(

W

)

i P

l =W  ⋅W =W  ⋅W + ⋅ R P ×W

i i p i P i i

a n x n ω a n (2.15)

From above Equation (2.15), an inverse Jacobian matrix can be found [20]

1 l=J− W_{q ,}  z α β ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦     W_q (2.16) where

(

)

(

)

, 1 , T W P _{x y} T W P _{x y} R J R − ⎡ × ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ × ⎢ ⎥ ⎣ ⎦ # # T P W 1,z 1 1 T P W 3,z 3 3 n a n n a n , subscript x, y, z (2.17)

The singular position will occur when det(J)=0, which may appear in the workspace. In this case, the trajectory planning needs to avoid the singularities place for precise work.

2.2.3. Inverse acceleration kinematics

The acceleration of ai is determined by taking time differentiating of Equation (2.14) [20]

(

)

W W P P R R = + × + × ×   W W P P i p P i p P i a x α a ω ω a _,

_[

_{0, 0,}

_]

T z =   W p x (2.18)

Therefore, the l_i can be easily found by differentiating Equation (2.15) respecting to time

(

)

(

)

i i l = ⋅ −l × × ⋅  W_ W W _ W i i i i i i a n ω ω n n (2.19) where

(

)

li = W × W i i i ω n a (2.20) i

obtained

(

2l_i

)

l_i

= W ×W − 

i i i i

α n a ω (2.21)

The angular velocity and acceleration variables ω and _i α are used to compute the _i link dynamics.

2.3 Inverse dynamics

2.3.1 Dynamics equation

The dynamics of parallel manipulators is complicated and highly nonlinear. There exist several approaches to build the dynamics, such as Newton-Euler formulation, Lagrangian formulation, and the principle of virtual work method. In this paper, Newton-Euler formulation is used to develop the dynamics equations of the system [23].

Fig. 2.3. Forces components and length expressions on link i

The links of the parallel platform are driven by hydraulic piston, which is controlled by servo valve. The link’s upper rod is a moving piston, which is attached to platform with universal joint. The piston is pushed up by hydraulically power pump, besides lower cylinder is stationary part attached to fixed base with universal joint.

The accelerations of the two parts can be derived as [23]

(

l_i l_u

)

(

)

(

l_i l_u

)

2 l_i l_i = − × × + − × + × + W W W W W iu i i i i i i i i a ω ω n α n ω n n (2.22)

(

)

l l l l = × × + × W W W il i i i i i a ω ω n α n (2.23)

where subscript u and l denote upper and lower part of link.

For developing the dynamic of hydraulic manipulator, the force interacted between links and platform is necessary. As seen in Fig 2.3., Fia and Fin are force acted on the spherical joint with platform in frame W: Fia is force component directed to limb axis, and Fin is force component normal to Fia. Therefore, the external force on link Fi is the summation of Fia and Fin.

i =

a n

i i

F F + F (2.24)

Moment of link by external force is equivalent to inertia moment [23]

(

)

( ) ( ) ( ) u i u l l i u l u l u i u l l m l l m l l I I I I m l l m l − × + × + × + = + − + × + − × + × W W W n W i i i i i W W W W i i i i iu i il n G n G n F M α ω ω n a n a (2.25) where mu and ml are mass of rod and cylinder. G is gravitational acceleration, Iu, Il are

inertia moment of mass of rod and cylinder respect to frame W. Mi is reaction moment, which is transmitted from universal joint, and written as

i m = W W i i M c (2.26)

where Wci is a unit vector normal to two revolute axes of universal joint, and mi is magnitude of this reaction moment. Here hydraulic piston and cylinder are assumed as asymmetric rigid bodies, like cylindrical rod.

⎛ _× ⎞ ⎜ ⎟ = × ⎜ _× ⎟ ⎝ ⎠ W W W W i i i i _{W W W} i i i b b n c b b n (2.27)

In order to make equation (2.25) compact, new algebraic variable Ni is assumed

i i i l m l + × = + × = W W n W W n W i i i i i i i M n F c n F N (2.28)

where Ni is from [23]

(

)

( ) ( ) ( ) u i u l l u l u l u i u l l m l l m l I I I I m l l m l = − − × − × + + − + × + − × + × W W W i i i i W W W W i i i iu i il N n G n G α ω ω n a n a (2.29)

Therefore, mi can be obtained by taking dot product of Equation (2.29) with Wni and presented

(

)

(

)

i m = ⋅ ⋅ W W i i W W i i N n c n (2.30)

Determined mi is introduced to equation (2.28) and normal force can be derived Fin .

(

i

)

i m l × − × = W W W W i i i i n i N n c n F (2.31)

For generalization, the entire force and moment, acting on platform respect to Frame

W, are consisted of force and moment from links and can be evaluated as [23]

3 3 1 1 a p i p i i m f m = = −

∑

−

∑

=  W W n P i i x n F G (2.32) where F_ia = f_iaWn , and _i 3 3 3 1 1 1 a W W i P P P P i i i f R R I I = = = −

∑

P ×W −

∑

P × n−

∑

= − × i i i i i P P P a n a F M α ω ω (2.33)

where mp and Ipare mass and inertia moment of platform. Hence, axial force of struts

can be funded from above two force equilibrium equations (2.32) and (2.33), which can be expressed as 3 3 1 1 a p p i i i m m f = = − −

∑

=

∑

 W n W P i i x G F n (2.34) and 3 3 3 1 1 1 W a W P P P i P i i i I I R f R = = = − + × −

∑

P × n−

∑

=

∑

P ×W P P P i i i i i α ω ω a F M a n (2.35)

Because this hydraulic platform moves on Z,a,b, there are only motions about these directions that need to be considered. Then, an inverse Jacobian matrix is found to replace equation (2.34) and (2.35) as

(

)

(

)

1 , , 3 a W W P _{x y} P _{x y} a f C R R f ⎛ ⎞ ⎡ _{⎤ ⎜ ⎟} ⎢ ⎥_{⎜ ⎟}= × × ⎢ _{⎥ ⎜ ⎟} ⎣ _{⎦ ⎝ ⎠} " # " W W 1,z 3,z P W P W 1 1 3 3 n n a n a n (2.36) where 3 1 3 3 1 1 _, p p i _z W P P P i i x y m m C I I R = = = ⎡ ⎛ _{− −} ⎞ ⎤ ⎢ ⎜_⎝ ⎟_⎠ ⎥ ⎢ ⎥ = ⎢_{⎛ ⎞} ⎥ − + × − × − ⎢_{⎜ ⎟} ⎥ ⎝ ⎠ ⎢ ⎥ ⎣ ⎦

∑

∑

∑

 W n P i P n P P P i i i x G F α ω ω a F M (2.37)

(

)

,

(

)

, T W W P _{x y} P _{x y} J R R − ₌⎡_⎢ ⎤_⎥ × × ⎢ ⎥ ⎣ ⎦ " " W W 1,z 3,z P W P W 1 1 3 3 n n a n a n (2.38)

Than the axial force fia of limb i, i=1, 2, 3 , can be obtained by multiplying Jocobian matrix 1 3 a T a f J C f ⎛ ⎞ ⎜ ⎟ = ⎜ ⎟ ⎜ ⎟ ⎝ ⎠ # (2.39)

The force fi of link i, i=1, 2, 3 , actuated by piston is determined by summing reaction force along to axial direction and expressed as

a

i u i u

f =m ⋅W − f −m ⋅W

iu i i

a n G n (2.40)

From above equations, actuation force of links F can be obtained as

(

)

(

)

1 3 u T u f m J C f m − ⋅ ⎛ ⎞ ⎛ ⎞ ⎜ ⎟ ⎜ ⎟ =_{⎜ ⎟}=_{⎜ ⎟}− ⎜ ⎟ ⎜ _{− ⋅} ⎟ ⎝ ⎠ ⎝ ⎠ # # 1u 1 3u 3 a G n F a G n (2.41)

Therefore, an inverse dynamic program for calculating link force with equations (2.1)-(2.41) can be written on computer. It is introduced in Section 2.3.2. In our system, hydraulic piston is independently controlled for tracking in joint space. Thus, the dynamical formulation of manipulator is developed for computing the force, so as

to reject force disturbance and control piston as general linear system. Control strategy of hydraulic piston will be discussed in Section 2.6.

2.3.2 Dynamics programming

With derivation of kinematics and dynamics equations (2.1) – (2.41), a PC is utilized to compute the force for parallel manipulator as it process some trajectories

(

, ,

)

Dynamics q q q

=  

F (2.42)

Here, the results of program simulation are presented. And its material data and design dimension of hydraulic machine are shown in Table. 2.1-2.2.

Table 2.1. Designate data

Mass Inertia (kg) Moment Inertia (kg-m2

)

Upper Limb 5 Iupperaxis 5

Lower Limb 5 Iloweraxis 7

Table 2.2. Platform and Base layouts

Upper platform Lower Bases

φu=800 mm φl=800 mm

θu=120° θl=120°

Link motion and force are simulated by computer and presented as Fig. 2.4 and 2.5. Below figures are divided into cylinder displacement that are computed from equations (2.1)-(2.12), and actuating force computed from equations (2.22)-(2.41) respectively. Two trajectories are tested and their motion function are

( )

( )

(

) ( )

(

)

Trajectory 1: 650, cos 1.2 , sin 1.2

15 15 Z t = α t = π πt β t = π πt

( )

( )

( )

(

)

Trajectory 2: 600 10 , 0, sin 0.4 18 Z t = + t α t = β t = π πt Joint 2 Joint 3 Joint 1 θl Y Joint 2 Joint 3 Joint 1 θu Y X X

( )

( )

(

) ( )

(

)

Trajectory 1: 650, cos 1.2 , sin 1.2

15 15 Z t = α t = π πt β t = π πt 0 2 4 6 8 10 12 14 16 18 20 600 650 700 C y li nd er 1 time Cylinder length (mm) 0 2 4 6 8 10 12 14 16 18 20 600 650 700 C y lin d e r 2 time 0 2 4 6 8 10 12 14 16 18 20 600 650 700 C y li nd er 3 time

Fig. 2.4. (a) Displacement of cylinder from equation (2.12)

0 2 4 6 8 10 12 14 16 18 20 95 100 105 110 C y lin d e r 1 time Cylinder force (Newton)

0 2 4 6 8 10 12 14 16 18 20 95 100 105 110 C y li nde r 2 time 0 2 4 6 8 10 12 14 16 18 20 95 100 105 110 C y li nde r 3 time

( )

( )

( )

(

)

Trajectory 2: 600 10 , 0, sin 0.4 18 Z t = + t α t = β t = π πt 0 2 4 6 8 10 12 14 16 18 20 600 650 700 750 800 850 C y li nd er 1 time Cylinder length (mm) 0 2 4 6 8 10 12 14 16 18 20 600 650 700 750 800 850 C y lin d e r 2 time 0 2 4 6 8 10 12 14 16 18 20 600 650 700 750 800 850 C y li nd er 3 time

Fig. 2.5. (a) Displacement of cylinder from equation (2.12)

0 2 4 6 8 10 12 14 16 18 20 101.5 102 102.5 C y lin d e r 1 time Cylinder force (Newton)

0 2 4 6 8 10 12 14 16 18 20 101.6 101.8 102 102.2 102.4 C y li nde r 2 time 0 2 4 6 8 10 12 14 16 18 20 101.6 101.8 102 102.2 102.4 C y li nde r 3 time

As inverse dynamics is derived, the force computation can be carried out. In figures, the joints receive larger force when cylinders are decreasing length. It is quite reasonable that joint takes more reacted force when downward acceleration, composed of gravity and downward acceleration, increases. The result can help us understand the system model and make correct force command when cylinders are controlled. Above two trajectories will be introduced to trajectory experiment in Chapter 4.

Nevertheless, the inverse dynamics is in an ideal situation and evaluated without considering the friction and some environment effects. For further development, adaptive controller, simplifying dynamical formulation, is implemented to estimate the friction parameter and increase the robustness of the system. The implementation of adaptive controlling will be discussed in Chapter 4.

2.4 Forward kinematics

Generally, link lengths are the only information obtained, so it’s necessary to derive forward kinematics to find the Cartesian coordinate vector of platform. However, the same condition of link lengths may have many different position of platform, the forward kinematics is more difficult to calculate. There are several approaches to find the forward kinematics; Raghavan and Tsai [20] had solved the forward kinematics with 40 possible solutions. Nevertheless, only one solution is consistent with actual position of platform. Therefore, Chin and Peng [25] proposed numerically iterative method, based on the Newton-Raphson method, to find out the approximate solution.

For solving the problem, a closed-loop function FLi(q) is defined as [25]

( )

3

(

( )

2 2

)

1 0 i i i FL L q l = =

∑

− = q (2.43)

where i denotes limb number and Li(q) is link length function, which determines the link length by inverse kinematics with coordinate vector q. Thus, Taylor expansion of the function is taken

( )

| ( )n

( )

| (n1) | (n1) FL FL FL q − − ⎛∂ ⎞ = + _{⋅⎜ ⎟} ∂ ⎝ ⎠ q=q q=q q=q q q ∆q (2.44)

where subscript n means iterative count. So, FLi

q ∂ ∂ is derived as ( 1) ( 1) 1 1 1 3 3 ( ) ( ) ( ) ( ) | | ( ) ( ) n n FL FL FL z FL FL FL z α β β − − ∂ ∂ ∂ ⎛ ⎞ ⎜ _{∂ ∂ ∂} ⎟ ⎜ ⎟ ∂ = ⎜ ⎟ ∂ _{⎜ ⎟} ∂ ∂ ⎜ ⎟ ⎜ _{∂ ∂} ⎟ ⎝ ⎠ # # " q=q q=q q q q q q q q (2.45)

As [20], FLi q ∂

∂ is the same to Jacobian matrix J

-1

. As a result, the desired vector q(n) can be easily computed as

( )

( )

(

( ) ( 1)

)

( )n = (n−1)+ ⋅J FL |q=qn −FL |q=qn−

q q q q (2.46)

2.4.1 Iterative step of Newton-Raphson method

There are several iterative steps as followings

Step 1. Set vector q and link length l as initial position and determine function FL Step 2. Compute deviation vector q∆ by derive J⋅ ∆F q

( )

Step 3. Add q∆ to q, q_{( )n} =q_(n−1)+ ∆ , and determine new function FL(n) q

Step 4. Repeat Step 2 to Step3, until the q∆ is smaller than acceptable error, and q is approximate to realistic position of platform.

Tsai [20] proposed that in addition to boundary, the singularity is occurred when Jacobian matrix of manipulator is singular, and then analytical solution will diverge. In hydraulic manipulator with triangular support, the singularity surface is rejected within workspace. Thus any trajectory in efficacious workspace is allowed and the singular of Jacobian matrix does not exist.

2.5 Forward dynamics

In contrast to inverse dynamics, forward dynamics equations derive the position, velocity, and acceleration condition from information of link force. As inverse

dynamics, several methods [22],[21] are proposed to derive forward dynamic, such as Newton-Euler method, principle of virtual work, and Lagrangian formulation.

Forward dynamics is commonly used for simulating motion process of manipulator. Here a simple dynamics formulation is funded, which can obtain manipulator information with link force input. [26]

( )

(

)

( )

M

= _a + _a _a + _a

F q q N q ,q G q (2.47)

where M is inertia mass matrix, N is vector of centrifugal/Coriolis force, and G is vector of gravitational force.The equations of inverse dynamics are derived and programmed, where program is as a dynamics function with dynamical parameters input

(

)

Dynamics

=  

F q,q,q (2.48)

As known, the analytical solutions of acceleration can be computed by multiplying inverse mass matrix M(qa) [26]

( )(

)

1 ' a M− = −  q q F F (2.49) where

(

_a _a

)

=

(

_a _a

)

+

( )

_a F' q ,q N q ,q G q (2.50)

The F’ program is a subroutine of dynamics program, and it is for computing coupling force effect, which is obtained by dynamics function given q=0 input.

(

0

)

Dynamics

=  =

F' q,q,q (2.51)

Then, for deriving mass matrix M(qa), dynamics function is re-executed with giving

0, 0

= =

q q and gravitational G=0 input. And further column of matrix M(qa) is

computed by input q_i =1and q_j =0 for i≠ j

( )

( )

(

0, 0,

)

| 0

i i G

M q =_⎣⎡# m q #_⎦⎤=Dynamics q= q= q_{i =} (2.52)

Hence, mass matrix can be obtained, and the analytical solution can be obtained. Thus, other velocity, position term are derived by numerically integration. The computer simulator process is similar to [26], and shown in Fig. 2.6

( )

1 a

M

−

q

(

_a, _a, 0

)

Dyn q q Integration (4thRunge Kutta− )

F



q

, 

q q

(

, 0, 0

)

i i m =Dyn q

[ ]

i M = m

q

Simulation

Block

F′

2.6 Dynamics of hydraulic piston and computed force control

The links of manipulator are hydraulic power element, which are combinations of servo valve and piston. In order to improve the manipulator precision, the hydraulic cylinder must be controlled more accurately. The research of hydraulic has been developed for a long time, and many control schemes for improving hydraulic

tracking were investigated and proposed, for instance, conventional PID controller [27] or the adaptive control of hydraulic cylinder [28]. Intuitionally, the load force may have negative contribution to piston, so the load force effect on hydraulic cylinder is concerned about in many papers. When the load is much smaller than allowable range the cylinder can hold, the load force, certainly, has slight effect on hydraulic. But for larger scale of load, large negative effect will dominate the performance of cylinder [4]. Therefore, to reject force effect and improve the performance of cylinder, computed force is proposed for our control system.

The servo valves of hydraulic manipulator are commanded by PC base, and feedback signal of piston length are read by potentiometers. The piston is pushed by oil pressure, and than its motion is involved with spool displacement, which can adjust the oil pressure.

V1 V2 P2 P1 In Out xv xp Mt Kc B F Q1 Q2

Merritt [4] had proposed a flow equations of hydraulic piston is related to spool displacement xv and load pressure PL

(

1 2

)

2

L v v p L

Q Q

Q = + =K x −K P (2.53)

, where QL is load flow which is seen in Fig. 2.7, Kvl and Kpt coefficients, and

(

1 2

)

2

L

P P

P = − . Also, the cylinder continuity equation is approximated by [4]

4 L p p L l L V Q A x P C P β =  +  + (2.54)

, where xp is piston displacement, Ap area of piston, V total volume of cylinder chamber, β effective bulk modulus of oil, and Cl leakage coefficient. Because the oil is compressibility flow, then term

4 L

V P

β  can be ignored, and equation (2.54)

becomes

L p p l L

Q =A x +C P (2.55)

Thus an equivalent equation of load flow QL is derived by combining equation (2.55) and (2.53)

L p p l L v v p L

Q =A x +C P =K x −K P (2.56)

The actuating piston force FL is approximatelyA P , so that piston velocity is relation _{p L} to spool displacement xp and load pressure FL by rearranging equation (2.56)

(

l p

)

v L p v p p p C K K F x x A A A + = −  (2.57)

The spool displacement is proportion to input voltage uv and the valve control input is obtained v iv v x =k u (2.58)

(

)

1 2 l p p v L v p p L iv iv v v iv p C K A x F u x k x k F k k K K k A + = =  + =  + (2.59)

where uv is voltage input of servo valve, kiv is the constant, and k1, k2 are simplified constant.

The FL is desired output force and a control concept of computed force is introduced 1 p 2 d v L u =k x +k F (2.60) p d

x is modified desired piston velocity

(

)

, , , d

p p d p p d p a

x =x +K x −x (2.61)

, where Kp is a proportion gain. So that the tracking error e is guaranteed to converge to zero when Kp is positive

, ,

0,

p p d p a

e K e+ = e x= −x (2.62)

The control strategy of computed force is shown in Fig. 2.8. Desired load force is added to input voltage command with modified desired piston velocity, and xp,a is feedback of piston length.

, p d x

x

v L F 1 k 2 k v u iv k Hydraulic cylinder p a, x p x Kp

Fig. 2.8 Computed force control strategy

The computed force control strategy can be applied on our hydraulic manipulator. For control of hydraulic manipulator, result of inverse dynamic is adopted for deriving actuating force FL. The computed force of hydraulic manipulator is shown in Fig. 2.9.

Link length vector l is identical to piston length, and the subscript d and a are desired and actual condition. Dynamic program is proposed in Sec. 2.3.2., and voltage input

uv of computed force controller is summation of u1 and u2

1 2 , 2 1 p d L k k = =  u F u x (2.63)

Fig. 2.9. Computed force control for manipulator

Combining with forward dynamic proposed in Sec. 2.5, computer simulator for hydraulic manipulator with dynamics of hydraulic actuators is completely developed, as seen in Fig. 2.11.

2.7 Analysis of workspace

Workspace means a territory in which the end-effectors can arbitrarily travel and move. Generally, the workspace of machine tool is as total orientation workspace, TOW. The TOW of parallel manipulator is determined by limits of links’ length. Therefore, define TOW of parallel manipulator with mathematical model is defined

{

| min max, max( ) , means valuable ,

}

T P Z z Z X X z X α β

Ω = ≤ ≤ ≤

Pong [25] evaluated the workspace by computing inverse kinematics with discrete value and iteratively examining constraints of link, which is applied in this paper. Analysis program is consisted of several loops to find out the value boundary and the values will be recorded. The flow chart of program and the analysis of workspace are as Fig. 2.12.

CHAPTER 3

MCCPM AND INTERPOLATION

3.0 Introduction

The MCCPM [10] (Multi-axes Cross-Coupled Pre-compensation) is an algorithm, which was developed by manufacturing laboratory, NCTU. It is for diminishing the trajectory error of manipulators. MCCPM provides a simplified calculation to derive the demand compensative velocity for cutter, if the cutter is not exact on trajectory. In this chapter, MCCPM method is adopted to find out the demand compensative

velocity; thereby the error for exact trajectory tracking is compensated previously. In the past, Chin and Lin [29] and Chin and Lu [10] had sequentially governed the MCCPM algorithm; Chin and Tsai [9] has accomplished the feedback gain

assignment. And, here, MCCPM is implemented in the system, observing its tracking performance. Behind MCCPM, the interpolation of hydraulic manipulator will be introduced.

3.1 MCCPM controller

MCCPM system includes calculations of contour error and compensative velocity. Trajectory error is defined the deviation in space between desired path and real position; MCCPM compensates trajectory error by calculating needed response velocity and adjusting link velocity to track ideal trajectory. Chin and Lin[29] and Chin and Lu [10] had proposed algorithm of cross-coupled pre-compensation for several years, and the algorithm for compensating of hydraulic manipulator is modified.

3.1.1 Trajectory contour error

The trajectory error is expressed as a 3-dimension vector with L1, L2, L3 elements in joint space. Fig. 3.1 is illustration of path contour error between desired path and actual position.

Fig. 3.1. Spatial path contour error

At first, it’s assumed that the machining surface is continuous spatial surface. Pa is actual position of pistons, and Pe is the point most close to Pa on the desired path. The vector E is position tracking error vector which is from actual position Pa to desired position Pd.

[

1 2 3

]

T d a E E E = − = E P P (3.1)

where subscript number means piston’s numbering. Er is the path contour error vector which is the shortest distance between desired trajectory and actual position

[

1 2 3

]

T r = e− a = Er Er Er

E P P (3.2)

Vector VJK is unit vector expressing the velocity from Pe to Pd. Because the curve is

approximately close to straight line, thus the vector VJK can be obtained by taking average of the velocities of Pd and Pa. [10]

a d a d + = +   JK   P P V P P (3.3)

Therefore, the path contour error Er, is derived as seen in Fig. 3.1

(

)

(

)

r = e− a = − d e= − ⋅ = − × × JJJJJK JK JK JK JK E P P E P P E E V V E V V (3.4) Thus Eris [10]

(

) (

)

(

) (

)

(

) (

)

1 3 1 1 3 2 3 2 2 3 1 2 3 1 3 3 1 2 1 2 2 1 3 3 2 3 3 2 1 2 1 1 2 r r r r ⎡ ⎤ ⎡ _{⎤ ⎢ ⎥} ⎢ _{⎥ ⎢ ⎥} ⎢ _{⎥ ⎢ ⎥} ⎢ _{⎥ ⎢ ⎥} ⎣ _{⎦ ⎢ ⎥} ⎣ ⎦ JJK JJK JJK JJK JJK JJK JJK JJK JJK JJK JJK JJK JJK JJK JJK JJK JJK JJK V E V - E V +V E V - E V E E = E = V E V - E V +V E V - E V E _{V E V - E V +V E V - E V} (3.5)

and its distance is

( )

2 2 2 3 1 1 3 1 3 2 2 3 2 3 1 2 2 1 1 ( ) 1 ( ) 1 ( ) r dist = − − + − − + − − JJK JJK JJK JJK JJK JJK JJK JJK JJK E V E V E V V E V E V V E V E V (3.6)

From equation (3.5), MCCPM can quickly obtain the actual error Er. Thus, the

compensating velocity can be derived by multiplying a gain value, such as reciprocal of sampling time.

3.2 Pre-compensation velocity

From equation (3.5), the total contour error Er is obtained

[

1 2 3

]

T r = Er Er Er

E (3.7)

Error vector Er is used to calculate the compensative velocity for diminishing contour error. PI controller is applied for modifying velocity. The adjustable velocity can be obtained as r r a K_v K_iv dt = + +

∫

JK JK JJK JJK V V E E (3.8)

and rewritten by vector [7]

1 1 1 1 2 2 2 2 3 3 3 3 a v r iv r a v r iv r a v r iv r K K dt V V K K dt V _{K K dt} ⎡ + + ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ =_{⎢ ⎥ ⎢}= + + ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ _{+ +} ⎣ ⎦

∫

∫

∫

JK V E E V V E E V E E (3.9)

Hence, the trajectory path can be gradually tracked and the contour error will be diminished.

3.3 Interpolator

The interpolator of hydraulic manipulator is made for trajectory planning, thus the information of machining surface is entered and the interpolator determines fitting trajectory of the tool. For general cutting machine, milling cutter is needed to keep perpendicular to the surface of work piece. In other words, the axis of cutter is normal to machining surface. So the interpolator needs two functions; one is to transform the contour of surface in global space to base frame, and then, from inverse kinematics, the desired lengths of links can be obtained. The other is to instantly calculate the working contour and derive tangential velocity, which will be combined with compensative velocity of MCCPM. In this section, the transformation and interpolation will be introduced and discussed separately.

3.3.1 Transformation of link trajectory

Like traditional CNC, surface of work piece is designed by smooth fitting curve and bi-cubic spline algorithm is popular fitting patch [30]. Thus, the surface can be expressed as segmental surface functions and with two parameters u,v

S( , )u v =[S_x S_y S_z] ( , )T u v (3.12) Since it is necessary that the mill cutter is perpendicular to surface, the normal vector

of surface n must be funded

( , ) S S v u n u v S S v u ∂ _×∂ ∂ ∂ = ∂ _×∂ ∂ ∂ (3.13)

Because the milling cutter is normal to platform, then the cutter vector based on effectors can be written as

[

0 0

]

effector

T

Cutter= C (3.14)

where C is length of cutter. Thus, vector n should be transformed to cutter vector by Coordinate transformation

0 cos 0 sin ( , )

0 sin sin cos sin cos ( , )

cos sin sin cos cos ( , )

x y z effector base n u v n u v C n u v β β α β α α β α β α α β − ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ _{= −}⎢ ₋ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ (3.15)

and the orientation a, b are conveniently obtained

-1 -1 2 2 tan y ; tan x z x z n _n n n n α = ⎛⎜ ⎞⎟ β = ⎛_⎜ ⎞_⎟ ⎜ ₊ ⎟ _{⎝ ⎠} ⎝ ⎠ (3.16)

Therefore, the path and orientation vector of moving platform can be written with parameters as

( , ) [ , , , , , ] ( , )T [S( , ); (n( , ))]T

q u v = x y zα β γ u v = u v dir u v (3.17)

As hydraulic manipulator is responsible for DOF of Z, a, b, then q is written

z

( , ) [ ] ( , )T [S ( , ) ( , ) ( , )]T

q u v = z α β u v = u v α u v β u v (3.18)

The vector q is inverted to link vector l by transforming with inverse kinematics. Thus, the link trajectory can be derived.

3.3.2 Interpolation of 3D surface

Lee [30] had proposed that surface equations, like curve, can be represented as parametric equation. The surface equation is expressed by bicubic patch method as a polynomial form 3 3 0 0 S( , ) _ij i j i j u v a u v = = =

∑∑

(0≤ ≤u 1, 0≤ ≤ ) (3.19) v 1

where parameters u and v are variables ranged 0 to 1. Rewrite P(u,v) as a matrix form

[

]

[

]

S(0, 0) S(0,1) S (0, 0) S (0,1) S(1, 0) S(1,1) S (1, 0) S (1,1) S( , ) ( ) ( ) S (0, 0) S (0,1) S (0, 0) S (0,1) S (1, 0) S (1,1) S (1, 0) S (1,1) v v T v v u u uv uv u u uv uv u v F u F v ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ (3.20)

where the blending function F is defined [30]

[

]

2 3 2 3 2 3 2 3

( ) 1 3 2 3 2 2

F u = −_⎣⎡ u + u u − u u− u +u − +u u ⎤_{⎦ (3.21)}

Tangential vectors respect to u and v is found

[

]

S S

[

]

S( , ) ( ) ( ) S S T v u u uv u v F u F v u ⎡ ⎤ ∂ ₌ ⎢ ⎥ ∂ _{⎣ ⎦}

[

]

S S

[

]

S( , ) ( ) ( ) S S T v v u uv u v F u F v v ⎡ ⎤ ∂ ₌ ⎢ ⎥ ∂ _{⎣ ⎦} (3.22)

where subscription u,v means differentiation with respect to u,v [30]

2 2 2 2

( ) [ 6 6 6 6 1 4 3 2 3 ]

u

F u = − +u u u− u − u+ u − +u u (3.23)

However, the bicubic patch can only determine the boundary of surface function, and the surface curve is yet unknown.