Robot Manipulators 2011 Part 2 doc

Then the so–called joint configuration space is specified by the vector while the pose position and orientation of the end–effector frame with respect to the base frame see Fig.. 3.1 Di

matrix, but maps into two unit quaternions, representing antipodes in the unit hypersphere

Also from (13) we can verify that (Rη ε,− =) R(η ε, )T and, as ⎡⎣η − ⎤ ∈εT⎦T S3 is the conjugate

(or inverse) of ⎡⎣η εT⎤ ∈⎦T S3, then we have that

Moreover, the premultiplication of a vector v∈ \ by a rotation matrix 3 R∈SO(3) produces

a transformed (rotated) vector w Rv= ∈ \ The same transformation can be performed with 3

quaternion multiplication using w= ⊗ ⊗ , where ξ v ξ∗ ξ∈ is the unit quaternion S3

corresponding to R and quaternions v , w H∈ are formed adding a null scalar part to the

corresponding vectors (v= ⎣⎡0 v T⎤⎦ , T w= ⎣⎡0 w T⎤⎦ ) In sum T

In the case of a time–varying orientation, we need to establish a relation between the time

derivatives of Euler parameters ξ=⎡⎣η ε T⎤⎦T∈\4 and the angular velocity of the rigid body

3

ω∈ \ That relation is given by the so–called quaternion propagation rule (Sciavicco &

Siciliano, 2000):

1( )

2E

where

4 3( ) ( )

3 Robot Kinematics and Dynamics

Serial robot manipulators have an open kinematic chain, i.e., there is only one path from one

end (the base) to the other (the end–effector) of the robotic chain (see Fig 3) A serial

manipulator with n joints, either translational (prismatic) or rotational, has n degrees of

freedom (dof) This section deals only with the modeling of serial manipulators

As explained in (Craig, 2005), any serial robot manipulator can be described by using four

parameters for each joint, that is, a total of 4n parameters for a n –dof manipulator Denavit

and Hartenberg (Denavit & Hartenberg, 1955) proposed a general method to establish those parameters in a systematic way, by defining n+ coordinate frames, one per each link, 1

including the base The four Denavit–Hartenberg (or simply D–H) parameters for the i -th

joint can then be extracted from the relation between frame i − and frame i , as follows: 1

θ : Angle between axes x i−1 and x i, about axis z i−1

Note that there are three constant D–H parameters for each joint; the other one (θi for rotational joints, d i for translational joints) describes the motion produced by such i -th

joint To follow the common notation, let q i be the variable D–H parameter corresponding

to the i –th joint Then the so–called joint configuration space is specified by the vector

while the pose (position and orientation) of the end–effector frame with respect to the base

frame (see Fig 3), denoted here by x , is given by a point in the 6–dimensional pose

configuration space \3×M3, using whichever of the representations for M3⊂ \ That is m

3 M3 3m p

3.1 Direct Kinematics

The direct kinematics problem consists on expressing the end–effector pose x in terms of

q That is, to find a function (called the kinematics function) h:\n→\3×M3 such that

Traditionally, direct kinematics is solved from the D–H parameters using homogeneous

matrices (Denavit & Hartenberg, 1955) A homogeneous matrix combines a rotation matrix

and a position vector in an extended 4 4× matrix Thus, given p∈ \ and 3 R∈SO(3), the

corresponding homogeneous matrix T is

4 4SE(3)

SE(3) is known as the special Euclidean group and contains all homogeneous matrices of the

form (21) It is a group under the standard matrix multiplication, meaning that the product

of two homogeneous matrices T T1, ∈2 SE(3) is given by

In terms of the D–H parameters, the relative position vector and rotation matrix of the i -th

frame with respect to the previous one are (Sciavicco & Siciliano, 2000):

where C x, S x stand for cos( )x , sin( )x , respectively

The original method proposed by (Denavit & Hartenberg, 1955) uses the expressions in (23)

to form relative homogeneous matrices i1

i T

− (each depending on q i), and then the composition rule (22) to compute the homogeneous matrix of the end–effector with respect

to the base frame, ( ) SE(3)T q ∈ :

An alternative method for computing the direct kinematics of serial manipulators using

