Damped-Rate Resolved-Acceleration Control for Manipulators

Copyrighl @ 1996 IFAC 13th Triennial World Cong~ss. San Francisco, USA

tb-I I I

.". - ..,

DAMPED-RATE RESOLVED-ACCELERATION CONTROL FOR MANIPULATORS 1 Sun-Li Wu and Shir-Kuan Lin

Instilute of Control Engineering, National Chiao rung University Hsinchu 300, Taiwan ROC

Abstract. The d~mped least-squares method has been used to solve the singularity problem of the resolved-acceleration control scheme. It works by damping only the joint accelerations, 50 that the joint accelerations in the degenerated directions are zero at a singular point. However, the joint velocities in the degenerated directions may be nonzcro in some situations. These joint velodties will make fluctuations around the singular point. In this paper, we propose the damped-rate resolved-acceleration control scheme (DRRAe) to overcome this drawback. Sirnulations were und ertaken to verify the proposed control scheme. Keywords. robot cartesian control , alngularities, damped least-squares method

1. INTRODUCTION

Resolved-rate control and resolved-accelera.tion control (Luh et al., 1980) are two general control sehemes for the control of robots in Cartesia.n space. The resolvedrate control scheme is suitable only for low speed motion control, since it ignores the ma.nipulator dynamics. In this paper , however, we consider only the re501vedacceleration control scheme. The resolved-acceleration control scheme will break down when the Jacobian ma.trix is singular. In the neighborhood of the singularity, very large joint speeds are required to produce small changes in some end-effector positions or orientations. An effective strategy t hat allows motion control of the manipulator in the neighborhood of aingularities is the damped least-squares method originally proposed by Nakamura and Hanafusa (1986) and Wampler (1986). 1 This paper was support.ed in part. by t he National Science Council, Ta.iwan, under grant no. NSC84-22I'2-E-009-028

349

Most researchers (Chiaverini, 1993; Chiaverini et al., 1994; Maciejewski and Klein , 1988) have used the damped least-squares method to solve the singular problem of the resolved-rate control scheme. Wampler and Leifer (1988) presented the application of the damped leastsquares method to the resolved-acceleration control scheme. Lin and Wu (1995) proposed another version of the damped least-squares method to deal with the sin· gularity of the resolved-acceleration control scheme. In this case, the degenerated directions of the manipulator are found and then the damped least-squa.res method is applied only to them. The accuracy of the motion along the other directions can be preserved, however, Lin and \Vu )g experiments (1995) show that there are fluctuations around the workspace boundary for the resolvedacceleration control scheme if the target is outside the works pace. In the industry, the robot operators usually do not know wh at the singular point is, and where it is. They may ask the end-effector pass outside the wQrkspace, then the fluctuations win occur around the singular point. In this paper, we analyze the reason for th e fluctuations and propose th e damped-rate resolved-

acceleration control (DRRAC) to overcome above drawback . Finally, the proposed control scheme is tested by simulations on the PUMA 560 manipulator.

ing in the joint coordinates is then guaranteed. This techniq ue is called the computed torque scheme. Luh et al. (1980) proposed the retlOlved-acceleration control scheme as

This paper is organized as follows . The resolved acceleration control scheme and the damped least·squares method are reviewed in Section 2. Section 3 states the motion problem of the damped-acceleration resolvedacceleration control scheme (DA RAC) and proposes DRRAC scheme to overcome this problem. Some simulations about motion near the singularity are reported in Section 4. Conclusion are drawn in a final section.

(6) where KD and Kp a re gain matrices, subscript lid" denotes the desired value, €,. = rd - r is the position error, and t e is the orientation error. The orientation error can be Ou, U tan 0/2 (Rodrigues parameters), u sin 0 (the parameters ofLuh et al.), or usin8/2 (Euler parameters), where 8 is the rota.tional angle, u is the unit vector of the rotational axis. Unfortunately, this control scheme breaks down when J- 1 does not exist, which occurs at. a. singular configuration.

2. BACKCROUND

2.1 Resolved-Acceleration Control The relationship between the end-effector velo cities and t he joint rates for the robotic m a nipulators can be represented as (1 )

2 .2 Damped Least-Squares Method

