A Fuzzy Controller for Multiple Hyper-Redundant Cooperative Robots

A Fuzzy Controller for Multiple Hyper-Redundant Cooperative Robots Mircea Ivanescu, Viorel Stoian. Nicu Bizdoaca Automatic and Computer Dept., University of Craiova, 1 100, Craiov a, Romania e-mail: [email protected] Abstract. A control system is proposed to solve the local control for a multi-chain robotic system formed by tentacle manipulators grasping a common object l1-ith hard point contacts. The two-level hierarchical control is adopted. The upper level coordinator gathers all the necessary information to resolve the force distribution. Then , the lower- level local control problem is treated as a open-chain reduruiant manipulator control problem. A closed loop control based on the inverse dynamic model and the DSMC procedure are proposed. The fuzzy rules are established. Simulation results are presented and discussed. Keywords: Fuzzy Control, Hyper-Redundant Robots

1. INTRODUCTION In the past few years, the research in coordinating robotic systems with multiple chains has received considerable attention . A tentacle manipulator is a manipulator with a great flexibility, with a distributed mass and torque that can take any arbitrary shape. Technologically, such systems can be obtained by using a cellular structure for each element of the arm. The control can be produced using an electrohydraulic or pneumatic action that deter- mines the contraction or dilatation of the peripheral cells. The first problem is the global coordination problem that involves coordination of several tentacles in order to assure a desired trajectory of a load. The second problem is the local control problem, which involves the control of the individual elements of the tentacle arm to achieve the desired position. To resolve this large-scale control problem, a two-level hierarchical control scheme (Cheng,1995) is used. The upper-level system collects all the necessary information and solves the inter-chain coordination problem. Then, the problem is decoupled into j lower-level subsystems, for every arm. The local fuzzy controllers are assigned to solve the local control. In order to obtain the fuzzy control an approximate model of the integral-differential equation that describe the tentacle arm is used. A closed loop control that is based on the inverse dynamic model is used. A decoupling controller is introduced and the conditions that assure the stability of the motion are determined. The control strategy is based on the Direct Sliding Mode Control which controls the trajectory towards the s~itching line and then the motion is forced directly to the origin , on the

switching line. A fuzzy controller is proposed and the fuzzy rules are established by using the DSMC procedures. Efficiently considerations of the method are discussed Numerical simulations for several control problems are presented. 2. MODEL FOR COOPERA TIVE TENTACLE ARMS A multiple-chain tentacle robotics system is presented in Fig. 1. With the chains of the system forming closed-kinematics loops, the responses of individual chains are tightly coupled with one another through the reference member (object or load). The complexity of the problem is considerable increased by the presence of the tentacle robots,