quaternions, instead of homogeneous matrices, was proposed recently by (Campa et al.,

2006) The method is inspired in one found in (Barrientos et al 1997), but it is more general,

and gives explicit expressions for the position p q( )∈ \ and orientation 3 ξ( ) Sq ∈ ⊂ \ in 3 4

terms of the original D–H parameters

Let us start from the expressions for i1

i R

− and i1

i p

− in (23) We can use (12) and some trigonometric identities to obtain the corresponding quaternion i1

i

ξ

− from i1

i R

− ; also, we can extend i1

i

p

− to form the quaternion i1

i p

− The results are

cos cos

0cos sin

cos( )

S

sin( )sin sin

ξ is the unit quaternion (Euler parameters) expressing the orientation of the end–effector

with respect to the base frame The position vector ( )p q is the vector part of the quaternion

( )

p q

An advantage of the proposed method is that the position and orientation parts of the pose

are processed separately, using only quaternion multiplications, which are computationally

more efficient than homogeneous matrix multiplications Besides, the result for the

orientation part is given directly as a unit quaternion, which is a requirement in task space

controllers (see Section 5)

The inverse kinematics problem, on the other hand, consists on finding explicit expressions for

computing the joint coordinates, given the pose coordinates, that is equivalent to find the

inverse funcion of h in (20), mapping \3×M3 to \n:

1( )

The inverse kinematics, however, is much more complex than direct kinematics, and is not

derived in this work

3.2 Differential Kinematics

Differential kinematics gives the relationship between the joint velocities and the

corresponding end–effector’s linear and angular velocities (Sciavicco & Siciliano, 2000)

Thus, if = d ∈\n

dt

q q is the vector of joint velocities, v∈ \ is the linear velocity, and 3 ω∈ \ 3

is the angular velocity of the end–effector, then the differential kinematics equation is given by

( )

( )( )

p o

J q v

( )

p q q

h q

q q

J q ∈ \ + × is known as the analytic Jacobian of the robot, and it relates the joint

velocities with the time derivatives of the pose coordinates (or pose velocities)

In the particular case of using Euler parameters for the orientation, φ( )q =ξ( ) Sq ∈ ⊂ \ , 3 4

The linear velocity v of the end–effector is simply the time derivative of the position vector

p (i.e v p=  ); instead, the angular velocity ω is related with ξ by (19) So, we have

find its inverse mapping, although this is possible only if such a matrix has full rank In the

case of a matrix J∈ \n m× , with n m > , then there exists a left pseudoinverse matrix of J ,

such that J J† = ∈ \I m m× On the contrary, if n m < , then there exist a right pseudoinverse

J = =J J−

3.3 Lagrangian Dynamics

Robot kinematics studies the relations between joint and pose variables (and their time

derivatives) during the motion of the robot’s end–effector These relations, however, are

established using only a geometric point of view The effect of mechanical forces (either

external or produced during the motion itself) acting on the manipulator is studied by robot

dynamics

As pointed out by (Sciavicco & Siciliano, 2000), the derivation of the dynamic model of a

manipulator plays an important role for simulation, analysis of manipulator structures, and

design of control algorithms There are two general methods for derivation of the dynamic

equations of motion of a manipulator: one method is based on the Lagrange formulation and

is conceptually simple and systematic; the other method is based on the Newton–Euler

formulation and allows obtaining the model in a recursive form, so that it is computationally

more efficient

In this section we consider the application of the Lagrangian formulation for obtaining the

robot dynamics in the case of using Euler parameters for describing the orientation This

approach is not new, since it has been employed for modeling the dynamics of rigid bodies

(see, e.g., Shivarama & Fahrenthold, 2004; Lu & Ren, 2006, and references therein)

An important fact is that Lagrange formulation allows to obtain the dynamic model

independently of the coordinate system employed to describe the motion of the robot Let us

consider a serial robot manipulator with n degrees of freedom Then we can choose any set

of m n k= + coordinates, say ρ∈ \ , with m k≥ holonomic constraints of the form 0

( ) 0

i

γ ρ = (i= , , , ), to obtain the robot’s dynamic model If 1 2… k k= we talk of independent 0

or generalized coordinates, if k ≥ we have dependent or constrained coordinates 1

