TrajectoryTrackingControlofthePhilipsExperimentalRobotArminthePort-HamiltonianFramework DiscreteTechnology&ProductionAutomation

(1)

Discrete Technology & Production Automation

Trajectory Tracking Control of the Philips Experimental Robot Arm in

the Port-Hamiltonian Framework

Master Thesis August 2014

Author: Supervisor:

Floorke Koops Prof. Dr. Ir. J.M.A. Scherpen

Student number: Second Supervisor:

s1639633 Prof. Dr. Ir. B.J. Kooi

Daily Supervisor:

M. Muñoz-Arias

(2)

Abstract

In this research a control model is designed for the first two shoulder joints of the Philips Experimental Robot Arm, such that the arm is able to smoothly follow a desired path. The research is a step in the direction of letting a robot perform simple household tasks. The arm is controlled in the port-Hamiltonian framework, since this framework is suitable for modeling complex systems. By a change of variables, we transform the original port-Hamiltonian system with a non-constant inertia matrix into an equivalent port-Hamiltonian form which has a constant inertia matrix. Furthermore, we apply a coordinate transformation in order to realize a trajectory tracking control strategy in our system. Before doing experiments on the arm, the model is tested with a simulation in Matlab Simulink. The maximum error between the desired and the real joint angles in the simulation is 0.8 degrees. For the experiments on the arm we use a sampling time of 1 ms and a force compensation when the arm is going down. The desired trajectory of the first joint is a sine wave with a period of 20 seconds and an amplitude of 40 degrees. The second joint is controlled to be constantly zero.

The maximum error in the first joint is 1.4 degrees and in the second joint the error is always less than 1 degree. The control model that is designed is a stable control system, since it works for a period of 10, 20 and 30 seconds and for amplitudes of 20, 30 and 40 degrees. Every time the scripts are run with the same parameters the arm has a comparable behavior and the scripts work for more than one cycle.

(3)

(4)

CONTENTS CONTENTS

G The scripts used in the PBC PERA Simulink model 67 G.1 The script for the ‘PERA dynamics’ function block . . . 67 G.2 The script for the ‘PERA controller’ function block . . . 69 H The scripts used in the TTC PERA Simulink model 72 H.1 The script for the ‘PERA’ function block . . . 72 H.2 The script for the ‘PERA controller’ function block . . . 74

I Computation of β 79

J Scripts of the TTC of the PERA 84

J.1 The script ‘PERA_Shoulder’ . . . 84 J.2 The script ‘PERA_Shoulder_Controller’ . . . 89

K Maxon DC motor specifications 95

L Robustness of the TTC scripts 96

(6)

1 INTRODUCTION

1 Introduction

In the present time with lots of technology, robotics is a fast-growing and popular field of engineering [12]. At the beginning, robots were mainly developed for industrial use to take over tasks of human beings, since they are faster, more accurate and cheaper [12]. However, this focus is changing rapidly to usage in human environments, for instance for assisting or serving humans and for entertainment [9, 12].

According to [13], the definition of a robot is ‘an autonomous system which exists in the physical world, can sense its environment, and act on it to achieve some goals’. A robot makes its own decisions, experiences the physical laws, gets information from its environment and responds to it in a useful manner.

The definition makes clear that a robot is a complex machine and understanding it requires knowledge of many scientific fields [23].

The robot that is used in this research is the Philips Experimental Robot Arm (PERA) shown in Figure 1.1. The robot is developed by Philips Applied Technologies and is made for research purposes [21]. Its kinematics corresponds to the anatomy of the human arm. By doing research on the arm the application of robots, for instance doing simple household tasks or caring for people, can be further improved.

Previous studies have been done on the PERA at the University of Groningen by both bachelor [15, 17, 11, 22] and master [2] students. We solely focus on the work of [2] here, where a lot of prework has been done. The scripts of that research are adjusted to a new form, as described in Chapter 3, and the description of the PERA helps to fully understand the robot. In the work of [2]

Figure 1.1: The PERA in the DTPA lab at the University of Groningen.

(7)

1 INTRODUCTION

controllers in the the Euler-Lagrangian framework were compared to controllers in the port-Hamiltonian (pH) framework.

Since the pH controller showed promising results, the PERA is in this research controlled only in the pH framework, which combines the classical La- grangian and Hamiltonian approach and emphasizes the physics of a system [10]. The pH system is ensured to be passive [25]. The framework is very useful to model complex systems, since a total system can be split into simple indepen- dently modeled subsystems that can be interconnected by using the description of the energy exchange between them [25].

The main objective of the project is to design and implement a trajectory tracking control (TTC) model for the PERA, first described in [7], such that the arm can smoothly follow a desired path. With TTC a non-constant inertia matrix can be changed to a constant form, which is helpful for controlling complex systems. The experiments are done on the first two shoulder joints of the PERA, due to time and extend limitations of the research, but the scripts are made in such a way that they can easily be extended to more degrees of freedom.

This research is a step in the direction of the general objective of controlling the arm in such a way that it can move towards an object and grasp it.

The theory behind the control of the PERA can be found in Chapter 2. The first section of the chapter explains the Denavit-Hartenberg (DH) convention and the second section describes the control methods in the pH framework.

The first section of Chapter 3 explains how the PERA works and how it can be controlled. The second and third section respectively describe the Matlab scripts for the simulations and the scripts for the experiments on the PERA. The results of the research are given in two sections, where the first section gives the results of the simulations and the second gives the results of the experiments on the PERA. Both sections can be found in Chapter 4. The results of the research are discussed in Chapter 5. The questions that are still unsolved and the recommendations for future work can be found here. In Chapter 6 the conclusion of the research is formulated.

(8)

2 PRELIMINARIES

2 Preliminaries

In this chapter the theory behind the control of the PERA is described. In Section 2.1 it is described how to use the DH convention. The use of the DH convention facilitates the computation of the inertia matrix of the PERA. A complete overview of the subject is given in [23]. In this study a control design methodology in the pH framework is realized for the PERA. Section 2.2 gives a brief explanation of the framework and the types of control that are tested on the PERA.

2.1 Denavit-Hartenberg convention

To control a system in the pH framework, we need to find the inertia matrix of that system. A convenient way to calculate the inertia matrix is by use of the DH convention. In this section it is described how to apply such a convention and how to derive the inertia matrix out of it. All the information is based on [23].