where r a.nd w are the end-effector velocity and the angula.r velocity, respectively) q is the joint rate vector) and J is the Jacobian matrix. Differentiate (1), we get

8: [ ..r ]

.. =Jq

+ J..q

The damped least-squares m ethod for the inverse problem of (1) is applied to solve the following optimization problem:

(2)

minOl Jq - v q

where r and a are the end-effector acceleration and the angula.r acceleration, respectively,

.

qp:;::::

be moddled in the form of

= M(q)'i· +f(q, q)

(8)

where (9) which a.lways exists for p f:. O. This solution is a compromise between the residual error, Jq - v , and the magnitudes of the j oint velocities, q) by p. The singular valu e decomposition (SVD) can provide insight into the singularities of the inverse Jacobian (Maciejewski and Klein , 1989). ]n th is paper, we consider nonredundant manipulators, 80 J is a 6 x 6 matrix. By the theory of SVD, t here are two orthogonal matrices U = IUl ... u,] and V = [v! ... v,], such that

(4)

where q. is the vector of the desired joint accelerations, so that

ij = ij.

JfpV

(3)

where M( q) is the positive defini te, symmetric inertia matrix, f (q, q) is the vector comprising Coriolis, centrifugal and gravity force, T is the vector of actuator force) q is the vector of joint displacements, The secondorder nonlinea.r coupled dynami c equa.tion (3) can be lineari.ed and decoupled by inputting the inverse dyna mi cs: T

(7)

where p is the damping factor. The solution to (7) is

It is well-known that the dynamics of a manipulator can

M (q)'i +f(q,q) = T

11' +p' 11 q 11')

,

J

(5)

= Ul:V = EO'iUiV; T

(10)

i=l

That me ans, adding the inverse dynamics as a compensator in the conventional controller, the trajectory track-

wh ere E :;:::: diag[C'l l"' ) 0'6], CTi are the singular values of J . The vectors and Vi are the i-th left singular

u,

350

Applying DARAC, if the manipulator is at the singularity, say u. 0, then '/. 0, sinte "/. O'.>'./(u~ + p').

vector and the i-th right singular vector, respectively.

=

Substituting (10) into (9), we obtain

=

=

This implies that the component of

ro. Thus, the component of

(11)

where :Et,

= diag[--'-'---+ trp7 ,'"

~) . , u:+?

When the manipulator is in the neighborhood of a sin-

gular point , it can be seen from (U) that the solution

q are

not infinitely large.

J-' in (6) is replaced with J~ to obtain the following control scheme (Kirc3nski and Bori c, 1993; Lin and Wu,

17;

_ JIp8 • -_L...J "

(T.) a V,.

~ Uj

i=1

O"j

+P

(12)

q'

Jq' _ ,,' 11' +p'lI q' 11')

two~link

lar value decomposition of the Jacobian matrix can be

found from Kircanski and Boric (1993) . Form there, we

(13)

know the singular vector

We shall name the control scheme (12) DARAC. Note

-sign(5,p)C12u21 - 512U,,] 11, = [ -sign(5,p)5 12 U21 + C"u"

that if ideal computed-torque co ntrol is used , the joint

acceleration

q is equal

to

qd.

is ze-

(1) The wrist-center degeneracy: We consider a arm shown in Fig.l(a), whose symbolic singu-

The vector qd4 in (12) is actually the solution to the optimization problem of

u,iu(1I

VI:

ulator with a spherical wrist into two parts: wrist·center degeneracy and orientation degeneracy. In the following discussion, we shall state the problems at these two degeneracy configurations.

6

".

along

is zero by (5) if

We shall classify degeneracy (or singularity) of a manip-

1995) qdl1 -

v.

ideal computed-torque control is used. In this configuration, the component of q along VI: is uncontrollable, i.e ., wc cannot change the valuc of q along Vk . This component of q may be nonzero in some situatlons. In general , if the target is oHtside the workspa.ce or there is an infeasible region between the initial position and the desired posi tion, the component of q along v k is nonzero when the end-effector approaches thc workspace boundary containing the singular point t7} = O. These nonzero components of cl are unnecessary and will create some problems which are stated as follows.