CfMI , j = l..k), the systems with, theoretically, a great mObility, which can take any position and orientation in space. In the Figure 2 is presented a plane model, a simplified structure which can take any shape in the XOZ plane. The dynamic equations for each chain of the system are (Ivanescu, 1984): TMJ : P jA j J[sin{qi _q.i

o

s

.

.

k

j2

+cos{qi - q,i .

+ pAg Jcos q} ds'H} = T} . j = 1.2

o

112

ft· j~s·+ (2.1 )

Li

.

Li (

\,

j rJds =Fj j

Li

wbere

.

_sinqJ ps+Fj j ros qJ ds 0 0

0

(2.2)

where we assume that each manipulator (TMi ) has a uniform distributed mass, with a linear density p ' and a section AJ. We denote by s the spatial variable upon the length of the arm, sE [0, U). We also use

Mo

is inertial matrix of the object and r

defines the Object coordinate vector

r = (x, z,

(2 .5 )

and r(t) represents the desired trajectory of the motion . The inequality constraints which include the friction constraints and the maximum force constraints may be associated to (2.3) (2.6)

,l:JIP 5,B

where Ai is a coefficient matrix of inequality constraints and B is a boundary-value vector of inequality constraints. The problem of the contact forces F.i can be treated as an optimal control problem if we associate to the relations (2.3) - (2.6) (2.7) an optimal index 'I' = re j F i Tbis problem is solved in several papers by the general methods of the optimization or by the specific procedures (Cheng, 1991 ).After all of the

Fil!Ve 2.

the notations: q1 - Lagrange generalised coordinate for

1Ml

(the

absolute

angle),

qi = qi (S,1),SE [O,Li ], 1 E [0,1 f], q,i = qi (s' , 1), S'E [0, Li ], 1 E [0, 1f

Fi are determinated, the dynamics of TMi are decoupled Now, the equations

contact forces

each arm (2.1), (2.2) can be interpreted as same decoupled

= TJ(S,1) - the distributed torque over the arm;

equations with a given 't(s), SE[O, U] acting on the tip of the arm. A discrete and simplified model of (2.1), (2.2) can be obtained by using a spatial discretization.

",i = ",i (S,1)

SI ' s2 ' .. . SN ;

];

Ti

- the distributed moment to give the

desired motion specified on the reference member. All these sizes are expressed in the coordinate frame of the arm

TMi. The k

integral equations are tightly

coupled through the terms ",l , F:J

' F) where all of

these terms determine the desired motion . We propose a two-level hierarchical control scheme for this multiple-chain robotic system The control strategy is to decouple the system into k lower-level subsystems that are coordinated at the upper level. The function of the upper-level coordinator is to gather all the necessary information so as to formulate tbe corresponding force distribution problem and then to solve tbis constrained, optimization problem sucb that optimal solutions for the contact forces FJ are generated. These optimal contact forces are then the set-points for the lowerlevel subsystems. We consider the hard point contact with friction and the force balance equations on the O object may be written as: F =rOD iFi (2.3)

Si - si-l

= t!. ; qi (si) - qi (sk

(2.8) ; i , k = 1, 2, ... ~ where eA. are constants and e is sufficiently small. We denote Si = it!.,

U = ~ t!. , qi (si

Tj (Si ) = Tij , -ri (S; )=

(2.9)

and considering the arm as a light weight arm, from (2.1), (2.2) it results (lvanescu, 1986):

q'

+ f(qi

F:J

) +

a(qi

) +

(qj. qj , ... q~J

113

)

= B Tj

);

-bsi~ ~

a..cr )=

-bsm/, +bsicy/'I'

-

(2. 12)

-bsirr! . +bsm/ . n1 ,.,1-1

bcosq(

b cos q i . - b cos q i .

(2.4 )

c(qi

(2. 11 )

ODfthe partial spatial transform from the coordinate

= GFO

F)

(2.10)

where qi = col

bcosq4 -bcosq(

by the form M Or

) = q~ ,

'tji

where: fD- the resultant force vector applied to object expressed in the inertial coordinate frame (0),

frame for the arm TMi to the inertial coordinate frame (0). The object dynamic equations are obtined

)I
nl

nl-l

(2.13 )

a(q~ XMj - SFj)+ c(q~ Xt.Fj - 8F!)= 0

I 0 ...... ···0 -I I

0 .. . ···0

B= 0 -I I 0 ... 0

(2.14)

+FJ d~iqJd .FJ)=(CfL d d:j J xd

(ooL (a:-L(3.5) J +F! "d d:j d

CXz

d

o

where d is an ) X n j matrix. Using (3.2) in (3.4) we obtain the following control equations:

0 ·· ·0 -I I

+(~I -K!J-KdIW =0 yj+(I_KJ ~,:-j=o

3. CONTROL SYSTEM The control problem asks for determining the

(I -Kd3-dK{3};/ -(Kd2+dAiM

manipulatable torques (control variable) T ~ such

K J Wj+K J

that the trajectory of the overall system (object and manipulators) will correspond as closely as possible to the behaviour. In order to obtain the control law for a prescribed motion, we shall use the inverse model. A closed-loop control system is used (Fig. 3). Let

q~, q~, q~

contact point of the object, and, qj,l/,

t/ ,Fj

hx

the

t./ =~ -/;

ti/=i/a-iMj =ij~ -ijj ;&:i=Fd-Ri;

(3 .1)

fIx

equations of the motion can be written as,

-(~t ~ tijj -(pjt Rj &jj =0

(3.6) and for the force

Kj

h

t.P j

+K j t:.f:j +(I-Kj tu;-j =0

h

fir

(3.7) where we used the notations, .J

i

l' =(I-K

33

-d

i

i i );Q =(Kn+d

KI3

The trajectory controller serves as the trajectory perturbation controller which generates the new

Ri = d (I - Ki

variations 8 qj, 8 qJ, 8 qj, 8 F j in order to

and we considered K}

assure the performances of the motion for the overall system. The control law is proposed as,

Kj

J A~J +KJ t.q.J +KJ A;;J . 8 qJ -- K 11'-"1 12 13'-"1 •

x

8 P~

31'-"1

fix

= K)

M 'j

11:.':

x

12x

x

f3x

x

(3 .2) where the relations (3 .2) define a complex control for the motion parameters and for the force. From (3 . 1), (3.2) results that the evolutions of direct and inverse system take place on the trajecto-

cl =q~ -tJqj; q =~ -!Xjj;(j =ii/t -!Jijj and q j =q~ _&]/q ) =qj _&jj;q i =q~ _&jj' (3.3)

ries,

if ) + j(rF)+F/a(q))+F/c(qi)=BT) Assuming thatcq i «q~; 51 <
q;

j -K

fa -

-Kj ' Kj

.

.

=K} =K}; fJz

fl

j -K

-Kj

f2z. - f2 ' f3x -

(3.9)

h

f1!, -

(3.4) q~,

Taylor-series expansion, neglecting the high-order (2 .11), (3.3) we have terms, from

to

be

stable,

j~ ~ 4K j3 (I - K }I )

and (3.10)

The relations 0.10) define the main conditions imposed to the controller in order to assure the global stability for the motion of the arm and for the force Rid at the terminal point of the arm. If the the inequality in (3.10) is easy to apply, the stability of the matrix (3.10) is more difficult to use. We can obtain a simplified procedure if we choose suitable matrices Kim,n(m,n =1,2,3) in the control law (3 .2):

I - Ki»__ - d Ki ,_ = Cl. I,

Cl. -

l

integer number ;

K32 +dK 12 =2~ ) ;d(I-I2 11 )-K31 = Q j i

then using the

~qj -&jj)+d(q~.FjXt.qj -&jj)+

(3.8) .

fIx

K

+ KJ fJi:) + K j /:}F} /::..:.:. J3::'