2.1.1 Joints

A robotic system consists of one or more links which are connected through joints. There are two types of joints, revolute joints and prismatic joints, which are shown in Figure 2.1. Revolute joints make the connected links rotate relative to each other and prismatic joints make the links move linearly with respect to each other. The joint variable of a revolute joint is the angle θ between two joints and the joint variable of a prismatic joint is the extension d of the link.

The degrees of freedom (DOF) of a system is the same as the number of joints the system has.

Since each joint in a system is located between two links, a nDOF robot exists of n + 1 links. Joint i is considered to be stuck to link i − 1, thus when joint i is actuated, link i moves. Each link has a coordinate frame attached in such a way that the coordinates of the link remain constant in that frame. This

Figure 2.1: On the left a revolute joint with variable θ and on the right a prismatic joint with variable d.

(9)

2.1 Denavit-Hartenberg convention 2 PRELIMINARIES

automatically means that when joint i is actuated, frame i changes with respect to the base frame.

2.1.2 Forward kinematics

There are two ways to describe the kinematics of a robot, namely with forward kinematics and with inverse kinematics. Forward kinematics means that the position of the end effector of the robot is obtained by giving the desired values of the joint variables. With inverse kinematics the joint variables have to be computed from the given desired position of the end effector. A problem based on inverse kinematics is often more complex than a problem based on forward kinematics, because the same end effector position can be obtained with different values of the joint variables.

In this study forward kinematics is used to control the PERA. The objective of using forward kinematics is to see what happens to the end effector position by changing the joint variables. The DH convention helps with the analysis of the forward kinematics of a more complex system.

2.1.3 Denavit-Hartenberg representation

Let the transformation matrix that transforms the coordinates of frame i into coordinates of frame i − 1 be defined by a matrix Ai. Ai is a function of the joint variable qi, which is θi if joint i is revolute and di in the case of joint i being prismatic. For a nDOF system, the matrix Tn=Qn

i=1Ai transforms the coordinates of the end effector in the end effector frame into the coordinates of the end effector in the base frame.

In the DH representation, Aiis defined as a product of four transformations:

two rotations and two transformations. As stated in [23] the transformation matrix in DH representation is given by

A_i = Rot_z,θ_iT rans_z,d_iT rans_x,a_iRot_x,α_i (1)

=







cos (θi) − sin (θi) cos (αi) sin (θi) sin (αi) aicos (θi) sin (θi) cos (θi) cos (αi) − cos (θi) sin (αi) aisin (θi)

0 sin (αi) cos (αi) di

0 0 0 1





 (2)

where Rotz,θ_i is a rotation of θi around the z-axis, T ransz,d_i is a translation of di in the direction of the z-axis, T ransx,a_i is a translation of aiin the direction of the x-axis and Rotx,α_i is a rotation of αi around the x-axis. Thus, in order to make the transformation matrices in DH representation, we need to find the parameters a, α, d and θ for each link of the robot.¹ However, before we can do this, we must first know how to define the coordinate frames of the links.

1Note that the parameter d is variable if the joint is prismatic and the parameter θ is variable if the joint is revolute.

(10)

Figure 2.2: Definition of the end effector frame [23].

2.1.4 Coordinate frames

By making the coordinate frames of the links, first the z-axes for link 0 to link n − 1have to be defined. Axis zi−1corresponds to the axis of actuation of joint i, which in case of a prismatic joint is the axis of translation and in the case of a revolute joint is the axis of rotation. The next step is to complete the base frame. In principle the origin of the base frame can be chosen arbitrary on axis z0, but often it is useful to choose it at the center of joint 1. Also axis x0 may be chosen at random, as long as it is normal to the z0-axis, but usually it is chosen in the direction of link 1 if this is not axis z0. Axis y0 is chosen in such a way that the frame is right handed.

In DH representation there are two constraints for the coordinate frames, which are that axis xi and axis zi−1 have to be perpendicular and that they intersect at some point in space. By using these constraints x-axes 1 to n − 1 - and therefore the right handed frames 1 to n − 1 - can be defined after setting up the base frame.

Frame n, the end effector frame, is always specified as shown in Figure 2.2.

The origin is determined exactly between the two end points of the gripper, axis zn is the direction in which the gripper approaches an object and axis ynis the direction in which the fingers slide when the gripper opens and closes. The x-axis is specified by making the frame right handed. The letters a, s and n, which stand for approach, sliding and normal, can also be used in stead of zn, yn and xn respectively.

2.1.5 Parameters

Having put the coordinate frames to the links, we can find the parameters of the DH representation. In Figure 2.3 the four parameters are given for two coordinate frames that obey the constraints for DH convention. Here we can see that parameter ai is given by the distance between oi−1 and oi measured along xi. Parameter αi is defined as the angle between zi−1and zi in the plane normal to xi, where a positive value of α is determined by the right hand rule.

The distance between oi−1and oi measured along zi−1gives the parameter di.

(11)

Figure 2.3: The definition of the four parameters needed to make the transformation matrices in DH representation [23].

Link ai αi di θi

1 0 90° 15 θ1

2 0 0° d2 90°

Table 2.1: Example of a table with all link parameters of a system. The first joint is revolute and the second joint is prismatic.

The last parameter θi is given by the angle between xi−1 and xi in the plane normal to zi−1, where again the positive value of θ is determined by the right hand rule.

Now a table can be made with all the parameters of all the links of the system. If joint i is prismatic the link parameter di is denoted as a variable in the table and if joint i is revolute the link parameter θi is a variable. An example of such a table is given in Table 2.1. The robot that corresponds to this table has a revolute joint (joint 1) and a prismatic joint (joint 2). The table can be used to make the transformation matrix Ai, which is given by (2), for each link.

2.1.6 Jacobian

In order to describe the kinematics of the robot, the Jacobian of the system has to be found. As described below, the linear part of the Jacobian is dependent on the vector on and the angular part of the Jacobian depends on the rotation matrix Rn. Therefore, we need to determine the matrix Tn, which gives us Rn

and on. As described in Section 2.1.3 the matrix Tn, which is a product of the transformation matrices A1 to An, can be used for a transformation of the coordinates in the n^th frame to the coordinates in the base frame. The matrix

