R ob u st T im e-o p tim al P a th Tracking C ontrol of R obots: T heory and E xp erim en ts. A idan Jam es Cahill. December 1995

Size: px

Start display at page:

Download "R ob u st T im e-o p tim al P a th Tracking C ontrol of R obots: T heory and E xp erim en ts. A idan Jam es Cahill. December 1995"

Sheila Small
5 years ago
Views:

1 R ob u st T im e-o p tim al P a th Tracking C ontrol of R obots: T heory and E xp erim en ts Im plem entation D etails A idan Jam es Cahill B.Sc. (Hons) M athem atics, N orthum bria (UK) December 1995 An addendum to a thesis submitted for the degree of Doctor of Philosophy of the Australian National University D epartm ent of Systems Engineering Research School of Information Sciences and Engineering The Australian National University

2 A b stract This document supplements the information contained in the thesis R obust Time- O ptim al P ath Tracking Control of Robots: Theory and Experim ents. Specifically, it deals w ith im plem entation of the theory described in C hapters 3, 4 and 5 of the m ain tex t which describes a m ethod for the planning of tim e-optim al trajectories th a t are robust to plant uncertainties and which ensures the tracking of paths to a specified tolerance. l

3 C ontents A b stra ct 1 In tro d u c tio n... 2 Identification of the Robot M o d e l... 3 Real-Time Control L a w s... 4 Im plem enting the Acceleration D isturbance T h e o ry Calculation of the Acceleration D isturbance S ig n a l Selecting the Controller Gains to Control the E r r o r s Calculating the Com pensation T orques... 5 Offline Trajectory P l a n n in g... 6 Closed-Loop Trajectory G e n e ratio n C alculation of the W orst-case Controllable Region B oundary Online T rajectory G eneration 13

4 1 In tro d u ctio n 1 1 In trod u ction This docum ent has been w ritten as a supplem ent to the thesis R obust Tim e-o ptim al P ath Tracking Control of Robots: Theory and Experim ents, subm itted for the degree of D octor of Philosophy of the A ustralian N ational University. The m ain purpose of the document is to provide additional inform ation regarding the im plem entation of the theory described in C hapters 3, 4 and 5 in the m ain text. The theory presented therein describes a m ethod for the planning of tim e-optim al trajectories th at are robust to plant uncertainties and which ensures the tracking of paths to a specified tolerance. The inform ation presented herein focusses m ainly on the m athem atical and com putational aspects of the im plem entation. T his docum ent does not consider either th e work involving the dynam ic program ming solution to the tim e-optim al trajectory planning problem (C hapter 2) or the work involving the use of singular configurations for high-speed pick and place operations (C hapter 6). The formulation and com putation of the dynamic program m ing solution are described adequately in the main text, and the details of the im plem entation of the solution are covered by the work described herein. Similarly, the algorithm s for the high-speed pick and place operations are based on a simple adaptation of the shooting m ethods of Bobrow et al. [1] discussed herein.