The next step is to express the total (kinetic and potential) energy of the mechanical system

in terms of the chosen coordinates Let (K ρ ρ, ) be the kinetic energy, and ( )U ρ the

potential energy of the system; then the Lagrangian function (L ρ ρ, ) is defined as

τ ∈ \ is the vector of (generalized or constrained) forces associated with each of

the coordinates ρ, vector ( )γ ρ is defined as

and λ∈ \ is the vector of k Lagrange multipliers, which are employed to reduce the k

dimension of the system, producing only n independent equations in terms of a new set

r

r r g

Now consider the total mechanical energy of the robot (E r r, =) K(r r, +) U( )r Using

properties (34)–(36), it can be shown that the following relation stands

And, as the rate of change of the total energy in a system is independent of the generalized

coordinates, we can use (37) as a way of convert generalized forces between two different

generalized coordinate systems

For an n –dof robot manipulator, it is common to express the dynamic model in terms of the

joint coordinates, i.e r q≡ ∈ \ , this leads us to the well–known equation (Kelly et al., 2005): n

Subindexes have been omitted in this case for simplicity; τ is the vector of joint forces (and

torques) applied to each of the robot joints

Now consider the problem of modeling the robot dynamics of a non–redundant (n≤ ) 6

manipulator, in terms of the pose coordinates of the end–effector, given by

3 S3 7

T

x=⎡⎣p ξ ⎤⎦ ∈\ × ⊂\ Notice that in this case we have holonomic constraints If n= 6

then the only constraint is given by (9), so that:

where matrices M x x( ), C x x x( , , ( )) g x x can be computed from M q( ), (C q q, , ( )) g q in (38)

using a procedure similar to the one presented in (Khatib, 1987) for the case of operational

ξ is the analytic Jacobian in (29) To obtain the previous equations we are

assuming, according to (37), that

Equation (39) expresses the dynamics of a robot manipulator in terms of the pose

coordinates ( p , ξ) of the end–effector In fact, these are a set of seven equations which can

be reduced to n , by using the 7 n − Lagrange multipliers and choosing a subset of n

generalized coordinates taken from x This procedure, however, can be far complex, and

sometimes unsolvable in analytic form

In theory, we can use (32) to find a minimal system of equations in terms of any set of

generalized coordinates For example, we could choose the scalar part of the Euler

parameters in each joint, ηi, (i= , , , ) as a possible set of coordinates To this end, it is 1 2… n

useful to recall that the kinetic energy of a free–moving rigid body is a function of the linear

and angular velocities of its center of mass, v and ω, respectively, and is given by

where m is the mass, and H is the matrix of moments of inertia (with respect to the center

of mass) of the rigid body The potential energy depends only on the coordinates of the

center of mass, p , and is given by

( )p =m g T p,U

where g is the vector of gravity acceleration Now, using (30), (31), we can rewrite (40) as

Considering this, we propose the following procedure to compute the dynamic model of

robot manipulators in terms of any combination of pose variables in task space:

1 Using (41), find the kinetic and potential energy of each of the manipulator’s links,

in terms of the pose coordinates, as if they were independent rigid bodies That is,

for link i , compute (Ki pi, , ξ ξi i) and ( ).Ui p i

2 Still considering each link as an independent rigid body, compute the total

Lagrangian function of the n –dof manipulator as

i i n

x

p x

x x

3 Now use (9), for each joint, and the knowledge of the geometric relations between

links to find 6n holonomic constraints, in order to form vector γ ρ( )∈ \ 6n

4 Write the constrained equations of motion for the robot, i e (32)

5 Finally, solve for the 6n Lagrange multipliers, and get de reduced (generalized)

system in terms of the chosen n generalized coordinates

This procedure, even if not simple (specially for robots with n> ) can be useful when 3

requiring explicit equations for the robot dynamics in other than joint coordinates

4 Path Planning

The minimal requirement for a manipulator is the capability to move from an initial posture

to a final assigned posture The transition should be characterized by motion laws requiring

the actuators to exert joint forces which do not violate the saturation limits and do not excite

typically unmodeled resonant modes of the structure It is then necessary to devise planning

algorithms that generate suitably smooth trajectories (Sciavicco & Siciliano, 2000)