(12)

Tn can be further specified, as given in [23], in the following way:

Tn=

Rn on

0 1

(3) in which Rn is the upper left 3 x 3 rotation matrix, on is the upper right 3 x 1 vector and 0 is the 1 x 3 zero vector. In this expression the parameters Rn and o_n, and therefore Tn, depend on the vector of joint variables q = (q1, . . . , q_n)^>. If the robot moves, the joint variables are time-dependent. Let us define the joint velocities as the time derivatives of the joint variables of the moving robot,

˙

q = ( ˙q₁, . . . , ˙q_n)^>. As described in Section 2.1.2, the aim of forward kinematics is to describe the position of the end effector given the joint variables. For a moving robot the end effector position is also a function of time and can be described with the linear and angular velocity. The Jacobian is used to couple the linear and angular velocity of the end effector to the joint velocities of the system. The linear velocity of the end effector is given by vn = ˙on, and the angular velocity is given by² S (ωn) = ˙RnR^>_n. Now we can find the Jacobian by stating that the linear velocity is described by vn = ˜Jvq˙ and the angular velocity is given by ωn = ˜Jωq˙, where ˜Jv and ˜Jω are 3 x n matrices. According to [23], to obtain the total 6 x n Jacobian matrix, the linear part should be put on top of the angular part:

J˜n=

" ˜Jv

J˜_ω

#

(4) There is a simple way to find the linear and angular part of the Jacobian.

Let us split the Jacobian in columns, such that ˜Jv = h ˜Jv₁. . . ˜Jv_ni and ˜Jω = h ˜Jω1. . . ˜Jωni. In case of a revolute joint³ the i^th column of the linear part of the Jacobian is given by ˜J_v_i = z_i−1× (on− oi−1), where zi−1is the projection of the z-axis of frame i − 1 onto the z-axis of the base frame. For a revolute joint the i^th column of the angular part of the Jacobian is given by ˜J_ω_i = z_i−1. The Jacobian matrix is now given by ˜J =h ˜J1. . . ˜Jni, where

J˜i=

z_i−1× (on− oi−1) z_i−1

(5)

2.1.7 Inertia matrix

Once we have found the Jacobian, the inertia matrix can be calculated. As stated before, the inertia matrix of the system is needed to control a robot. As

2If ωn=



 a b c



, then S (ω_n) =





0 −c b

c 0 −a

−b a 0



.

3Since all joints in the PERA are revolute, we only consider the case of a revolute joint here.

(13)

Link a_i α_i d_i θ_i a_c_i d_c_i

1 0 90° 20 θ1 0 10

2 30 0 0 θ2 15 0

Table 2.2: Example of a table with all link parameters of a system, including the center of mass parameters. In this case the center of mass is in the middle of the links.

shown in [23] the inertia matrix M is given by⁴

M =

n

X

k=1

mkJ˜_v^>_kJ˜v_k+ ˜J_ω^>_kRkIkR^>_kJ˜ω_k (6) Here, mk is the mass of link k, ˜Jv_k is linear Jacobian of the k^th link, ˜Jω_k is the angular Jacobian of the k^th link, Rk is the rotation matrix of link k and Ik is the principal moment of inertia of link k. In this formula

J˜_v_k= [z₀× (o_k− o₀) . . . z_n−1× (o_k− o_n−1)] (7)

and J˜ω_k = [z0. . . zn−1] (8)

which are 3 x n matrices. For both the linear and angular Jacobian, the i^th column is the 3 x 1 zero vector if i > k.

2.1.8 Center of mass

For a more realistic model not only the length of the links, but also the center of mass of the links should be considered. In this case it is useful to put two extra columns to the DH convention table, as shown in Table 2.2. With considering the center of mass of the links, the linear Jacobian, and therefore the inertia matrix, changes slightly.

The transformation matrices of the center of mass Aciare comparable to the transformation matrices Aigiven by (2), only aci and dci should be substituted for ai and di, respectively. The center of mass matrix Tcn becomes

T_c_n = A₁A₂. . . A_n−1A_c_n= T_n−1A_c_n (9) Like in (3) we can specify the matrix Tc_n as

Tc_n=

Rc_n oc_n

0 1

(10) The linear Jacobian of (7) takes the form

J˜v_k = [z0× (oc_k− o0) . . . zn−1× (oc_k− on−1)] (11) where the i^th column again is the 3 x 1 zero vector if i > k. With taking the center of mass into account, the inertia matrix of (6) can be calculated by taking the linear Jacobian of (11).

4To not confuse the i^thcolumn of the Jacobian with the Jacobian of the i^thlink, we choose to denote the links with k instead of i here.

(14)

2.2 Port-Hamiltonian framework 2 PRELIMINARIES

2.2 Port-Hamiltonian framework

Once the inertia matrix of the system is found, it can be used in the pH framework to control the system. This section describes the pH system and two different types of control. The theory of energy shaping (ES) is given in Sec- tion 2.2.3 and is described in more detail in [18]. Section 2.2.4 gives the theory behind TTC, which is discussed in more detail in [7].

2.2.1 Port-Hamiltonian system

According to [19], a nDOF pH system with dissipation and m actuators can be described by











˙

x = [J (x) − R (x)]∂H (x)

∂x + g (x) u y = g (x)^>∂H (x)

∂x

(12)

In this formula J (x) is the 2n x 2n interconnection matrix, R (x) is the 2n x 2n damping matrix, H (x) is the Hamiltonian of the system and g (x) is the 2n x m input matrix. The 2n x 1 vector x = (q, p)^> describes the states of the system, the column vector u = (u1, . . . , um)^> gives the control inputs and y = (y1, . . . , ym)^> is the output vector.

The upper part of (12) can for standard mechanical systems be written in matrix form as follows:

q˙

˙ p

=

0 I

−I 0

−

0 0

0 D (x)







∂H (x)

∂q

∂H (x)

∂p





+

0

G (q)

u (13)

where q = (q1, . . . , qn)^> is the vector of generalized position, p = (p1, . . . , pn)^>

