On the generation of smooth three-dimensional rigid body motions

576 IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 14, NO. 4, AUGUST 1998

On the Generation of Smooth Three-DimensionalRigid Body Motions

Milo s Zefran, Vijay Kumar, and Christopher B. Croke

Abstract—This paper addresses the problem of generatingsmooth trajectories between an initial and a final position andorientation in space. The main idea is to define a functionaldepending on velocity or its derivatives that measures smoothnessof trajectories and find a trajectory that minimizes this functional.In order to ensure that the computed trajectories are independentof the parameterization of positions and orientations, we usethe notions of Riemannian metric and covariant derivative fromdifferential geometry and formulate the problem as a variationalproblem on the Lie group of spatial rigid body displacements,SE(3): We show that by choosing an appropriate measure ofsmoothness, the trajectories can be made to satisfy boundaryconditions on the velocities or higher order derivatives. Dy-namically smooth trajectories can be obtained by incorporatingthe inertia of the system into the definition of the Riemannianmetric. We state the necessary conditions for the shortest distance,minimum acceleration and minimum jerk trajectories. Analyticalexpressions for the smooth trajectories are derived for somespecial cases. We also provide several examples of the generalcase where the trajectories are computed numerically.

Index Terms—Euclidean group, Lie groups,SE(3), trajectorygeneration, variational calculus.

I. INTRODUCTION

T HERE are many applications in which the problem ofgenerating smooth trajectories for a rigid body in

is encountered. In robotics, it is frequently necessary to planmovements between a given (start) end-effector position andorientation and a desired (goal) position and orientation [1].In general, we have to compute actuator forces that achievethe specified displacement. But when the dynamic model ofthe system is not available or difficult to derive, it is betterto separately plan the kinematic (task space) trajectory anduse some other method to compute the corresponding actuatortorques. Smooth trajectories are preferred because:

1) the electro-mechanical system is limited by the size ofthe actuators and their control bandwidth so it cannotproduce large velocities and accelerations;

Manuscript received November 22, 1995; revised April 9, 1998. This workwas supported by NSF Grants BCS 92-16691, MSS 91-57156, CISE/CDA88-22719, DMS 95-05175, ARPA Grant N00014-92-J-1647, and a fellowshipfrom the Institute for Research in Cognitive Science, University of Pennsyl-vania. This paper was recommended for publication by Associate Editor J.Wen and Editor S. Salcudean upon evaluation of the reviewers’ comments.

M. Zefran was with the Department of Computer Science, University ofPennsylvania, Philadelphia, PA 19104 USA. He is now with the Departmentof Mechanical Engineering, California Institute of Technology, Pasadena, CA91109 USA.

V. Kumar and C. B. Croke are with the University of Pennsylvania,Philadelphia, PA 19104 USA.

Publisher Item Identifier S 1042-296X(98)05018-6.

2) movements with high acceleration and/or jerk can excitethe structural natural frequencies in the system.

Planning of smooth task space trajectories is also employedin the programming of industrial robots for tasks such aswelding and painting where a “teaching” process is employedto record intermediate positions and the final trajectory isobtained by interpolation [1]. Similarly, in computer animationit is necessary to generate a smooth trajectory passing througha set of key frames specifying positions and orientations [2].In this case, smoothness is required to obtain realistic motionsor motions that “look” natural.

There are several factors that need to be considered whendeveloping a trajectory planning method. It is desirable thatthe trajectories are independent of the choice of coordinatesfor the space. In this way, computations performed withdifferent choices of coordinates will produce consistent results.In addition, to describe motion of a rigid body in space, aninertial and a body fixed reference frame must be chosen. Wewould therefore also like to find a planning method that doesnot depend on the choice of these two frames. And finally,the computed trajectories should have good performance forthe chosen task.

Coordinate independence of the trajectories is assured ifthey are computed using the intrinsic geometric (i.e., coor-dinate free) properties of the space. Appropriate tools areprovided by differential geometry and the theory of Lie groups.Differential geometry offers a consistent way of extending thenotion of differentiation from Euclidean space to an arbitrarymanifold so that we can define acceleration, jerk and differentmeasures of smoothness of the trajectories. The theory of Liegroups provides a framework for investigating the invarianceof the trajectories with respect to the choice of the inertial andbody fixed frames.

There is an extensive literature on trajectory generationin kinematics, robotics and computer graphics. In order togenerate a smooth motion for a robot arm from an initial toa final position, Whitney [3] and Pieper [4] advocated usinga screw motion. Waldron [5] developed an algorithm that isbased on a slight variation of Pieper’s scheme so that thevelocity profile along the trajectory is trapezoidal. In all theseschemes, although the screw motion is invariant with respect torigid body transformations, it does not optimize a meaningfulcost function. Furthermore, the translational part of a screwmotion between two points is in general not a straight line. Paul[1] decomposes the desired displacement into a translation andtwo rotations, each of which is smoothly parameterized withrespect to time. The motion of the end-effector is obtained

1042–296X/98$10.00 1998 IEEE

ZEFRAN et al.: GENERATION OF SMOOTH THREE-DIMENSIONAL RIGID BODY MOTIONS 577

by a composition of these three displacements. He employsa fourth-order polynomial to obtain a smooth motion. Al-though there is some justification for the proposed trajectory,the approach will lead to different trajectories if a differentparameterization is chosen for the rotation or if the coordinateframes in which the trajectory is computed are changed. Thereis also no attempt to develop a measure of smoothness forthree-dimensional (3-D) motions.

Shoemake [6] proposed a scheme for interpolating rotationswith Bezier curves. This idea was extended by Ge and Ra-vani [7] to spatial motions and proposed for computer-aidedgeometric design. In both cases, the interpolating curves arescrew motions and therefore invariant with respect to thechoice of reference frames. However, the interpolating schemeproduces a motion that does not possess these invarianceproperties. Also, these motions are not of minimal length forany meaningful metric. In contrast, Park and Ravani [8] use ascale-dependent left-invariant metric to design Bezier curvesfor 3-D rigid body motion interpolation.

