Sie sind auf Seite 1von 16

Dynamic Control of Redundant Manipulators*

Ping Hsu, John Hauser, and Shankar Sastry Department of Electrical Engineeering and Computer Science, Electronics Research Laboratory, University of California, Berkeley, CA 94720 Received February 19, 1988; accepted November 22, 1988

Redundant manipulators provide increased flexibility for the execution of complex tasks. Redundancy is often required to maintain manipulability and avoid obstacles while completing the required task. Self-motion is the internal (joint) motion of the manipulator that does not contribute to the end effector motion. In this article we provide a dynamic feedback control law that guarantees the tracking of a desired end effector trajectory and provides redundancy resolution by making the self-motion of the manipulator flow along the projection of a given arbitrary vector field. By choosing this vector field to be the gradient of a cost function, for example, the manipulator can be made to seek an optimum configuration. The effectiveness of the control law is illustrated with simulation results.

*Research supported in part by NSF under PYI grant DMC 84-51129, the Schlum- berger Foundation, and the Berkeley Engineering Fund.

Journal of Robotic Systems, 6(2), 133-148 (1989)

@ 1989 by John Wiley & Sons, Inc.

CCC 0741-22231891020133-15$4.00

134

Journal of Robotic Systems-1

989

I. INTRODUCTION

Redundant manipulators provide increased flexibility for the execution of complex tasks. The redundancy of such manipulators can be effectively used to avoid obstacles, avoid singularities, and maintain a high degree of manipu- lability while performing the desired end effector task. The extra degrees of freedom of a redundant manipulator are exhibited as joint velocities that do not contribute to the velocity of the end effector. This redundant joint velocity produces self-motionof the manipulator. Much work has been done in the area of redundant manipulators. - (See refs. 1-18,21 for a sampling of recent results.) Most methods for resolving redundancy in manipulation involve defining a cost function, such as manipu- lability defined by Yoshikawa,’* that has a minimum or maximum value at a desirable configuration. Then, for a given end-effector position, using the gradient (or its negative) of this function to control the joint velocity in the redundant directions, the manipulator will tend to seek the optimal configura- tion.1ZsI3,18While many authors have discussed how to specify such joint velocities (this might be termed “kinematic control”), most ignore the fact that manipulators are actually controlled by specifying the joint torques (to provide the desired acceleration). A notable exception is the work of Nakamura et aL21 In their control scheme, the manipulator task is divided into an ordered sequence of sub-tasks. The control law guarantees that all the subtasks (and, in particular, the top priority task) that can be simultaneously realized are accomplished. The control input to the remaining space is chosen to be close (in a least squares sense) to the one that is required to accomplish the remaining subtasks. The actual motion of the manipulator in this subspace was not discussed. In particular, they do not give conditions to guarantee that the internal or self-motion of the manipulator will be satisfactory. In this article, the authors provide a dynamic control law that guarantees the tracking of a given end effector trajectory and also provides for the control of the manipulator self-motion. The desired ‘redundant joint velocity’ can be specified to optimize a cost function over the configurations that achieve the desired end effector position.

II. DYNAMIC CONTROL ALGORITHM

Redundant manipulators have a larger number of degrees of freedom than the dimension of the workspace so that the number of joint space coordinates, rn, exceeds the number of workspace coordinates, n. The effect of the extra degrees of freedom is easily seen by considering the differential relation given by the manipulator Jacobian

X = J( 8) d.

Here, J is an n X m matrix with rn > n since the manipulator is redundant. The null space of J therefore has dimension of at least m - n. Any joint space

Dynamic Control of Redundant Manipulators

135

velocity, i, in the null space of J does not contribute to the workspace velocity, i, The manipulator is thus free to move in this configuration- dependent subspace. This type of motion is called self-motion since it is not observed at the end effector. Thus, there are generally an infinite number of inverse kinematic solutions, 8, for a given workspace position, x. Given a desired workspace trajectory, Xd(f), it is difficult to choose a reasonable joint trajectory, b( t), that also satisfies additional requirements and constraints imposed on the manipulator (e.g., stability, obstacle avoidance, etc.). We shall therefore use the differential relation (1) between joint space and workspace velocities given by the Jacobian to develop a control law similar to the resolved acceleration method presented in ref. 20. The general form of manipulator dynamics is given by