is the vector of generalized momenta, I is the n x n identity matrix, 0 is the n x n zero matrix, D (x) is the n x n damping matrix⁵ and G (q) is the n x m input matrix. The Hamiltonian is given by H (x) = K (x) + V (q), where K (x) = ¹₂p^>M (q)⁻¹pis the kinetic energy and V (q) is the potential energy.

The lower part of (12) in matrix form is given by

y =h

0 G (q)^>

i







∂H (x)

∂q

∂H (x)

∂p





 (14)

If the number of actuators m is equal to the degrees of freedom n of the system, we call the system fully actuated. In this case, the input matrix G (q)

5In case of the damping being linear, the damping matrix takes the form D (q, p) = diag (d1, . . . , dn).

(15)

is the n x n diagonal identity matrix and (13) and (14) can be written out as

˙

q = ∂H (x)

∂p (15)

˙

p = −∂H (x)

∂q − D (x)∂H (x)

∂p + u (16)

y = ∂H (x)

∂p (17)

From (15) and (17) it is easy to see that in this case the output matrix y is equal to the time derivative of the generalized coordinates of the configuration

˙

q. Since the PERA is fully actuated, from now on we only consider the theory in case of a fully actuated system.

2.2.2 Control by interconnection

In this study, the pH system is controlled by a method known as control by interconnection, as discussed in [20]. A schematic model of this method is given in Figure 2.4. The figure shows a pH system with a Hamiltonian Hd(x), an input vand an output y. This closed-loop system consists of two pH subsystems, one plant with Hamiltonian H (x) and one controller with Hamiltonian Hc(q). The pH system of the controller has an input uc and an output yc. The input of the plant is u = v − yc, which is the input of the closed-loop system minus the output of the controller. The output y of the plant is the same as the output yof the closed-loop system and also the same as the input uc of the controller.

As given by [20], we get the following equations for a system controlled by

interconnection:

v = u + y_c

y = u_c (18)

2.2.3 Energy Shaping and Damping Injection

We first controlled the PERA with ES. In this method of control a virtual spring, with spring constants Kp = diag (kp₁, . . . , kp_n), and a virtual damper, with damping coefficients C = diag (c1, . . . , cn), are implemented into the system.

The input u of the system is given by [18]

u = ∂V (q)

∂q − Kp(q − qd) − C ˙q + v (19) where qd= (qd₁, . . . , qd_n)^> is a 1 x n vector with the desired values for the joint variables q = (q1, . . . , qn)^>. By substitution of (19) in (16), we get the following expression for the time derivative of the generalized momentum vector of the closed-loop system

˙

p = −∂H (x)

∂q − D (x)∂H (x)

∂p +∂V (q)

∂q − Kp(q − qd) − C ˙q + v (20)

(16)

Figure 2.4: A schematic model of control by interconnection. The pH system with Hamiltonian Hdhas input v and output y, the system with Hamiltonian H has input u and output y and the system with Hamiltonian Hc has input ucand output yc.

Although the inertia matrix M (q) is non-constant, we assume it is constant here, which will be discussed in Section 5.1. The Hamiltonian of the plant is then given by H (x) = ¹₂p^>M⁻¹p + V (q)and the derivative of the Hamiltonian of the plant with respect to q is given by⁶

∂H (x)

∂q = ∂V (q)

∂q (21)

By substituting (21) into (20) and taking (15) into account, we obtain an expression for ˙p as

˙

p = −K_p(q − q_d) − (D (x) + C)∂H (x)

∂p + v (22)

According to [18], the Hamiltonian of the closed-loop system is given by Hd(x) = H (x) + Hc(q) − V (q), thus the derivative of Hd(x)with respect to p is equal to the derivative of H (x) with respect to p:

∂H_d(x)

∂p =∂H (x)

∂p (23)

The Hamiltonian of the controller is given by Hc(q) = ¹₂(q − qd)^>Kp(q − qd), therefore the derivative of the Hamiltonian of the closed-loop system with respect to q is

∂Hd(x)

∂q =∂H (x)

∂q +∂Hc(q)

∂q −∂V (q)

∂q = Kp(q − qd) (24)

6If M (q) is non-constant, then the control law implies ^∂H(x)_∂q = _∂q^∂ ¹₂

p^>M (q)⁻¹p

+

∂V (q)

∂q .

(17)

We now substitute (23) and (24) into (22). The dynamics of ˙p is then given by

˙

p = −∂Hd(x)

∂q − (D (x) + C)∂Hd(x)

∂p + v (25)

Substituting (23) in (15) and (17) gives us the following expression for ˙q and y:

˙

q = y =∂Hd(x)

∂p (26)

In matrix form, ˙q and ˙p of the whole system can be written as

q˙

˙ p

=

0 I

−I −(D (x) + C)







∂H_d(x)

∂q

∂Hd(x)

∂p





+

0 I

v (27)

2.2.4 Trajectory Tracking Control

After using ES, the PERA is controlled by using the method of TTC. Here the inertia matrix M (q) is not assumed constant, but the non-constant M (q) is transformed to a constant form by a change of variables. This method of expressing the system in new coordinates is first mentioned by [24], while generalized canonical transformations were first described by [6] and [7]. By using generalized canonical transformations a time-varying generalized Hamiltonian system is transformed into another one [6] and the error system of the Hamil- tonian system can be stabilized [7].

According to [7], a generalized canonical transformation is given by a set transformations

¯

x = Φ (x) (28)

H (¯¯ x) = H (x) + U (x) (29)

¯

y = y + α (x) (30)

¯

u = u + β (x) (31)

where ¯x is the new state, ¯H (¯x)is the new Hamiltonian, ¯y is the new output and

¯

uis the new input. The error system is constructed in such a way that when the new state is zero, ¯x = 0, then the old state is equal to the desired state, x = x_d. The new system is now described by [7]











˙¯

x =J (¯¯ x) − ¯R (¯x) ∂H (¯¯ x)

∂ ¯x

>

+ ¯g (¯x) ¯u

¯

y = ¯g (¯x)^>∂ ¯H (¯x)

∂ ¯x

> (32)

Consider a coordinate transformation given by [16]

¯ x =

q¯

¯ p

=

q − qd

T (q)⁻¹p − T (q)^>q˙d

(33)

(18)

where T (q) is the lower triangular matrix specified by

M (q) = T (q) T (q)^> (34)