In this paper, the trajectory planning problem is posed asone of finding maximally smooth trajectories between an initialand a final position and orientation. The measure of the lack ofsmoothness is chosen to be the integral over the trajectory ofa cost function depending on velocity or its higher derivatives.Boundary conditions on the derivatives of desired order canbe enforced by appropriately choosing the cost function. Forexample, by minimizing the norm of the velocity we obtain theshortest distance paths. The minimum acceleration (minimumjerk) trajectories can be made to satisfy boundary conditions onthe velocities (accelerations). Dynamically smooth trajectoriescan be obtained by incorporating the inertia of the system intothe cost function. A simple extension of the ideas in this paperallows the inclusion of intermediate positions and orientationsand lends itself to motion interpolation.

Necessary conditions for smooth curves on general mani-folds were derived by Noakeset al. [9], and in parallel withour work by Camarinhaet al. [10] and Crouch and SilvaLeite [11]. In [9], necessary conditions for cubic splines whichcorrespond to our minimum acceleration curves are derived foran arbitrary manifold. These results are extended in [11] to thedynamic interpolation problem. In [10], necessary conditionsfor curves minimizing the integral of the norm of an arbitraryderivative of velocity are derived. None of these works dealsspecifically with computing the trajectories on nor dothey address the choice of the metric for the space. Since thereis no natural metric for [12], [13], the choice of metricfor trajectory planning becomes an important issue.

The paper is organized as follows. We first review somepreliminary concepts on Lie groups and space kinematics,including the ideas of a left-invariant metric, connection andthe covariant derivative. This material is standard and can befound in texts such as [13]–[15]. In Section III, we addressthe choice of metric for We propose a left-invariantmetric given by the kinetic energy of a rigid body andderive the expressions for the covariant derivative given bythis metric. We use these geometric constructs to formalizethe ideas of acceleration and jerk on Most of theseresults are presented here for the first time. In Section IV,

Fig. 1. The inertial (fixed) frame and the moving frame attached to the rigidbody.

we discuss the variational problems that need to be solved inorder to calculate the shortest distance, minimum accelerationand minimum jerk trajectories. While some of these resultswere derived in [9] and [10], we present alternative proofsand specialize the results to In Section V, we deriveanalytical solutions for the smooth trajectories in some specialcases. For more general situations, we compute numericalsolutions. Section VI provides some concluding remarks.

II. K INEMATICS, LIE GROUPS, AND DIFFERENTIAL GEOMETRY

A. The Lie Group

Consider a rigid body moving in free space. Assume anyinertial reference frame fixed in space and a framefixed to the body at a point as shown in Fig. 1. At eachinstant, the configuration (position and orientation) of therigid body can be described by a homogeneous transformationmatrix corresponding to the displacement from frame toframe The set of all such matrices is called thespecial Euclidean group of rigid body transformations in threedimensions

It is easy to show [13] that is a group for the standardmatrix multiplication and that it is a manifold. It is thereforea Lie group [15].

On a Lie group, the tangent space at the group identitydefines a Lie algebra. The Lie algebra of denoted by

is given by

(1)

A 3 3 skew-symmetric matrix can be uniquely identifiedwith a vector so that for an arbitrary vector

where is the vector cross product operationin Each element can be thus identified with avector pair


Given a curve an element ofthe Lie algebra can be associated to the tangent vector

at an arbitrary point by

(2)