&j l «q~; t.q i «q~; Aijj«

31

[_/-IR _p~IQ]

J A~ J + K32'-Af J A;' J + K33'-Af J A;; J .• j 8 F J = K M J +KJ. yj +K j t.F J K

) - Ki 11

From (3 .6), (3.7) we obtain immediately, Proposition L The control law for the motion and force control requires

= Ki 1tltJ J + Ki 2t.q J + Ki3!lii J

so .. J u q -

r.

K i3 - d K/3) these

For the nesingular matrix (I -

same sizes measured on the real system, the error of the feedback system is given by:

hx

K J WJ+K J Y!+(I_K J \':-)=0 hz z f 2z f 1z ) <-

Dijj

be the desired parameters of the

trajectory, Fct' the desired force applied at the j -

8 qJ

.

d

(3.11)

:=: j

Where

=diaJf,I . 4~ ); rei = diaJroI .L;)~)

(3.12) The equations (3.6) become .

An}

.

.

?!!}A'}

.?

114

.

}-A,,}

a . ' 4 i - """ i uq i + (j) i

'4i

=0

(3.13)

The equations (3.6) for the control of the arm parameters and (3.7) for the control of the force offer a simple control for a Direct Sliding Mod Control (DSMC), (lvanescu, 1995). The DSMC is a control method which operates in two steps. First step assures the motion towards the switching line Sq (or SF),

M:'J + p!F MJ by

the

=

general

&if + p/ t:.q/ =0

0

(3.14)

stability

conditions