The time derivative of the coordinates given by (15) can be written as ˙q = M (q)⁻¹p. When we substitute (34) in this expression, we obtain an expression for the momenta

p = M (q) ˙q = T (q) T (q)^>q˙ (35) By substituting (35) in (33), the new state of (28) can be expressed as

¯ x =

q − qd

T (q)^>( ˙q − ˙qd)

(36)

In (29), the scalar function U (x), which derivation is given in Appendix A, can be denoted as [16]

U (x) = −1

2 p^>q˙d+ ˙q^>_dp +1

2q˙^>_dT (q) T (q)^>q˙d+ ¯Unc(q − qd) (37) where ¯Unc(q − qd)plays the role of a virtual potential energy [7]. By substituting (37) in (29), we obtain an expression for the Hamiltonian of the new system [16]

H (¯¯ x) = 1

2p¯^>p + ¯¯ U_nc(q − q_d) + V (¯q) (38) The input of the new system is given by (31), where the control input is given by [16]

u = − ¯G (q)⁻¹

T (q)⁻¹f (x) + T (q)⁻¹Kp(q − qd)

− C ¯y (39) where Kpis the spring constant matrix and C is the damping coefficient matrix, as mentioned in Section 2.2.3. The input matrix ¯G (q)is the inverse of the lower triangular matrix T (q) when the system is fully actuated. The control input (39) can therefore be simplified as

u = −f (x) − K (q − qd) − C ¯y (40) The internal forces are given by

f (x) = −∂H (x)

∂q − D (x)∂H (x)

∂p (41)

where the first part is the gravity compensation and the second part is the compensation of the friction force, which is specified in further detail in the

(19)

next section. The free parameter β (x) can be expressed as

β (x) = T (q)

∂

T (q)^>q˙_d

∂t +∂ ˙q^>_dM (q) ˙qd

∂q

>

−



T (q)

∂

T (q)⁻¹p − T (q)^>q˙_d

∂q + D (x)



q˙d

+∂ ¯Unc(q − qd)

∂q

>

(42) For the complete derivation of β (x) we refer to Appendix (B), which is gathered from the work of [16].

2.2.5 Friction force compensation

The performance of a controlled robot is limited by the friction that is present inside the robot [5]. The friction can be overcome by using model-based friction compensation, which means that a force is applied that is opposite in sign to the present friction force [5]. The friction compensation that is used in this research is a combination of three friction models, namely the Coulomb friction model, the viscous friction model and the Stribeck friction model.

The velocity response of the friction models is given in Figure 2.5. The Coulomb friction model represents the friction between two dry surfaces and is, in its simplified form, given by [1]

F = Fcsign ( ˙q) (43)

where Fc is the Coulomb sliding friction force and v is the velocity. This is the most commonly used friction model [1] and it only depends on the sign of the velocity and not on its magnitude [5]. The viscous friction model is a linear function of the velocity and is given by [5]

F = Fvq˙ (44)

where Fv is the viscous coefficient. It represents the behavior of a fluid between two surfaces [5], but it can also depict damping rather well if the viscous coefficient is tuned [1]. The last part of the friction compensation is the Stribeck friction model. The Stribeck friction represents the effect of lubricated contacts, which means that at low velocities the friction decreases with increasing velocity [5]. The complete friction compensation, which is a combination of the three models, is now given by [1]

F =

Fc+ (Fs− Fc) e⁻⁽^{| ˙}^q|^/^qs^˙ ⁾^δ

sign ( ˙q) + Fvq˙ (45) where Fs is the maximum static friction force, ˙qs is the Stribeck velocity and δ is the form factor [5].

(20)

Figure 2.5: The three friction models: a. Coulomb friction, b. Coulomb + viscous friction, c. Coulomb + viscous + Stribeck effect. [5]

Figure 2.6: The effect of α on the complete friction model. By decreasing α, the approximation comes closer to the sign function.

Caution should be taken when using sign ( ˙q), since the sign function can cause problems in simulations [1]. In order to avoid the problems, an approximation for the sign function can be made as follows [8]

sign ( ˙q) ' q˙

(α + ˙q²)¹^/² (46)

where α controls the smoothness of the step function. With decreasing α the corners of the step function get sharper and when α = 0 the approximation equals the sign function, as shown in Figure 2.6.

As mentioned in Section 2.2.4, in the model of TTC the friction force is compensated via the internal forces, where in (41) the part containing damping matrix D (x) represents the friction force. By substituting (46) in (45) and taking into account (15), we find the following expression for damping matrix D (x)

D (x) =

F_c+ (F_s− F_c) e⁻⁽^{| ˙}^q|^/^qs^˙ ⁾^δ

α + ˙q²^−0.5

+ F_v (47)

(21)

3 EXPERIMENTAL SETUP

3 Experimental Setup

In this chapter we describe the experimental setup of the research. The kinematics and the mechatronics of the PERA are described in Section 3.1, which gives an insight in how the arm can move and how it is controlled. Section 3.2 outlines the methods and the scripts that are used to make simulations of the PERA behavior. The scripts for PD control of the PERA developed by [2] are adjusted to a new TTC form and Section 3.3 describes the new scripts that are used to control the PERA.

3.1 Philips Experimental Robot Arm

The PERA is a robot for research purposes [21] that corresponds to the anatomy of the human arm. Some of the applications of research on the PERA could be the development of a robot arm that can do simple household tasks or care for people [21]. The upcoming sections explain the kinematics and mechatronics behind the PERA. The information is gathered mainly from the instruction manual of the arm [21].

3.1.1 Kinematics

The PERA consists of four links⁷, that correspond to the human body, upper arm, lower arm and hand. The links are connected through seven joints that are all revolute. An outline of the kinematics of the PERA is given in Figure 3.1, where the allowed joint movements are depicted by blue arrows. The letters R, P and Y stand for roll, pitch and yaw and the subscripts S, E and W for shoulder, elbow and wrist, respectively. The - for the robot - right arm is shown in the posture with all joint variables equal to zero. Taking this posture as a reference, the maximum and minimum position of the joint angles, which roughly correspond to the limitations of the human arm, can be specified as given in Table 3.1. The positions are also given in the form of figures in Appendix C.