A curve on physically represents a motion of the rigidbody. If is the vector pair corresponding tothen physically corresponds to the angular velocity of therigid body while is the linear velocity of the origin of theframe both expressed in the frame In kinematics,elements of this form are called twists [13] and thuscorresponds to the space of twists. It is easy to check that thetwist computed from (2) does not depend on the choice ofthe inertial frame For this reason, is called the left-invariant representation of the tangent vectorAlternatively,the tangent vector can be identified with a right-invarianttwist (invariant with respect to the choice of the body-fixedframe In this paper, right-invariant twists will not beconsidered, but all the developments are parallel to those forthe left-invariant twists.

Since is a vector space, any element can be expressedas a 6 1 vector of components corresponding to a chosenbasis. The standard basis for is

The twists and represent instantaneous rotationsabout, and and instantaneous translations along,the Cartesian axes and respectively. The componentsof a twist in this basis are given precisely by thevelocity vector pair,

The Lie bracket of two elements is definedby

It can be easily verified that if and are vectorpairs corresponding to the twists and the vector pair

corresponding to their Lie bracket is given by

(3)

In kinematics, this product operation is called themotorproduct of the two twists.

The Lie bracket of two elements of a Lie algebra isan element of the Lie algebra and can be expressed as alinear combination of the basis vectors. The coefficients

corresponding to the Lie brackets of the basis vectors are calledthe structure constantsof the Lie algebra [14]

(4)

B. Left-Invariant Vector Fields

A (differentiable) vector field on a manifold is a (smooth)assignment of a tangent vector to each element of the manifold.At each point, a vector field defines a uniqueintegral curveto which it is tangent [15]. Formally, a vector field is a(derivation) operator which, given a differentiable function

returns its derivative (another function) along the integralcurves of In other words, if is a curve tangent to avector field at a point then

(5)

On a matrix Lie group, an example of a (differentiable)vector field, is obtained by setting

(6)

where belongs to the Lie algebra of the group. Such a vectorfield is called aleft-invariant vector field. We use the notation

to indicate that the vector field is obtained by left translatingthe Lie algebra element The set of all left-invariant vectorfields is a vector space and by construction it is isomorphicto the Lie algebra. Right-invariant vector fields can be definedin an analogous way. In general, a vector field need not beleft- or right-invariant.

We now concentrate on the group Sinceare a basis for the Lie algebra the set of the left-

invariant vector fields is a basis of the spaceof the left-invariant vector fields. In addition, we have [14]

(7)

Finally, because at any point the vectorsform a basis of the tangent space at that

point, any vector field can be expressed as

(8)

where the coefficients vary over the manifold – if theyare constant then is left-invariant. This implies that we canassociate a vector pair defined by

to an arbitrary vector field

C. The Exponential Map and Local Coordinates

A motion of the rigid body in is described by a curve,on If is the vector field tangent tothe vector pair associated with corresponds

to the instantaneous twist for the motion. Associated witha twist is theinstantaneous screw axis[16]. In general, the


twist changes with time. Motions for which the twistis constant, and therefore, the instantaneous screw axis

is fixed, are known in kinematics asscrew motions. If thevector pair is interpreted as Plucker coordinates of aline in space, it is not difficult to see that the screw motionphysically corresponds to a rotation about this line with aconstant angular velocity and a concurrent translation alongthe line with a constant translational velocity.

Let the twist be represented by a vector pairand let be a screw motion with the screw axissuch that We definethe exponential map

by

(9)

Using (2) we can show that the exponential map agrees withthe usual exponentiation of the matrices in

(10)

where S denotes the matrix representation of the twistThesum of this series can be computed explicitly and the resultingexpression, when restricted to is known as Rodrigues’sformula. The formula for the sum in is derived in [13,pp. 413].

D. Riemannian Metrics on Lie Groups

If a smoothly varying, positive definite, bilinear, symmetricform is defined on the tangent space at each point on themanifold, we say the manifold is Riemannian. The bilinearform is an inner product on the tangent space at each pointand is called a Riemannian metric.

On a Lie group, and thus on an inner product on thetangent space at the identity can be extended to a Riemannianmetric (everywhere on the manifold) using the idea of lefttranslation. Assume that the inner product of two elements

is defined by

(11)

where and are the 6 1 vectors of components ofand with respect to some basis and is a positive

definite matrix. If and are tangent vectors at an arbitrarygroup element the inner product on thetangent space can be defined by

(12)

The metric obtained in such a way is called aleft-invariantmetric [15].

Physically, left invariance corresponds to independence ofthe choice of the inertial frame. Let and representtwo motions of a rigid body that pass through a pointat and let andbe the corresponding velocity vector fields. Let describea displacement of the inertial reference frame. In the newreference frame, the motions become and

and the velocity vector fields andThen

(13)

We could similarly define a right-invariant Riemannian metricand in this case the metric would be independent of the choiceof the body-fixed frame.

E. Affine Connections and Covariant Derivatives

The motion of a rigid body is represented by a curve,on The velocity at an arbitrary point is the

tangent vector to the curve at that point. In order to obtainthe acceleration, or to engage in a dynamic analysis, we needto be able to differentiate a vector field along the curve. Ateach point the value of a vector field belongs tothe tangent space and to differentiate a vector fieldalong a curve, we must be able to subtract vectors from tangentspaces at different points on the curve. But tangent spaces atdifferent points are not related. We thus have to specify howto transport a vector along the curve from one tangent spaceto another. This process is calledparallel transport and isformalized with theaffine connection[15].

A derivative of a vector field along a curve is definedthrough the parallel transport. Let be a vector field definedalong and let stand for Denote bythe parallel transport of the vector to the pointThecovariant derivativeof along at the point is

(14)

By taking covariant derivatives along integral curves of avector field we obtain a covariant derivative of the vectorfield with respect to the vector field This derivative isalso denoted by

(15)

where is taken along the integral curve of passingthrough at . It is clear that in order to computeat a point, we have to know how changes in a neighborhoodof that point. The affine connection, is therefore not atensor.

The covariant derivative of a vector field is another vectorfield, so it can be expressed as a linear combination of the basisvector fields. The coefficients of the covariant derivativeof a basis vector field along another basis vector field

(16)

are called theChristoffel symbols.1 Note the reversed order ofthe indices and

1In the literature, different definitions for the Christoffel symbols can befound. Some texts (e.g., [14]) reserve the term for the case of the coordinatebasis vectors. We follow the more general definition from [15] in which thebasis vectors can be arbitrary.


Given a Riemannian manifold, there exists a unique con-nection [15] which is compatible with the metric:2

(17)

and symmetric

(18)

This connection is called theLevi-Civita or Riemannian con-nection.

The velocity, of the rigid body moving along the curveis given by the tangent vector field

The acceleration, is the covariant derivative of thevelocity along the curve

(19)

Note that the acceleration depends on the choice of theconnection. We can also define jerk, as the covariantderivative of the acceleration

(20)

F. The Curvature Tensor

The curvatureof a Riemannian manifold is a correspon-dence that associates to a pair of vector fields anda mapping

(21)

where is a vector field and is the Riemannian connec-tion on 3 Unlike the affine connection, the curvature is apointwise object. That is, the value of at a pointdepends only on the vectors and it is notimportant how the vector fields change in the neighborhood of

Thecurvature tensoris a multi-linear mapping which mapsa quadruple of vectors ( ) into a real number. Thevalue of the curvature tensor on the quadruple ( ) isgiven by If is a basis, the componentsof the curvature tensor are given by

(22)

III. T HE RIEMANNIAN STRUCTURE ON

A. The Choice of Metric

A desired property of a planning method is that the gen-erated trajectories are invariant with respect to the choice ofthe reference frames. One family of such invariant trajectoriesare screw motions [17]. But it can be shown [17] that screwmotions are not the shortest length curves for any Riemannianmetric so they do not minimize any physically meaningful

2Note thatXhY; Zi is a derivative of the functionhY;Zi along the integralcurves ofX (see Section II-B).

3Sign convention in the definition of the curvature in the literature varies.Here we follow [14].

cost function. Since the trajectories that we propose in thepaper will depend on a Riemannian metric, another possi-bility to obtain invariant trajectories is to choose a metricthat is bi-invariant (i.e., both, left- and right-invariant) andthus independent of the choice of the reference frames (seeSection II-D). However, does not admit a bi-invariantRiemannian metric (see [12] and in the context of robotics[18], [19]). For this reason, we focus on the left-invariantmetrics that are independent of the choice of the inertialreference frame, thus sacrificing the independence of thecomputed trajectories with respect to the choice of the body-fixed reference frame.

A metric that is attractive for trajectory planning can beobtained by considering the dynamic properties of the rigidbody. The kinetic energy of a rigid body is a scalar that doesnot depend on the choice of the inertial reference frame. Itthus defines a left-invariant metric. For this metric, the matrix

in (11) is the inertia matrix and corresponds tothe kinetic energy of the rigid body moving with a velocityIf the body-fixed reference frame is attached at the centroidand aligned with the principal axes, then we have

(23)

where is the mass of the rigid body and is the matrix

with and denoting the moments of inertia aboutthe and axes, respectively. If is the vector pairassociated with the vector this vector pair represents theinstantaneous twist associated with the motion, expressed inthe body-fixed reference frame. The norm of the vectorthusassumes the familiar expression

(24)

Now assume that the body fixed frame is displacedby the matrix

to a new frame The kinetic energy does not changeif the body-fixed frame is changed. It is not difficult to checkthat this implies that the matrix defining the energy metricfor the new description of the motion of the rigid body is

(25)

where is the skew-symmetric matrix corresponding to thevector This is therefore the most general form of the inertiamatrix and can be viewed as a spatial version of Steiner’sparallel-axis theorem.

If we desire a trajectory that can be used for differentobjects, we can abstract the inertial properties by setting


and in (23), where and are two arbitrarypositive scalars. In this way, the matrix becomes

(26)

This was the metric proposed by Park and Brockett [20] for thestudy of kinematic dexterity of robot mechanisms. In additionto being left-invariant, this metric is also bi-invariant whenrestricted to the group of rotations, The two scalars,

and act like scaling factors for angular velocities andlinear velocities. Inkinematic analysisthere is noa priorijustification for choosing them.

Metrics (23) and (26) are not right-invariant. Specifically,they will depend on the choice of the origin of the body-fixedreference frame.

Remark 1: Let be the matrix of metric coefficients, thatis In [17] it is shown that for a product metricon induced by a left-invariant metric onand the standard Euclidean metric on the matrix hasthe form

(27)

The metric (23) [and thus (26)] has this form and is thereforea product metric. In other words, there is an isometry between

endowed with any of these metrics and the productspace with appropriately defined metrics onand Although (25) is not a product metric with respectto this splitting, it is isometric to a metric of the form (23).Consequently, any metric induced by the kinetic energy willbe isometric to a product metric. These isometries do notpreserve the group structure of they are isometries inthe sense of manifolds. But since none of the functionals thatwe later use to define the smoothness of a curve depends onthe group structure of the calculations in the examplescould be simplified by performing them on the product space

However, the key results in this paper are derivedfor a general metric and are not limited to product metrics.There are important applications of such general metrics. Forexample, if the metric is defined so that it reflects the dynamicproperties of the mechanical system to which the object isattached, it will in general not be a product metric. For thisreason, the product structure of equipped with themetric induced by the kinetic energy metric (23) will not beused in the derivations.

B. The Riemannian Connection

In this section, we find the Riemannian connections thatcorrespond to the left-invariant metrics (23) and (26). We startwith an elementary result relating the Christoffel symbols andthe structure constants for an arbitrary Lie group. It can beshown [14] that if is the Riemannian connection then forany three vector fields and

(28)

This immediately implies:Proposition 2: If is the Riemannian connection com-

patible with a left-invariant metric described by a matrixthe Christoffel symbols for the basis are given

by

(29)

where are the structure constants of the Lie algebra and

Any vector field on can be expressed as a linearcombination of left-invariant vector fields (with possibly vary-ing coefficients) according to (8). If and

are any two vector fields, then

(30)

where is the derivative along the integral curve ofand are obtained from (29).4 But instead of computingthe we derive expressions for the Riemannian connectiondirectly from (28). First, we prove the following lemma for

Lemma 3: Let and bethree arbitrary vector fields and let the corresponding vectorpairs be and respectively. Ifis the Riemannian connection corresponding to a left-invariantRiemannian metric then

(31)

Proof: The result of the Lemma follows directly from(28). The Lie bracket of any two vector fields is

where denotes the action of the vector field on a scalarfunction (See Section II-B). Rewritten in terms of the pairs

and the first term becomes

Thus, in (28)

4Starting from this point we use the Einstein summation convention tosimplify the notation.


Furthermore, if are the entries of in (11)

If we similarly expand all terms in (28) and add them, theresult in (28) follows.

Proposition 4: Let and be twoarbitrary vector fields. If is the Riemannian connectioncorresponding to the Riemannian metric (23), then

(32)

where is the derivative along the integral curve ofThe translational component of the expression is

independent of the choice of matrix and thus independentof the choice of the metric on

Proof: We use Lemma 3 and compute the right handside of (28) using the metric (23)

Since the above is true for an arbitrary this proves theproposition.

By substituting we obtain the following corollary:Corollary 5: Let and be two

arbitrary vector fields. If is the Riemannian connectioncorresponding to the Riemannian metric (26), then

(33)

where is the derivative along the integral curve ofRemark 6: Note that the expression for the Riemannian

connection corresponding to the metric (26) is independentof the scaling constants, and

C. The Curvature

In the subsequent sections we will also need expressions forthe Riemannian curvature of for the metric (26).

Proposition 7: If and are three arbitrary vec-tor fields on with the associated vector pairs

and and has the Rie-mannian connection defined in (33), then the Riemanniancurvature is

(34)

Proof: The result directly follows from (21) and (33).

D. The Acceleration and Jerk in Three-Dimensional Motions

Having a formula for the covariant derivative, we cancompute the expressions for the acceleration and jerk. We usethe scale-dependent left-invariant metric from (26) to illus-trate this. Since the connection coefficients and the covariantderivative are independent of the choice of the constantsand the resulting expressions for acceleration and jerk willalso be independent of these scale factors.

If is the velocity (tangent to the curve) associated with themotion of a rigid body and if is the correspondingvelocity pair, it immediately follows from (19) and (33) thatthe acceleration is given by

(35)

The third derivative of motion, jerk, can be computed from(20) and (33)

(36)

Remark 8: The resulting expression for the accelerationcorresponds to the acceleration that is used in kinematics. Thesame is true for the jerk. Given that the acceleration and jerkdepend on the connection and therefore on the metric, thisresult is due to the special choice of the metric (26) and doesnot hold, for example, for a general form of the metric (23).See [17] for a discussion of this phenomenon.

IV. NECESSARYCONDITIONS FOR SMOOTH TRAJECTORIES

A. Variational Calculus on Manifolds

In this section, we consider trajectories between a startingand a final position and orientation that minimize an integralcost function while possibly satisfying additional boundaryconditions on the velocities and/or accelerations. The costfunction can be the kinetic energy of the rigid body, orsome other measure of smoothness involving velocity or itshigher derivatives. Specifically, we will be interested in curves

that minimize integrals of the form


where boundary conditions on and its derivatives may bespecified at the endpoints and The function returns avector field and usually involves one or more recursive appli-cations of the covariant derivative. To obtain trajectories thatare independent of the choice of the inertial reference frame

we will use a left-invariant metric and the correspondingRiemannian connection.

We adapt methods from the classical calculus of variationsto the differential geometric setting [14]. Noakeset al. [9]use such a framework to derive expressions for cubic splineson a general manifold and they provide the formulas for thegroup of rotations The cubic splines correspond toour minimum acceleration curves and we derive the resultsfrom [9] using a more direct approach. We will illustrate thisapproach by deriving the necessary conditions for minimumjerk curves. These necessary conditions were independentlyobtained by Camarinhaet al. [10], who extended the resultsby Noakeset al. to higher order splines.

In the calculus of variations, the first-order necessary con-ditions for the minimal solution are derived by studyingvariations of the optimal trajectory. Let be a curve on

and let be a differentiablemapping such that Such a mapping is called avariation of the curve [14]. A variation is called proper,if any curve satisfies the given boundaryconditions at and For a variation we can definethe vector fields andThe value of the cost functional on a curve is

If the curve is a stationary point of thenthe first variation must vanish for and thisgives us the first order necessary condition for the optimaltrajectories.

B. Minimum Distance Curves—Geodesics

Given a Riemannian metric, the length of a curvebetween the points and is defined to be

(37)

We are usually interested in finding the shortest curve (thecurve that minimizes between two points. It can be shown[14], that if there exists a curve that minimizes the functional

this curve also minimizes the so-calledenergy functional

The critical points of the energy functional are calledgeodesicsand are given by [14]

(38)

whereTo solve (38) and find the geodesics on for the metric

(23), we express as a linear combination of left-invariant

vector fields according to (8). The coefficients ofthe linear combination form the vector pair which ingeneral varies over the manifold.

Proposition 9: A curve on equipped with themetric (23) is a geodesic if and only if the vector paircorresponding to the velocity vector field satisfiesthe equations

(39)

The second equation in (39) can be simplified to

(40)

Proof: A curve is a geodesic if and only if (38)is satisfied. Substituting for from (32), and letting

we get (39). The secondequation in (39) can be written as

By writing and and using the identitywe obtain

which provesRemark 10: According to Hamilton’s principle, the trajec-

tory that minimizes the kinetic energy is obtained by solvingthe dynamic equations of motion. It therefore comes as nosurprise that the first equations in (39) are the Euler equationswhile (39) is Newton’s equation in the absence of externalforces.

Corollary 11: A curve

on equipped with the metric (26) is a geodesic if andonly if the vector pair corresponding to the velocityvector field satisfies

(41)

The second equation in (41) can be simplified to

Remark 12: It is worth noting that the above result isindependent of the choice of the scale factorsand Thenecessary conditions for minimum acceleration and minimumjerk curves derived in subsequent subsections will also havethe same property. However, the curves do depend on thechoice (of the origin) of the body-fixed reference frame.


C. Minimum Acceleration Curves

We derive the necessary conditions for the curves thatminimize the square of the norm of the acceleration byconsidering the first variation of the acceleration functional

(42)

where and is a curve on the manifold.The initial and final points as well as the initial and finalvelocities for the motion are prescribed. Noakeset al. [9]derived the following theorem.

Theorem 13 (Noakes et al. [9]):Let be a curve on aRiemannian manifold that satisfies the boundary conditions(that is, it starts and ends at the prescribed points with theprescribed velocities) and let If minimizesthe functional then

(43)

Proof: The proof of the theorem is similar to the proofof Theorem 16 and will be omitted in the interest of space.Noakeset al. use a slightly different approach for their proof.

We can directly apply Theorem 13 to with theRiemannian connection computed from the metric (26).

Proposition 14: Let

be a curve between two prescribed points on that hasprescribed initial and final velocities. If is the vectorpair corresponding to the curve minimizes thecost function derived from the metric (26) only if thefollowing equations hold:

(44)

where denotes the th derivative ofProof: We start by using (35) and (34) to compute the

second term in (43)

(45)

By repeated application of (33) the term can besimplified. The rotational part of the above equation thus re-duces to the first equation in (44). To simplify the translationalcomponent, we first observe that the translational componentof can be written as (see Proposition 9)

It follows that the translational component of is

Similarly, the translational component of can besimplified to get

from which the second equation in (44) follows.Remark 15: As observed in [9], the first equation (44) can

be integrated to obtain

(46)

However, this equation cannot be further integrated analyti-cally for arbitrary boundary conditions. In Section V-B, wewill show how to obtain the solution for a particular choiceof the initial and final velocities.

D. Minimum Jerk Curves

The minimum jerk curves between two points are obtainedby minimizing the norm of the Cartesian jerk, provided thatthe appropriate boundary conditions are given. In particular,it is possible to solve for minimum jerk trajectories when theinitial and final velocities and the initial and final accelera-tions are specified. Minimum jerk trajectories are particularlyuseful in robotics where one is generally able to control theacceleration of the end effector of a robot (and therefore thevelocity and position) but the electro-mechanical actuatorscannot produce sudden changes in the acceleration.

The jerk cost functional is

(47)

where The curve must start and end at thedesired points on the manifold and with the desired velocitiesand accelerations. We arrive at the necessary conditions for thesolution by following the same approach as in the previoussubsection.5

Theorem 16:Let be a curve on a Riemannian mani-fold that satisfies the boundary conditions (that is, it starts andends at the prescribed points with the prescribed velocitiesand the prescribed accelerations) and let If

minimizes the functional then

(48)

Proof: See Appendix A.The expressions for the minimum jerk trajectories on

for the metric (26) immediately follow.Proposition 17: Let be a curve between two pre-

scribed points on that has prescribed initial and finalvelocities and initial and final accelerations. If isthe vector pair corresponding to the curveminimizes the cost function for the metric (26) only if

5A generalized version of Theorem 16 was derived in parallel with ourwork by Camarinhaet al. [10].


the following equations hold:

(49)

Proof: The proof follows the same lines as the proof ofProposition 14. We use formulas (33) and (34) to evaluate thethree terms in (48) and the result follows in a straightforwardmanner.

V. SOLUTIONS FOR OPTIMAL TRAJECTORIES

A. Shortest Distance Paths on

According to Proposition 9, the rotational components forthe minimum distance curves corresponding to the metric(23) are the Euler equations (see Remark 10). In general,these equations do not have an analytical solution and mustbe solved numerically. However, for the metric (26), theequations simplify and the minimum distance curves can becomputed analytically. Using properties of the Riemanniancovering maps, Park showed [18] that for the metric (26),the geodesics can be obtained by lifting the geodesics from

(zero pitch screw motions) and (straight lines). Wecome to the same result constructively using (41).

Proposition 18 (Park [18]): Given two configurations

a shortest distance path (minimal geodesic)

between them with respect to the metric (26) is given by

(50)

(51)

where

The path is unique unless when thereexist two geodesics of equal minimum length (see Remark19).

Proof: The result follows from Proposition (11). The firstequation in (41) can be readily integrated to obtain

(52)

Let be the skew symmetric matrix representation of thevector From (2) we have

(53)

Equation (52) can thus be integrated

(54)

From the initial condition we get and from theboundary condition The function log is theinverse of the exponential function. See [13, p. 414] for theformula on

The expression for the vector is obtained by integratingthe equation 0 twice. As a result, we get

and (51) immediately follows from the initial and final condi-tions on

Remark 19: The log function on is multi-valued.If yields a solution where is a unit vectoralong the axis of rotation and is the angle of rotation,then is also a solution for any integer Themultiplicity of the solution can be avoided by restrictingto lie in the interval (the interval is coveredby using the axis The geodesic computed by restricting

to lie in the interval can be shown to give theunique minimal-length geodesic [18] unless If wethink of the representation of as a unit hyper-spherein with antipodal points identified, the minimal-lengthgeodesic between two points and is unique, exceptwhen and there exist two geodesics ofequal minimum length.

According to Chasles’s theorem there is a unique6 screwmotion between any two given positions and orientations. Ascrew motion will be a geodesic for the metric (26) only in thespecial case in which the screw axis for the screw displacementfrom the initial position and orientation to the final positionand orientation passes through the originIn [17], we showthat there is no Riemannian metric whose geodesics are screwmotions. Furthermore, it is shown that there is a family ofnondegenerate (but not positive-definite) bi-invariant 2-formsfor which the screw motions satisfy the geodesic equation (38).These forms can be viewed as generalizations of the Kleinand Killing forms and they are the only 2-forms for which thegeodesic equation is satisfied by the screw motions.

Fig. 2 shows various trajectories for a motion in the plane0. Fig. 2(a) shows a screw motion which in the planar

case corresponds to rotation about a fixed point in the plane.A geodesic for the metric (26) is shown in Fig. 2(b). Forplanar motions, the geodesic for the metric (23) will be thesame. Since the trajectory is computed by using a left-invariantmetric, it does not change if the inertial reference frame

6An argument similar to that in Remark 19 shows that the screw motion isunique if we limit the angle of rotation to[0; �):


(a) (b) (c)

Fig. 2. Motions in a plane: (a) a screw motion, (b) a geodesic for metrics (26) and (23), and (c) a geodesic after the body-fixed framefMg is displaced.

(a) (b) (c)

Fig. 3. Geodesics for different metrics onSE(2): (a) product metric,�1 = �2 = 1, (b) a metric with�1 = 1; �2 = 10, and (c) a metric with�1 = 5; �2 = 1:

is moved. But the trajectory changes if we change the body-fixed frame The trajectory for a different body-fixedreference frame is shown in Fig. 2(c) and is different fromthe curve shown in Fig. 2(b). We also show the motion of thenew body-fixed frame. The figure clearly shows that the newbody-fixed frame follows a geodesic for the metric (26), butthe rigid body will move along a curve that is different fromthe geodesic on Fig. 2(b). Examples of 3-D motions can befound in [21], [22].

It is also interesting to compare the geodesics for the metric(23), which are products of geodesics on and withgeodesics for a nonproduct metric. For illustrative purposeswe present motions in the plane and thus the geodesics on

A generalized form of the metric (23) for is

(55)

The rows correspond to components and respec-tively. When this metric is not a product metric (seeRemark 1). Such a metric might be used, for example, to planthe end-effector trajectories for a gantry mechanism that hasdifferent dynamic characteristics for motions in theanddirections.

Fig. 3 shows the geodesics for different choices ofandFig. 3(a) shows a geodesic when In this case, the

metric becomes the same as the metric (26) and the geodesicis a product of geodesics on and The other twofigures show geodesics for the cases when and themetrics are not product metrics. In this case the rotationaland translational components of the motion are coupled. Inparticular, the translational motion does not follow a straightline. These geodesics were computed numerically.

Remark 20: To obtain trajectories satisfying the necessaryconditions for a general variational problem, it is neces-sary to solve a two-point boundary value problem. To solvethese boundary-value problems numerically, we used a finite-difference method [23]. Typically, the solution for approxima-tion with a grid of 100 points takes less than 5 s to computeand is robust with respect to the choice of the initial guess.More details, including some 3-D examples, are presented in[22].

B. Minimum Acceleration and Minimum Jerk Trajectories

In general, the rotational components of the minimumacceleration curves (44) and minimum jerk curves (49) cannotbe computed analytically. However, in the special case whenthe initial velocities and accelerations are collinear with theinitial velocity of the geodesic between the two endpoints,and the final velocities and accelerations are collinear with thefinal velocity of the geodesic, it is easy to obtain a solutionfor these trajectories in terms of the geodesic curve. If thegeodesic curve can be computed analytically, so can minimumacceleration and minimum jerk curves. This is true not only for

with the metric (26) but for any geodesically completeRiemannian manifold.

Proposition 21: Given an initial point and a final pointon a Riemannian manifold, let be a

geodesic connecting these two points such thatand Let and Ifthe boundary conditions for the minimum acceleration curveare of the form

(56)

then the minimum acceleration curve is given by


(a) (b) (c)

Fig. 4. Minimum acceleration motions in the plane for different boundary conditions: (a)V (0) = V (1) = f0; 0; 0gT , (b)V (0) = f�1;3; 10gT ; V (1) = f2; 2; 5gT , and (c) V (0) = f1;10;5gT ; V (1) = f�1;�10;�5gT : The triple V = f!z ; vx; vyg denotesthe velocity components for the planar motion.

where is a third degree polynomial that satisfies

(57)

whereProof: Assume that the minimum acceleration curve

has the form where is an arbitrary scalarfunction, It is easy to see that

Let Since is a geodesic,0. It then follows:

(58)

But is a derivative of along so Itimmediately follows that:

(59)

Using the linearity of the curvature, we also get

(60)

Equation (43) therefore reduces to

(61)

Since is a tangent vector for a geodesic and therefore nevervanishes, we must have

(62)

The solution of this differential equation is a polynomial ofdegree 3 and the boundary conditions transform into (57).

The following proposition can be proved along similar lines:Proposition 22: Given an initial point and a final pointon a Riemannian manifold, let be a

geodesic connecting these two points such thatand Let andIf the boundary conditions for the minimum jerk curve are ofthe form

(63)

then the minimum jerk curve is given by whereis a fifth degree polynomial that satisfies

(64)

For this special form of the boundary conditions, the mini-mum acceleration, minimum jerk, and minimum distance pathsare therefore the same; only the parameterization along thepath varies. Fig. 4 shows that for more general boundaryconditions the path of the minimum acceleration curve doesnot follow a geodesic. Furthermore, the path changes with theboundary conditions. The figure shows minimum accelerationmotions in the plane 0 for different choices of the initialand final velocities. We consider equipped with themetric (26). In Fig. 4(a), the initial and final velocities are 0,so the object follows the geodesic path shown in Fig. 3(a), butwith a different velocity profile. The initial and final velocitiesfor Fig. 4(b), (c) are not collinear with the initial and finalvelocities of the geodesic in the Fig. 3(a) and the paths aredifferent.

VI. CONCLUSION

This paper addressed the problem of generating smoothtrajectories for a rigid body between an initial and a finalposition and orientation. The main idea was to define ameasure of the smoothness of a trajectory in the form ofa functional and find trajectories that minimize this costfunctional. Using some basic tools from differential geometry,the problem was formulated as a variational problem on theLie group of rigid body displacements We definedan inner product on the Lie algebra leading to a left-invariant Riemannian metric on This metric gaverise to a Riemannian connection and a covariant derivative.We derived analytical expressions for the covariant derivativeand the curvature of The covariant derivative wasused to define acceleration and jerk for spatial rigid bodymotions. We stated the necessary conditions for minimumdistance, minimum acceleration and minimum jerk trajectoriesand specialized these conditions for We computedthe analytical solutions for the minimum distance trajectoriesby choosing an appropriate basis for the space of the vectorfields. We also found analytical solutions for the minimumacceleration and minimum jerk trajectories for a special classof boundary conditions.

In addition to these results, we showed how canbe naturally endowed with a product metric or with metricsthat are isometric to a product metric. We provided severalnumerical examples to illustrate how the generated solutionswere affected by:


1) the metric;2) choice of the body-fixed reference frame;3) boundary conditions.