(Fig.

variables,the control coefficients ~/,

four fuzzy variables on the normalized universe.

,;7 j

= Fi* j = { 0,

0.5, 1.0, 1.5, 2.0, 2.5 } where

the range of the values is chosen such that

,;! ,and for the force:

for the damping coefficient 1

) r ) 2 ( )12. ~i < min~(Oi s

r,

B

1

2

1,

(4.1)

=

1

K}2 ~2(K}3(1-KjJ)2;

Kj2 ' will use

4).

1

s

interval. The membership functions for these quantities are shown in Figure 5. The fuzzy output

(315) When the trajectory penetrates Sq (or SF ), the damping coefficients ,; / ' K 1

;;/ > max

s

1

~/2(s)]2: K}2 >2(K}3~-K;J2

4. FUZZY CONTROLLER A fuzzy control is proposed by using the control of the damping coefficient ~j, K j 2 in (3.15) - (3.16) . We consider a DSMC strategy with the switching of a control variable on the switching line (3 .14),

AQ. I

JIB

:ps

o

(Figure 4). We shall let the errors ~q~, ~FJ and the

j be defined by eight linguistic

variables: NB,NM,NS,NZ,PZ,PS,PM,PB partitioned on the error spaces [-~qm ,~qm]' [-~Fm ,~Fm] and the error rate spaces

B Bn BD Bn

S

S

S

S

B Bn Bn

S

S

S

S

BD Bn B BD

S

S

S

S

[-.&j m ,~q m 1[- M:'m' Mm]

l'l

BD BD ~n B

NZ

sn sn SD SD

.n,[

£

rz

I'll!

III

-1 .0

0

Figure 5.

1.0

Bn B IIn ~D

S

S

S

NM

S

S

S

S Bn Bn B Bn

NB

S

S

S

S

Bll Bn Bn

B

5. NUMERICAL RESULTS The purpose of this section is to demonstrate the effectiveness of the method. This is illustrated by solving a fuzzy control problem for a two tentacle manipulator system which operates in XOZ plane

m

2.1) -

B Bn Bn Bn

S

M , .po

·2.0

sn SD sn sn

NS

where all these quantities are normalized at the same .E

2

The memberships of the output variables are represented in Figure 6, where STI, BTl define linguistic variable: SMALLER THAN I and BIGGER THAN l,respectively. According to the theoretical results obtained in the previous part of the paper, we can generate the control rules which establish a fuzzy control for a DSMC control (Table 1). The main idea is to assure the normal control towards the switching line and direct control when the trajectory penetrates this line. A standard defuzzification procedure based on the centroid method is then used. .uj Table I. A~ INB NM NS NZ l'Z PS .6. F) PM PB

PM BD

&j!, M

15

Fi!!1ll'e o.

j = 1,2 (3.16) The system is moving towards the origin, directly, on the switching line Sq (or SF)'

error rates

1

lit;

j 2 are increased,

nIl.

.

(li~ 1. (li~) 115

, .."

--

lY ,:tmjedD

,

I

TM2

~ Fi,re 7.

.

(Fig. 7). These two manipulators form a closedchain robotic system by a common object which is manipulated. An approximate model (2.10) with t. j

= 0.06 m and n = 7 is used. Also, the length and the mass of the object are 0.2 m and I kg, respectively. The inertial positions of the arms expressed in the inertial coordinate frame are presented in Table 2. The trajectory is defined by x = xo + a sin
Zo + b cos
with xo= 0.2 m, Zo =0.1 m, a= 0.3 m,

b= 0.1 m,
Table 2. TM" ";(0) qjo) 4(,(0) ~) qjo) TM' ..r; TM1 nr;

~) ~)

"lIJ 7"JiJ2. bB 11115 ~

0

4115 3lIK 3lIK be

11

4It)

REFERENCES

work envelopes of the both arms and does not go through any workspace singularities. The maximum force constraints are defined by

FX s; FMAX =SO N .• FZ :<;; FMAX = 50 N