Figure 3.2 is a symbolic representation of Figure 3.1. If the coordinate frames are constructed as described in Section 2.1.4, the parameters for the DH convention can be obtained with the method described in Section 2.1.5. Table 3.2 gives the parameters that we found for the PERA. The table shows that for joints 1, 2, 4 and 6 both ai and di are zero, which means that the lengths of the corresponding links are zero. Therefore, there is one group of three joints and there are two groups of two joints that are at the same location. This is necessary to create a realistic model of the human arm, since the human shoulder has three degrees of freedom and the human elbow and wrist have both two degrees of freedom. A joint with two or three degrees of freedom can always be expressed as two or three joints, respectively, with links of length zero between them [23].

7Actually the PERA has eight links, but four of them have zero length, which is explained later in this section.

(22)

3.1 Philips Experimental Robot Arm 3 EXPERIMENTAL SETUP

Figure 3.1: Graphical representation of the PERA [21].

Name Joint Minimum Maximum

S₁ R_S 0° +90°

S₂ P_S -90° +90°

S₃ Y_S -90° +90°

E₁ P_E -90° +55°

E₂ Y_E -105° +105°

W1 PW -57° +57°

W2 YW -45° +45°

Table 3.1:Minimum and maximum rotation angles for the PERA, taking the posture of Figure 3.1 as the zero posture [21].

Name Link ai αi di θi ac_i dc_i

S₁ 1 0 90° 0 θ₁ 0 0

S₂ 2 0 -90° 0 θ₂ 0 0

S₃ 3 0 90° 320 θ3 0 160

E1 4 0 -90° 0 θ4 0 0

E2 5 0 -90° 280 θ5 0 140

W1 6 0 90° 0 θ6 0 0

W2 7 200 -90° 0 θ7 100 0

Table 3.2: The DH-convention table of the PERA.

(23)

Figure 3.2: Symbolic representation of the PERA.

The parameters aciand dciin Table 3.2 give the length to the center of mass of link i. The center of mass of the links in the PERA is considered to be exactly in the middle of the link, since the components of each link are considered to be generally mass distributed [2]. The length of links 3, 5 and 7 is non-zero. Link 3 corresponds to the human upper arm and has a length of 320 mm. Its weight is roughly 2.9 kg and the center of mass is at 160 mm. The length of link 5, the lower arm, is 280 mm, thus its center of mass is at 140 mm. Its weight is approximately 0.8 kg. The hand, which is represented by link 7, has a length of 200 mm. The center of mass is therefore at 100 mm and its weight is about 0.2 kg.

3.1.2 Differential Drive

Only one of the seven joints of the PERA is a direct rotate, namely the third shoulder joint S3. All the other joints are in pairs driven by a differential drive.

The first two shoulder joints S1 and S2 are driven by one differential drive, as well as the two elbow joints E1 and E2 and the two wrist joints W1 and W2.

A schematic representation of the differential drive is given in Figure 3.3. It consists of two motors, which are connected to two gears [2] depicted in orange in the figure. The gears are both connected to a single output gear, which is depicted in blue. For the shoulder joint the output gear is connected to the upper arm (link 3), for the elbow joint it is connected to the lower arm (link 5) and for the wrist joint it is connected to the hand (link 7).

The inputs of the differential drive are the motor rotations ϕmotor1 and ϕ_motor2. The outputs are the single gear rotations ϕout1 and ϕout2, which are two orthogonal rotations. The relationship between the inputs and the outputs

(24)

Figure 3.3: Schematic representation of the differential drive that is used in the PERA [21].

is given by [21]

ϕout1 = ϕmotor1+ ϕmotor2

2 (48)

ϕout2 = ϕmotor1− ϕmotor2

2 (49)

From (49) it shows that when the motors rotate with speeds that are the same in magnitude and sign, then the single gear rotation ϕout2 = 0, which means that the gear does not rotate around its axis and only rotates around the axis of the motors. From (48) it shows that when the motors rotate with speeds of equal magnitude, but opposite sign, the single gear rotation ϕout1 = 0and therefore the gear does only rotate around its own axis and not around the axis of the motors. These two situations correspond to a movement of only one of the joints that are controlled by the differential drive. Any other combination of motor rotations actuates both of the joints.

3.1.3 Torques

The relationship between the torques of the motors and the output torques is given by the following expression [21]

τout1 = τmotor1+ τmotor2 (50)

τ_out2 = τ_motor1− τ_motor2 (51)

The PERA is controlled with a script that computes the output torques that are needed to let the arm go to a certain position (see Section (3.3)). In order to let the arm make a desired movement, the torques that we want from the

(25)

motors must be calculated from the desired output torques. Therefore we have to rewrite (50) and (51) as

τmotor1 = τout1+ τout2

2 (52)

τmotor2 = τout1− τout2

2 (53)

By using (52) and (53), the torques that are needed from the motors can be calculated. A current must be sent to the motors in order to let the motors produce these torques. In the next section it is described how the current can be calculated from the torques.

3.1.4 Motors

The PERA is controlled with 7 Maxon DC motors⁸, of which an example is given in Figure 3.4. The input Pel is converted into a mechanical power Pmech

and power losses PJ due to winding [14]. The formula for the power balance can be given as [14]

Pel = Pmech+ PJ (54)

In more detail, the power balance can be written as [14]

U_mot· I_mot= π

30000τ_motor· n + R · I_mot² (55) where Umotis the voltage applied to the motor in V, Imotis the current running through the motor in A, τmotor is the torque from the motor in mNm, n is the speed at which the motor rotates in rpm and R is the resistance in Ω. The torque of the motor relates to the current of the motor by a torque constant kM

like [14]

τmotor = kM · Imot (56)

By substituting (56) in (55) and taking into account Ohm’s law, we can write the power balance as

Imot= π

30000 ·n · kM

R +τmotor

kM (57)

By using the formula, it can be calculated how much current has to be sent to the motor in order to get the desired torque from the motor.

3.1.5 Boards

The computer and the PERA communicate with each other via RT-Motion USB controller boards. A schematic overview of the tasks of the boards and the connection between the computer and the boards is given in Figure 3.5. It can be seen from this figure that:

8There is an additional Maxon DC motor for the gripper, which is not taken into account here.

(26)

Figure 3.4: Example of a Maxon DC motor with input Pel and outputs Pmech and PJ [14].