A simple extension of the ideas in this paper allows theinclusion of intermediate positions and orientations and lendsitself to motion interpolation [10]. The presented methodsalso have immediate applications in computer graphics andplanning of trajectories for robots and other machines.

APPENDIX

THE PROOF OF THEOREM 16

The proof is similar to the derivation of the first variation forthe energy functional [14]. Similar reasoning as in this proofcan be used to prove Theorem 13. We will use the followingidentities.

1) .2) .3) (since and

are derivatives with respect to coordinate curvesand

4) for.

5)6)

The first and the fourth identities express the fact that avector field is a differential operator. The second and thethird identities state that is a Riemannian connection, thuscompatible with the metric 2) and symmetric 3). Identity 5)is just the definition of the curvature operator when0, while 6) is one of the symmetry properties of the curvaturetensor [14]. In the proof, the numbers above the equal signsindicate which identities were employed.

We first obtain the expression for the first variation of thefunctional

Since the initial and final positions, velocities and accelera-tions are fixed, and

vanish at the endpoints. Thus theintegral in the above equation must vanish for an arbitraryvariation (that preserves the boundary conditions). But this isonly possible if (48) holds, so the Theorem is proved.

ACKNOWLEDGMENT

The authors thank Dr. F. C. Park for discussions on priorwork in this area, and for pointing out [9].