P }min (7 F J

and the optimal indexes min ( ~/ are

used.

The

F

generalized

trajectories

qj (t), i/ (t), iF (t) are obtained by the constraints q) 1

, (t) = 0 ; q7 - (t) =

-11:

and by the condition min IIql

(t)1I The solution of this problem is given by solving the nonlinear differential equation (Schilling, 1990),

qi (t)=

[v iT (q) T

Vi (q)Jl viT (q}w(t)

where

.

w= (x,z) and V(q) is the lacobian matrix of the arms ( j = 1,2 ). A decoupling control procedure (3.11)- (3.12) is used, where the following parameters are selected,

a =3;1;(

=2.5,

w( =l&i =1·· ·7;j =1,:2

A fuzzy controller which applies the general rules of the DSMC procedure (Tablc 1) is used. Figure 8 shows the evolution of

Ss

the position error rate

1

and the position error and 1

aqs'

(dash line) are

represented in Figure 9. Figure 10 presents the phase . . ( I .1) plane. The lorce ~ trajectory 10 \&7S,aq5 error a Fl , 1

and the force error rate M"x are presented

in

Figurell and Figure 12 exhibits the phase trajectory 1

for the force Fx . We consider a plan-parallel motion of the object, without any rotations. Figure 13 presents the final trajectory. We can remark the error during the 1th cycle and the convergence to the nd

proposed trajectory during the 2 cycle. 6. CONCLUSIONS

The two-level hierarchical control procedure is constructed in this paper to solve the large-scale control problem of a chain robotic system formed by tentacle manipulators grasping a common object. The upper-level coordinator collects all the necessary information to solve the inter-chain coordination problem, the force distribution. Then , the problem is decoupled into j lower-level subsystems, for every arm. The local fuzzy controllers are assigned to solve the local control. A local decoupling controller is introduced and the conditions that assure the stability of the motion are determined. A DSMC procedure is used and the fuzzy rules are established. The simulation problem for two closed-chain tentacle robotic system has also been studied.

Ivanescu,M.,(l984) "Dynamic Control for a Tentacle Manipulator", Proc. of 1nl. Conf, Charlotte, USA, 1984. Ivanescu,M.,(l986), "A New Manipulator Arm: A Tentacle Model", Recent Trends in Robotics, Nov. 1986, pp. SI-57. Silverman,L.M.( 1969) "Inversion of Multivariable Linear Systems", IEEE Trans. Aut. Contr., Vol AC -14,1969. Cheng, Fan - Tien,(199S) "Control and Simulation for a Closed Chain Dual Redundant Manipulator System", Journal of Robotic Systems, pp. 119 - 133,199S. Zheng, Y.F., J.Y.S ., Luh, (1988) "Optimal Load Distribution for Two Industrial Robots Handling a Single Object", Proc. IEEE 1nl. Conf Rob. Autom ., pp 344 - 349,1988. Mason, M. T.,(l981) "Compliance and Force Control", IEEE Trans. Sys. Man Cyb., Nr. 6,1981, pp. 418 - 432. Cheng, F. T ., D. E., Orin,(l991) "Optimal Force Distribution in Multiple-Chain Robotic Systems", IEEE Trans. on Sys. Man and Cyb. ", Jan.1991, vol. 21, pp. 13 - 24. Cheng, F. T ., D. E ., Orin,(l99I) "Efficient of the Force Distribution Formulation Equations for Simple Closed - Chain Robotic Mechanisms", IEEE Trans on Sys. Man and Cyb., Jan.1991, vol. 21, pp. 25 -32. Wang, Li-Chun T.,(1996) "Time-Optimal Control of Multiple Cooperating manipulators", 1. of Rob.Sys.,'96,229-24l. Khatib,D.E.(1996) "Coordination and Descentralisation of Multiple Cooperation of Multiple Mobile Manipulators, Journal of Robotic Systems, 13 (11) , 75~ - 764. Schilling, R. 1.,(1991) "Fundamentals of Robotics",Prent. Hall, '90. Ross,T.J.(199S) "Fuzzy Logic with Engineering Applications", Mc. Grow Hill, Inc .

116

Ivanescu,M.. V., Stoian,( 1995) "A Variable Structure Controller for a Tentacle the 1995 IEEE Manipulator", Proc. of Int.Con! on Robotics and Aut. , Nagoya, Japan, May 21- 27,1995, vol. 3, pp. 3155 - 3160. Ivanescu,M., V., Stoian,(l996) "A Sequential Distributed Variable Structure Controller for a T~ntacle Arm", Proc. of the 1996 IEEE Intern.

Con! on Robotics and Aut., Minneapolis, April 1996, vol. 4, pp. 370 I - 3706. Ivanescu,M.,V.Stoian,(l996), '1\ Micro-Tentacle Manipulator ~ith ER-Fluids", Micro-Robot World Cup Soccer Tournament, MIROSOT, Proceedings, November 9-12,1996, Taejon, KOREA, vol. 1, pp. 74 - 79.

~'"

1.5.--_..,--_-r-_--,-_-,

2~r-T-'-~~~~~~rn I

I

I

I

I

I

I

I

I

-r-~-j--:--:--~-~-~--:-

U

~-~ ~--:--~-~-~-~--:-

U

I

I

I

I

I

I

I

I

I

I

I

I

I

f

I

I

I

1

1: t )1~ J J~ ~:~ ~ ~ ~ ~ ~ I~ J~ ~:~ lA

~

I

I

I

I

I

I

I

J I

I

I

I

I

I

I

I

I

I

I

I

I

I

I

- ~

0.1)

I

I

I

I

~

I

I

I

I

~

~

~

~

I

I

I

I

I

I

I

I

I

I

I

I

I

I

I

I

I

I

I

_.J _.J __ , __ L _ L _

~

~

\ -1

I

1

2

~

1

5

,

e

'!

I

9

J

\ I, . , ~L __ ~ ___ . __ L _____ : . ___ _ ,

\;,

_ .J _ -, __

-1.5 '=-0--:2~.5:---:!5:----=7:'-::,5"---:-10~

o~~~~~~~~--~~~~

o

:

-0.5 ---f---!--- ---1-------i -----

-:- -

I

I

-

~

I

' o I___ "=_,:....l..;~-~----4-___l

-r-1-~--~-r-r-l-1-~--

02

_____ J ______ _______ ! ____ _

I

~ ~ - ~ - -~ - ~ - ~ - ~ - ~ -

-

0.5

jr --:- - - - - --:- -

I

0.1

~--~-~-~-.-~-~-

I

-----~------1-------·-----

10

F,,.1T,,,9_

~&.

1.5 .--_-;--_-.--_-;-_-, I

05

I

I

~I

, I

(I

r-

-

-0

I

I

I

I

t

I

I

"\ I

I I

I

-~r

I

I

I

I

I

I

I

I

I

I

I

I

I I

I I

I I

I I

\.

)

:

:

I

I

!- -:- -:

,\:

I

I

, -:-~-,

-I -

-1

-:-

,

-1.5

-I;:J1-~~-'---1.--1.---'_'--~

-U-fl.4-U 0 U O.4CIii U F~10.

1 12

-2~--~--__--~~~

o

, --~---!--~---!--~I I I I I

__

I ~---L--~-_-L--~-_

,

"

--~---.--4---.--4-I

I

I

I

I

I

I

I

I

I

82

--'---T--~---T--~--

o

-~---~------~--~-­

I

I

I

I

,

-02 -0.4

-

-

~-

-

-

-

-

... -

I

-

-1- -

-

... _ _ _ 1_ -

~

-:- -

-

~

I"

I

I

- - .JI - - -

~I

I

I

- - -,- - - r - -

-0Ai -0.8

,

-

I

-,~

-

-

I

-:-1I

. . - .. - - -,-

I....

-

I

- - -'- - 'LI ' I I

----J' --

I

...... .....

I

_ 1L---------------------~~

o

0.2

fJ.~

O.t>

ot

,

-,- -,- -,- -,-

-:- ,:- -:- -,I

_____ l,______ ~-------l----. ,

1

I

-~-r-r-r-'--I--'--I-I I I I I I I I , I I I I I I

1

1 .2

FJgUJe 12. 117

2.S 5 Figure 11.

7,5

10