Figure 3.5: Connection of the RT-Motion USB boards and the computer [21].

• board number 1, which is called board 0 in the software, communicates with the motors of the differential drive of the shoulder and their encoders

• board number 2, which is called board 1 in the software, communicates with the motors of the differential drive of the elbow and their encoders

• board number 3, which is called board 2 in the software, communicates with the motor of the third shoulder joint and its encoder and the gripper motor and encoder

• board number 4, which is called board 3 in the software, communicates with the motors of the differential drive of the wrist and their encoders A detailed overview of the wiring between the computer and the motors and sensors is given in Appendix D.

Since it is not possible to send a current from the computer to the motors directly, the computer has to send a reference signal to the board, whereafter

(27)

Figure 3.6: Relationship between the reference signal coming from the computer and the current going to the motors [21]. On the vertical axis the motor current is given in mA and on the horizontal axis the reference signal is given in counts.

the board sends a corresponding current to the motors. There is no linear relationship between the reference signal that is coming from the computer and the current that is sent to the motors. Figure 3.6 shows the relationship between the reference signal and the current for the first board, that is, for the motors of the first two joints of the shoulder. Since we only control the first two joints of the PERA, due to limitations of time and extend, we do not consider the graphs of the other boards here, but they can be found in [21].

With the script that is used for controlling the PERA, the current that has to be sent to the motors for a particular movement is computed. To find the reference signal that the computer has to send to the board corresponding to the desired motor current, the axes of the graph of Figure 3.6 must be exchanged.

A graph of this is shown in Figure 3.7. The dotted black line gives the curve corresponding to the one in Figure 3.6, the solid colored lines are approximations of the curve. The first approximation is the solid green line, which is a linear approximation and has the formula

SW Ref erence = 40 · Imot (58)

This seems to be a poor approximation, but since we only work in the reference signal range of counts between 0 and 16000, it is not deviating much from the other approximations. For the second approximation, the solid red line, the true relationship is broken down in 7 parts that are assumed to be linear. This approximation seems to fit the curve best, but up to a desired current of 62.5 mA the reference signal is 0 counts. This offset has to be overcome, since it delays the response of the arm at the start of the movement. The solid blue

(28)

3.2 Matlab models for simulation 3 EXPERIMENTAL SETUP

Figure 3.7: Relationship between the desired motor current and the reference signal that must be sent to the board. The dotted black line gives the real curve, the green line gives the linear approximation, the red line gives the approximation in linear parts and the blue line gives the polynomial approximation.

line is the polynomial approximation. It is given by a second order polynomial trend line through the curve and its formula is

SW Ref erence = −0.0116 · I_mot² + 36.96 · Imot+ 1593.2 (59) To see which approximation works best, they are all tested on the PERA. The results are shown in Section 4.2.1.

3.2 Matlab models for simulation

Before testing TTC on the PERA, some models are made in Matlab to predict its behavior. A realistic model of the PERA is needed in order to get a natural predicted behavior. In Section 3.2.1 it is discussed how a 3D figure is made with the DH convention of the PERA. Two models are used to test the behavior of the PERA, Section 3.2.2 gives the ES model and Section 3.2.3 describes the TTC model.

3.2.1 Matlab Robotics Toolbox

With the Robotics Toolbox for Matlab [3] it is possible to make a dynamic 3D figure of the PERA, where the joint variables can be changed. In this way it is easy to visualize how the robot moves in space. We used the toolbox to see if the DH-convention made for the PERA is right and to see how the robot acts when it is controlled.

(29)

Name Link a_i α_i d_i θ_i a_c_i d_c_i

S1 1 0 -90° 0 θ1 0 0

S2 2 800 90° 0 θ2 250 0

Table 3.3: DH-convention for only the first two links of the shoulder assuming all other joints fixed.

A robot can be easily created in Matlab using the Robotic Toolbox. The link parameters for each link of the robot have to be specified. To create a link Li, type Li=link([αi ai θi di σi oi]) in the command window of Matlab, where αi and θi are specified in radians and ai and di are specified in meters. The parameter σidetermines whether the joint corresponding to the link is prismatic or revolute. If the joint is revolute σi should be equal to zero and in case of a prismatic joint σi should be a non-zero constant. With oi it is possible to give the joint variable an offset, which must be given in radians if the joint is revolute or in meters if the joint is prismatic. By the command Li.qlim=[qmin

q_max], the limits of the joints can be specified, where qminis the lower limit and q_max is the upper limit of the joint variable.

Once the links are created, it is possible to create a nDOF robot by typing r=robot({L1L2. . . Ln}) in the command window. The command plot(r) makes a static plot of the robot, which is often not very useful. However, with the command drivebot(r) it is possible to make a dynamic plot of the robot, where it is possible to change the joint variables.

Table 3.2 on page 22 gives the DH-convention we found for the PERA. The last two columns give the specifications for the center of mass, which are not needed for the configuration of the 3D model of the PERA. With the script given in Appendix E.1 a dynamic model of the PERA is created⁹, which is shown in Figure 3.8. In the model it is possible to change the joint variables and to see how the PERA would move. With this model it becomes clear that the DH-convention that is found for the PERA is right.

In this study the control methods are tested on only the first two joints of the shoulder of the PERA due to time and extend limitations of the research. A new DH-convention is made for only those two links. The convention is given in Table 3.3, where the center of mass of the second link is calculated by assuming that the arm is in the straight position. The script given in Appendix E.2 is used to make the model of the first two links with the Robotics Toolbox. No figure is given of this, because it is just a single rod with two joints at the beginning.

3.2.2 Energy Shaping and Damping Injection

With ES the first two links of the PERA are controlled in such a way that the joint variables q = (q1, q₂)^> go to some desired constants qd = (q_d₁, q_d₂)^>. To

9To clarify the model: When the red bar is pointing into the screen, the x-axis is at the bottom and the y-axis is at the left, the model is comparable to seeing the PERA from the back.

(30)

Figure 3.8: Model of the PERA made with the Robotics Toolbox for Matlab.

control the shoulder, the inertia matrix of the first two joints of the PERA has to be found. By using the parameters as given in Table 3.3, the linear Jacobian matrix of (11) and the angular Jacobian matrix of (8) can be calculated as given in Appendix F. By putting the Jacobians in (6), the inertia matrix M (q) of the first two joints of the PERA is calculated as

