Proceedings of the 2000 IEEE International Conference on Robotics a Automation

San Francisco, CA April 2000

Preview and Stochastic Controllers for Motion Control of Robotics Manipulator with Control Input Constraints

Mohamed M. M. Negm, Member, IEEE Faculty of Engineering Ain-Shams University

Cairo, Egypt

Abstract -A preview controller and a stochastic controller are synthesized in this paper to control the motion of an n-degrees of freedom direct drive robotics manipulator. The behavior of the two controllers are indicated with deterministic and stochastic disturbances. The proposed preview controller is implemented off line and the optimal problem is solved at an arbitrary operating point of a prespecified optimal trajectory. While design of the proposed stochastic controller is based on the minimum variance control with model reference depends on the nominal values of the position and angular velocity of the manipulator’s joints. The least squares sense is used on line to estimate the parameters of the stochastic controller. In the proposed two controllers, the endeffector’s velocity of the manipulator is monitored in a servo loop, as well as the position and angular velocity of each joint of the manipulator are compelled to track their nominal values. These two controllers comprise n-input vector of the actuators torque, and 6-output vector of the linear and angular endeffector’s velocities. The actuators of the direct drive manipulator are taken as dc servo motors. Their dynamics and constraints are introduced in the proposed two controllers. Computer simulation results are made to demonstrate the applicability, robustness and the tracking performance of the preview and stochastic controllers. Comparison between their performance is also given.

I. INTRODUCTION

The general approach often adopted when designing conventional controller for multi-variable systems, is to find a suitable nominal model for the plant, which is often a very difficult task, and then to design a controller based upon this model. If, however, large unexpected structural changes subsequently occur in the system, sever limitations in practical performance may occur since conventional control schemes usually do not have the ability to control systems which are subject to unplanned extreme changes. In particular, the control of these systems which contain unpredictable events, changing environments, and difficult to model internal dynamics, is a challenging topic. One of such systems is the n-degrees of freedom robotics manipulator. The difficulties that arise in the control of this complex dynamic system can be classified under three aspects: (i) system complexity: there exists a large number of degrees of freedom which have to be used to describe the system’s dynamic of each joint which are compelled to track their nominal property; (ii) non-linearity: nonlinear dynamics existing in a complex system is either modeled or unmodeled, however, there is a great deal of difficulties associated with designing non-linear control systems using traditional concepts; (iii) uncertainty: only partial or even no a priori information concerning internal