Suppose that p is moderately small. When the manipulator is far from singularity (i.e., 17; > p), then 17;/(17; + p') '" 1/17;, which implies that J~ '" J-' (see (11».

of joint velocities

q along

qda

by (5) .

=

where 5, sin 9" I, + I" u"

p

3. DAMPED-RATE METHOD

=

5'J=; 17,/(12 (f' + '){12 ;= 17, - 0",),

= 17,

sin I, -

17,,",,17; - q/(I,vul- un·

(16)

cos (9, + 9,), and u"

=

Rewriting (14) and (15) as

3 .1 Motivation

In here, we state the motion problem of DARAC in the

a" =>.,u, +~2U,

neighborhood of the singularity and analyze the reMon

0'1.\.1

tors in Cartesian space can be rep resented as a Unear combination of the left singular vectors U t, .. " Us and any vectors in joint space can be represented as a linear combination of th e right singular vectors VI, ... , V6.

on the y-axis to a target point that is also on y-axis and outside the workspace. When the tip reaches the singular point B on the boundary of the workspace (see Fig. I(b)), then u, = [0 _I)T and >., is nonzero since the position error along U2 is non zero. However , the compon~nt. of Q.dl1 along V 2 is forced to zero since 0'2 = 0 (see

6

(14)

i=l

v,

(18), then the component of q along will become zero by (5) if ideal computed-torque control is used. Whereas

Substituting (11) and (14) into (12), we get

the component of it along V2 is nonzero, so that link 1 moves continuously in a counterclodcwise direction and link 2 moves in a clockwise direction. Thus the tip is

6

Cida

=

L

'YiVi

(18)

The tip of the two-link arm will move from a point A

Thus , in (6), a' can be represented as

= LAiUi

0"2..\2

qd. = ut + p' v, + O'~ + p' v,

for this problem. First, from (12) , we know that any vec-

a·

(17)

(15)

;=1

drawn from the singular point B to point C (Fig.l(c)). As this happens, the controller

351

will command the arm to

y

Infr:uiblc Rq)oo

....

y

3.2 Control Scheme

Tltlet

" .

The above analysis reveals that a good robot Cartesian control scheme should restrict the joint velocities as well as the joint accelerations. It is known that the joint accelerations are the derivative of the joint velocities. If the joint velocities are restricted, then so are the joint accelerations. The converse is, however, not true. This concept motivated us to propose a control scheme that solves the following optimization problem:

(a)

J
ITlin(1I qo

----.l1--I~x «)

(d)

a"

11' +p~ 11 q" 11')

(19)

In discrete-time control, we always use the back difference to approximate the derivative, so that

Fig. 1. The motion of a two-link arm from (a), (b), (c) to (d) through the singular point B.

q"(k)

= .2..(q"(k) 81

qO(k - 1))

(20)

where 81 is the sampling time. Note that, by the assumption of ideal computed-torque control, the measured value of q(k) in this sampling interval is equal to the command q'(k - I) in the last sampling interval. For co nvenience the index le is dropped in the subsequent expressions , hence (20) becom es Fig. 2. The spherical wrist.

q"

return to the target , link 1 then moves in a clockwise direction and link 2 in a cou nterclockwise direction. When the tip returned to the singular point B (Fig. I(d)), the velocities of two Ilnks will remain nonzero, the tip will leave the singular point B again . So there are slowly decaying fluctuations around the singular point B.

= .2..(q"(k) _ q(k)) = _1 (q" 81 Ll.I

q)

(21)

Substituting (21) into (19), the solution of the optimization problem (19) is


(2) The orientation degeneracy: We consider a spherical wrist which is shown in Fig. 2. The singularity OCClUS when the first and third rotational axes are colinear, i.e. , the angle of the second joint is zero. In this configuration, the component of the angular velocity of the wrist along the first rotational axis can be achieved by infinitely many combinations of the joint velocities of the first and third axes, since one of tlle5e two joint velocities can be vanished by the other in the opposite directions. We order the spherical wrist to stay in the singular co nfiguration . When the angle of the second joint approaches zero, th e joint accelera.tions of the first and third joints also approaches zero by DARAC. At the same time, the joint velocities of the first and third joints are uncontrollable. If one of th ese two joint velocities are non zero, due to the external t orque or the numerical error of the controller computa.tion, the other j oint will rotate with the same speed in the opposite direction. The controller cannot stop them since they are uncontrollable, so these two j oints will rotate indefinitely in the singular configuration.