REFERENCES

[1] R. P. Paul,Robot Manipulators, Mathematics, Programming and Con-trol. Cambridge, MA: MIT Press, 1981.

[2] J. Hoschek and D. Lasser,Fundamentals of Computer Aided GeometricDesign. New York: A.K. Peters, 1993.

[3] D. E. Whitney, “The mathematics of coordinated control of prostheticarms and manipulators,”ASME J. Dynam. Syst. Contr., vol. 94, pp.303–309, 1972.

[4] D. L. Pieper,The kinematics of manipulators under computer control,Ph.D. dissertation, Stanford Univ., Stanford, CA, 1968.

[5] K. J. Waldron, “Geometrically based manipulator rate control algo-rithms,” Mech. Mach. Theory, vol. 17, no. 6, pp. 379–385, 1982.

[6] K. Shoemake, “Animating rotation with quaternion curves,”ACMSiggraph, vol. 19, no. 3, pp. 245–254, 1985.

[7] Q. J. Ge and B. Ravani, “Computer aided geometric design of motioninterpolants,”ASME J. Mech. Design, vol. 116, pp. 756–762, 1994.

[8] F. C. Park and B. Ravani, “Bezier curves on Riemannian manifolds andLie groups with kinematics applications,”ASME J. Mech. Design, vol.117, no. 1, pp. 36–40, 1995.

