R ob u st T im e-O p tim al P a th Tracking

C ontrol o f R obots:

T heory and E xp erim en ts

Im plem entation D etails

A idan Jam es CahillB.Sc. (Hons) M athem atics, Northum 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

A b strac t

This document supplements the information contained in the thesis “R obust Time-

O ptim al P a th 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

C ontents

A b stra c t

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 .....................................

4.1 Calculation of the Acceleration D isturbance S ig n a l ........................

4.2 Selecting the Controller Gains to Control the E r r o r s ....................

4.3 Calculating the Com pensation T o rques................................................

5 Offline Trajectory P l a n n in g .................................................................................

6 Closed-Loop Trajectory G e n e ra tio n ....................................................................

6.1 Calculation of the W orst-Case Controllable Region Boundary . .

1

2

6

8

8

9

10

11

12

12

6.2 Online Trajectory Generation 13

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 a th Tracking Control of Robots: Theory and Experim ents” , subm itted for the degree

of Doctor of Philosophy of the A ustralian National 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 a t 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.

This docum ent does not consider either the work involving the dynamic 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 tex t, 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 adap tation of the shooting

m ethods of Bobrow et al. [1] discussed herein.

2 Id en tifica tio n o f th e R ob ot M od el 2

2 Id en tifica tion 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 a t 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. Thus, equations (1) may 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 sample 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 the values of q ^ :̂ , q ^ , q ^ and T ^ are

inserted into the discrete equivalent of equations (2)

w ( q W , q W , q W ) $ = T<*> . (4)

Next, bo th sides of equations (4) are low-pass filtered to reduce the effects of noise

introduced by taking the numerical derivatives (3). This yields

(w(qW, qW, qW)) i $ = ( r W ) i , (5)

2 Identification o f the R obot M odel 3

where (•)/ represents the low-pass filter w ith 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 a t we identify the model param ­

eters in two stages. Firstly, we identify the friction param eters by using the “slow”

joint position tra jectory

sin(O.lf) + cos(0.25t) \q r = 60000 t = 0 : A t : 12s (7)sin(0.25t) + sin(0.4f) I

as reference input 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” jo in t position t r a ­

jectory

q r = 30000 3 [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” tra jec to ry

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 bo th slow

and fast dynamics.

The “goodness of fit” of the identified model param eters <1> can be estim ated graph­

ically by comparing the input torque trajectories for an experim ent w ith the torque

predicted by the discrete equivalent of equation (1)

M(qW)qW + n(qW ,q |fc)) = T « ( 10)

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 pa th 1 in C hapter 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 icc

CL- -200

— actual

— pred ic ted \ j13.5 4 4.5 5

path position s5.5 6

2 200£✓ ---- V 0CO -- actual

^ -200 — pred ic ted

,-------- .3 3.5 4 4.5 5 5.5 6

path position s

Figure 1: Actual and Estim ated A ctuator Torques

Note that:

• The original software for the real-tim e controller implem enting the PD control

law (8) was developed by Mr 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 the 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 .

2 Id en tifica tio n o f th e R ob ot M od el 5

• The original software for the identification procedure was developed by Mr Paul

Logothetis and implemented in MATLAB.

— 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.

— The least-squares fit to the model param eters equation (6), is imple­

m ented using the pseudo-inverse function pinv.

• A lthough 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.

• The development of backlash, described in C hapter 4 of the m ain tex t, necessi­

ta te 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

tex t) differed noticeably from those identified for the original model (Table 3.1 in

the m ain tex t). These differences are not a result of deficiencies in the identifi­

cation algorithm . R ather, they are a ttribu tab le to the addition of ex tra parts to

the robo t’s structure during its refurbishm ent.

3 R ea l-T im e C on tro l Laws 6

3 R ea l-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:

• S tandard 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 ?

• Com 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 nu­

merical differentiation of the joint positions q(A:) . We use the Euler-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 a t for m anipulators possessing low-resolution encoders and when

using low sampling frequencies, the velocity estim ates would contain sufficiently high

3 R ea l-T im e C on tro l Laws 7

levels of noise to cause instability. Such a result would necessitate a filter to be intro­

duced when calculating q ^ . However, this would invalidate the theory on which the

m ethods in the m ain tex t 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

compromise, leaving sufficient margin 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 au thor to implement

the control law (12).

• The order of conversion of joint position m easurem ents from encoder counts to ra ­

dians 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).

4 Im p lem en tin g th e A cce lera 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 tex t 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 T p(j ( s ) .

All of the work was implemented in MATLAB by the author, although the MATLAB

code executes the tra jectory 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 o f 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 ou tpu t joint positions q ^ , k = 1 to k f , from

the previous experim ent (kf = ^ + 1, where t f is the tim e over which the experim ent

is run and A t 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)A t A t

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).

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 Se lec t ing th e C o n tro lle r G a ins to C o n tro l th e E r ro r s