(2)

where M(.) is an m X m,symmetric, positive definite mass matrix, N(., .) is an m x 1 vector containing nonlinear terms such as coriolis and gravity, and T is an m x 1 vector of joint torques. From (2), it is obvious that we select the joint torque inputs to effect the joint accelerations and that joint velocities are not the controller output as some authors have indicated. Given a desired tra- jectory, &('), we want to choose T so that the actual trajectory tracks the desired one. Additionally, we shall want to control the joint $ace trajectory to achieve subtasks such as singularity and obstacle avoidance. Differentiating (l),we obtain the differential relation for accelerations

M(e)e+N(e, e)=

x = je+ J8.

(3)

Since J is not square (m> n), we must use the pseudo-inverse, J+ defined in (9,to obtain the inverse relation

,j= j+(i- je) + &,

(4)

where &, is a vector in the null space of J. The pseudo-inverse, J+, is defined as the unique matrix such that"

JJ+J = J,

(J'J)'

= J+J,

J+JJ+ = J+, (JJ*)T = JJ+.

When JE W""" (m> n) has full rank (the manipulator is not in a singular configuration), we write

J+ = JT( JJT)-'

(6)

so that J+ satisfies JJ+ = I (I is the identity matrix) as well as Eq. (5). Using Eq. (4) with (2) we are ready to specify a control law to track a given workspace trajectory. This is the subject of the following proposition.

136

Journal of Robotic Systems-1

989

Proposition 1

Let the control, T, be given by

where e4 xd - x is the tracking error, K, and Kp are constant feedback gain matrices, and 4Nis any vector in the null space of J. If the manipulator does not go through a singularity, then the control law (7) guarantees that the tracking error converges to zero.

Proof

The closed loop system is given by

~8

which simplifies to

+ N = M{J+(xd + ~,e+ Kpe - jb) + b}+ N

8 = J'(Xd

+'K,e 4- Kpe - .fb) + &

since M is uniformly positive definite. Combining (4) with (9) we get

J+(e+K,e +Kpe)= (iN- &.

Premultiply (10) by J to obtain

e +K,e +Kpe = 0

since JJ+ = I when J has full rank and & - & belongs to the null space of J.

The proper

a Hurwitz polynomial) in (11) implies that e goes to zero, exponentially. 0

choice of K, and Kp (e.g., K, = k,l and Kp = kpl with s2 + ks + kp

In general, we cannot expect that the manipulator will avoid singularities while following a given trajectory. At a singularity, the pseudo-inverse, J+, is still defined so that the control law given by (7) is still well-defined. In this case, the pseudo-inverse of J can be found using the singular value decom- position.Ig However, since JJ' # I at a singularity, (11) will only be true if

e + K,e + Kpe is in the range of J. Fortunately, a cleverly chosen & (in the

null space of J) can help the manipulator avoid singularities. There is another reason to choose & with some care. When (x,x) is. considered as the output of the system, the components of joint velocities in the null space of J are unobservable. Unless we use 4N to control these components of joint velocities, the system may have undesirable behavior or even become unstable. See the next section for simulation examples illustrat- ing this point. We are interested in controlling the joint velocities in the null space of J to

Dynamic Control of Redundant Manipulators

137

achieve good system behavior (e.g., stability) and to accomplish a given subtask such as the avoidance of obstacles or singularities. We will first consider (Proposition 2) the case where we want the null space joint velocity to track any given null space velocity. Then we will show (Proposition 3) how a clever choice of such a null space velocity can help achieve certain subtasks.

(which

may be a function of time, the current state, etc.) and we want the null space

joint velocity to track the projection of g onto the null space of J. Since (Z-J+J) projects vectors onto the null space of J, this is the same as asking that

Consider the case where we are given a vector function g( .) E R"

converge to zero. The following proposition shows how to choose & to get the desired result.

Proposition 2

Assume that the manipulator does not go through a singularity. Let the control be given by Eq. (7) with

& = (I - J+J)(g +KNeN) - (J+jJ++P)J(g - i)

(13)

where KN is a positive definite feedback matrix. Then the tracking error e (as defined in Proposition 1) converges to zero and the joint velocity converges to g in the null space of J, i.e., eN+O.

Proof

First, note that c$~ given by (13) belongs to the null space of J (when J has full rank) since

and

J(Z - J+J) = J - J = 0

(14)

J(J+JJ++j+)= jJ++Jj+= - (JJ+)= - I = 0.

dt

dt

d

d

Therefore, by Proposition consider

1, the tracking error e converges to zero. Now,

138

Journal of Robotic Systems-I

989

Substituting for 8 from Eqs. (9) and (13), we get

since (I-J+J)J+=O and & belongs to the null space of J. Define a Lyapunov function v( * ) by

since

(I-J+J)T = (I-J+J).

(I - J+J)(I- J+J) = (I- J+J),

and

(I - J+J)J+= 0.

Since z) is positive definite and d is negative definite, IIeNII goes to zero,

monotonically.

0

There are many possibilities for the function g( * ). One could choose g = 0, for example. Then, as long as the manipulator avoids singularity, the null space velocity, (I-J+J)d will go to zero. However, this choice of control makes no provision for avoidiing ~ingularities.~If the manipulator gets to a singularity, the control law given by (7) and (13) is no longer well defined since J+ is discontinuous at this point. Singularity avoidance should thus be an important objective in choosing the function g( - ) for the control law. The function (I- J+J)g can be thought of as the desired null space joint velocity. Many authors have discussed the selection of the null space joint velocity for the purpose avoiding singularities, avoiding obstacles, and ac- complishing other sub task^.'^*'^*'^ A common method is to define a function, f (@),for which a lower value is associated with a more desirable configuration. For example, we might choose f( 6) = -det(JJT) to maximize the manipu- lability of the manipulator." Then, choosing g = -Vf would tend to keep the manipulator away from singularities. The following proposition states that, for

Dynamic Control of Redundant Manipulators

139

a given cost function f over the joint space, if the end effector is kept at a fixed position and if the initial joint space configuration 8(0)is close to a point 8* which locally optimizes the cost function f subject to the constraint imposed by the end effector position, then e(t) converges to 8". We shall denote the set of joint configurations corresponding to a given end-effector position by

where F( .) is the forward kinematic function.

Proposition 3

Suppose that x(0) = 0 and xd(t) = x(0) for all t 2 0. Let f: W"-,

W be a C2

function such that f(.) restricted to the constraint set q(x(0)) has an isolated local minimum at 8*. Let the control 7 be given by Eqs. (7) and (13) with

g = -Vf (0). If the manipulator does not go through a singularity, then there exists E > 0 and a neighborhood A = q of 8* such that

implies

Proof

Consider

f= Vf'l

= vf T( I - J'J) 0+Vf TJ'J9

= -VfT(Z-

J'J)Vf

+VfT(Z-

J'J)(Vf

= -)I( I - J'J)Vf

[I2- Vf'eN

+VfTJ+Jh.

By proposition 1, the initial

all

t 2 0 so that

condition and control

+d)+VfTJ'Jb

(25)

law imply that i(t) = 0 for

and O(t) E q for t L 0. Using Eq. (26) and the fact that f?N is in the null space of J, Eq. (25) leads to

140

Journal of Robotic Systems-I989

Assume, without loss of generality, that f(8*) = 0. Since 8* is an isolated local minimum of f( .) restricted to q,there exists a c > 0 such that the set

A A { 8 E 9 :f(8) 5 c, 8 path-connected with 8*}

(28)

is compact and contains only one stationary point of f(.) restricted to T, namely 8*. Thus, (I - J'J)Vf# 0 for 8 E A, 8 # 8*. Define a sequence of sets

sia{eEA:f(e) < c. 2-9,

i=

(29)

We will show that, for each i, there is a ti 2 0 such that 8(t) E Si for t 2 ti provided 11eN(0)ll is small enough. Since (I - J'J)Vf is continuous and A - Si is compact for each i,

is well-defined and Si > 0 for each i. Set E = 6112 so that }\eN(t)ll< 6J2 for t20. Then, O(0)E A implies that O(t) E A for tz 0 since, by Eq. (27), fl-6:/2 on A - S1 and any trajectory escaping A must exit through A - S1.

Now, by Proposition 2, lieN(t)l( decreases monotonically to zero. Therefore, for

each

i, there is a 6 2 0 such that (1eN(t)l( 5 Si/2 for t 2 6. This implies that

f5 -6:/2 on A - Si for t 2 6. Thus, there is a ti 2 [ such that f(8(t)) < c * 2-'

(i.e., 6(t) E Si) for all t 2 ti. Since this is true for each ir 1, we have that

e( t) + 8*.

0

Remarks (1) If f is relatively small (i.e., J'Jh

in Eq. (25) is relatively small), it is easy

to see that the system should behave similarly. This point is illustrated in the simulation section. However, for the case where i f0, proof of convergence is much more difficult since both ? and 8* depend on the end effector position. Much stronger assumptions on the function f( a) are probably needed. (2) Clearly, the size of the set A in Proposition 3 depends on the cost function f(-). If f(-) reasonably well behaved, the set A may be quite large. For example, if f( .) is convex, the A is the entire constraint set (see the example in the next section).

HI. SlMULATlON

We consider a three-link manipulator moving in the plane as shown in Figure 1. Each of the three links has length one and is modeled as a point mass at the end of a massless link. Note that the joint angles are measured with

Dynamic Control of Redundant Manipulators

Figure 1.

The three link planar manipulator used for simulations.

141

respect to the horizontal (x) axis. Four sets of simulation results are provided in this section. Each simulation uses a different choice for & in the control law (7). The desired end effector trajectory for each simulation is

xd( f) = 1+sin(3t)

i.e., a circle of radius one centered at the point (x, y) = (1, 1). The simulations are given in order of increasing complexity and demonstrate why the self- motion of a redundant manipulator must be controlled and provide examples of how this may be done to achieve the desired system performance. In the first simulation, & is chosen to be zero. In other words, no control is applied to the joint velocity in the null space of J. As shown earlier in the Proposition 1, regardless of the choice of & (as long as & belongs to the null space of J), the end effector trajectory will converge to the desired trajectory. In Figure 2, (a) shows the trajectory of the end effector, (b) shows the joint trajectory, and (c) gives the determinant of JJT. The determinant of JJT has been called the manipulability index'* since it provides a measure of how close

a manipulator is to a singularity. Figure 2(d) shows several snapshots of the

system configuration at selected times. These snapshots are shown together in

a single picture in Figure 2(e). Note that, although the end point of the

manipulator follows the desired trajectory closely, the motion of the joints exhibits instability and is completely unacceptable. Thus, we see that the self-motion of the manipulator must be actively controlled to prevent such undesirable behavior. In the second simulation, & is given by Eq. (13) with g set to zero. With this &, the control law causes the components of joint velocity in the null space of J to go to zero. The simulation results are shown in Figure 3. Note

142

Journal of Robotic Systems-1

(a) l=x 2=y (end effector trajectory)

1.5

0.

5.

-15.

0.

1.5

0.

2.5

5.

5.

.s

7:5

989

10.

n

frame 290 (1-7.25)

frame 294 I1-7.35)

frame 298 (1-7.45)

frame 302

11-7.55)

frames 290 294 296 302

Figure 2.

(i.e., no redundant joint velocity control).

Legend for Figures 2-5: (a) end effector trajectory, x(t), (b) joint trajectory, O(t), (c) manipulability measure, JJT, (d), (e) selected frames showing manipulator configura- tion.

Simulation results with &GO

that the joint motion (Figure 3(b)) is smooth and reasonable compared to Figure 2(b). A conjecture given in ref. 11 states that with this choice of joint velocity (no component in null space of J), the manipulator will automatically avoid singularities. This conjecture has been disproved3 and this fact is also indicated by Figure 3(c) which shows that the manipulator comes very close to a singularity several times. It is likely that some other desired trajectory will cause the manipulator to go through a singularity. We must therefore always

Dynamic Control of Redundant Manipulators

, (a) l=x

1.5.

0.

5

-15.

1.5

0.

-

$-

frame 290 11-7.25)

O-Z

frame 298 0-7.45)

2=y

(end effector trajectory)

2------ci------=

- 3-

-3

2:5

frame 294 ft-7.353

&

frame 302 (1-7.55)

5.

-1-

7.5

lrames 290 294 298 302

143

10.

Figure 3.

Simulation results with g( - ) = 0 (i.e., the desired redundant joint velocity is

zero).

be careful to select & in a manner that keeps the manipulator away from singularities. In the third simulation, & is given by Eq. (13) where g is

144

Journal of Robotic Systems-1989

Note that g is the negative of the gradient of the cost function

In other words, the control law tries to minimize the difference between

O2 - el. The angle 6; is just the relative angle of link

three with respect to link two (this is another common method of specifying the joint coordinates). Figure 4 shows the simulation results. Note that the determinant of JJT is kept well away from zero and that the manipulator seeks the optimum configuration of 8; = gz. In fact, this cost function and -det(JJT) have the same minimizing point for each constraint set *(x). The final simulation is an example where obstacle avoidance is also im- portant. Furthermore, in this example the obstacle is not stationary but has a time-varying trajectory of its own. Two manipulators working in the same area

is an example of such a system. In this example, the obstacle is a disk following

a trajectory given by

e3A O3 - e2 and &A

yoh(t)= 1.7+0.75 cos(3t).

Since we would like the manipulator to maintain a reasonable posture and yet

avoid the obstacle, we choose g = -Vfob

with fobs defined as

11

fobs = f +z+E

where d2 and d3 are the distance between the obstacle center and joint two and three, respectively. In a real application, a more careful computation of the distance between the manipulator and obstacle would be made. Figure 5 shows the simulation results using the time-varying vector field. Figures 5(d) and 5(e) show how the manipulator is deflected from its ideal posture as the distance function component of the vector field becomes stronger due to the proximity of the obstacle. The effect of this deflection on manipulability is seen in Figure 5(c). Comparing Figures 4 and 5, we see that the distance function component of the cost function causes little effect on the configuration of the system when the obstacle is not close to the manipulator. In each of these simulations we have presented a few pictures of the system configuration at selected times. These frames actually come from a reaI-time movie of each simulation that was displayed on a SUN workstation. These movies provide an effective tool for understanding the dynamics of a complex system. This is especially true for systems where much of the action is coupled and several variables are of interest.

Dynamic Control of Redundant Manipulators

145

( RL2.5 25. &1.5,n:10.B

1.5

0.

1

1

(b) l=thetal

2=theta2

3=theta3

(joint

1

2

trajectory)

(a) l=x 2=y

(end effector

trajectory)

-15.

0.

2.5

5.

7.5

10.

0.

1

0.

2.5

5.

1.5

10.

n

frame 290 (1-7.25)

frame 298 (1-7.45)

n

frame 294 It-7.35)

n

frame 302 (1-7.55)

frames 290 294 298 302

Figure 4.

Simulation results with g( * ) = -Vf

(f chosen to achieve an optimal pos-

ture).

-

i

-

;

-

'

-

;

146

 

(a)

l=x 2=y

1.5

0.

(

 

(b)

l=thetal

-

* 5

-15. 0.

1.5

0.

(

(4

a

Journal of Robotic Systems-1

989

(end effector trajectory)

2.5 5.

2=theta2 3=theta3

21RRh1.52

1

10.

(joint trajectory)

$--gz

2.5

5.

1.5

10.

2.5

5.

1.5

10.

f-

(el

trame 115 6-2.801

1

frame 125(1-2.60)

trame 135p-3.00)

Figure 5.

posture with obstacle avoidance).

Simulation results with g(

) = -Vfob

frames 105 115 125 135

(fob. chosen to achieve an optimal

Dynamic Control of Redundant Manipulators

IV. CONCLUSION

147

Redundant manipulators are often used in executing tasks in a confined workspace where extra freedom is needed to avoid obstacles, maintain good manipulability, etc. In this article, the authors have presented a dynamic control law that guarantees the asymptotic tracking of a desired work space trajectory and also provides an effective means of resolving redundancy to accomplish desirable subtasks. The effectiveness of the control law has been demonstrated with dynamic simulations.

References

1. J. Baillieul, “A constraint oriented approach to inverse problems for kinematically

redundant manipulators,” Proc. of ZEEE Znt. Conf. on Robotics and Automation,

Raleigh, NC, 1827-1833 (1987).

2.

J. Baillieul, “Avoiding obstacles and resolving kinematic redundancy,” Proc. of

IEEE Int. Conf. on Robotics and Automation, San Francisco, CA, 1698-1704

(1986).

3.

J. Baillieul, J. Hollerbach, and R. Brockett, “Programming and control of kine-

matically redundant manipulators,” Proc 23rd Conf. on Decision and Control, Las Vegas, NV, 768-774 (1984).

4.

D.

R. Baker, and C. W. Wampler, 11, “Some facts concerning the inverse

kinematics of redundant manipulators,” Proc. of ZEEE Int. Conf. on Robotics and

Automation, Raleigh, NC, 604-609 (1987).

5.

P. H. Chang, “A closed-form

solution for the control of

manipulators with

kinematic redundancy,” Proc. of IEEE Znt. Conf. on Robotics and Automation, San Francisco, CA, 9-14 (1986).

6.

S. L. Chiu, “Control of redundant manipulators for task compatibility”, Proc. of

IEEE Int. Conf. on Robotics and Automation, Raleigh, NC, 1718-1724 (1987).

7.

R.

Dubey, and J. Y. S. Luh, “Redundant robot control for higher flexibility,”

Proc. of ZEEE Int. Conf. on Robotics and Automation, Raleigh, NC, 1066-1072

(1987).

8.

J. M. Hollerbach, “Optimum kinematic design for a seven degree of freedom

manipulator,” in Robotics Researck The Second International Symposium, MIT

Press, Cambridge, MA, pp. 216-222.

9.

J.

M. Hollerbach, and K.C. Suh, “Redundancy resolution of manipulators through

torque optimization,” Proc. of IEEE Int. Conf. on Robotics and Automation, St.

Louis, Missouri, 1016-1021 (1985).

10.

0. Khatib, “Dynamic control of manipulators in operational space,” Sixth CZSM-

ZFToMM Congress on Theory of Machines and Mechanisms, New Delhi, India,

December 1983.

11.

C.

A. Klein, and C. H. Huang, “Review of pseudoinverse control for use with

kinematically redundant manipulators”, IEEE Transactions of Systems, Man, and Cybernetics,SMC-13,245-250 (1983).

12.

A. Ligeois, “Automatic supervisory control of the configuration and behavior of

multibody mechanisms,” ZEEE Transactions on Systems, Man, and Cybernetics,

SMC-7,868-871 (1977).

13.

A. A. Maciejewski, and C. A. Klein, “Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments,” The International

Journal of Robotics Research, 4, 109-117 (1985).

14.

Y. Nakamura, and H. Hanafusa, “Task priority based control of robot manipula-

tors,” in Robotics Researck- The Second International Symposium, MIT Press,

Cambridge, MA, pp. 155-161.

148

15.

Journal of Robotic Systems-1989

K. C. Suh, and J. M. Hollerbach, “Local versus global optimization of redundant

manipulators,” Proc. of IEEE Int. Conf. on Robotics and Automation, Raleigh,

NC, 619-624 (1987).

16.

C. W. Wampler 11, “Inverse kinematic functions for redundant manipulators,”

Proc. of IEEE Int. Conf. on Robotics and Automation, Raleigh, NC, 610-617

(1987).

17.

0. S. Yahasi, and K. Ozgoren, “Minimal joint motion optimization of manipula-

tors with extra degrees of 325-330 (1984).

freedom,”

Mechanisms and

Machine

Theory, 19,

18.

T.

Yoshikawa, “Analysis and control of robot manipulators with redundancy,” in

Robotics ResearckThe First International Symposium, MIT Press, Cambridge,

MA, 735-747 (1984).

1.9.

G. H. Golub, and C. F. Van Loan, Matrix Computations, The Johns Hopkins Press, Baltimore, MD, 1983.

20.

J. Y. S. Luh, M.W. ‘Walker, and R. P. C. Paul, “Resolved-acceleration control of

mechanical manipulators,” IEEE Transactions on Automatic Control, AC-25,

468-474 (1980).

21.

Y.

Nakamura, H. Hanafusa, and T. Yoshikawa, “Task-priority based redundancy

control of robot manipulators,” 3-15 (1987).

The International Journal of Robotics Research, 6,