[9] L. Noakes, G. Heinzinger, and B. Paden, “Cubic splines on curvedspaces,”IMA J. Math. Contr. Inform., vol. 6, pp. 465–473, 1989.

[10] M. Camarinha, F. S. Leite, and P. Crouch, “Splines of classck on

non-Euclidean spaces,”IMA J. Math. Contr. Inform., vol. 12, no. 4, pp.399–410, 1995.

[11] P. Crouch and F. S. Leite, “The dynamic interpolation problem: OnRiemannian manifolds, Lie groups, and symmetric spaces,”J. Dynam.Contr. Syst., vol. 1, no. 2, pp. 177–202, 1995.


[12] J. Cheeger and D. G. Ebin,Comparison Theorems in RiemannianGeometry. Amsterdam, The Netherlands: North-Holland, 1975.

[13] R. M. Murray, Z. Li, and S. S. Sastry,A Mathematical Introduction toRobotic Manipulation. Boca Raton, FL: CRC, 1994.

[14] M. P. do Carmo,Riemannian Geometry. Boston, MA: Birkhauser,1992.

[15] B. F. Schutz,Geometrical Methods of Mathematical Physics. Cam-bridge, U.K.: Cambridge Univ. Press, 1980.

[16] K. H. Hunt, Kinematic Geometry of Mechanisms. Oxford, U.K.:Clarendon, 1978.

