JOURNAL IEEE

OF ROBOTICS AUTOMATION, AND

VOL. NO. RA-3,

1, FEBRUARY 1987

43

A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation
forces. The magnitude of these dynamic forces cannot be ignored when large accelerations and fast motions are considered. Controlling the end-effector contact forces in some direction can be strongly affected by the forces of coupling created by the end-effector motion that can take place in the subspace orthogonal to that direction. The description of the dynamic interaction between end-effector motions and the effects of these motions on the end-effectors behavior in the direction of force control are basic requirements for the analysis and design of high-performance manipulator control systems. Obviously, these characteristics cannot be found in the manipulator joint space dynamic model, which only provides a description of the interaction between joint moI. INTRODUCTION tions. High-performance control of end-effector motion and contact forces requires the description of how motions along ESEARCH in dynamics of robot mechanisms has largely different axes are interacting, and how the apparent or focused on developing the equations of joint motions. equivalent inertia or mass of the end-effector varies with These joint space dynamic models have been the basis for configurations and directions. various approaches to dynamic control of .manipulators. The description, analysis, and control of manipulator sysHowever, task specification for motion and contact forces, tems with respect to the dynamic characteristics of their enddynamics, and force sensing feedback are closely linked to the effectors has been the basic motivation in the research and end-effector. The dynamic behavior of the end-effector is one development of the operational space formulation. The endof the most significant characteristics in evaluating the effector equations of motion [ 131, [141 are a fundamental tool performance of robot manipulator systems. The problem of for the analysis, control, and dynamic characterization [181 of end-effector motion control has been investigated, and almanipulator systems. In this paper, we will discuss, from the gorithms resolving end-effector accelerations have been develperspective of end-effector control, the issue of task descripoped V I , [111, 1221, [3Ol, 1331. tion, where constrained motions and contact forces are The issue of end-effector dynamic modeling and control is involved. The fundamentals of the operational space formulayet more acute for tasks that involve combined motion and contact forces of the end-effector. Precise control of applied tion are presented, and the unified approach for the control of end-effector forces is crucial to accomplishing advanced robot end-effector motion and contact forces is developed. Treated within the framework of joint space control assembly tasks. This is reflected by the research effort that has systems, redundahcy of manipulator mechanisms has been been devoted to the study of manipulator force control. generally viewed as a problem of resolving the end-effector Accommodation [35], joint compliance [26], active stiffness desired motion into joint motions with respect to some criteria. [311, impedance control [9], and hybrid position/force control [28] are among the various methods that have been proposed. Manipulator redundancy has been aimed at achieving goals such as the minimization of a quadratic criterion [29], [34], the Force control has been generally based on kinematic and avoidance of joint limits [5], [21] the avoidance of obstacles static considerations. While in motion, however, a manipula[4], [6], [20], kinematic singularities [23], or the minimization tor end-effector is subject to inertial, centrifugal, and Coriolis of actuator joint forces [8]. The end-effector equations of motion for a redundant manipulator are established and its behavior with respect to generalized joint forces is described. Manuscript receivedAugust 6 , 1985; revised May 29, 1986. This work was supportedinpart by the NationalScienceFoundation and inpart by the The unified approach for motion and active force control is Systems Development Foundation. then extended to these systems. The authoris with the Artificial Intelligence Laboratory, Computer Science Kinematic singularities is another area that has been Department, Cedar Hall, Stanford University, Stanford, CA 94305. considered within the framework of joint space control and IEEE Log Number 8610323.
Abstract-A framework for the analysis and control of manipulator systems with respect to the dynamic behavior of their end-effectors is developed. First, issues related to the description of end-effector tasks that involve constrained motion and active force control are discussed. Thefundamentals of the operational space formulation are thenpresented, and the unified approach for motion and force control is developed. The extension of this formulation to redundant manipulator systems is also presented,constructing the end-effector equations of motion and describing their behavior with respect to joint forces. These results are used in the development of a new and systematic approach for dealing with the problems arising at kinematic singularities. At a singular configuration, the manipulator is treated as a mechanism that is redundant with respect to the motion of the end-effector in the subspace of operational space orthogonal to the singular direction.

0882-4967/87/0200-0043$01 .00@1987 IEEE

44

IEEE JOURKAL OF ROBOTICS AND AUTOMATION, VOL. RA-3, NO. 1, FEBRUARY 1987

Fig, 1. Constrainedend-effectorfreedom of motion. (a) Fivedegrees of freedom. (b) Four degrees of freedom. (c) Three degrees of freedom.

formulated in terms of resolution of the task specifications into lated object forms, with the fixture or constrained object, a joint motions. Generalized inverses and pseudo-inverses have pair of two rigid bodies linked through a joint. A constrained been used, and recently an interesting solution based on the motiontask can be described, for instance, by a spherical, singularity robust inverse has been proposed [25]. Inthis planar, cylindrical, prismatic, or revolute joint. However, when viewed from the perspective of endpaper, a new approach for dealing with the problem of kinematic singularities within the operational space framework effector control, two elements of information are required for is presented. In the neighborhood of a singular configuration a complete description of the task. These are the vectors of total force and moment that are to be applied to maintain the the manipulator is treated as a redundant mechanismwith respect to the motion of the end-effector in the subspace of imposed constraints, and the specification of the end-effector operational space orthogonal to its singular direction. Control motion degrees of freedom and their directions. of the end-effector for motions along the singular direction is Let f d be a unite vector, in the frame of reference (Ro((3, x,,, based onthe use of the kinematic characteristic of the Jacobian yo, ZO), along the direction of the force that is to be applied by matrix. the end-effector. The positional freedom, if any, of the constrained end-effector will therefore lie in the subspace 11. GENERALIZED TASK SPECIFICATION MATRICES orthogonal to f d . The end-effector motion and contact forces are among the A convenient coordinate frame for the description of tasks most important components in the planning, description, and involving constrained motion operations is a coordinate frame control of assembly operations of robot manipulators. The 6if(0,xf, y f , z f ) obtained from Gio by a rotation transformaend-effector configuration is represented by a set of m tion described by Sf such that zf is aligned with fd. For tasks where the freedom of motion (displacement) is restricted to a parameters, X I , xz, * x , , specifying its position and orientation in some reference frame. In free motion opera- single direction orthogonal to f d , one of the axes Oxf or Oyf tions, the number of end-effector degrees of freedom mo is will be selected in alignment with that direction, as shown for defined [ 131 as the number of independent parameters required the task in Fig. 2. Let us define, in the coordinate @if,the position specificato specify completely, in a frame of reference (Ro, its position and orientation. A set of such independent configuration tion matrix 0 0 parameters forms a system of operational coordinates. In constrained motion operations, the displacement and (1) rotations of the end-effector are subjected to a set of geometric constraints. These constraints restrict the freedom of motion where ox, o;, and a, are binary numbers assigned the value 1 (displacements and rotations) of the end-effector. Clearly, when a free motion is specified along the axes O x , , Oyf, and geometric constraints will affect only the freedom of motion of (3zf, respectively, and zero otherwise. A nonzero value of a, the end-effector, since static forces andmoments at these implies a full freedom of the end-effector position. This case constraints can still be applied. The number of degrees of of unconstrained end-effector position is integrated here for freedom of the constrained end-effector is given by the completeness. The coordinate frame ( R f in this case is assumed difference between mo and the number of independent to be identical to Bo,and the matrix Sf is the identity matrix. equations specifying the geometric constraints, assumed to be The directions of force control are described by the force holonomic. Examples of five-, four-, and three-degree-of- specification matrix associated with Cf and defined by freedom constrained end-effectors are shown in Fig. 1. An interesting description of the characteristics ofendcf=I-cf (2) effectors and their constraints uses a mechanical linkage representation [ 5 ] , 1241. The end-effector, tool, or manipu- where I designates the 3 X 3 identity matrix.
e ,

=f=(T 2 ; )

zf

MOTION

KHATIB:

CONTROL

FORCE MANIPULATORS OF ROBOT

45 The task specification matrices Q and can be constant, configuration-varying, or time-varying matrices. Nonconstant generalized task specification matrices correspond to specifications that involve changes in the direction of the applied force vector and/or moment vector, e.g., moving the endeffector while maintaining a normal force to a nonplanar surface. Cl and have been expressed here with respect to the frame of reference OXo. For control systems implemented for tasks specified with respect to the end-effector coordinate frame, these matrices will be specified with respect to that coordinate frame as well. 111. END-EFFECTOR EQUATIONS OF MOTION

XI

x0

YO

Fig. 2. One-degree-of-freedom motion.

Let us now consider the case where the end-effector task involves constrained rotations and applied moments. Let 7 d be the vector, in the frame of reference aO(O, xo, yo, zo), of moments that are tobe applied by the end-effector, and &,(U, x,, y,, z,) be a coordinate frame obtained from CRo(O, xo,yo, zo) by a rotation S, that brings z, into alignment with the moment vector Td. In a,, the rotation freedom of the endeffector lies in the subspace spanned by { x,, y , ) . To a task specified in terms of end-effector rotations and applied moments inthe coordinate frame a,, we associate the rotation and momentspecification matrices C, and X,, defined similarly to Cf and gf. For general tasks that involve end-effector motion(both position and orientation) and contact forces (forces and moments) described in the frame of reference Boy we define the generalized task specification matrices

and

associated with specifications of motion ahd contact forces, respectively. Q and fi act on vectors described in the reference frame a0. A position command vector, for instance, initially expressed in a0is transformed by the rotation matrix Sf to the task coordinate frame The motion directions are then selected in this frame by the application of Cp Finally, the resulting vector is transformed back in cRo by Sf'. The construction of the generalized task specification matrices is motivated by the aim of formulating the selection process in the same coordinate frame (reference frame CRo) where the manipulator geometric, kinematic, and dynamic models are formulated. This allows a more efficient implementation of the control system for real-time operations. Control systems using specifications based only on the matrices C , and X, will require costly geometric, kinematic, and dynamic transformations between the reference frame and the task coordinate frames.

Joint space dynamic models, which establish the equations of manipulator joint motions, provide means for the analysis and control of these motions, and for the description of the configuration dependency and interactive nature of these mechanisms. However, the control of end-effector motion and contact forces, or the analysis and characterization of endeffector dynamic performance requires the construction of the model describing the dynamic behavior of this specific part of the manipulator system. The end-effector motion is the result of those combined joint forces that are able to act along or about the axes of displacement or rotation of the end-effector. These are, indeed, the forces associated with the system of operational coordinates selected to describe the position and orientation of the end-effector. The construction of the end-effector dynamic model is achieved by expressing the relationships between its operational positions, velocities, accelerations, and the virtual operational forces acting on it. First, let us consider the case of nonredundant manipulators, where a set of operational coordinates can be selected as a system of generalized coordinates for the manipulator. The manipulator configuration is represented by the column matrix q of n joint coordinates, and the end-effector position and orientation is described, in a frame of reference OXo, by the mo X 1 column matrix x of independent configuration parameters, i.e., operational coordinates. With the manipulator nonredundancy assumption we have the equality n = mo. Now let us examine the conditions under which a set of independent end-effector configuration parameters can be used as a generalized coordinate system for a nonredundant manipulator. In the reference frame R0, the system of mo equations expressing the components of x as functions of joint coordinates, i.e., the geometric model, is given by
X=

G(g).

(5)

Let qi and Qi be, respectively, the minimal and maximal bounds of the ith joint coordinate q i . The manipulator configuration represented by the point q in joint space is confined to the hyperparallelepiped

Obviously, for arbitrary kinematic linkages, and general joint boundaries, the set of functions G defined from Dq to the

46 domain 9, of the operational space given by

IEEE JOURNAL OF ROBOTICS AUTOMATION, AND

VOL. NO. RA-3,

1 , FEBRUARY 1987

where p ( x , x) is the vector of end-effector centrifugal and Coriolis forces given by pj(x, X)=X~rIj(x)X,

i= 1,

.*. , mo.

(15)

is not one-to-one. Different configurations of the manipulator links can, in fact, be found for a given configuration of the end-effector. The restriction to a domain where G is one-to-one is therefore necessary to construct, with the operational coordinates, a system of generalized coordinates for the manipulator mechanism. In addition, for some configurations of the manipulator, the end-effector motion is restricted by the linkage constraints and its freedom of motionlocally decreases. These are the singular configurations, which can be found by considering the differentiability characteristics of the geometric model G . Singular configurations q E are those where the Jacobian matrix J ( q ) involved in the variational or kinematicmodel associated with G ,
= J(q)Aq,

The components of the mo x mo matrices rIj(x) are the Christoffel symbols a i , j k given as a function of the partial derivatives of A(x) with respect to the generalized coordinates x by
1 ah, ahik a j , j k = - -+---

axjk
axi

axk

axj

1.

(16)

The equations of motion (14) establish the relationships between positions, velocities, and accelerations of the endeffector and the generalized operational forces acting on it. The dynamic parameters in these equations are related to the parameters involved in the manipulator joint space dynamic model. The manipulator equations of motion injoint space are given by
m ) g + b ( q , q)+g(q)=r

(8)

(17)

is singular. The end-effector behavior at singular configurations is treated in Section VIII. Let 5, be the domain obtained from 9, by excluding the manipulator singular configurations and such that the vector function G of (5) is one-to-one. Let a ) , designate the domain

S,= G ( B q ) .

(9)

The independent parameters X I , x2, * * , xmoform a complete set of configuration parameters for a nonredundant manipulator, in the domain a ) , of operational space and thus constitute a system of generalized coordinates for the manipulator system. The kinetic energy of the holonomic articulated mechanism is a quadratic form of the generalized operational velocities
(X,

where b(q, q ) , g ( q ) , and r represent, respectively, the Coriolis and centrifugal, gravity, and generalized forces in joint space. A ( q ) is the n X n joint space kinetic energy matrix. The relationship between the kinetic energy matrices A ( q ) and A(x) corresponding, respectively, to the joint space and operational space dynamic models can be established [131, [14] by exploiting the identity between the expressions of the quadratic forms of the mechanism kinetic energy with respect to the generalized joint. and operational velocities, A(x)=J-*(q)A(q)J-(q). (18)

X) = - XTA(x)X

1 2

(10)

The relationship between the centrifugal and Coriolis forces b(q, q ) and p ( x , X) can be established by the expansion of the expression of p(x, X) that results from (1 l),
p(x,

where A(x) designates the mo x mo symmetric matrix of the quadratic form, i.e., the kinetic energy matrix. Using the Lagrangian formalism, the end-effector equations of motion are given by

X) = h(x)X - V T ( x , X ) .

(19)

Using the expression of A(x) in (18), the components of p(x,

x) in (19) can be written as

A ( x ) X = J - T ( q ) A(414- A ( q ) h ( q , 4 ) + j - T ( Q ) A ( q ) 4

v m , X)= J - = ( q > l ( q 4 , ) + j-* ( S ) A(414

where the Lagrangian L ( x , X) is
L (x, X) = T ( x , X)- U ( x )

(20)

where (12) and 1 li(Q, 4)=- q=Aqi(q)q, 2 i=l,

*.-,

h(q, 4 ) = j ( q M

(21)

and U ( x ) represents the potential energy due to gravity. F is the operational force vector. Let p ( x ) be the vector of gravity forces p ( x )= V U ( x ) . (13)

n.

(22)

The end-effector equations of motion in operational space can be written [ 131, [14] in the form A ( xX )f )+ p ( ~ x, )=F
(14)

The subscript q i indicates the partial derivative with respect to the ith joint coordinate. Observing from the definition of b(q, q ) that
b(q1

4>=A(s)4-4s,4)

(23)

KHATIB: MOTION AND FORCE CONTROL OF ROBOT MANIPULATORS

47
Nonlinear dynamic decoupling in operational space is obtained by the selection of the following control structure,

yields
Ax,

4= J- =(q)b(q,4 ) - N q ) h ( q , 4).

(24)

The relationship between the expressions of gravity forces can be obtained using the identity between the functions expressing with the gravity potential energy in the two systems of generalized coordinates and the relationships between the partial derivatives with respect to these coordinates. Using the definition of the Jacobian matrix (8) yields where A(x), effector equations of motion (14), i.e., A, p , p , are expressed in terms of joint coordinates. This resolves the ambiguity in defining the configuration of the manipulator corresponding to a configuration of the end-effector in the domain DX of -(7). With these expressions, the restriction to the domain 6,, where G is one-to-one, then becomes unnecessary. Indeed, the domain of definition of the end-effector dynamic model of a nonredundant manipulator can be extended to the domain a ) , defined by

F = Fm+ Fccg

(29)

Fm= A(x)P$

P (x, x),

and j (x) represent the estimates of

In the foregoing relations, the components involved inthe end- A(x), p(x, x), and p (x). FZ is the command vector of the

decoupled end-effector. With a perfect nonlinear dynamic decoupling, the end-effector becomes equivalent to a. single unit mass Zmo, moving in the mo-dimensional space. To simplify the notations, the symbol will be dropped in the following development. At the level of the decoupled end-effector, I$, various control structures can be selected. For tasks where the desired motionof the end-effector is specified, a linear dynamic behavior can be obtained by selecting
P~=z,,Xd-kP(X-Xd)-ku(X-Xd)

a , = G(a),)

(26)

(31)

where is the domain resulting from a ) , of (6) by excluding the kinematic singular configurations. Finally, let us establish the relationship between generalized forces, i.e., F and r. Using (18), (24), and (25) the endeffector equations of motion (14) can be rewritten as
J - T(q)rA ( Q ) q +M q ,

$ 3 ,

4 ) +g(q)l= F.

(27) (28)

Substituting (17) yields

where x d , i d , and & are the desired position, velocity, and acceleration, respectively, of the end-effector. Zmois the mo X mo identity matrix. kp and k, are the position and velocitygain matrices. An interesting approach for tasks that involve large motion to a goal position, where a particular path is not required, is based on the selection of the decoupled end-effector command vector F; as

r = J T ( q )F

FZ = - k , ( i where

Kid)

(32)

which represents the fundamental relationship between operationaland joint forces consistent with the end-effector and manipulator dynamic equations. This relationship is the basis for the actual control of manipulators in operational space. IV. END-EFFECTOR MOTION CONTROL The control of a manipulator in operational space is based on the selection of the generalized operational forces F as a command vector. These forces are produced by submitting the manipulator to thecorresponding joint forces I ' obtained from (28). As with joint space control systems, the control in operational space can be developed using a variety of control techniques. In operational space control systems, however, errors, performance, dynamics, simplifications, characterizations, and controlled variables are directly related to manipulator tasks. One of the most effective techniques for dealing with these highly nonlinear and strongly coupled systems is the nonlineardynamicdecouplingapproach [36],[37], which fully exploits the knowledge of the dynamic model structure and parameters. Within this framework of control and at the level of the uncoupled system linear, nonlinear, robust [32], and adaptive [3] control structures can be implemented.

(33)
This allows a straight line motion of the end-effector at a given speed V,=. The velocity vector x is in fact controlled to be pointed toward the goal position while its magnitude is limited to V,,,. The end-effector will then travel at V,,, in a straight line, except during the acceleration and deceleration segments. This command vector is particularly useful when used in conjunction with the gradient of an artificial potential field for. collision avoidance [151. Using the relationship between generalized forces given in (28), the joint forces corresponding to the operational command vector F, i n (29) and (30), for the end-effector dynamic decoupling and control, can be written as

r =J=(q)A(q)F:+

4 )+ d q )

(34)

where 6(q,4) is the vector of joint forces under the mapping into joint space of the end-effector Coriolis and centrifugal

48

JOURNAL IEEE

OF ROBOTICS AND AUTOMATION, VOL. RA-3, NO. I , FEBRUARY 1987

force vector p(x, i). To simplify the notation, A has also been This approach has also been proposed [lo] for real-time used here to designate the kinetic energy matrix when dynamic control of manipulators in joint space. expressed as a function of the joint coordinate vector q. 6(q, V. CONSTRAINED MOTION OPERATIONS 4 ) is distinct from the vector of centrifugal and Coriolis forces The matrix s2 defined earlier specifies, with respect to the b(q, q ) in(17)that arises whenviewingthe manipulator frame of reference 030, the directions of motion (displacement motion in joint space. These vectors are related by and rotations) of the end-effector. Forces and moments are to 6(q, 4 ) = H q , 4 ) - J T ( q ) A ( q ) h ( q ,4 ) . (35) be applied in or about directions that are orthogonal to these A useful form of 6(q, q ) for real-time control and dynamic motion directions. These are specified by the matrix 8. An important issue related to the specification of axes of analysis can be obtained by a separation of its dependency on rotation and applied moments is concerned with the compatiposition and velocity. bility between these specifications and the type of representaThe joint space centrifugal and Coriolis force vector b(q, tion used for the description of the end-effector orientation. In of (17) can, in fact, be developed in the form fact, the specification of axes of rotations and applied moments b(q7 4 ) =B(q)[441+ c(q)[q21 (36) in the matrices C, and 2, are only compatible with descriptions of the orientation using instantaneous angular rotations. where B ( q ) and C ( q ) are, respectively, the n X n(n - 1)/2 However, instantaneous angular rotations are not quantities and n x n matrices of the joint space Coriolis and centrifugal that can be used as a set of configuration parameters for the forces associated with b(q, 4 ) . [qq] and [q2]are the symbolic orientation. Representations of the end-effector orientation notations for the n(n - 1)/2 X 1 and n X 1 column matrices such as Euler angles, direction cosines, or Euler parameters, are indeed incompatible with specificationsprovided by C, and [441=[4142 4143 ' * * 4 n - 1 @ n I T

q,

[42] =[

q:

4;

E,.

* * *

Qi]T.

(37)

With [4q] and [g2],the vector h(q, q ) can be developed in the form

h(q7 4 ) = ~ l ( 4 ) [ 9 Q 1 + ~ 2 ( ~ ) c 4 2 1

(3 8)

where the matrices .Hl(q)and H2(q)have, respectively, the dimensions n x n( - 1)/2 and n x n. Finally, the vector g(q, q ) can be written as
&qY

4 ) =B(q)[(i@l+ aq)[421

(39)

where B ( q ) and c(q) are the n X n(n - 1)/2 and n X n matrices of the joint forces under the mapping into joint space of the end-effector Coriolis and centrifugal forces. These matrices are

Instantaneous angular rotations have been used for the description of orientation error of the end-effector. An angular rotation error vector 64 that corresponds to the error between the actual orientation of the end-effector and its desired orientation can be formed from the orientation description given by the selected representation [ 131, [22]. The time derivatives of the parameters corresponding to a representation of the orientation are related simply to the angular velocity vector. With linear and angular velocities is associated the matrix Jo(q), termed the basic Jacobian, defined independently of the particular set of parameters used to describe the end-effector configuration

B(q)=B(q)-JT(g)A(q)H1(q)

ei(41 = C(q)- JT(q>A(q)H2(q)

(40)

The Jacobian matrix J ( q ) associated with a given representation of the end-effector orientation x, can then beexpressed in the form [13] J ( q )= ExrJo(4) (43)

Withthe relation (39), the dynamic decoupling of the endeffector can be obtained using the configuration dependent dynamic coefficients A ( q ) , B ( q ) , and g(q). The joint force control vector (34) becomes

c(q),

~ = J ~ ( ~ ) A ( ~ ) F I T ,++ eB (m (~ 2) i+ [~ m ~ .I

(41)

where the matrix Ex, is simply given as a function of x,. For end-effector motions specified in terms of Cartesian coordinates and instantaneous angular rotations, the dynamic decouplingandmotion control of the end-effector can be achieved [131 by

By isolating these coefficients, end-effector dynamic decoupling and control can be achieved in a two-level control system architecture [15]. The real-time computation of these coefficients can then be pacedby the rate of configuration changes, which is much lower than that of the mechanism dynamics. This leads to the following architecture for the control system a low rate dynamic parameter evaluation level: updating the end-effector dynamic parameters; a high rate servo control level: computing the command vector (41) using the updated dynamic coefficients.

r = J,T(s>Ao(x)F: + 5o(q, 4 ) + g ( 4 )

(44)

q ) are defined similarly to A ( q ) and where &(q) and 60(q, 6(q,4 ) with J ( q ) being replaced by Jo(q). Using the relationship (43) similar control structures can be designed to achieve dynamic decoupling and motion control with respect to descriptions using other representations for the orientation of the end-effector. The unified operational command vector for end-effector dynamic decoupling, motion, and active force control can be

KHATIB: MOTION AND FORCE CONTROL OF ROBOT MANIPULATORS

49

ll

Fig. 3. Operationalspacecontrol system architecture.

written as

manipulator, and the dynamic behavior of the entire redundant system cannot be represented by a dynamic model in coordiF = Fm+ Fa+ Fccg (45) natesonly of the end-effector configuration. The dynamic and Fccgare the operational command vectors of behavior of the end-effector itself, nevertheless, can still be where Fm, Fa, motion, active force control, and centrifugal, Coriolis, and described, and its equations of motionin operational space can still be established. gravity forces given by The end-effector is affected by forces acting along or about the axes of its freedom of motion. These are the operational F m = Ao(Q)Qp: forces associated with the operational coordinates selected to Fa=fiFz+Ao(q)fiFz describe its position and orientation. Let us consider the endeffector dynamic response to the application, on the endFccg = 60(0(4, 4 ) + g ( q ) (46) effector,oran operational force vector F. In this case of where F: represents the vector of end-effector velocity redundant manipulator systems, the joint forces that can be damping that acts in the direction of force control. The joint used to produce a given operational force vector are not force vector corresponding to F in (45) is unique. The joint force vector r=J,T(q)[Ao(q)(QF:+ OF:) + f i F z I + 60(q1 4 ) +g(q). (47) I'=JT(q)F (28)

The control system architecture is shown in Fig. 3, where kf represents the force error gain and k,f denotes the velocity gain in F,*. An effective strategy for the control of the endeffector during the transition from free to constrained motions is based ona pure dissipation of the energy at the impact. The operational command vector Fa during the impact transition control stage is Fa=Ao(q)8F:. (48)

represents, in fact, one of these solution. The application of the joint forces (28) to the manipulator (17), and the use of the relation

a=J(q)q+h(q,4)

(49)

allow us to establish [13] the equations of motion of the endeffector

AAq)f+ PA49

4 ) +PA41 = F

(50)

where

The duration of the impact transition control is a function of the impact velocity and the limitations on damping gains and actuator torques (this duration is typically on the order of tens of milliseconds). Force rate feedback has also been used inF;. A more detailed description of the components involved in this control system, real-time implementation issues, and experimental results can be found in [19]. VI. REDUNDANT MANIPULATORS The configuration of a redundant manipulator cannot be specified by a set of parameters that only describes the endeffector position and orientation. An independent set of endeffector configuration parameters, therefore, does not constitute a generalized coordinate system for a redundant

h,(q)=[J(q)A-'(q)JT(q)l-'
Pccr(Q,

4 ) = J T ( q ) b ( q 94 ) - & ( 4 ) h ( q ,
PA41 = . f T ( 4 k ( q >

4)
( 5 1)

and
~i(4)=A-'(s)JT(4)A,((I).
(52)

J ( q ) i s actually a generalized inverse of the Jacobian matrix corresponding to the solution that minimizes the manipulator's instantaneous kinetic energy. Equation (50) describes the dynamic behavior of the endeffector when the manipulator is submitted to a generalized

50

IEEE JOURNAL OF ROBOTICS AND AUTOMATION, VOL. NO. RA-3,

1, FEBRUARY 1987

joint force vector of the form (28). The m X m matrix &(q) can be interpreted as a pseudo-kinetic energy matrix correspending to the end-effector motion in Operational 'pace* h ( q 9 0)represents the and forces acting On the end-effector and p r ( q )the gravity force vector. The effect on the end-effector of the application of arbitrary joint forces, can be determined by (50) which can be rewritten

Stability Analysis In the command vector (58), and with the assumption of a
"perfect" compensation (or noncompensation) ofthe centrifugal and Coriolis forces, the manipulator can be considered as a conservative system subjected to the dissipative forces due to the velocity damping term ( - kui) in F.+. These forces are

Substituting (17 ) yields

D ( q )= - k u J T ( q ) ~ r ( q ) J ( q (6 ). 0)
~=~=(q)r.
(54)

Lyapunov stability analysis leads to the condition

This relationship determines how the joint space dynamic forces are reflected at the level of the end-effector. Lemma: The unconstrained end-effector (50) is subjected to the operational force F if and only if the manipulator (17) is submitted to the generalized joint force vector

qTD(q)qsO

(61)

which is satisfied, since D ( q ) is an n x n negative semidefinite matrix of rank m. However, the redundant mechanism can still describe movements that are solutions of the equation

qTD(q)q=o. (62) where I,, is the n X n identity matrix, J ( q ) is the matrix given An example of such behavior is shown in Fig. 4(a). The endin (52) and r0is an arbitrary joint force vector. effector of a simulated three-degree-of-freedom planar manipWhen the applied forces r are of the form (55), it is ulator is controlled under (58). The end-effector goal position straightforward from (54) to verify that the only forces acting coincides with its current position, while the three joints are on the end-effector are the operational forces F produced by assumed to have initially nonzero velocities (0.5 rad/s has the first term in the expression of r. Joint forces of the form been used). [I,, - J T ( q ) J T ( q ) ] rcorrespond 0 in fact to a null operational Asymptotic stabilization of the system can be achieved by force vector. the addition of dissipative joint forces [ 1 3 ] . These forces can The uniqueness of (55) is essentially linked to the use of a be selected to act in the null space of the Jacobian matrix [161. generalized inverse ] ( q ) that is consistent with the dynamic This precludes any effect of the additional forces on the endequations of the manipulator and end-effector. The form of the effector and maintains its dynamic decoupling. Using (55) decomposition (55) itself is general. A joint force vector r can these additional stabilizing joint forces are of the form always be expressed in the form of (55). Let P ( q )be a generalized inverse of J ( q ) , and let us submit rn,=[Z,-JT(q)JT(q)Ir,. (63) the manipulator to the joint force vector By selecting
If, for any yields

r = J T ( q ) ~ + [ z , - J T ( q ) J T ( q ) ] r o (55)

ro,the end-effector is only

rs= (64) -ku,A(q)4,

subjected to F, (56) the vector r,, becomes

. I ( q ) A - ' ( q ) = [ J ( q ) A - ' ( q ) J T ( q ) 1 P T ( q(57) )

with which implies the equivalence of P ( q ) and J ( q ) . VII. CONTROL OF REDUNDANT MANIPULATORS As in the case of nonredundant manipulators, the dynamic decoupling and control of the end-effector can be achieved by selecting an operational command vector of the form of (29), (30): The corresponding joint forces are

rns=rs+JT(q)Ar(q)Frs (65)

(66) Fr,= kuqi.

Finally, the joint force command vector can be written as

r=J'(q)ar(q)(F.+.+Fr,)+r,+b;(q, q ) + g ( q ) (67)
Under this form, the evaluation of the generalized inverse of the Jacobian matrix is avoided. The matrix D ( q ) corresponding to the new expression for the dissipativejoint forces rdis in the command vector (67) becomes

where &(q, q ) is defined similarly to 6(q,4). The manipulator joint motions produced by this command vector are those that minimize the instantaneous kinetic energy of the mechanism.

D ( q ) = - [ ( k , - k u , ) J T ( q ) A , ( q ) J ( q ) + k u , A ( q ) l (68) .
Now, the matrix D ( q ) is negative definite and the system is asymptotically stable. Fig. 4(b) shows the effects of this

KHATIB: MOTION AND FORCE CONTROL OF ROBOT MANIPULATORS

51

(a)

(b)

Fig. 4. Stabilization of redundant manipulator.

stabilization on the previous example of a simulated threedegree-of-freedom manipulator.

hood of a given type of singularity 9, can be defined as

Constrained Motion Control

The extension to redundant manipulators of the results obtained in the case of nonredundancy is straightforward. The generalized joint forces command vector becomes

as=~ ~ 1 1 s ( q ) l ~ ~ o l

(70)

r = J~(q)[ii,,(q)(w; + i j ~ +F,J+ ; ij~;] + r, + gr0(q,q ) + g ( q )

(69)

where &,(a) and b;o(q, q ) are defined with respect to the basic Jacobian matrix Jo(q). VII. SINGULAR CONFIGURATIONS A singular configuration is a configuration q at which some column vectors of the Jacobian matrix become linearly dependent. The mobility of the end-effector can be defined as the rank of this matrix [ 5 ] . In the case of nonredundant manipulators considered here, the end-effector at a singular configuration loses the ability to move along or rotate about some direction of the Cartesian space; its mobilitylocally decreases. Singularity and mobility can be characterized, in this case, by the determinant of the Jacobian matrix. Singularities can be further specified by the posture.of the mechanism at which they occur. Different types of singularities can be observed for a given mechanical linkage. These can be directly identified from the expression of the determinant of the Jacobian matrix. The expression of this determinant can, in fact, be developed into a product of terms, each of which corresponds to a type of singularity related to the kinematic configuration of the mechanism, e.g., alignment of two links or alignment of two joint axes. To each singular configuration there corresponds a singular direction. It is in this direction that the end-effector presents infinite inertial mass for displacements or infinite inertia for rotations. Its movements remain free in the subspace orthogonal to this direction. This behavior extends, in reality, to a neighborhood of the singular configuration. The extent of this neighborhood can be characterized by the particular expression s ( q ) in the determinant of the Jacobian matrix that vanishes at this specific singularity. The neighbor-

where so is positive. The basic concept in our approach to the problem of kinematic singularities canbe formulated as follows. In the neighborhood 9, of a singular configuration q , the manipulator is treated as a mechanism that is redundant with respect to the motion of the end-effector in the subspace of operational space orthogonal to the singular direction. For end-effector motioninthat subspace, the manipulator is controlled as a redundant mechanism. Joint forces selected.from the associated null space are used for the control of the end-effector motion along the singular direction. When moving out of the singularity, this is achieved by controlling the rate of change of s ( q ) according to the value of the desired velocity for this motion at the configuration when Is(q)( = SO. Selecting the sign of the desired rate of change of s(q) allows the control of the manipulator posture among the two configurations that it can generally take when moving out of a singularity. A position error term on s ( q ) is used in the control vector for tasks that involve a motion toward goal positions located at or in the neighborhood of the singular configuration. Using polar or singular value decomposition, this approach can be easily extended to redundant manipulator systems. The extension to configurations where more than one singularity is involved can be also simply achieved. An example of a simulated two-degree-of-freedom manipulator is shown in Fig. 5(a). The manipulator has been controlled to move into andoutof the singular configuration while displaying two different postures. The time-response of the motionin the singular direction x(t) is shown in Fig. 5@). IX. SUMMARY AND DISCUSSION A methodology for the description of end-effector constrained motion tasks based on the construction of generalized task specification matrices has been proposed. For such tasks where both motion and active force control are involved, a unified approach for end-effector dynamic control within the operational space framework has been presented. The use of the generalized task specification matrix has provided a more

52

IEEE JOURNAL OF ROBOTICS AND AUTOMATION, VOL. RA-3, NO. I , FEBRUARY 1987

Fig. 5.

Control at singular configuration.

efficient control structure for real-time implementations, further enhanced by a two-level control architecture. Results of the implementation of this approach have shown the operational space formulation to be an effective means of achieving high dynamic performance in real-time motion control and active force control of robot manipulator systems. This approach has been implemented in an experimental manipulator programming system COSMOS (Control in Operational Space of a Manipulator-with-ObstaclesSystem): Using a PUMA 560 and wrist and finger sensing, basic assembly operations have been performed. These include contact, slide, insertion, and compliance operations [17]. With the recent implementation of COSMOS in the multiprocessor computer system NYMPH [2], where four National Semiconductor 32016 microprocessors have been used, a low-level servo rate of 200 Hz and a high-level dynamics rate of 100 Hz have been achieved. The impact transition control strategy was effective in the elimination of bounces at contact with a highly stiff surface. The end-effector normal velocities at impact were up to 4.0 in/ s. Experiments with square wave force input have also been conducted, and responses with rise times of less than 0.02 s andsteady force errors of less than 12 percent have been observed. This performance hasbeen obtained despite the limitations in controlling the manipulator joint torques [27]. Accurate identification of the PUMA 560 dynamic parameters [l] has contributed to a nearly perfect dynamic decoupling of the end-effector. For redundant manipulator systems, the end-effector equations of motion have been established, and an operational space control system for end-effector dynamic decoupling and control has been designed. The expression of joint forces of the null space of the Jacobian matrix consistent with the endeffector dynamic behavior has been identified and used for the asymptotic stabilization of the redundant mechanism. The resulting control system avoids the explicit evaluation of any generalized inverse or pseudo-inverse of the Jacobian matrix. Joint constraints, collision avoidance [121, [ 151, and control of manipulators postures can be naturally integrated in this framework of operational space control systems. Also, a new systematic solution to the problem of kinematic singularities has been presented. This solution constitutes an effective

alternative to resolving end-effector motions intojoint motions generally used in joint space based control systems. ACKNOWLEDGMENTS The author would like to thank Brian Armstrong, Harlyn Baker, Joel Burdick, Bradley Chen, John Craig, Ron Fearing, Jean Ponce, and Shashank Shekhar for the discussions, comments, and help during the preparation of the manuscript. REFERENCES
[l] B. Armstrong, 0 . Khatib, and J. Burdick, The explicit dynamic model and inertial parameters of the PUMA 560 arm, in Proc. I986 ZEEE Znt. Conf. Robotics and Automation, pp. 510-518. [2]B. Chen, R. Fearing, B. Armstrong, and J. Burdick, NYMPH: A multiprocessor for manipulation applications, in Proc. I986 ZEEE Znt. Conf. Robotics and Automation, pp. 1731-1736. [3] J. J. Craig, P. Hsu, S . S . Sastry, Adaptive controlofmechanical Znt. Conf. Robotics and manipulators, in Proc. 1986 ZEEE Automation, pp.190-195. [4] B. Espiau and R. Boulic, Collision avoidance for redundantrobots Znt. Symp. Robotics with proximity sensors, in Preprints 3rd Research, 1985, pp. 94-102. [5] A. Fournier, Gtntrationde mouvements en robotique-application des inverses gtntralisbes et des pseudo inverses, Thtse detat, Mention Science, Univ. des Sciences et TechniquesduLanguedoc, Montpellier, France, Apr. 1980. [6] H. Hanafusa, T. Yoshikawa, and Y. Nakamura, Analysis and control of articulated robot arms with redundancy, in Proc. 8th ZFAC World Congress, vol. XIV, 1981, pp.38-83. [7] J. R. Hewit and J . Padovan, Decoupled feedback control of a robot and manipulator arms, in Proc. 3rd CZSM-ZFToMMSymp. Theory and Practice of Robots and Manipulators. NewYork: Elsevier, 1979, pp. 251-266. resolution of [SI J. M. Hollerbach and K. C. Suh, Redundancy manipulators through torque optimization, in Proc. I985 Znt. Conf. Robotics and Automation, pp. 1016-1021. [9] N. Hogan, Impedance control of a robotic manipulator, presented at the Winter Annual Meeting of the ASME, Washington, DC, 1981. [lo] A. Izaguirre, and R. P. Paul, Computation of the inertial and gravitational coefficients of the dynamic equations for a robot manipulator with a load, in Proc. I985 Znt. Conf. Robotics and Automation, pp.1024-1032. [ l l ] 0 . Khatib, M. Llibre, and R. Mampey, Fonction dtcision-commande dun robot manipulateur, Rapport Scientifique 2/71-56, DERA-CERT, Toulouse, France, July 1978. [12] 0 . Khatib and J. F.Le Maitre, Dynamic control of manipulators operating in a complex environment, in Proc. 3rd CZSM-ZFToMM Symp, Theory andPractice of Robots and Manipulators. New York: Elsevier, 1979,pp.267-282. [13] 0. Khatib, Commande dynamique dans Iespace optrationnel des robots macipulateurs en prtsence dobstacles, these de docteuringtnieur, Ecole National Suptrieure de 1ACronautique et de IEspace (ENSAE). Toulouse, France, 1980..

KHPrTIB: MOTION AND FORCE CONTROL OF ROBOT MANIPULATORS

53
in [30]M. Renaud and J.Zabala-Iturralde,Robotmanipulatorcontrol, in

t 141 -,

Dynamiccontrol of manipulators in operationalspace,

Proc. 6th CZSM-ZFToMM Congress on Theory of Machines and Mechanisms. New York: Wiley,1983, pp. 1128-1131. , Real-timeobstacleavoidancefor manipulators and mobile robots, in Proc. ZEEE Int. Conf. Robotics and Automation, pp.

Proc. 9th Int. Symp. Industrial Robots, 1979, pp. 463-475.

[31] J. K. Salisbury, Active stiffness control of a manipulator in Cartesian coordinates, presented at the 19th IEEE Conf. Decision and Control, Albuquerque, NM, Dec. 1980. [32] J. J. Slotine and 0. Khatib, Robust control in operational space for goal positioned manipulator tasks, presented at the 1986 American Control Conf., Seattle, WA, June 1986. [33] K.Takase,Task-orientedvariablecontrol of manipulator and its softwareservoingsystem, in Proc. ZFAC Znt. Symp., 1977, pp. 139-145. [34] D. E. Whitney, Resolved motion ratecontrol of manipulators and human prostheses, ZEEE Trans. Man-Machine Syst., vol. MMS-2, pp. 47-53, June 1969. [35] D. E. Whitney, Force feedbackcontrol of manipulator fine motions, ASME J. Dynamic Syst., Meas., Contr., pp. 91-97, June 1977. [36] J. Zabala-Iturralde, Commande des robots manipulateurs B partir de la modelisation de leur dynamique, these de 3 cycle, Univ. Paul Sabatier, Toulouse, France, July 1978. [37] E. Freund,Thestructure of decoupled nonlinear systems, Znt. J. Contr., vol. 21, no. 3, pp. 443-450, 1975.

, t 161 -

500-505. Operationalspace formulation in the analysis,design, and control of manipulators, in Preprints 3rd Znt. Symp. Robotics Research, 1985, pp. 103-110. t 171 0 . Khatib, J. Burdick, and B. Armstrong, Robotics in three acts1 (film), Artificial Intell. Lab., Stanford Univ., Stanford, CA, Part 1 1985. 0.Khatib and J. Burdick, Dynamic optimization in manipulator design: Theoperatioqalspaceformulation, presented at the 1985 ASME Winter Annual Meeting, Miami, FL. -, Motion and force control of robot manipulators,, in Proc. 1986 ZEEE Znt. Conf. Robotics and Automation, pp. 1381-1386; M. Kircanski and M. Vukobratovic, Trajectory planning for redundant manipulators in presence of obstacles, in Preprints 5th CZSM-

ZFToMM Symp. Theory and Practice of Robots and Manipulators,

1984, pp. 43-58. A. Liegois, Automation supervisory control of the configuration and behavior of multibody mechanisms, ZEEE Trans. Syst., Man, Cybern., vol. SMC-7, Dec. 1977. and P. Paul, Resolved acceleration J. Y. S. Luh,M.W.Walker, control of mechanical manipulators, ZEEE Trans. Automat. Contr., pp. 468-474, June 1980. J. Y. S. Luh and Y. L. Gu, Industrial robots with seven joints, in Proc. 1985ZEEEZnt. Conf. Robotics and Automation, pp. 10101015. M. T. Mason, Compliance and force control for computer controlled manipulators, TEEE Trans. Syst., Man, Cybern., vol. SMC-11, pp. 418-432, 1981. Y. Nakamura, Kinematical studies on the trajectory control of robot manipulators, Ph.D. dissertation, Kyoto, Japan, June 1985. R.P. Paul and B. Shimano, Compliance and control, in Proc. Joint Automatic Control Conf., pp. 694-699, July 1976. L. Pfeffer, 0 . Khatib, and J. Hake, Joint torque sensory feedback in the control of a PUMA manipulator, presented at the 1986 American Control Conf., Seattle, WA, June 1986. M. Raibert and J. Craig, Hybrid position/force control of manipulators, ASME J. Dynamic Syst., Meas., Contr., June 1981. la modklisation et de la M. Renaud, Contribution B 16tude de commmande des systemes mkcaniques articules, these de docteur ingenieur, Univ. Paul Sabatier, Toulouse, France, Dec. 1975.