To avoid confusion, a path denotes simply a locus of points either in joint or pose space; it is

a pure geometric description of motion A trajectory, on the other hand, is a path on which a

time law is specified (e.g., in terms of velocities and accelerations at each point of the path)

This section shows a couple of methods for designing paths in the task space, i.e., when

using Euler parameters for describing orientation

Generally, the specification of a path for a given motion task is done in pose space It is

much easier for the robot’s operator to define a desired position and orientation of the end–

effector than to compute the required joint angles for such configuration A time–varying

position ( )p t is also easy to define, but the same is not valid for orientation, due to the

difficulty of visualizing the orientation coordinates

Of the four orientation parameterizations mentioned in Section 2, perhaps the more intuitive

is the angle/axis pair To reach a new orientation from the actual one, employing a minimal

displacement, we only need to find the required axis of rotation u∈S2 and the

corresponding angle to rotate θ \ Given the Euler parameters for the initial orientation, ∈

[η εT]T∈S3

o o , and the desired orientation, [η εT]T∈S3

d d , it can be shown, using (11) and quaternion operations, that (Campa & de la Torre, 2006):

We can use expressions in (42) to define the minimal path between the initial and final

orientations A velocity profile can be employed for θ in order to generate a suitable ( )tθ

However, during a motion along such a path, it is important that u remains fixed Also

notice that when the desired orientation coincides with the initial (i.e., η ηo= d, εo=εd), then

 0

θ= and u is not well defined

Now consider the possibility of finding a path between a given set of points in the task space

using interpolation curves The simplest way of doing so is by means of linear interpolation

Let the initial pose be given by T T T

o o

For the position part of the motion, the linear interpolation is simply given by

( ) (1 ) o d

but for the orientation part we can not apply the same formula, because, as unit quaternions

do not form a vector space, then such ( )ξ t would not be a unit quaternion anymore

Shoemake (1985) solved this by defining a spherical linear interpolation, or simply slerp, which

is given by the following expression

sin([1 ] ) sin( )( )

It is possible to show (Dam et al., 1998) that such ( )ξ t in (43) satisfies that ξ( ) St ∈ , for all 3

0≤ ≤ (Dam et al., 1998) is also a good reference for understanding other types of t 1

interpolation using quaternions, including the so called spherical spline quaternion

interpolation or squad, which produces smooth curves joining a series of points in the

orientation manifold

Even though these algorithms have been used in the fields of computer graphics and

animation for years, as far as the authors’ knowledge, there are few works using them for

motion planning in robotics

5 Task-space Control

As pointed out by (Sciavicco & Siciliano, 2000), the joint space control is sufficient in those

cases where it is required motion control in free space; however, for the most common

applications of interaction control (robot interacting with the environment) it is better to use

the so–called task space control (Natale,2003) Thus, in the later years, more research has been

done in this kind of control schemes

Euler parameters have been employed in the later years for the control of generic rigid

bodies, including spaceships and underwater vehicles The use of Euler parameters in robot

control begins with Yuan (1988) who applied them to a particular version of the resolved–

acceleration control (Luh et al., 1980) More recently, several works have been published on

this topic (see, e.g., Lin, 1995; Caccavale et al., 1999; Natale, 2003; Xian et al., 2004; Campa et

al., 2006, and references therein)

The so–called hierarchical control of manipulators is a technique that solves the problem of

motion control in two steps (Kelly & Moreno-Valenzuela, 2005): first, a task space kinematic

control is used to compute the desired joint velocities from the desired pose trajectories;

then, those desired velocities become the inputs for an inner joint velocity control loop (see

Fig 4) This two–loop control scheme was first proposed by Aicardi et al (1995) in order to

solve the pose control problem using a particular set of variables to describe the end–

effector orientation

The term kinematic controller (Caccavale et al., 1999) refers to that kind of controller whose

output signal is a joint velocity and is the first stage in a hierarchical scheme On the other

hand dynamic controller is employed here for those controllers also including a position

controller but producing joint torque signals

In this section we analyze two common task–space control schemes: the resolved–motion rate

control (RMRC), which is a simple kinematic controller, and the resolved–acceleration control

(RAC), a dynamic controller These controllers are well–known in literature, however, the