[17] M. Zefran, V. Kumar, and C. Croke, “Metrics and connections for rigidbody kinematics,”Int. J. Robot. Res., to be published.

[18] F. C. Park, “Distance metrics on the rigid-body motions with applica-tions to mechanism design,”ASME J. Mech. Des., vol. 117, no. 1, pp.48–54, 1995.

[19] J. Loncaric, “Geometrical analysis of compliant mechanisms inrobotics,” Ph.D. dissertation, Harvard Univ., Cambridge, MA, 1985.

[20] F. C. Park and R. W. Brockett, “Kinematic dexterity of robotic mecha-nisms,” Int. J. Robot. Res., vol. 13, no. 1, pp. 1–15, 1994.

[21] M. Zefran and V. Kumar, “Planning of smooth motions on SE(3),” inProc. 1996 Int. Conf. Robot. Automat., Minneapolis, MN, Apr. 1996,pp. 121–126.

[22] M. Zefran, “Continuous methods for motion planning,” Ph.D. disserta-tion, Univ. Pennsylvania, Philadelphia, 1996.

[23] W. H. Press, S. A. Teukolsky, W. T. Vetterling, and B. P. Flannery,Numerical Recipes in C. Cambridge, U.K.: Cambridge Univ. Press,1988.

Milo s Zefran received the M.Sc. degree in electricalengineering from the University of Ljubljana, Slove-nia, in 1992, the M.Sc. degree in mechanical engi-neering and the Ph.D. degree in computer sciencefrom the University of Pennsylvania, Philadelphia,in 1995 and 1996, respectively.