352

= (JTJ

+p~W1JT(a" + LJq) - ~!q

(22)

where the damping factor Pr := p8! . (22) is DRRAC. Actually, DRRAC (22) has one more t erm than DARAC (12) . Rewrit.ing (22) as


=(JTJ + pm-

,

1

JT a' -

~t (JTJ + p~Wlq(23)

If P. is equal to p of DARAC, the first term on right side in a.bove equation is equal to the solution of DARAe, the second term OD right side (q~) is an additional term from DRRAC . We rewrite
··0

qc =

I 1J..t

6

,

t: (1; +p;' ( '"

-p.

T.)

Vi

q

(

Vi

24)

If th e manipulator is far away from any singular point, .

I.e.,

p'

fJ'i "

Pr > 0, (1t+p~ ~ 0, i

= 1"",6,

q;

is zero

0.2

and then DRRAC is identical to DARAC. Whell the manipula.tor is near or at the singular point, cri :> p,. > O,i = 1,··· , k and eT; "" O, j = k + 1,·· ·, 6, then (24) can be reduced to

,

0.1

nreuti8'

RegIon \

o

,

(25)

....

q~

is a sum of the negative mu ltiples of the components of q along Vk+l,' .• , V6. From the discussion of the last section, we deduce that the components of q along Vj I j ::: k + 1" . . I 6, are unnecessary. DRRAC use q; to remove the unnecessary component.s of q.

.().2

I.!.:==L_. . .

_-'-_..:T~a~r~

~.2

-0.1

0

0.1

(a)

4. STMULATIONS

WcrI
Three examples are given in this section . DAMe and DRRAC are compared for the application to the PUMA 560 six-joint robot . The input commands of all simulations are step commands, and the controller parameters are defined as follows : Kp = 641, KD = 161, th e sampling period is 3 ms. p of DARAC and p, of DRRAC are both chosen the same as (Lin and Wu, 1995)

O.02e -1250( IJ- O.02)2 where

CT

0.2

X (m)

,,

0.1

l>

;'

0

.0.1

Inf~ Region "

,

_.......-,, ' ,.:

........... J

..... ~ .... ,....

(26)

;'

./

..... ,

Tar

<>.2 <>.2

0.

0.1

0.2

X (m)

is the smallest singular value of J. (b)

The first example illustrates t he responses of the manipulator if there is an infeasible region between the desired position and the initiaI position . The initial and desired positions are [-0.1 0.2 0.8] and [0.15 - 0.150.6]' The orientation is ignored. The result of DARAC is shown in Fig. 3(a) . The singular circle in the figure is the boundary of the workspace, the infeasible region is inside it, and the workspace of manipulator is outside it. When the end-effector touches the circle, it leaps away from the circle immediately, but after it has touched the singular circle thrice times, the e nd- ~ffector reaches the desired position. Fig. 3(b) is the simulation result of DRRAC . After the end-effector touch es the singular cirde, it moves along the circle until it reaches the target without fluctuations .

Fig. 3. Example I, the trajectory projected on X-Y plane for (a) DARAC (b) DRRAC. boundary without fluctuations. Finally, the end-effector converges to a singular point nea.rest to the target. Example 3 illustrates the response of the manipulator in the orientation degeneracy. The initial position is [-0 .10.20.8]' the target is [-0 .02030 .15010.8649)' and the orientation holds towards +Z axis. At the target, q5 is zero, i.e., a singular point. The joint displacements of the joints 4, 5, and 6 for DARAC are shown in Fig. 5(a). In this figure, when q. = 0, joints 4 and 6 rotate with the same speed in th e opposite directions. This is because DA RAC forces q''; and 96 zero when qs = 0, however, q, and q. are nonzero. The result of DRRAC is shown in Fig.5(b). When q. approaches 0, hoth joints then stopped at the same time.

In example 2, the target is outside the workspace of th e manipulator. The initial position is [-0.1 0.2 0.8], the target is [-0.05 0.05 0.8]. The damping factor of two methods still uses (26). The simulation result for DARAC is shown in Fig.4(a). After the end-effector has touched the singular circle, the fluctuatiolls occur around the workspace boundary. Result of DRRAC is shown in Fig.4(h) . After the end-effector has touched the circle, the end-effec tor moves along the workspace

5. CONC!'uSION This paper presents DRRAC to overcome the problem of the resolved-acceleration control scheme in the neigh-

353

~r--------------------

tion at the orientation degeneracy occurred in DARAC,

but nor in DRRAC. These simulation results show that DRRAC makes the resolved-acceleration control scheme more practical for the industrial manipulator .

•

REFERENCES

Totget

-0.1

0 X (rn)

Chiaverini , S. (1993). "Estimate of the two smallest sin-

0.1

gular value of the Jacobian matrix: application to damped least-squares inverse kinematics,lt J. Robotic Systems, vol. 10, no. 8, pp. 991-1008. Chiaverini, S. , B. Siciliano, and 0 , Egeland (1994) . "Revi ew of the damped least-squares inverse kinematics with experiments on an industrial robot manipulator," IEEE Tran. Control Sy.tem. Technology, vol. 2,

(a)

no. 2, pp. 123-134. Kir canski , M. and M. Dj. Boric (1993). "Symbolic sin~

gul ar value decomposition for a PUM A robo t a.nd its application to a robot operation near singularities," 1nt. J. Robotics Research, vol. 12, no. 5, pp . 460-472. Kircanski, M., N. Kircanski, Dj. Lckovic , and M. Vuko-

AogiorI -0.1

0 X(rn)

bratovic (1994) . "An experimental study of resolved

(b)

acceleration control in ~ingularities: damped least squares approach ," Proc. IEEE 1nt. Conl Robotics

Fig. 4. Example 2, the trajectory for (a) DARAC, (b) DRRA~C~.~______________- ,

~.

u

u

u

-_J U

1

U

U

and Automation, pp.2686-2691. Lin , S. K. and S. L. Wu (1995). "Implementation of of the damped resol ved acceleration control for a manipulator near singularity," J. Systems Engineering) vol. 5, no. 3, I'p . 174-1 91. Luh , J. Y. S. , M. W. Walker, and R. P. Paul (1980) . "Resol ved-acceleration control of mechanical manipulators," IEEE Trans. Automatic control, vol. AC-25,

u

pp . 195-200. Maciejewski, A. A. and Ch. A. Klein (1988). "Numer-

(a)

ical filtering for the operation of robotic manipulators through kinematically singular configura.tions ," J. Robotic Systems, vol. 5, no. 6, pp. 527-552.

Maciejcwski, A. A. and Ch. A. Klein (1989). "The sin-

-_J

gul ar value decomposition: computation and applications to robotics," 1nL. J. Robotics Research, vol. 8, no. 6, PI'. 63-79. Nakarnura, Y. and H. Hanafusa (1986). "Inverse kinematic solutions with singularity robustness for robot manipulator control," Trans . ASME}. Dynamic Sys., M eas ., Contr., vol. 108, pp .163-171. Wampler, C. W. (1986) . "Manipulator inverse kinematic sol ution based on vector formulations and damped least-squares methods," IEEE Trans. Sys., Man, Gybern, vol. 16, no. I, pp.93-101. Wam pi€r, C. W . and L. J. Leifer (1988). "Applications of damped least-squares metho ds to resolved-rate and resolved -acceleration control of rnanipulators / ' ASME J. DYlla mic SY'.' Meas. , Contr., vol. 1I0, pp.31-

~~'--~U~~U~~U~~U~~'--~U~-'~A--J" (b)

Fig. 5. Example 3, the histories of DARAC, (b) DRRAC.

Q4,

q" q. of (a)

borhood of a singular point. The proposed method damps the velocities directly instead of the accelerations such that the unnecessary joint velocities are removed in the singular point. The three situations including two from wrist- tenter degeneracy and one from orientation degeneracy are simulated by DARAC and DRRAC, respectively, on the PUMA 560 manipulator. The fluctuations at the wrist- center degenera.cy a.nd the simultaneous rotation of joints 4 and 6 in the opposite direc-

38.

354