CRC Press - Robotics and Automation Handbook Episode 1 Part 3 ppsx

Nagurka Marquette University Ben-Gurion University of the Negev 4.1 Introduction Scope • Background 4.2 Theoretical Foundations Newton-Euler Equations • Force and Torque Balance on an Is

Trang 1

3-28 Robotics and Automation Handbook

/* This is the forward reduction */

for(i=0;i<n;i++) {

coef = a[i][i];

for(j=n-1;j>=0;j ) {

y[i][j] /= coef;

a[i][j] /= coef;

}

for(k=i+1;k<n;k++) {

coef = a[k][i]/a[i][i];

for(j=n-1;j>=0;j ) {

y[k][j] -= coef*y[i][j];

a[k][j] -= coef*a[i][j];

}

/* This is the back substitution */

for(i=n-1;i>=0;i ) {

for(k=i-1;k>=0;k ) {

coef = a[k][i]/a[i][i];

for(j=0;j<n;j++) {

y[k][j] -= coef*y[i][j];

a[k][j] -= coef*a[i][j];

}

return y;

}

B.6 File ‘‘matrixproduct.c’’

/* This file is "matrix product.c"

*

* This file multiples a and b (both square, n by n matrices) and

* returns a pointer to the matrix c

*

*/

double** matrixproduct(double **a,double **b,double **c,int n) { int i,j,k;

for(i=0;i<n;i++)

for(j=0;j<n;j++)

c[i][j] = 0.0;

Trang 2

Inverse Kinematics 3-29

for(i=0;i<n;i++)

for(j=0;j<n;j++)

for(k=0;k<n;k++)

c[i][j] += a[i][k]*b[k][j];

return c;

}

B.7 File ‘‘inversekinematics.h’’

/*

*/

#include<stdio.h>

#include<stdlib.h>

#include<math.h>

#define MAX_ITERATIONS 1000

#define EPS 0.0000000001

#define PERTURBATION 0.001

#define DIAGONAL_EPS 0.0001

#define N 6

double** forwardkinematics(double *alpha, double *a, double *d,

double *theta, double **T);

double** matrixinverse(double **J, double **Jinv, int n);

double** pseudoinverse(double **J, double **Jinv);

double** computejacobian(double* alpha, double* a, double* d,

double* theta, double** T, double** J);

double** matrixproduct(double **a,double **b, double **c, int n); double** homogeneoustransformation(double alpha, double a, double d, double theta, double **T);

B.8 File ‘‘dh.dat’’

0 0 0

-90 0 0

0 24 6

-90 2 24

90 0 0

-90 0 0

Trang 3

B.9 File ‘‘Tdes.dat’’

-0.707106 0 0.707106 12

0 -1 0 12

0.707106 0 0.707106 -12

0 0 0 1

B.10 File ‘‘theta.dat’’

24.0

103.0

145.0

215.0

29.2

30.0

References

[1] Craig, J.J (1989) Introduction to Robotics Mechanics and Control Addison-Wesley, Reading, MA.

[2] Denavit, J and Hartenberg, R.S (1955) A kinematic notation for lower-pair mechanisms based on

matrices, J Appl Mech., 215–221.

[3] Murray, R.M., Zexiang, L.I., and Sastry, S.S (1994) A Mathematical Introduction to Robotic Manip-ulation, CRC Press, Boca Raton, FL.

[4] Gupta, K.C (1997) Mechanics and Control of Robots Springer-Verlag, Heidelberg.

[5] Paden, B and Sastry, S (1988) Optimal kinematic design of 6 manipulators Int J Robotics Res.,

7(2), 43–61

[6] Lee, H.Y and Liang, C.G (1988) A new vector theory for the analysis of spatial mechanisms

Mechanisms and Machine Theory, 23(3), 209–217.

[7] Raghavan, M (1990) Manipulator kinematics In Roger Brockett (ed.), Robotics: Proceedings of Symposia in Applied Mathematics, Vol 41, American Mathematical Society, 21–48.

[8] Isaacson, E and Keller, H.B (1966) Analysis of Numerical Methods Wiley, New York.

[9] Naylor, A.W and Sell, G.R (1982) Linear Operator Theory in Engineering and Science

Springer-Verlag, Heidelberg

Trang 4

Newton-Euler Dynamics of Robots

Mark L Nagurka

Marquette University

Ben-Gurion University of the Negev

4.1 Introduction

Scope • Background

4.2 Theoretical Foundations

Newton-Euler Equations • Force and Torque Balance on an Isolated Link • Two-Link Robot Example • Closed-Form Equations

4.3 Additional Considerations

Computational Issues • Moment of Inertia • Spatial Dynamics

4.4 Closing

4.1 Introduction

4.1.1 Scope

The subject of this chapter is the dynamic analysis of robots modeled using the Newton-Euler method, one of several approaches for deriving the governing equations of motion It is assumed that the robots are characterized as open kinematic chains with actuators at the joints that can be controlled The kinematic chain consists of discrete rigid members or links, with each link attached to neighboring links by means

of revolute or prismatic joints, i.e., lower-pairs (Denavit and Hartenberg, 1955) One end of the chain is fixed to a base (inertial) reference frame and the other end of the chain, with an attached end-effector, tool, or gripper, can move freely in the robotic workspace and/or be used to apply forces and torques to objects being manipulated to accomplish a wide range of tasks

Robot dynamics is predicated on an understanding of the associated kinematics, covered in a sepa-rate chapter in this handbook An approach for robot kinematics commonly adopted is that of a 4× 4 homogeneous transformation, sometimes referred to as the Denavit-Hartenberg (D-H) transformation (Denavit and Hartenberg, 1955) The D-H transformations essentially determine the position of the origin and the rotation of one link coordinate frame with respect to another link coordinate frame The D-H transformations can also be used in deriving the dynamic equations of robots

4.1.2 Background

The field of robot dynamics has a rich history with many important developments There is a wide literature base of reported work, with myriad articles in professional journals and established textbooks (Featherstone, 1987; Shahinpoor, 1987; Spong and Vidyasager, 1989; Craig, 1989; Yoshikawa, 1990; McKerrow, 1991;

Trang 5

Newton-Euler Dynamics of Robots 4-5

where the mass moments of inertia are with respect to each link’s centroidal frame Superscript 0 is used

to denote the inertial reference frame R.

Equation (4.9) can be used to find the torques at the joints, as follows:

0T1,2=0T2,3+0d1,2× m2 a2+0p1,2×0F2,3

−0d1,2× m2 g + I2 α2+0ω2×

I2 ω2

(4.12)

0T0,1=0T1,2+0d0,1× m1 a1+0p0,1×0F1,2

−0d0,1× m1 g+ I1 α1+0

ω1×

I1 ω1

(4.13)

An expression for0F1,2can be found from Equation (4.7) and substituted into Equation (4.13) to give

0T0,1=0T1,2+0d0,1× m1 a1+0p0,1× m2 a2+0p0,1×0F2,3−0p0,1× m2 g

−0d0,1× m1 g + I1 α1+0ω1×

I1 ω1

(4.14)

For a planar two-link robot with two revolute joints, as shown in Figure 4.3, moving in a horizontal plane, i.e., perpendicular to gravity, several simplifications can be made in Equation (4.11) to Equation (4.14):

(i ) the gyroscopic terms0ω n× (CIn ω n ) for n= 1, 2 can be eliminated since the mass moment of inertia

matrix is a scalar and the cross product of a vector with itself is zero, (ii) the gravity terms0dn−1,n × m n g

disappear since the robot moves in a horizontal plane, and (iii) the torques can be written as scalars and act

perpendicular to the plane of motion Equation (4.12) and Equation (4.14) can then be written in scalar form,

0T1,2=0T2,3+0

d1,2× m2 a2

· ˆu z+0

p1,2×0F2,3

· ˆu z + I2 α2 (4.15)

0T0,1=0T1,2+0

d0,1× m1 a1

· ˆu z+0

p0,1× m2 a2

· ˆu z+0

p0,1×0F2,3

· ˆu z + I1 α1 (4.16)

where the dot product is taken (with the terms in parentheses) with the unit vector ˆuzperpendicular to the plane of motion

A further simplification can be introduced by assuming the links have uniform cross-sections and are made of homogeneous (constant density) material The center of mass then coincides with the geometric center and the distance to the center of mass is half the link length The position vectors can be

0F0,1

0p0,1

0T1,2

0T2,3 0F2,3

0p1,2

0F1,2

q2 x1

x2

x0

y0

y1

y2

FIGURE 4.3 Two-link robot with two revolute joints.

Trang 6

written as,

0p0,1= 20d0,1, 0p1,2= 20d1,2 (4.17) with the components,

0p 0,1x

0p 0,1y

= 2

0d 0,1x

0d 0,1y

=

l1C1

l1S1

,

0p 1,2x

0p 1,2y

= 2

0d 1,2x

0d 1,2y

=

l2C12

l2S12

(4.18)

where the notation S1 = sin θ1, C1 = cosθ1, S12 = sin(θ1 + θ2); C12 = cos(θ1 + θ2) is introduced; and

l1, l2are the lengths of links 1,2, respectively

The accelerations of the center of mass of each link are needed in Equation (4.15) and Equation (4.16) These accelerations can be written as,

0a1=0α1×0d0,1+0ω1×0

ω1×0d0,1

(4.19)

0a2= 20a1+0α2×0d1,2+0ω2×0

ω2×0d1,2

(4.20)

Expanding Equation (4.19) and Equation (4.20) gives the acceleration components:

0a 1x

0a 1y

=

−1

2l1S1θ¨1−1

2l1C1θ˙2 1

2l1C1θ¨1−1

2l1S1θ˙2

(4.21)

0a 2x

0a 2y

=

−

l1S1+1

2l2S12

¨

θ1−1

2l2S12θ¨2− l1 C1θ˙2−1

2l2C12( ˙θ1+ ˙θ2)2

l1C1+1

2l2C12

¨

θ1+1

2l2C12θ¨2− l1 S1θ˙2−1

2l2S12( ˙θ1+ ˙θ2)2

(4.22)

Equation (4.15) gives the torque at joint 2:

0T1,2=0T2,3+ m20

d 1,2xˆux0+0d 1,2yˆuy0

×0

a 2xˆux0+0a 2yˆuy0

· ˆu z

+0

p 1,2xˆux0+0p 1,2yˆuy0

×0

F 2,3xˆux0+0F 2,3yˆuy0

· ˆu z + I2 α2

where ˆux0, ˆuy0 are the unit vectors along the x0, y0axes, respectively Substituting and evaluating yields,

0T1,2=0T2,3+1

2m2l1l2C2+1

4m2l2+ I2¨

θ1+1

4m2l2+ I2¨

θ2+1

2m2l1l2S1θ˙2

Similarly, Equation (4.16) gives the torque at joint 1:

0T0,1=0T1,2+ m10

d 0,1xˆux0+0d 0,1yˆuy0

×0

· ˆu z

+ m20

p 0,1xˆux0+0p 0,1yˆuy0

×0

· ˆu z

+0

p 0,1xˆux0+0p 0,1yˆuy0

×0

F 2,3xˆux0+0F 2,3yˆuy0

· ˆu z + I1 α1 which yields after substituting,

0T0,1=0T1,2+1

4m1l2+ m2 l2+1

2m2l1l2C2+ I1¨

θ1+1

2m2l1l2C2

¨

θ2

−1

2m2l1l2S2θ˙2−1

2m2l1l2S2θ˙2− m2 l1l2S2θ˙1θ˙2

Trang 7

Spong, M.W and Vidyasager, M., Robot Dynamics and Control, John Wiley & Sons, New York, 1989.

Swain, A.K and Morris, A.S., A unified dynamic model formulation for robotic manipulator systems,

J Robotic Syst., 20, No 10, 601–620, 2003.

Vukobratovic, M., Potkonjak, V., and Matijevi, V., Dynamics of Robots with Contact Tasks, Kluwer,

Dordrecht, 2003

Walker, M.W and Orin, D.E., Efficient dyamic computer simulation of robotic mechanisms, Trans ASME,

J Syst., Meas Control, 104, 205–211, 1982.

Yoshikawa, T., Foundations of Robotics: Analysis and Control, MIT Press, Cambridge, MA, 1990.

Trang 8

Lagrangian Dynamics

Miloˇs ˇZefran

University of Illinois at Chicago

Francesco Bullo

University of Illinois at

Urbana-Champaign

5.1 Introduction 5.2 Preliminaries

Velocities and Forces • Kinematics of Serial Linkages

5.3 Dynamic Equations

Inertial Properties of a Rigid Body • Euler-Lagrange Equations for Rigid Linkages • Generalized Force Computation

• Geometric Interpretation

5.4 Constrained Systems

Geometric Interpretation

5.5 Impact Equations 5.6 Bibliographic Remarks 5.7 Conclusion

5.1 Introduction

The motion of a mechanical system is related via a set of dynamic equations to the forces and torques the

system is subject to In this work we will be primarily interested in robots consisting of a collection of rigid links connected through joints that constrain the relative motion between the links There are two main formalisms for deriving the dynamic equations for such mechanical systems: Newton-Euler equations that are directly based on Newton’s laws and Euler-Lagrange equations that have their root in the classical work

of d’Alembert and Lagrange on analytical mechanics and the work of Euler and Hamilton on variational calculus The main difference between the two approaches is in dealing with constraints While Newton’s equations treat each rigid body separately and explicitly model the constraints through the forces required

to enforce them, Lagrange and d’Alembert provided systematic procedures for eliminating the constraints from the dynamic equations, typically yielding a simpler system of equations Constraints imposed by joints and other mechanical components are one of the defining features of robots so it is not surprising that the Lagrange’s formalism is often the method of choice in robotics literature

5.2 Preliminaries

The approach and the notation in this section follow [21], and we refer the reader to that text for additional details A starting point in describing a physical system is the formalism for describing its motion Since

we will be concerned with robots consisting of rigid links, we start by describing rigid body motion Formally, a rigid bodyO is a subset of R3where each element inO corresponds to a point on the rigid

body The defining property of a rigid body is that the distance between arbitrary two points on the rigid

body remains unchanged as the rigid body moves If a body-fixed coordinate frame B is attached to O, an

Trang 9

arbitrary point p ∈ O can be described by a fixed vector pB As a result, the position of any point in O

is uniquely determined by the location of the frame B To describe the location of B in space we choose

a global coordinate frame S The position and orientation of the frame B in the frame S is called the

configuration of O and can be described by a 4 × 4 homogeneous matrix gSB:

gSB=

RSB dSB

, RSB∈ R3 ×3, dSB∈ R3, R TSBRSB= I3, det(RSB)= 1 (5.1)

Here I n denotes the identity matrix in Rn ×n The set of all possible configurations ofO is known as SE(3), the special Euclidean group of rigid body transformations in three dimensions By the above argument, SE(3) is equivalent to the set of homogeneous matrices It can be shown that it is a matrix group as well as a smooth manifold and, therefore, a Lie group; for more details we refer to [11, 21] It

is convenient to denote matrices g ∈ SE(3) by the pair (R, d) for R ∈ SO(3) and d ∈ R3(recall that

SO(3) = {R ∈ R3×3|R T R = I3, det(R)= 1})

Given a point p ∈ O described by a vector pBin the frame B, it is natural to ask what is the corresponding

vector in the frame S From the definition of gSBwe have

pS= gSBpB where for a vector p∈ R3, the corresponding homogeneous vector p is defined as

p=

p

1

The tangent space of SE(3) at g0∈ SE(3) is the vector space of matrices of the form ˙g(0), where g(t) is

a curve in SE(3) such that g (0) = g0 The tangent space of a Lie group at the group identity is called the

Lie algebra of the Lie group The Lie algebra of SE(3), denoted by se(3), is

se(3)=

v

| ∈ R3 ×3, v∈ R3, T = −

(5.2)

Elements of se(3) are called twists Recall that a 3 × 3 skew-symmetric matrix can be uniquely identified

with a vectorω ∈ R3so that for an arbitrary vector x ∈ R3,x = ω × x, where × is the vector cross

product operation inR3 Each element T ∈ se(3) can be thus identified with a 6 × 1 vector

ξ =

v ω

called the twist coordinates of T We also write = ω and T = ξ to denote these transitions between

vectors and matrices

An important relation between the Lie group and its Lie algebra is provided by the exponential map It

can be shown that forξ ∈ se(3), exp(ξ) ∈ SE(3), where exp : R n ×n→ Rn ×nis the usual matrix exponen-tial Using the properties of SE(3), it can be shown that, for ξ T = [v T ω T],

exp(ξ) =





I3 ∅v

exp(ω) 1

ω2[(I3− exp(ω))(ω × v) + ωω T v]

, ω = 0

(5.3)

where the relation

exp(ω) = I3+sinω

ω ω +

1− cos ω

Trang 10

Lagrangian Dynamics 5-5

For serial linkages the forward kinematics map has a particularly revealing form Choose a reference configuration of the manipulator, i.e the configuration where all the joint variables are 0, and let ξ1, , ξ n

be the joint twists in this configuration expressed in the global coordinate frame S If the i th joint is revolute and l = {p + λω ∈ R3| λ ∈ R, ω = 1} is the axis of rotation, then the joint twist corresponds to the screw (l , 0, 1) and is

ξ i =

−ω × p ω

(5.6)

If the i th joint is prismatic and v∈ R3,v = 1, is the direction in which it moves, then the joint twist

corresponds to the screw ({λv ∈ R3| λ ∈ R}, ∞, 1) and is

ξ i=

v

0

One can show that

gSBi (q ) = exp(ξ1 q1)· · · exp(ξ i q i )gSBi,0 (5.7)

where gSB i,0∈ SE(3) is the reference configuration of the ith link This is known as the Product of Expo-nentials Formula and was introduced in [7].

Another important relation is that between the joint rates and the link body and spatial velocities A

direct computation shows that along a curve t → q(t),

VSBs

i (t) = J s

SBi (q (t)) ˙q (t)

where J s

SBi is a configuration-dependent 6× n matrix, called the spatial manipulator Jacobian, that for

serial linkages equals

JSBs i (q ) = [ξS,1(q ) ξ S,i (q ) 0 0]

Here the j th column ξ S, j (q ) of the spatial manipulator Jacobian is the j th joint twist expressed in the global reference frame S after the manipulator has moved to the configuration described by q It can be

computed from the forward kinematics map using the formula:

ξ S, j (q )= Ad(exp(ξ1q1 )···exp(ξ j−1q j−1))ξ j

Similar expressions can be obtained for the body velocity

VSBb i (t) = J b

where J b

SBi is a configuration-dependent 6× n body manipulator Jacobian matrix For serial linkages,

JSBb i (q ) = [ξB i,1(q ) ξBi ,i (q ) 0 0] (5.9)

where the j th column ξBi , j (q ), j ≤ i isthe jthjointtwistexpressedinthelinkframeB iafter the manipulator

has moved to the configuration described by q and is given by

ξBi , j (q )= Ad−1

(exp(ξ j q j)···exp(ξ i q i )g SBi ,0)ξ j

v = 1, is the direction in which it moves, then the joint twist

corresponds to the screw ({λv ∈ R3< /small>| λ ∈ R}, ∞, 1) and is... 10

Lagrangian Dynamics 5-5

For serial linkages the forward kinematics map has a particularly revealing form Choose a reference... 0, and let ξ1, , ξ n

be the joint twists in this configuration expressed in the global coordinate frame S If the i th joint is revolute and

Tiêu đề	Newton-Euler Dynamics of Robots
Tác giả	Mark L. Nagurka
Trường học	Marquette University
Chuyên ngành	Robotics and Automation
Thể loại	bài báo
Năm xuất bản	2005
Thành phố	Boca Raton

Định dạng
Số trang	14
Dung lượng	587,96 KB