He is currently a NSF Postdoctoral Scholar atthe California Institute of Technology, Pasadena.His research interests include robotics, control, andbiomechanics.

Dr. Zefran was a fellow of the Institute forResearch in Cognitive Science, University of Pennsylvania, between 1993and 1996.

Vijay Kumar received the M.Sc. and Ph.D. degreesin mechanical engineering from The Ohio State Uni-versity, Columbus, in 1985 and 1987, respectively.

He has been on the Faculty of the School ofEngineering and Applied Science, University ofPennsylvania, Philadelphia, since 1987 and is cur-rently an Associate Professor. His research interestsinclude kinematics, robotics, biomechanics, mecha-nism design, and control.

Dr. Kumar is the recipient of the 1991 NationalScience Foundation Presidential Young Investigator

award. He has served on the editorial board of the IEEE TRANSACTIONS ON

ROBOTICS AND AUTOMATION. He is currently on the Editorial Board of theJournal of Franklin Instituteand an Associate Editor of the ASMEJournalof Mechanical Design.

Christopher B. Croke received the M.S. andPh.D. degrees in mathematics from the Universityof Chicago, Chicago, IL, in 1975 and 1978,respectively.

He was on the Faculty of the University ofCalifornia, Berkeley, between 1978 and 1980. Since1980, he has been on the Faculty of the Universityof Pennsylvania, Philadelphia, where he has beena Professor since 1986, and currently serves asthe Graduate Group Chair in the MathematicsDepartment. His research has been in the area

of Differential Geometry.Dr. Croke serves on the editorial board of theProceedings of the American

Mathematical Society. He has been a Fannie and John Hertz FoundationFellow from 1974 to 1978 and a Sloan Research Fellow from 1983 to 1985.

Documents

On the generation of smooth three-dimensional rigid body motions