M (q) =

ml²_ccos²(q₂) + I₁+ I₂ 0 0 ml_c²+ I₂

(60) where m is the mass of the arm, lcis the length from the shoulder to the center of mass of the arm, q2= θ2is the variable of joint 2 and I1and I2are the principle moments of inertia of link 1 and link 2 respectively. The inertia matrix is used in the scripts described in the following paragraphs.

The model given in Figure 3.9 is made in Simulink to control the shoulder with the method of ES. In the model some values - position q, momentum p and output u - are the inputs of a block called ‘PERA dynamics’. The dynamics block calls a Matlab script, which is given in Appendix G.1. In the script the system constants are defined and the mechanics of the robot arm are described in the pH framework. The damping in (25) is assumed to be linear and the damping constants are given by d1 = d2 = 15. The output (27) of the script is the vector ˙x = ( ˙q, ˙p)^>. The output is integrated in the Simulink model to obtain the vector x, which is used in the upper loop as the input for the ‘PERA dynamics’ block.

In the lower loop, both ˙x and x are used as the input for a new Matlab function block called ‘PERA controller’. The block calls a Matlab script, given in

(31)

Figure 3.9: The ES model of the PERA made in Simulink.

Appendix G.2, which is made to control the robot arm. The controller matrices K_p and C, described in Section 2.2.3, are defined as kp1 = k_p₂ = 15 and c1 = c₂ = 2. Furthermore, the desired joint variables qd are defined as qd1 = q_d₂ = 90°. The output u of the script, which is given by (19), is used as an input for the ‘PERA dynamics’ Matlab function block.

The output vector x is not only used in the control loop, but is as well written to the Matlab Workspace. It is now possible to let the model of the arm, made with the Robotics Toolbox as described in Section 3.2.1, move in the way the robot arm would move when controlled with the Simulink model. To do this, type drivebot(r,q) in the command window of Matlab. Since the time is written to the Workspace as well, a plot of the joint variables with respect to time can be made in Matlab. The results are given in Section 4.1.1.

3.2.3 Trajectory Tracking Control

After the ES simulations, it is evaluated how the PERA would behave when controlled with the method of TTC. With TTC again we want to control the arm in such a way that the joint variables q go to some desired values qd. However, now the desired values do not represent constants in space, but they describe a certain trajectory that the arm has to follow. The Simulink model that is made for the simulations is given in Figure 3.10. The lower loop of this model is a bit more complex than for the ES model, but the upper loop is very similar.

The script that is called by the Matlab function block ‘PERA’ is given in Appendix H.1. As in the ES model, the script has the position q, momentum p and ‘PERA controller’ output u as its inputs. In addition, the speed ˙q is used as an input, since in the theory of TTC it is needed in the damping matrix D (x), as described in Section 2.2.5. The output ˙x of the script is split in the Simulink model in the vectors ˙q and ˙p, where ˙q is thus used as an input for the

(32)

Figure 3.10: The TTC model of the PERA made in Simulink.

‘PERA’ block. Like in the ES model, output ˙x is integrated to obtain the vector x, which is split in q and p to serve as an input for the ‘PERA’ function block again.

The output u of the ‘PERA controller’ function block is generated by the script that is given in Appendix H.2. The script not only has the inputs x and

˙

x, but in addition it has the inputs qd, ˙qd, and ¨qd. As mentioned before in this section, the desired values qd describe a trajectory in space, that is, they are time dependent. The values can therefore not be put inside the script, but have to be generated outside the function block. As shown in the Simulink model, we have chosen to control the arm with a sine wave. This because the sine wave is a complex function for the arm to follow, but it is simple to differentiate and integrate. The formula of the sine wave that is used during the research is given in Section 4.1.2, but the scripts work for any sine wave that is within the limitations of the PERA.

In the TTC model the damping matrix D (x) in (41) and (42) is not assumed to be linear, but depends on the speed as in (47). The damping parameters are found experimentally and are given in Table 3.4. As described in Section 5.2, the parameters still need to be fine-tuned. The form factor δ is assumed to be 1and therefore not used in the formula. The damping matrix coefficients that are used in both scripts ‘PERA’ and ‘PERA controller’ are now

di =

0.5 + (0.9 − 0.5) e⁻^{| ˙}^qi|^/^0.02

0.0001 + ˙q_i²−0.5

+ 0.5 (61)

(33)

Parameter Value

Fc 0.5

Fs 0.9

˙

qs 0.02

α 0.0001

Fv 0.5

Table 3.4: The values of the parameters that are used in the damping matrix in the TTC model.

according to (47).

Based on [21], the moving parts of the PERA have a total mass of 3.9 kg.

However, it is found out that the controller has a better performance while using a mass of 3.8 kg. This difference could be due to rounding, since the mass of the PERA is only specified for the upper arm, lower arm and hand separately.

The mass of the arm is therefore 3.9 ± 0.15 kg. The center of mass is calculated as

lc =(2.9 · 160 + 0.8 · (320 + 140) + 0.2 · (320 + 280 + 100))

3.9 = 249 mm (62)

by using the specifications given in Section 3.1.1. The inertia’s I1 and I2 of the system are obtained experimentally and still need to be fine-tuned.

As mentioned in 2.2.4, in the model of TTC the dynamics of the system is not described in terms of the inertia matrix M (q), but in terms of the lower triangular matrix T (q). Since the inertia matrix (60) of the PERA is diagonal, the lower triangular matrix is simply the square root of the elements of M (q)

T (q) =

"

ml_c²cos²(q2) + I1+ I2^0.5

0 0 ml_c²+ I2^0.5

#

(63)

The main difference between the controller scripts of PCB and TTC is that with TTC the new output u is given by

u = v − β (x) (64)

which corresponds to (31) in Section 2.2.4. It is not feasible to put β (x) into the script in the form of (42), since it contains a derivation with respect to time.

By rewriting β (x) it is possible to eliminate this derivation, as shown for the

TrajectoryTrackingControlofthePhilipsExperimentalRobotArminthePort-HamiltonianFramework DiscreteTechnology&ProductionAutomation

Discrete Technology & Production Automation