stability analysis presented here is done employing Euler parameters as space–state

variables for describing the closed–loop control system

Figure 4 Pose–space hierarchical control

5.1 Pose Errror and Control Objective

For the analysis forthcoming let us consider that the motion of the manipulator’s end–

effector is described through its pose, given by x q( ) ⎡p q( )T ξ( )q T⎤T 3 S3

= ∈\ × , and its linear and angular velocities referred to the base frame, p q( )∈\3 and ω( )q ∈ \ , respectively 3

These terms can be computed from the measured joint variables (q q, ∈ \n) using the direct

kinematics function (20) and the differential kinematics equation (27)

Also, let the desired end–effector trajectory be specified by the desired pose

3 3( ) ( )T ( )T T S

with matrix operator ( )E ⋅ defined in (18) The desired pose x d can be considered as

describing the position and orientation of a desired frame with respect to the base frame, as

shown in Fig 5

The position error vector is simply the difference between the desired and actual positions

the robot’s end–effector, that is p= p d−p; the linear and angular velocity errors are

computed in a similar way

d d

However, as unit quaternions do not form a vector space, they cannot be subtracted to form

the orientation error; instead, we should use the properties of the quaternion group algebra

Let ξ= ⎣⎡η ε T⎤⎦T be the Euler parameters of the orientation error, then (Lin, 1995):

d d

T d d

Figure 5 Actual and desired coordinate frames for defining the pose error

Taking the time derivative of (46), considering (17), (44) and properties (3), (4), and (8), it can

be shown that (Fjellstad, 1994):

01

( )

T

d S

Notice that, considering the properties of unit quaternions, when the end-effector

orientation equals the desired orientation, i.e ξ ξ= d, the orientation error becomes

Let us consider a control scheme such as the one in Fig 4 As the robot model we take the

joint–space dynamics given in (38)

The RMRC is a kinematic controller first proposed by Whitney (1969) to handle the problem

of motion control in operational space via joint velocities (rates) Let 6

d

x x, ∈ \ be the actual and desired operational coordinates, respectively, so that x x= d−x is the pose error in

operational space The RMRC controller is given by

[ ]

†( )

†( ) d p d

K p p

which has a similar structure than (49) but now we employ the geometric Jacobian instead

of the analytic one, and the orientation error is taken as ε (the vector part of the unit

quaternion ξ ), which becomes zero when the actual orientation coincides with the desired

K K, ∈ \ are positive definite gain matrices for the position and orientation parts, ×

respectively

In order to apply the proposed control law it is necessary to assume that the manipulator

does not pass through singular configurations —where ( )J q losses rank— during the

execution of the task Also it is convenient to assume that the submatrices of ( )J q , J q p( )

and ( )J q o , are bounded, i.e., there exist positive scalars k J p and k J o such that, for all q∈ \ n

we have

Finally, for the joint velocity controller in Fig 4 we use the same structure proposed in

d d

K p p

K p p

where we have employed (47) Fig 6 shows in a detailed block diagram how this controller

is implemented

Substituting the control law (52) in the robot dynamics (38), and considering (53) we get

0

v

considering (27), and (45) we get two decoupled equations

Figure 6 Hierarchical control: RMRC + inverse–dynamics velocity controller

The closed–loop equation of the whole controller can be obtained taking p , η and ε as

states for the outer (task space) loop and v for the inner loop The state equation becomes

( )1

21

System (57) is non–autonomous due to the term ωd( )t , and it is easy to see that it has two

Notice that both equilibria represent the case when the end–effector’s pose has reached the

desired pose, and the joint velocity error, v , is null However, E1 is an asymptotically stable

equilibrium while E2 is an unstable equilibrium

The Lyapunov stability analysis presented below for equilibrium E1 of system (57) was

taken from (Campa et al., 2006) More details can be found in (Campa, 2005)

Let us consider the function:

Notice that α> is enough to ensure that (0 V p, , ,η ε v is positive definite in ) D rr, around the

equilibrium E1, that is to say that (0 1 0 0) 0V , , , = and there is a neighborhood B⊂D rr

around E1 such that (V p, , , >η ε v) 0, for all p T η εT v T T ⎡0T 1 0T 0T⎤T B

& & & &

     & & & &