Assuming th a t the actuators do not satu rate , 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 a t the disturbance signal q y(l is known, the estim ates

of the errors are calculated as follows:

• The second order dynamics (17) re-expressed as the equivalent first order state-

space system

( x \X2x 3

\ i 4 J

( 0 0 1 ° \0 0 0 1

— kp i 0 —2 0\ 0 kp2 0 2 yj kp2 )

( X \ ^X2X 3

+

/ 0 0 \ 0 0 1 0

\ x 4 J \ 0 ! J(18)

where x \ = e i, X2 = e.2, ^3 = ei and x 4 = e.2 .

• Equations (18) are discretised by replacing the continuous derivatives w ith the

Euler-step approxim ations

*(*+!)x ix{k+1) xi___

f >A tk = 1 to k i — 1 to 4 ,

4 Im p lem en tin g th e A cce lera tio n D istu rb a n ce T h eory 10

to give

(*+l)1 = x f 0 + A t x 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 a t the com pensation torques be defined as a function of the pa th posi­

tion s. Because the path position s is known at each sample instant k , the com pensation

torques T p(J are also defined as functions of the pa th 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 T p(i over the interval [5(fc~50)} s(fc+50)].

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 tex t requires a m ethod for the calculation

of reference joint trajectories. Because of its simplicity of understanding and imple­

m 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 main 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 re­

expressing them as the equivalent first order state-space system

x iX2

where x \ = s and X2 = s, and then discretising, replacing the continuous deriva­

tives w ith the Euler derivatives

(fc+i) ffc),(t+1) = x 1 and = x(fc+1) ( l )

— Xc, k = 1 to k f ,

to give

r (fc+l)X1 II "S

(fc+i)x 2

(k)= x \

*<*> + A t (*<*>)

where A t is the sample period.

• The sample period A t must be consistent w ith th a t used in real-tim e. In the real­

tim e system, we use a sampling frequency of f s = 300Hz, i.e. a sample period of

A t — —1— s30Qb.

The shooting m ethod was implem ented in c by the author.

6 C lo se d -L o o p T r a je c to r y G e n e r a t io 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 worst-

case 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 alcu lation o f 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 them as the equivalent first order state-space system

*1X2

0 1 0 0

x iX 2

+ §-rnax i

where x \ = s and X2 = s, and then discretising, replacing the continuous derivatives

w ith the Euler derivatives

*<*> =

.(fc+i) — x;and x

(k + 1 ) (Jfc)(fc) _ x \ ’ - x \ ’1 A 4-

k — 1 to k 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 pa th 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 closed-

loop 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 experiment. However, there may be a significant

6 C losed -L oop T rajectory G en eration 13

degradation in the performance of the closed-loop system if A t is taken as larger than

th a t used in the real-time closed-loop system , because the online disturbances q (i(t)

may then differ adversely from those anticipated by the previous experim ent.

W hen m apping the swc from time t to pa th position s, the pa th position intervals

A s should be sufficiently fine so th a t 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 f a = 300Hz, i.e. a sample

period of A t = ^ qS. W hen m apping swc a t tim e t to the pa th position s(t) , we use a

pa th position interval of size As = 0.001. Also, because s(t) may not equal an integer

m ultiple of A s, the value of swc a t a pa th position s is taken to be the value to which

s(t) m ost closely corresponds.

6.2 O nline T rajectory G eneration

The algorithm for calculating the online pa th acceleration s is as follows:

Given: The current joint positions q (4 path position s^) and path velocity s ^ .

• Estim ate the current pa th 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(sW)

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 pa th acceleration s ^ using s^ ' \

q^^ and in equations (5.14) and (5.15) in the m ain text.

6 C lo se d -L o o p T r a je c to r y G e n e r a t io 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 Euler-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 = - -3 - —).

• Calculate the reference joint acceleration qr*+1 ̂ as

q(A.+l) = f " ( s W ) ( 5 W ) 2 + f ' ( s W ) s W .

Calculate the input torque T ,A:+1, as

_(A:+1) _ (fc+l) , X(A:+1)' ' m od ' ' vd 1m od * ' pd

where is the model-based part of the com puted-torque control law calcu­

lated as

T m o d ^ = M(q<*+1>)q<fc> + n(qW,qW) .

Integrate s ^ and using an Euler-step approxim ation to obtain and

g(A:+l) viz;

s (fc+i) = s W + A t ( s (fc))

s ( k + i ) = .

The algorithm is term inated when s ^ is equal to a predefined final pa th position S f .

B ibliography

[1] J.E. Bobrow, S. Dubowsky and J.S. Gibson. Time-Optimal control of robotic ma­

nipulators along specified paths. In The International Journal of Robotics Re­

search, volume 4 (3), pages 3-17, Fall 1985.

15