structures can be used to mathematically model the system. Therefore, the system models are uncertain in terms of the unknown parameters and dynamic structure. A control system which is capable of dealing with the above three categories of complexities is qualified as intelligent control system, such as adaptive tracking control [ 11-[2], intelligent control [3], self tuning type neural network [4]-[5], and fuzzy control [6 ] .

The drawback of these control systems are their complexity and implementation on-line. Therefore, this paper proposes simple and robust two controllers to control one of such complex systems. This is the optimal preview controller and the adaptive controller. The objective of this paper is to control the motion of an n-degrees of freedom robotics manipulator using these two controllers with control input constraints. In the proposed two controllers, the tracking formulation is implemented such that the manipulator’s endeffector velocity is monitored in a servo loop with integral action to compensate inaccuracies in the structure and to attain zero steady state tracking error. In addition, the position and velocity of each joint of the manipulator are compelled to track their nominal values through state feedback loop to compensate the system complexity and nonlinearity. The proposed preview controller is implemented off line and the optimal problem is solved at an arbitrary operating point of a prespecified optimal trajectory. Design of the proposed stochastic controller is based on minimum variance control with model reference depends on the nominal values of the position and angular velocity of the manipulator’s joints. These values are calculated off-line using trajectory planning and optimal inverse kinematics. The parameters of the adaptive controller are estimated on line using the least squares algorithm. The preview feedforward steps are introduced in the preview controller to improve the transient response instead of the input constraints. The actuators of the direct drive manipulator are taken as dc servo motors. Their dynamics and constraints are considered in the proposed two controllers. In addition the dynamic nonlinearties, geometric nonlinearties and physical nonlinearties, as well as dynamic coupling present in the manipulator are taken into account. Intensive simulation work is carried-out using MATLAB to demonstrate the applicability of the proposed controllers on a two-degrees of freedom robotics manipulator.

11. PREVIEW CONTROLLER

The nonlinear dynamics of an n-degrees of freedom robotic manipulator is written in (I).

0-7803-5886-4/00/$1 O.OO@ 2000 IEEE 3020

. . M(6) 6 + N(6,B) 6 + g(6) = T(t) (1)

where 6, 6, and 0 are the position, velocity and acceleration n-vectors of the joints coordinate, respectively. The nxn inertia matrix M, is symmetric and itdepends nonlinearly on the manipulator configuration expressed in terms of the generalized joint coordinates 6. The nxn matrix N, denotes the total coriolis and centrifugal forces which depend nonlinearly on the joints velocity and joints position, while the n-vector g, gives the contribution from the gravitational forces which depends nonlinearly on 6. The n-vector T(t), represents controlling input torques which provided by the robot actuators that might be located at each joint.

The kinematics equation of n-degrees of freedom robotic manipulator may be written as,

v(t) = J(t) 0(t) (2)

where J(t) is 6x11 nonsingular Jacobian matrix. The trajectory of endeffector frame may be represented by a 6- order vector v(t).

Linearizing (1) about an arbitrary operating point of a prespecified optimal trajectory, utilizing Taylor's expansion, gives (3), [SI.

0(t) =A'(@ 0(t) + B'(0) .t(t) + D'(0) (3)

The augmented equation (4), is obtained from (3).

x(t) = Ao(0) x(t) + Bo(6) T(t) + Do(6) (4)

where

E'(6)=D'(0)+[A'(B)-In /TIC)" (t)+e0 (t-l)/T

The superscript "O", denotes the nominal value of the joints coordinate, which is calculated from the optimal trajectory planning. The parameters of the robotics manipulator are given by the nxn matrices A'(0) and B'(6) and the n-vector D'(6), while I, denotes a unit matrix of order n.

The discrete state space model and the 6-vector output signal y(k), of the manipulator can be derived from (4) and (2), respectively, as given below.

x(k+l) = A(k) x(k) + B(k) u(k) + D(k) (5)

(6) U&) = [UI (k) .. un (k)l' = [TI ( k ) k l .. Tn (k)knI'

y(k) = v(k) = C(k) x(k) +Co(k) (7)

where ui(k); i =1,2,..n is the armature current of the dc servo motor connected with joint i of the manipulator, and k,i ; i =1,2,..n is the corresponding torque coefficient. Note that the input vector is constrained, such that

where Uimm is the maximum permissible armature current of the dc motor at joint i . Also,

C(k) = [0 J(k)]; 6x2n matrix

Coo<) = C(k) [e" &)I 0" (k) '1' ; 6-vector

The superscript "'" denotes the transposition. 1

To produce a form of integral action for a multivariable desired output yd(k), with its response y(k), consider the integral of the output error:

Substituting (7) into (9) gives:

Z(k+I) = Z(k)-C(k) x(k)-Co(k)+yd(k) (10)

The augmented matrices ( I I)-( 12), are obtained from (S),

The preview controller with control input constraints U(k), (13)' can be implemented by minimizing the following criterion Jn, subject to the constraints given by (1 1) and (12), [SI.

m

J, = C [ X(k)' Q X(k) + U(k)' R U(k) ] k==O

M U(k) = G X(k) + C W(i) E(k+j-I)

j=1 where

3021

Feedback gain: G = - y O ' K Q , Feedforward gains: W( 1) = - y 0' K

WO) = W(i-1) KI; j =3,4,..,M W(2) = - y 0' 0' h

Q =

KI = K-I Q,* h, and the matrices K, h and y are the steady state solution of the following Riccati equation.

-

K(i) = Q + Q,'A(i+l) Q,

h(i+l) = K(i+l)[IZn+6- 0 y(i+l) 0' K(i+l)]

y(i+l) = [R + 0' K(i+l) 0 ] - I

The weighting matrices Q and R are given by:

['I. 0 1 01

; R = ["."I 0 rn

The real time optimal preview controller can be extended from (13):

k U(k) = GI A8(k) + G2 A8(k) - G3 AV(k) + Gs Z AvQ)

j=O M

j =1 + X {wl(j) El(k+j-l) + w2Q) E2(k+j-1)} (14)

where

G = [GI G2 G3]; Av(k) = vd(k) - ~ ( k )

A9(k) = 8 (k) - e"(k); Ae(k) = 8 (k) - Bo(k)

The feedforward terms w,(i).El(k+j-l) and w2(j).E2(k+j-1) are included in W(j).E(k+j-1) in (13), and they include the desired value of the endeffector's velocity and the nominal values of the joints velocity, respectively.

The optimal preview control system structure is implemented from (14) as illustrated in Fig.2a.

111. STOCHASTIC CONTROLLOER

First, consider the discrete state equation of (3) is corrupted with the process noise v,(k), and that of (2) is corrupted with the observation noise v2(k). Secondly, the stochastic autoregressive moving average model with auxiliary input, ARMAX, of the discrete state equation for d-step ahead is given by [7],

A(q-* ) y(k) = B(q-') u(k-d) + M(q-') D(k-d)

The delay of the stochastic control system is d , and the 2-vector g is added to compensate for any change in the robot parameters or unmodelled dynamics . The symbol k means the sampling time kT, and q-" denotes the backward shift operator of order n. Assume the matrix - C(d' ) has zeros on or inside the unit circle, and a 2x1 noise sequence o(k), which depends on vl(k) and v2(k), satisfies the following assumptions.

The optimal d-step ahead predictor of y(k) for the system of (1 5 ) satisfies the following relation.

where

The superscript" " " denotes the predicted value of the output y(k), while E{ ... Nk} is the expectation operation conditioned on the available measurements up to and including time k , where Yk denotes the sigma algebra generated by {y(k), y(k-1) ,...., y(O)}.

Define, The matrices G(q-') and F(q-') are specified as follows.

where G(q-')=g,+g,q-' +. . .+g"q-"

-d+l - F(q-') = 12 f~ q-' + ... -t fld-1 q

Expanding (1 7) to get _F(q-' ) and _G(q-' ), and then (1 8) is expanded to get C(q-' ) and F(q-' ).

F(q-' 1 C((9-I 1 = C(q-' 1 !3q-' 1 (18)

3022

where 111.2 Minimum Variance Controller

Finally, the polynomial matrix G(q-') can be calculated from the following equality.

wq-' 1 = qd [c(q-L) - F(Q' 1 Ah-' 11 (19)

From (16) the optimal 1-step ahead prediction, d=l, can be written as

yo(k+l/k) = 0,' +'(k) (20)

e,' = [a1 9 a2 > a3 > Po $1 $2 7 c1 $2 9 Yo 3 Y1 , Y2 9 hl; (2x20)

where

+'(k)' = [ y(k)' ,y(k-l)' ~ (k -2 ) ' ,u(k)' ,U(k-l)t ,~(k-2)',

-y"(k/k-l)', -y0(k-l/k-2)', D(k), D(k-l),

D(k-2), 13; (1x20)

111.1 Least Squares Sense

The estimated value of the predicted output can be calculated from (20) as written in (21).

- Y(k+l) = WO' $6) (21)

where

Note that, Q'(k) given in (20) is +(k) in (21) with yo(j/j-d) , - d=l, replaced by the corresponding a prioryestimates yc), for all j20. Then, the least squares variant of the

pseudo linear regression algorithm is firnished by the following recursive equations, where (k) refers to the estimated value of y(k), ... , etc., [7].

P(k-2) Q(k-1)[ y(k)' - +(k-1)' B(k-1)] &k) = g(k-1) +

YI / "12 + Mk-1)' P(k-2) $@-I)

(22)

P(k-l)= [P(k-2) - Y Y I

P(k-2) +(k-1) Q(k-1)' P(k-2)

Y I 1 ~ 2 + +(k-l)' P(k-2) 4G-l) where

The one step ahead stochastic control law u(k), given in (23), can be derived from minimizing the following criterion J,, while satisfying (16):

= [Po' Q Bo + E I-' Po' Q [ Yd - ;I ~ ( k ) - - azy(k-1) - a 3 y(k-2) + C I yo(k/k-1) -

+ c2y0(k-l/k-2)- PI u(k-I) - & u(k-2) -

(23) - yo D(k) - 71 D(k-I) - 7 2 D(k-2) - T; J

where 11 . )I denotes the norm value, a l s o 2 > 0 and _R 1 0 are the weighting matrices given by:

The stochastic adaptive control system structure is implemented from (23) as demonstrated in Fig.2b.

IV. TWO LINK PLANAR MANIPULATOR

The feasibility of the proposed optimal preview controller and adaptive controller with control input constraints are demonstrated by applying them on a 2-degrees of freedom planar manipulator shown in Fig.1. Pursuing the previous procedures indicated in section-I1 and section- 111, to derive the nonlinear dynamics of the 2-degrees of freedom planar manipulator, taking n=2, and d=l.

Accordingly, @(k), O(k) and E(k) in (1 l), are 6x6 matrix, 6x2 matrix and 6-vector ,respectively, while the augmented state vector is:

The input vector is:

The output vector is:

The endeffector velocity in x and y coordinates are v, and vy , respectively, while ul and u2 are the armature currents of the two actuators located at the two joints. The nonlinear Jacobian matrix J(k) is 2x2 matrix.

The proposed preview controller and the stochastic controller with control input constraints are calculated from

3023

(14) and (23), respectively, with n=2 and d=l.

V. SIMULATION RESULTS

The proposed two control systems comprise optimal preview controller and adaptive controller and a 2-link planar manipulator. This manipulator, shown in Fig. I , has the following parameters.

l1 = I2 = 30 cm; IC, = Ic2 = 15 cm; dl = d2 = 2.5 cm; ml = 3.7 kg, m2 = 2.2 kg; Ulmax= 22.5 A, U Z , , , ~ = 3.7 A; T ~ , , , ~ = 16.47 kg-m2, T~~~~ = 3.04 kg-m2.

Figs.3-8, indicate the digital computer simulation results of the proposed two control systems with control input constraints, (14) and (23). Figs. 3-6, illustrate effect of the optimal preview controller, while Figs. 7-8, depict effect of the adaptive controller on the control system. The sampling time in these figures is taken T=10 msec. For the optimal preview controller, the elements of the weighting matrices Q and R , given in (13), are ql= q2=E+6, q3=0.0001 and rl=r2=0.001. Figs. 3-4, are taken without preview steps (M=O), while Figs. 5-6, are obtained with one step ahead preview controller ( M=l). For the adaptive controller, the elements of the weighting matrices 4 and _R , given in (23), are gl= q2=1000, and rl=~2=0.00001. The forgetting factors in (22) are yl = y2 = 1.0 and Po = 2000, while the variance of the white noise in (1 5) is cr = 1.365. In these

figures, the nominal values OIo(k), O?(k); 8: (k), e; (k) and the desired values v,“ (k), v,” (k), given in (24), are obtained off line from the solution of the optimal inverse kinematics of the manipulator at a pre-specified planning path. The state response and the output response of the planar manipulator are calculated from (5) and (7), respectively.

The nominal signals and desired signals which are given in these figures are indicated as dotted lines, while their responses are demonstrated on the same graphs by solid lines. The endeffector is selected to move on a path that making a right angle in the x-y plane as indicated in Figs.3 and 5, while a part of a circle as shown in Fig.7. The velocity of the endeffector is monitored in a servo loop to track a trapezoidal speed time curve for both x and y coordinates as indicated in Figs. 4, 6 and 8. Figs. 3, 5 and 7, illustrate the desired position of the endeffector and its response in the x-y plane which give goodcoincidental responses. Figs.4, and 6, depict the performance of the control system based on optimal preview controller, while Fig.8, is based on the adaptive controller. The horizontal line in these figures represents the time in msec, while the vertical lines give from up to down, the endeffector’s velocity in x-direction and its desired value v,, v,d , respectively; the endeffector’s velocity in y-direction and its desired value vy , v,” , respectively; the position of joint- 1 and its nominal value el , elo , respectively; the position of joint-2 and its nominal value O2 , OZo , respectively; the angular velocity of joint-1 and its nominal value

el , OIo , respectively; the angular velocity of joint-2 . .

. . and its nominal value 02, OZo ,respectively. Finally, the torque (or armature current) of the two dc servo motors, TI , T~ (or uI , u2) are also indicated in these figures. The torque of joint-I is represented by the dotted line, while the solid line indicates the torque of joint-2. As illustrated from these results good coincidental responses are achieved with only one step preview controller, M=l, also the performance of the optimal preview controller on the control system is better than that of the performance of the adaptive controller withone step ahead (d=l).

VI. CONCLUSION

Two controlled systems based on a preview controller and a stochastic controller are synthesized in this paper to control the motion of an n-degrees of freedom direct drive robotics manipulator. In the proposed two systems, the manipulator’s endeffector velocity is monitored in a servo loop with integral action to compensate inaccuracies in the structure and to attain zero steady state tracking error. In addition, the position and velocity of each joint are compelled to track their nominal values through state feedback loop to compensate the system complexity and non-linearity. The synthesis method depends on the solution of the optimal inverse kinematics, and the tracking technique. The dynamics and constraints of the dc servo motors are introduced in the proposed controlled systems. The dynamic non-linearties, geometric non-linearties and physical non-linearties, as well as dynamic coupling present in the manipulator are also taken into account. Good coincidental responses are demonstrated by computer simulation results.

REFERENCES

[1] D.E. Miller and E.J. Davison, “An adaptive tracking problem with a control input constraint,” Automatica 29 (4), 1993, pp.877-887.

[2] M. Chang and E.J. Davison, “ Adaptive proportional - integral controllers with control input constraints, ” The Ninth Yale Workshop on Adaptive and Learning Systems,l996, pp.161-166.

I31 L. Jin, M.M. Gupta and P.N. Nikiforuk, ‘I Intelligent control for nonlinear systems using dynamic neural networks with robotic applications, ” Intelligent Automation and Soft Computing, vol. I , no.2, 1995, pp.123-144.

[4] J.Yuh, “ A self-tuning type neural net controller for robotic manipulators, ” Intelligent Automation & Soft Computing, vol. 1, no.2, 1995, pp.221-230.

[5 ] P.C.Y.Chen, J.K.Mills and K.C. Smith, “An approach to certainty compensation using a neural network for multi-manipulator system control,” Proceed. of 1ROS.94, v01.2, 1994, pp.1048-1055.

[6] S.A. Bentalba, El-Haijaji and A. Rachid, “ Controller design and stability analysis of fuzzy systems application to mobile robot ,”IFAC-IFIP- IMACS Conference on Control of Industrial Systems, 1997, pp.716-721.

[7] G.C. Goodwin and K.Sang Sin, Adaptive Filtering Prediction and Control, Prentice-Hall, Inc., NJ: 1984

[8] A. Kheireldin and M. M. Negm, “ Preview feedforward optimal tracking robotic manipulator motion control,” Proceedings of IEEE Conference on Sensorial Integration for Industrial robots, 1989,

[9] M.M.Negm and A.Kheireldin, “Properties of adaptive optimal and preview controllers based on MVC and LQG optimal controller- Application to robotic manipulator , ” Proceedings of the IEE lntemational Conference on Control, U.K., vol.1, 1991, pp.323-329.

pp.73-78.

3024

Fig. 1. The two-link planar manipulator.

w

Figfa The optimal preview control system.

L MODEL

3

Fig.2b. The stochastic adaptive control system.

3025

Fig.3. The cndcfeector’s position in x-y plane (M-0).

, . l o 1 5 - 1 0 a i J 3s i J .3

Fig.5. The endefccctor’s position in x-y plane @+I).

‘ T I , ‘ I

10 1

- . 72

.Y) -1;- , TI I I 0 ’ 5 ~ 0 1 5 W 3 1 1 3 S 4 Q

Tm( r(0 mrc)

FIg 6 Simulation results of opbmal preview control system @+I)

d d la) V, ,v, (cm/sec) ; (b) vY ,vr (cmlsec) ; . . . .

(e) e, ,elo (rad./sec) ; (0 e2 ,820 (radhec) ; (g) T~ , T~ (~.m.)

3026

............. ............................. ............. .............. .............. .............. 10 - i i L ..-.......... i i i i......-..

% -30 -20 -10 0 10 20 w, 4b

0 -

I

(4

Fig.7. The endefeector’s position in x-y plane (di.1).

0 -

500 [ ......................... ....................................

1 (4 0i-

. . 0 10 20 30 40 50 60 m -500

(e)

....................................................................................... .... 200 ....

0 - 1

0-

1 (b) I 0 10 20 30 40 50 60 m -200

(€9