5 2 Id en tifica tio n o f th e R ob ot M od el 2 2 Id en tification o f th e R ob ot M od el In our experim ents, we utilise 2 degrees of freedom of the 4 degree of freedom SCARA m anipulator th a t we have in our laboratory. For the identification of the model param eters of these 2 degrees of freedom, a standard least-squares identification technique is employed. The technique is based on noting th at whilst the dynamic equations M(q(*))q(t) + n(q(t), q(t)) = T (1) are highly nonlinear in the joint positions and velocities, they are linear in the param eters. T hus, equations (1) m ay be re-expressed as w(q( ),q(t),q(f))$ = r{t), ( 2) where <I> = ( / n, / 12,, ^2j>, n)t is the vector containing the model param eters. An experim ent is run in which known and discrete torque values T ^, k = 1 to kf, are applied as input and the resulting joint positions q ^, k = 1 to kf, are measured. Here, kf = ^ + 1, where ty is the tim e over which the experim ent is to be run and A t is the sam ple period. Estim ates of joint velocities q ^ and accelerations q(*:} are taken using the Euler- step approxim ations n (fc) _ n (fc l ) a (*0 _ /,( -!) q(*> = q - q and q<*> = - S , k = 1 to kf, (3) M At At 1 where q( ) = q(0) and q( ) = 0, and then th e values of q ^ :^, q ^, q ^ and T ^ are inserted into the discrete equivalent of equations (2) w(qw,qw,qw)$ = T<*>. (4) Next, both sides of equations (4) are low-pass filtered to reduce the effects of noise introduced by taking the num erical derivatives (3). This yields (w(qw,qw,qw))i$ = ( r W ) i, (5)

6 2 Identification of the R obot M odel 3 where ( )/ represents the low-pass filter with cut-off frequency lrad /s, and where the constant model param eter vector <I> is unaffected by the filtering. Finally, equations (5) are inverted as $ = (w (q W,q < ),q<*>)), (6) to provide a least-squares fit to the model param eters <I>. Here, ^ w ( q ^, q(fc), is the pseudo-inverse of ^ w (q (^, q ^ ), q(d )J^ In our case, we differ from this procedure only in th at we identify the model param eters in two stages. Firstly, we identify the friction param eters by using the slow joint position trajectory sin(o.lf) + cos(0.25t) \ q r = t = 0 : A t : 12s (7) sin(0.25t) + sin(0.4f) I as reference inp u t to the PD control law r<*> = kf, (q( >- q<*>) + k q(t>, 1 to (8) Secondly, we identify the remaining param eters by using the fast joint position tra jectory q r = [sin(1.5 ) cos(2t)] 3 [sin(t) cos(2.5t)] + 4sin(0.3t) 0 : A t : 12s (9) as reference input to (8), replacing the friction param eters resulting from the subsequent least-squares fit replaced with those values identified from the first slow trajectory experim ent. The rationale for choosing these particular experim ents is th a t each exercises a large part of the 2-dimensional planar workspace, as well as together exercising both slow and fast dynam ics. The goodness of fit of the identified model param eters <1> can be estim ated graphically by comparing the input torque trajectories for an experim ent with the torque predicted by the discrete equivalent of equation (1) M(qW)qW + n(qw,q fc)) = T «( 10)

7 2 Id en tifica tio n o f th e R ob ot M od el 4 using the param eters 4?, the joint positions m easured during the experim ent and the estim ated joint velocities q ^ and accelerations q ^ :^. An example of this is presented in Figure 1 where the input to a real experim ent (the initial disturbance identification experim ent for random p a th 1 in C h ap ter 4) is com pared to the torque predicted by equation (10). Clearly, for our robot the identification procedure works well. g 200 V... /V, 0 V i cc actual CL predicted \ j path position s V 0 CO -- actual ^ -200 predicted, path position s Figure 1: A ctual and E stim ated A ctu ato r Torques Note that: The original software for the real-tim e controller implem enting the PD control law (8) was developed by M r Paul Logothetis. Because of the order of conversion of joint position m easurem ents from encoder counts to radians in the controller software, large m ultiplying factors are present in th e reference joint trajectories (7) and (9). In the PD control law (8), we use kp = (0.0004,0.0004)T and kv = (0.04,0.04)T. These gain settings are small also because of the order of conversion of joint position m easurem ents from encoder counts to radians. In our experim ents, we use a sample rate of 300Hz and thus A t = ^ s.

8 2 Id en tificatio n o f th e R ob o t M od el 5 The original software for the identification procedure was developed by Mr Paul Logothetis and im plem ented in M ATLAB. Coefficients for the discrete filter which corresponds to the continuous first order low-pass filter ^ are obtained using the function bilinear. Here l is the cut-off frequency - we use l = 60rad/s. The discrete filter is im plem ented using the function filter. T he least-squares fit to the m odel param eters equation (6), is im plem ented using the pseudo-inverse function pinv. Although the identification of an accurate dynamic model has contributed to the quality of the experim ental results, it is not central to the theory proposed in the m ain text. The proposed theory assumes th a t a model is given. T h e developm ent of backlash, described in C h ap ter 4 of the m ain te x t, necessitate d the re-identification of the model param eters after the backlash had been fixed. The inertial term s of the new model param eters (Table 4.12 in the m ain text) differed noticeably from those identified for the original model (Table 3.1 in th e m ain tex t). These differences are not a result of deficiencies in the identification algorithm. R ather, they are attributable to the addition of extra parts to the robot s structure during its refurbishm ent.

9 3 R eal-t im e C on trol Laws 6 3 R eal-t im e C ontrol Laws In addition to the PD control law (8), which is used for tracking of the reference joint position trajectories (7) and (9) during model param eter identification, there are two other control laws used: Standard PD control T «= k (q<*> - q<*>) + k (qw - q<*>), (11) which is used for comparative purposes, i.e. What is the best tracking using standard PD control? C om puted-torque control T «= M(q<*>) {q<*> +k (q W -qw ) + kp (q<*>-q<*>) } + n ( q «, q<fc>), (12) which is the model-based control law used by the theory presented in the main text. Only the joint positions q ^ are directly available from the SCARA m anipulator in our laboratory. The m anipulator does not possess tachom eters (or accelerometers) and so the joint velocity estim ates required by both control laws m ust be obtained by num erical differentiation of the joint positions q(a:). We use the E uler-step approxim ation m q ^ ^ q* *= , k = 1 to k f > (13) where A t is the sample period, and q^0^ = q(0) and q^()* = 0. Because our m anipulator possesses relatively high-resolution joint encoders (360,000 counts per revolution), the joint velocity estim ates q ^ have low levels of noise and do not cause instability. Thus, we have found it unnecessary to filter the estim ates. It is possible th at for m anipulators possessing low-resolution encoders and when using low sam pling frequencies, the velocity estim ates would contain sufficiently high

10 3 R ea l-t im e C on trol Laws 7 levels of noise to cause instability. Such a result would necessitate a filter to be introduced when calculating q ^. However, this would invalidate the theory on which the m ethods in the m ain text are founded and a new or am ended theory would need to be developed. For our system, it is possible to vary the sampling frequency /., between 100Hz and 500Hz. Frequencies below 100Hz produce instability while those above 500Hz did not allow sufficient tim e for controller com putations. We selected f s = 300Hz as a suitable com prom ise, leaving sufficient m argin for anticipated additional processing. Note that: The real-tim e controller software implementing the control law (11) was developed by Mr Paul Logothetis and subsequently am ended by the author to implement the control law (12). The order of conversion of joint position m easurem ents from encoder counts to radians was am ended to remove the large m ultiplying factors present in the original software. This produced controller gains of order k p ~ 0(100).

11 4 Im p lem en tin g th e A ccelera tio n D istu rb a n ce T h eory 8 4 Im p lem en tin g th e A cceleration D istu rb an ce T h eory In this section, we describe the im plem entation of the acceleration disturbance theory presented in C hapter 3 of the m ain text and applied in the iterative open-loop and closed-loop algorithm s presented in C hapters 4 and 5. The three areas we address are how to calculate the acceleration disturbance signal q d( ), then, using this signal, how to select the controller gains kp to produce tracking to a specified tolerance, and finally, using the tracking errors predicted by these controller gains, how to calculate the com pensation torques Tp(j(s). All of the work was implemented in MATLAB by the author, although the MATLAB code executes the trajectory planning c program for the open-loop m ethod and the worst-case controllable region boundary c program for the closed-loop m ethod. 4.1 C alculation of th e A cceleration D istu rb an ce Signal The acceleration disturbance signal q (i(t) is defined as q d(t) = M Hq(t)) { r (t) - M (q(t))q(t) - h (q (t),q (t)) j. (14) We obtain the input torques and output joint positions q ^, k = 1 to kf, from the previous experim ent (kf = ^ + 1, where tf is the tim e over which the experim ent is run and At is the sample period). Estim ates of joint velocities q ^ and accelerations q ^ are taken using the Euler- step approxim ations n (fc) (fc-1) A.(k) _ i(fc-l) = and äw = ^ k = 1 to kf, (15) At At where q ^ = q(0) and q^()^= 0, and then the values of q*a:', q ^, q ^ and T ^ are put into the discrete equivalent of equations (14) qw = M _1(q(fc)) {rw - M (q W )q(fc) - n (q (fc),q (fc))}, (16) to obtain the discrete approxim ation q ^ to the acceleration disturbance signal q d(t).

12 4 I m p le m e n tin g th e A c c e le ra tio n D is tu rb a n c e T h e o ry 9 N o te : Although the numerical differentiation processes (15) introduce noise into the derivatives and, and hence into the acceleration disturbance signal q j ^, we do not filter q ^ and q^ ' before putting them into equations (16) because this would invalidate the theory. 4.2 Selecting the Controller Gains to Control the E rrors Assuming th at the actuators do not saturate, the error behaviour of the dynamic system M(q(t))q(t) + n(q(t),q(t)) = r(t) under com puted-torque control is e(t) + k ve(t) + kpe(t) = q d(t). (17) Knowledge of the acceleration disturbance signal q (j(t) allows equations (17) to be used to select the controller gain settings kp and k which result in errors e(t) satisfying the accuracy criteria max* e(t) = e for some pre-specified tolerances e. For specified values of k?> (remembering th a t the theory assumes critical dam ping, i.e. k w= 2^/kp), and assuming th at the disturbance signal q y(l is known, the estim ates of the errors are calculated as follows: T he second order dynam ics (17) re-expressed as the equivalent first order statespace system ( x\ X2 x3 \ i 4 J ( \ kpi \ 0 kp2 0 2 yj kp2 ) ( X\ ^ X2 X3 + / 0 0 \ \x4 J \ 0! J (18) where x\ = ei, X2 = e.2, ^3 = ei and x4 = e.2. Equations (18) are discretised by replacing the continuous derivatives w ith the E uler-step approxim ations *(*+!) xi x{k+1) xi At k = 1 to kf > i 1 to 4,

13 4 Im p lem en tin g th e A ccelera tio n D istu rb a n ce T h eory 10 to give (*+l) 1 = x f 0 + A tx f') (19) (k+1) 2 = x ^ + A t x f (20) \k+l) 3 = A t (kj,[x^1+ 2 \ J j + A t («S») (21),(*+!) 4 = 4 * - A t [kv2^2 *+ 2 ^ > ) + A t («1?) (22) where A t is the sample period. The errors are obtained by iterating equations (19) to (22) using the initial values = ci(0), = 6 2(0 ), 3 ^ = e i( 0 ) and = <=2(0 ). The controller gains kp which provide maxjt e ^ = e are found using a bisection m ethod starting with very small and very large values of kp. In our im plem entation, we search for the kp which provides max*; e ^ G [e 10%, e + 10%]. 4.3 C alculating th e C om pensation Torques T he controller gains k p which resulted in errors satisfying m axfcje^] = e when iterating (19) to (22), and the resulting errors and their derivatives are used to calculate the com pensation torques as a function of tim e via r<3 = M (qw ) {20^e<*> + k e ;»}. We require th at the com pensation torques be defined as a function of the path position s. Because the path position s is known at each sample instant k, the com pensation torques Tp(J are also defined as functions of the path position s^'k The values of T_pd and Tp(i in the interval [s ^, s (fc+ 50)] are taken to be the envelope defined as the maximum absolute value of Tp(i over the interval [5(fc~50)}s(fc+50)].

14 5 O ffline T rajectory P lan n in g 11 5 Offline T rajectory P lan n in g The work described in Chapter 4 of the m ain text requires a m ethod for the calculation of reference joint trajectories. Because of its sim plicity of u nderstanding and im plem entation, we have used the shooting m ethod of Bobrow et al. [1]. However, it should be noted th a t we could have used any one of a num ber of other m ethods available (see the Bibliography in the m ain text) The inform ation relevant to our im plem entation of the shooting m ethod in [1] is as follows: The second order dynamics s = u are integrated forwards in tim e by first reexpressing them as the equivalent first order state-space system xi X2 where x\ = s and X2 = s, and then discretising, replacing the continuous derivatives w ith the Euler derivatives (fc+i) ffc),(t+1) = x 1 and = x (fc+1) ( l) Xc, k = 1 to kf, to give r (fc+l) X1 (fc+i) x2 II " S (k) = x\ *<*> + A t (*<*>) where A t is the sample period. The sample period A t must be consistent with th a t used in real-tim e. In the realtim e system, we use a sampling frequency of f s = 300Hz, i.e. a sample period of A t 1 s 30Qb. The shooting m ethod was implem ented in c by the author.

15 6 C lo se d -L o o p T r a je c to r y G e n e r a tio n 12 6 C losed-l oop T rajectory G eneration In this section, we describe the im plem entation of the closed-loop algorithm presented in C hapter 5. The two areas we address are how to calculate the boundary to the worstcase controllable region Cwc and how to apply the closed-loop algorithm in real-tim e. Both of these were implem ented in c by the author. 6.1 C alculation of th e W orst-c ase C ontrollable R egion B ou n d ary The second order dynamics s = smax(s, s) are integrated backwards in tim e by first re-expressing th em as the equivalent first order state-space system *1 X xi X2 + -rnax i where x\ = s and X2 = s, and then discretising, replacing the continuous derivatives w ith the E uler derivatives *<*> =.(fc+i) (k+1 ) (Jfc) x; (fc) _ x\ - x \ and x k 1 to k 1 A 4- f» to give where A t is the sample period. This provides swc(t) and swc(t). These are then m apped into the s s phase-plane to give the boundary to the worst-case controllable region Cwc which expresses lim its on the path velocity s as a function the path position s, i.e. swc = swc(s). The sample period A t should be consistent w ith th a t used in the real-tim e closedloop system. There is little advantage to be gained by integrating using a smaller A t - the A t we use are fine enough to minimise approxim ation errors - because the driving input smax is defined as a function of the com pensation torques T p(i which are defined by the previous closed-loop experim ent. However, there m ay be a significant

16 6 C losed -L oop T rajectory G en eration 13 degradation in the performance of the closed-loop system if At is taken as larger than th a t used in the real-time closed-loop system, because the online disturbances q (i(t) m ay th en differ adversely from those anticipated by the previous experim ent. W hen m apping the swc from time t to path position s, the path position intervals A s should be sufficiently fine so th at Si,JC(s) contains no significant anomalies from the actual worst-case controllable region boundary. In the real-tim e system, we use a sampling frequency of fa = 300Hz, i.e. a sample period of At = ^ qs. W hen m apping swc at tim e t to the path position s(t), we use a path position interval of size As = Also, because s(t) may not equal an integer m ultiple of As, the value of swc at a path position s is taken to be the value to which s(t) m ost closely corresponds. 6.2 O nline T rajectory G eneration T he algorithm for calculating the online p a th acceleration s is as follows: Given: T he current joint positions q (4 p a th position s^) and p a th velocity s ^. Estim ate the current path velocity q*a:) using the Euler step (A*4-1) # # # (fc+1) Calculate the reference joint positions qf and velocities qf from the path description q = f(s) as qit+1) = f(s W) q(m-l) = f '( sw )gw. (fc+1) Calculate the online compensation torques Tpd via.(* + 1) pa = M(q!*+1)) (k (q<*+1>- q (fc>) + k (q<fc+1) - q w ) }. Calculate bounds s ^ and on the path acceleration s ^ using s^'\ q^^ and in equations (5.14) and (5.15) in the m ain text.

17 6 C lo se d -L o o p T r a je c to r y G e n e r a tio n 14 If an interval [s^),s* ^] exists Search for the maximum E [s ^, s ^ ] which will not violate the boundary of Cw c if applied. This is done by integrating s ^ and s^) forwards in time using an E uler-step approxim ation to obtain s ( k + l ) = sw+at (*<*>) s<*+u = «W+A and comparing ^(fc+1)(s(fc+1)) with swc(s^fc+1^). A bisection m ethod is used to reduce the search interval for. Else Decelerate (we use = ). C alculate the reference joint acceleration qr*+1^ as q(a.+l) = f"(s W )(5W )2 + f '(s W )sw. C alculate the input torque T,A:+1, as _(A:+1) _ (fc+l), X(A:+1) ' ' mod od '* '' vd pd 1 where is the m odel-based p a rt of the com puted-torque control law calculated as T m o d ^ = M(q<*+1>)q<fc>+ n(qw,qw). Integrate s ^ and using an E uler-step approxim ation to obtain and g(a:+l) viz; s(fc+i) = sw + At(s(fc)) s(k+i) =. T he algorithm is term in ated when s^ is equal to a predefined final p a th position Sf.

18 B ibliography [1] J.E. Bobrow, S. Dubowsky and J.S. Gibson. Time-Optimal control of robotic manipulators along specified paths. In The International Journal of Robotics Research, volume 4 (3), pages 3-17, Fall

A L A BA M A L A W R E V IE W

A L A BA M A L A W R E V IE W Volume 52 Fall 2000 Number 1 B E F O R E D I S A B I L I T Y C I V I L R I G HT S : C I V I L W A R P E N S I O N S A N D TH E P O L I T I C S O F D I S A B I L I T Y I N