Robust bucket position tracking for a large hydraulic excavator

Mechanism and Machine Theory 40 (2005) 1–16

Mechanism and Machine Theory www.elsevier.com/locate/mechmt

Robust bucket position tracking for a large hydraulic excavator q A.S. Hall, P.R. McAree

*

Co-operative Research Centre for Mining Technology and Equipment (CMTE), The University of Queensland, Brisbane 4072, Australia Department of Mechanical Engineering, The University of Queensland, Brisbane 4072, Australia Received 15 March 2002; received in revised form 14 April 2004; accepted 25 May 2004 Available online 19 August 2004

Abstract The device we study is the excavation arm of a large hydraulic mining shovel having a multi-loop kinematic form. We describe an iterative algorithm that allows the position of the bucket to be tracked from measurements of the linear actuator extensions. The important characteristic of this algorithm is that it is numerically well-behaved when the linkage is close to singular configurations. While we focus on a specific device, the algorithm is easy to adapt to other multi-loop linkages.
2004 Elsevier Ltd. All rights reserved. Keywords: Hydraulic excavator; Mining equipment; Multi-loop linkages; Robust forward kinematics; Singularities

1. Introduction The linkage shown in Fig. 1 finds application as the excavation arm on large open-cut mining shovels. The arm has three primary links, termed boom, stick, and bucket, connected by rotary q

Supported by CMTE project RM61. Corresponding author. Address: Co-operative Research Centre for Mining Technology and Equipment (CMTE), The University of Queensland, Brisbane 4072, Australia. Tel.: +61 73 365 3589; fax: +61 73 365 4799. E-mail addresses: [email protected] (A.S. Hall), [email protected] (P.R. McAree). *

0094-114X/$ - see front matter
2004 Elsevier Ltd. All rights reserved. doi:10.1016/j.mechmachtheory.2004.05.005

2

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

Fig. 1. Kinematic layout of the excavation arm. The arm is actuated by hydraulic cylinders ce, dj, and hl. Link bf is a rigid cylinder. Solid circles indicate revolute joints.

joints at a, i, and k. Actuation is by extension and retraction of three pairs of hydraulic cylinders, ce (boom cylinder), dj (stick cylinder) and hl (bucket cylinder), commanded independently by operator joysticks. The three degrees of freedom of the excavation arm are augmented by swing motion about the axis mn passing through the machine house. An interesting feature of the arm design is the use of the intermediate triangular link efh, pivoting at g, which serves to maintain the orientation of the bucket when the boom and stick cylinders are actuated. While greatly simplifying machine operation, the arrangement results in a multi-loop kinematic structure whose forward kinematic solution (the determination of bucket position from the cylinder extensions) is non-trivial. This paper describes an algorithm to facilitate tracking of the bucket position from cylinder lengths. Specifically, we describe a method for tracking the angles of the boom, stick and bucket links relative to a global reference frame (marked X and Y of Fig. 1). Having determined these angles, and with knowledge of the (easily measured) pivot angle, the spatial position of the bucket can be readily determined by applying, for example, the transform method of Denavit and Hartenberg, see Refs. [1,2], or BrockettÕs product-of-exponentials method, see Refs. [3,4]. The underlying motivation for this work is to develop a computer-based system for monitoring how these and similar machines are used in the many and varied conditions of an open-cut mine. The variables of interest, such as digging forces and bucket-fill, are intimately linked to the spatial trajectory of the bucket. Robustness of the kinematic tracking algorithm is particularly important. By ÔrobustÕ we mean the numerical computation remains well-behaved over extended periods without intervention or re-initialization, so that it can be left to work as part of an unattended computer-based system at remote mine sites. The principal threat to robustness comes from the so-called singular configurations of the mechanism which give rise to numerical instabilities in the solution procedure when approached. The main work of the paper is to describe a method for achieving robust tracking by allowing the kinematic reference (i.e. the internal model of the linkage used by the tracking algorithm) to

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

3

ÔdeformÕ in the neighborhood of singularities in such a way that the integrity of the underlying numerical method is preserved. In this way we are able to robustly track motion of the bucket through the full range of motions of the arm. While our discussion focusses on the linkage of Fig. 1, the approach we describe can, with appropriate changes, be adapted to other multi-loop linkages including spatial linkages such as the Stewart platform, see for instance Refs. [5–8]. We thus see the main contribution of this work being the presentation of an exemplar to serve as a guide to the approach. The work builds on some ideas first presented in Ref. [9].

2. Linkage geometry; kinematic constraints Our analysis starts with the observation that as the cylinders extend and retract, bucket motion must be in harmony with the constraints imposed by the geometry of the arm mechanism. The four vector loops shown in Fig. 2 serve to define these motion constraints. Using positive numbers di for vector lengths and unit complex numbers zi = cos hi + j sin hi to describe their directions, these loops are given by d 1 z1 þ d 2 z2 þ d 3 z3 þ d 4 z4 ¼ 0; d 5 z5 þ d 6 z6 þ d 3 z3 þ d 7 z7 ¼ 0; d 8 z8 þ d 9 z9 þ d 10 z10 þ d 11 z11 ¼ 0;

ð1Þ

d 12 z12 þ d 13 z13 þ d 14 z14 þ d 15 z15 ¼ 0: A fifth relation is introduced to reduce complexity: d 12 z12  d 8 z8 þ d 1 z1 þ d 16 z16 ¼ 0:

Fig. 2. Closed loops which construct constraint equations.

ð2Þ

4

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

Substituting Eq. (2) into Eq. (1) and expanding into real and imaginary parts gives a total of eight constraint equations (see Eq. (3)) that serve as the basis for our analysis. We denote these by C = C(d, h) where d is the vector of lengths di, h is the vector of directions hi, and ci and si are used as shorthand for cos hi and sin hi. 8 d 1 c1 þ d 2 c2 þ d 3 c3 þ d 4 c4 ¼ 0; > > > > > > > > d 1 s1 þ d 2 s2 þ d 3 s3 þ d 4 s4 ¼ 0; > > > > > d 5 c5 þ d 6 c6 þ d 3 c3 þ d 7 c7 ¼ 0; > > > > > > < d 5 s5 þ d 6 s6 þ d 3 s3 þ d 7 s7 ¼ 0; Cðd; hÞ ¼ ð3Þ > > d c þ d c þ d c þ d c ¼ 0; 8 8 9 9 10 10 11 11 > > > > > > > d 8 s8 þ d 9 s9 þ d 10 s10 þ d 11 s11 ¼ 0; > > > > > > d 8 c8  d 1 c1  d 16 c16 þ d 13 c13 þ d 14 c14 þ d 15 c15 ¼ 0; > > > > : d 8 s8  d 1 s1  d 16 s16 þ d 13 s13 þ d 14 s14 þ d 15 s15 ¼ 0: The tracking problem amounts to solving Eq. (3) and to do this we used an adaptation of the Newton–Raphson method for non-linear systems of equations, see for example Ref. [10, Section 9.6]. The formulation follows from Taylor series expansion of Eq. (3) for perturbations (Dd, Dh) about the configuration (d, h), i.e. !k 1 m m X X 1 X o o Dd i þ Dhi Cðd; hÞ ¼ 0; ð4Þ Cðd þ Dd; h þ DhÞ ¼ k! i¼1 od i i¼1 ohi k¼0 where m is the number of vectors used to define the geometry of the linkage, here 16. In taking this expansion note that we have included all of the characterizing parameters, including those which are fixed in their values by the design. This is intentional; our method for robust tracking works by perturbing both fixed and varying parameters when the linkage is near to singular to avoid ill-conditioning of the numerical solution. We denote the parameters fixed by design as dc and /c, and those which vary in normal motion by dv and hv. The parameters dv are the lengths of the three hydraulic cylinders and the hv are the time-varying angles dependent on dv. Explicitly: dc ¼ ½d 1 ; d 2 ; d 3 ; d 4 ; d 6 ; d 7 ; d 8 ; d 9 ; d 11 ; d 13 ; d 14 ; d 16 T ; dv ¼ ½d 5 ; d 10 ; d 15 T ; /c ¼ ½/4 ; /6 ; /7 ; /8 ; /11 ; /13 ; /16 T ¼ ½h4 ; h6  h2 ; h7 ; h8  h1 ; h11  h1 ; h13  h9 ; h16  h2 T ; T

hv ¼ ½h1 ; h2 ; h3 ; h5 ; h9 ; h10 ; h14 ; h15  : Considering only to first order terms, Eq. (4) can be written in vector matrix form as Cðd þ Dd; h þ DhÞ ¼ Cðd; hÞ þ

oC oC oC oC Ddc þ Ddv þ Dhv þ D/c ; odc odv ohv o/c

ð5Þ

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

5

where 0

oC odc

oC odv

c1 c2 B s1 s2 B B 0 0 B B B 0 0 ¼B B 0 0 B B 0 B 0 B @ c1 0 s1 0 0 0 0 B0 0 B B B c5 0 B Bs B 5 0 ¼B B 0 c10 B B 0 s10 B B @0 0 0 0 0

c3 s3 c3 s3 0 0 0 0

0 0 c7 s7 0 0 0 0

0 0 0 0 c8 s8 c8 s8

0 0 0 0 c9 s9 0 0

0 0 0 0 c11 s11 0 0

0 0 0 0 0 0 c13 s13

0 0 0 0 0 0 c14 s14

1 0 0 C C 0 C C C 0 C C; 0 C C C 0 C C c16 A s16

ð6Þ

ð7Þ

0

0

0

0

0

0

0

0

0

0

0

0

d 6 s6

d 7 s7

0

0

0

0

d 6 c6

d 7 c7

0

0

0

0

0

0

d 8 s8

d 11 s11

0

0

0

0

d 8 c8

d 11 c11

0

0

0

0

d 8 s8

0

d 13 s13

d 16 s16

0

0

d 8 c8

0

d 13 c13

d 16 c16

0 0

0 0 c6 s6 0 0 0 0

1 0 0 C C C 0 C C 0 C C C; 0 C C 0 C C C c15 A s15

d 4 s4

B d 4 c4 B B B 0 B oC B B 0 ¼B o/c B 0 B B 0 B B @ 0

c4 s4 0 0 0 0 0 0

d 1 s1

B d 1 c1 B B B 0 B B 0 oC B ¼B ohv B d 8 s8  d 11 s11 B B d c þd c 11 11 B 8 8 B @ d 8 s8 þ d 1 s1 d 8 c8  d 1 c1

1 C C C C C C C C; C C C C C A

ð8Þ

d 2 s2

d 3 s3

0

0

0

0

d 2 c2

d 3 c3

0

0

0

0

d 6 s6 d 6 c6

d 3 s3 d 3 c3

d 5 s5 d 5 c5

0 0

0 0

0 0

0

0

0

d 9 s9

d 10 s10

0

0

0

0

d 9 c9

d 10 c10

0

d 16 s16

0

0

d 13 s13

0

d 14 s14

d 16 c16

0

0

d 13 c13

0

d 14 c14

0

1

C C C 0 C C 0 C C C: 0 C C 0 C C C d 15 s15 A 0

d 15 c15 ð9Þ

6

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

3. Forward kinematic tracking using a multi-dimensional Newton–Raphson solver The tracking problem amounts to determining the time-varying trajectory hv from measurements of the cylinder lengths dv which we assume to be sampled at a constant rate. We achieve this from Eq. (5) by holding the design-fixed parameters constant, i.e. Ddc = 0, D/c = 0, and seeking angles hv which result in C(d + Dd, h + Dh) = 0 given current values of (d, h) and perturbations Ddv to the cylinder lengths resulting from the operatorÕs command since the previous iteration. This gives Cðd; hÞ þ

oC oC Ddv þ Dhv ¼ 0; odv ohv

which can be rearranged in terms of Dhv:  1   oC oC Dhv ¼  Cðd; hÞ þ Ddv : ohv odv

ð10Þ

The angles hv can be solved for using Eq. (10) in a multi-dimensional Newton–Raphson solver. A single-iteration implementation is given as Algorithm 1. Algorithm 1. Forward kinematic tracking using Newton–Raphson for ever do wait for timer interrupt read dv,k Ddv,k = dv,kdv,k1   oC 1  oC hv;k ¼ hv;k1  oh C þ j Dd  k1 v;k : odv k1 v k1

Here hv,k gives the values hv at step  k given Ddv,k, the measured change in cylinder lengths from oC  oC  the previous step. The notation oh  and od  indicates that these matrices (Eqs. (7) and (9)) are v k1 v k1 evaluated for values dk1 and hk1. Note that for each measurement the Newton–Raphson equation (Eq. (10), and line 4 of Algorithm 1) is iterated once only. This computation can be repeated several times to increase the accuracy of solution, terminating for instance, when kCk1k reaches some tolerance value. The iterative structure of Algorithm 1 means the computed reference will always lag the true position of the linkage, the error increasing as the update rate decreases. Later, in Section 8.1, we find lower bounds on the sample rate by considering the relationship between update rate and tracking error for Algorithm 1. 4. Linkage singularities oC is well-conditioned. When this matrix is close to being Algorithm 1 is well-behaved provided oh v singular, its inverse becomes numerically ill-conditioned causing the solution to diverge. oC correspond to configurations where the linkage loses first order constraint/ Singularities of oh v acquires first order freedom. At these configurations the bucket is able to undertake small motions

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

7

Fig. 3. The excavation arm at a nearly singular configuration z14 and z15 are nearly collinear.

oC described by dhv in the kernel of the singular oh . Singular configurations can be identified from the v condition   oC ¼ ðc14 s15  s14 c15 Þðc9 s10  s9 c10 Þ½d 2 ðs3 c5  c3 s5 Þðs1 c2  c1 s2 Þ det ohv

þ d 6 ðc5 s6  s5 c6 Þðc1 s3  s1 c3 Þ ¼ 0:

ð11Þ

Each factor in Eq. (11) defines a variety of singularity. The first factor is zero when z14 and z15 are collinear, see Fig. 3. The second when z9 and z10 are collinear. The third condition is more general but can be visualized through specific examples, e.g. pairs z1, z2 and z3, z5 being simultaneously collinear.

5. Kinematic tracking in the vicinity of singularities The limitation on Algorithm 1 imposed by linkage singularities can be overcome by allowing oC the parameters d and h of the kinematic reference to vary from their true values so that oh remains v well-conditioned. Let dhv, d/c, ddv, and ddc be first order perturbations from the true value. For valid configurations C(d + Dd, h + Dh) = C(d, h) = 0, and Eq. (5) reduces to the constraint oC oC oC oC ddc þ ddv þ dhv þ d/ ¼ 0; odc odv ohv o/c c which can be rearranged as  1   oC oC oC oC dhv ¼  ddc þ ddv þ d/ : ohv odc odv o/c c

ð12Þ

oC . As a measure of We seek perturbations d/c, dhv, ddv, and ddc that maintain the condition of oh v oC condition, we use the smallest singular value r of ohv obtained by the singular value decomposition (SVD), see Refs. [11,10,12]. To the first order, the perturbations d/c, dhv, ddv, and ddc alter r by

8

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

dr ¼

or T or T or T or T dhv þ d/c þ ddv þ ddc : ohv o/c odv odc

Substituting Eq. (12) into Eq. (13) gives    T  or T or T oC 1 oC or or T oC 1 oC dr ¼   d/c þ ddc o/c ohv ohv o/c odc ohv ohv odc  T  or or T oC 1 oC þ  ddv : odv ohv ohv odv Vectors

or or or , , , ohv o/c odc

r ¼ uT

and

or odv

ð13Þ

ð14Þ

in Eq. (14) can be computed by first observing

oC v; ohv or

where u and v are the left and right singular vectors associated with r. The partial derivative ohi is then   or oC T o ¼u v; ohi ohi ohv   oC where oho i oh is the matrix obtained by differentiating Eq. (9) with respect to angle hi so that v   1   1 0 0 oC oC T o T o u v u v B o/4 ohv C B oh1 ohv C C B B   C   C C B B B uT o oC v C B uT o oC v C C C B B or or o/6 ohv C; oh2 ohv C: ¼B ¼B C C B B o/c B ohv B C C .. .. C B B .  C .  C C B B @ @ oC A oC A T o T o u v u v o/16 ohv oh15 ohv Similarly   or oC T o ¼u v; od i od i ohv giving   1 o oC u v B od 1 ohv C B   C B o oC C C B uT B or B od oh v C C; 2 v ¼B C odc B C . . B .  C C B A @ o oC uT v od 16 ohv 0

T

  1 o oC B u od 5 ohv v C B   C C or B B T o oC C u v C: B odv B od 10 ohv C B   C @ oC A T o u v od 15 ohv 0

T

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

9

As shorthand set: T

ory or T or T oC1 oC ¼  ; o/c ohv ohv o/c o/c T

ory or T or T oC1 ¼  odc ohv ohv odc T yT or or or T oC1 ¼  odv ohv ohv odv

oC ; odc oC ; odv

so that Eq. (14) simplifies to T

dr ¼

T

T

ory ory ory d/c þ ddc þ ddv : o/c odc odv

ð15Þ

The problem is to compute suitable perturbations so that r is always above a defined threshold rt . That is, when, for the current values of the kinematic reference, r < rt we seek d/c, ddc, and ddv that generate the deficit between rt and r, i.e. dr ¼ rt  r:

ð16Þ

These are added to the current values /c, hv, dc, and dv to distort the kinematic reference so that it is suitably conditioned. In choosing these perturbations, we seek to minimize the weighted sum of squares cost  1 ð17Þ J ¼ ddTv ddv þ a ddTc ddc þ b d/Tc d/c 2 where a and b are weights penalizing ddc and d/c. (The selection of a and b is discussed in Section 8.3.) The values of ddv, ddc, and d/c which minimize Eq. (17), subject to the constraint of Eq. (15), can be found from the theory of Lagrange multipliers by finding the minimum of  1 Lðddc ; ddv ; d/c ; kÞ ¼ ddTv ddv þ a ddTv ddv þ b d/Tc d/c 2 ! T T T ory ory ory þ k dr  d/c  ddc  ddv : o/c odc odv This expression is minimized when the partial derivatives of L with respect to ddc, ddv, d/c, and the Lagrange multiplier k are all zero, i.e. oL ory ¼ ddv  k ¼ 0; o ddv odv oL ory ¼ a ddc  k ¼ 0; o ddc odc oL ory ¼ b d/c  k ¼ 0; o d/c o/c T T T oL ory ory ory ddc  ddv  d/c ¼ 0: ¼ dr  ok odc odv o/c

10

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

Arranging in matrix form we have 1 0 ory 0 0  B I odc C C0 B 1 0 1 B y C 0 or C ddv B aI 0  CB B 0 C B B odv CB ddc C B 0 C C C B C ¼ B C; y CB B or A A @ @ 0 d/ C B 0 c 0 bI  C B o/ B cC dr k C B A @ ory T ory T ory T 0 odc odv o/c which can be solved for using the method of Schur complements [11,13] to give 1 ory dr; W odv 1 ory dr; ddc ¼ aW odc 1 ory dr; d/c ¼ bW o/c ddv ¼

ð18Þ

where T

W¼

T

T

ory ory 1 ory ory 1 ory ory þ þ : odv odv a odc odc b o/c o/c

ð19Þ

6. Robust tracking by distorting the kinematic reference Eqs. (5)–(19) are used in Algorithms 2 and 3 to achieve singularity robust tracking. (The division into two parts is for clarity.) Algorithm 2 closely resembles Algorithm 1 and is driven by changes in cylinder lengths Ddv (calculated here relative to the possibly deformed kinematic reference). Algorithm 2 extends on Algorithm 1 by using Eq. (15) to predict the smallest singular value, r, oC when the Ddv are applied. If the prediction is below the threshold value, rt , the linkage is of oh v deemed singular, causing the invocation of Algorithm 3. Algorithm 3 gives a sequence of computations that ÔdistortÕ the kinematic reference so that r P rt thereby maintaining well-conditioned calculations using Eq. (18). Note that the distortions accumulate from step to step while the linkage is near to singular. A single iteration of the Newton–Raphson equation is applied at the final stage of Algorithm 2 using the current (possibly deformed) kinematic reference to compute hv. The following notes may further assist the reader to interpret and implement these algorithms. • The prefix D on quantities such Dhv,k indicate Pchange resulting from linkage motion; d indicates a perturbation computed at this step; and the cumulative perturbation.

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

11

• Algorithm 3 updates values of the kinematic reference at step k  1 using perturbations computed at step k, see in particular the last four lines. • The boolean flag ÔSingularÕ is set ÔTRUEÕ when the physical mechanism is in the region of singularity set by rt . • Algorithms 2 and 3 are tuned by four parameters: rt , a, b, and the measurement update rate. The selection of these parameters is discussed in Section 8. oC such as those pre• Line five of Algorithm 2 assumes a method for computing the SVD of oh v sented in Refs. [10,12]. Algorithm 2. Robust kinematic tracking by deformation for ever do wait for timer interrupt read dv,k  P Ddv,k = dv,k + dv,k1h dv,k1   oC  ½rk1 ; uk1 ; vk1  ¼ min svd oh  v

i

k1

ory odv

  

T

Ddv;k Estimate new r resulting from Ddv,k using: re;k ¼ rk1 þ k1 If re;k < rt or Singular == TRUE then Predicted that reference will become singular or that linkage is singular execute Algorithm 3 (possibly perturbed) kinematic reference Compute hv using the  current    oC 1  oC  Ck1 þ odv  Ddv;k hv;k ¼ hv;k1  ohv  k1 k1 dc,k = dc,k1 /c,k = /c,k1. Algorithm 3. Compute non-singular kinematic reference Estimate the contribution of cumulative perturbations to r using Eq. (15):    T T T P P P P ory  ory  ory  /c;k1 þ odc  dc;k1 þ odv  dv;k1 rk ¼ o/  c k1 k1 k1 P If re;k  rk < rt then Linkage singular. Determine perturbations to kinematic reference so that r ¼ rt using Eqs. (16), (18), and (19); update cumulative perturbations P P y 1 or  ddc;k ¼ Wk1 ðrt  re;k Þ; dc,k = dc,k1 + ddc,k odc  k1 y P P 1 or  ddv;k ¼ Wk1 ðrt  re;k Þ; dv,k = dv,k1 + ddv,k odv  k1 P P y 1 or  d/c;k ¼ Wk1 o/  ðrt  re;k Þ; /c,k = /c,k1 + d/c,k c

k1

Set Singular = TRUE else Linkage no singular. Remove and zero cumulative perturbations. P longer P ddc,k =  dc,k1; dc,k = 0

12

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

P P ddv,k =  Pdv,k1; Pdv,k = 0 d/c,k =  hv,k1; /c,k = 0 Set Singular = FALSE Determine dhv resulting from d/c, ddv, and ddc using Eq. (12)       oC1  oC  oC  oC  dhv;k ¼  ohv  ddc;k þ odv  ddv;k þ o/  d/c;k odc  c k1

k1

k1

k1

Update kinematic reference by adding computed perturbations: dc,k1 = dc,k1 + ddc,k dv,k1 = dv,k1 + ddv,k /c,k1 = /c,k1 + d/c,k hv,k1 = hv,k1 + dhv,k.

7. Distortion of computed end-point trajectories Because Algorithms 2 and 3 distort the geometry of the arm, the computed bucket position differs from its actual location. Fig. 4 illustrates this, comparing the perturbed bucket-tooth trajectory with the ÔactualÕ trajectory for a dig-cycle in which the arm comes close to the singularity-type shown in Fig. 3. The trajectory is shown in the plane of the arm, that is swing motion has been ignored. The data used to construct this plot is based on measurements made on an O&K RH200 hydraulic excavator during field trials, with the cylinder lengths sampled at a rate of 200 Hz. 12 Actual

10 Distorted

Vertical Distance (m)

8

6

4

2

Start

End 0

8

9

10

11

12

13

14

15

Horizontal Distance (m)

Fig. 4. Comparison of actual and distorted bucket-tooth trajectory.

16

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

13

0.185 0.18

Minimum Singular Value

0.175 0.17 0.165 0.16 Distorted 0.155 0.15 0.145 0.14 Actual 16

16.5

17

17.5

18

18.5

19

19.5

20

Time (s)

Fig. 5. Comparison of actual and distorted minimum singular value.

The ÔactualÕ trajectory in Fig. 4 was computed using Algorithm 1. The cycle is one in which the arm approaches singularity, but not so close that Algorithm 1 diverges, providing a comparative baseline. To enable the perturbation to be clearly seen, a higher-than-usual threshold value of rt (=0.15) is used. It is seen that the two trajectories (actual and perturbed) track each other closely, except in the region of highest bucket reach where the arm approaches singularity. Similar perturbations are observed when the algorithm is applied to other dig-cycles. Fig. 5 compares r for the actual trajectory with that of the perturbed trajectory. The plot shows the region in which Algorithm 3 is active and illustrates that r is maintained above the threshold rt . 8. Algorithm tuning to minimize end-point perturbation Algorithms 2 and 3 are tuned by four parameters: the update rate, the threshold minimum singular value rt , and the cost-function weights a and b. It is desirable to choose these parameters so that the deviation of the computed end-point (e.g. bucket-tooth) trajectory from the ÔactualÕ trajectory is minimized. In this we are aided by the observation that most dig-cycles follow similar trajectories. The parameters to be chosen to minimize deviations on a typical path, e.g. that of Fig. 4, with good performance for most paths. In Sections 8.1–8.3 issues relevant to the selection of these parameters are discussed, using the data of Fig. 4 as a typical path.

14

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16 0.35 0.3 RMS Error

0.25 0.2 0.15 0.1 0.05 0

0

20

40

60

80

0

20

40

60

80

100

120

140

160

180

200

100

120

140

160

180

200

0.15 0.14

Inf (σ)

0.13 0.12 0.11 0.1 0.09

Update Rate (Hz)

Fig. 6. Variation in RMS tracking error and infðrÞ with update rate.

8.1. Update rate selection The update rate refers to the number of times per second the tracking algorithm runs. Because Algorithms 1 and 2 are iterative, the computed linkage configuration lags the true configuration, resulting in tracking error. A decrease in update rate increases the tracking error with the possibility of numerical ill-conditioning should the computed configuration lag in such a way that it becomes close to singular. For these reasons, it is desirable to update at the highest rate possible. The highest rate is governed by the available computational resource. Fig. 6 indicates the acceptable lower bound on update rate by showing how the RMS tracking error and the infimum of r vary with the update rate. These graphs were generated by applying Algorithm 1 to sub-sampled cylinder length measurements. Observe the tracking error increases and infðrÞ decreases as the update rate is decreased. The graphs suggest that for the machine in question, good tracking is achieved for update rates above 40 Hz. 8.2. Threshold singular value, rt The threshold value, rt determines when Algorithm 3 is active. This threshold trades numerical conditioning against end-point deviations. For the candidate machine it was found that Algorithm 1 diverges when r < 0:05. Setting rt = 0.1 provides a good balance between end-point deviations and acceptable numerical conditioning, yielding robust computation.

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

15

0.35

RMS Error

0.3 0.25 0.2 0.15 0.1 500 400 0

300 100 200

200 300

100 400 α

0

500

β

Fig. 7. Variation of RMS tracking error with a and b.

8.3. Cost weighting parameters, a and b Algorithm 3 is tuned by parameters a and b. Fig. 7 shows the variation in RMS trajectory error with these parameters. Note the error becomes smaller with decreasing a and increasing b, suggesting that only the parameters dc should be allowed to distort, i.e. that the cost function of Eq. (17) be reduced to 1 ð20Þ J ¼ ddTc ddc : 2 This results in some straight-forward simplifications in Algorithm 3. The full derivation has been provided for generality.

9. Conclusions The problem we have solved is to provide a computationally robust method for tracking the end-point motion of multi-loop linkages close to singularities. The specific example of an excavation arm has been examined in some detail. The methodology can, however, be applied to other multi-loop linkages by straight-forward adaptation. The key to the formulation is to identify kinematic loops within the linkage and use these to describe the constraints on motion imposed by the linkage structure. For planar mechanisms, complex numbers (as used here) provide a concise means of deriving and expressing these con-

16

A.S. Hall, P.R. McAree / Mechanism and Machine Theory 40 (2005) 1–16

straints. For spatial mechanisms, 3d vector loops can be used to achieve the same end. Other approaches are also possible, see Ref. [9]. It is important to recognize that the algorithm we have described is unable to track motion of the linkage should it pass through a singularity into another closure. The algorithm can be thought of as placing a Ôpotential wallÕ around linkage singularities which keeps the kinematic reference to the same closure. This limitation is not as significant as it might first appear, since the closure of a multi-loop linkage cannot be determined from the control inputs alone. If additional sensor information capable of determining the closure is available, there is likely to be a simpler, possibly closed form, solution to the forward kinematics which would be favored over the method described here. We see an attractive feature of our approach being its derivation from basic principles of calculus. Some workers have questioned whether the same end could be achieved by simpler means, for example by imposing limit constraints on cylinder lengths near to singular configurations so that the reference does not become singular. The functional discontinuities that such approaches introduce are, to us, antithetical to the calculus-theoretic formulation which underpins the Newton–Raphson method. For this reason, we see elegance in the approach we have described. Moreover, often singular configurations involve a complex inter-relation between the inputs, such limit constraints are inappropriate. Our approach treats all singular configurations in the same way.

References [1] J. Denavit, R.S. Hartenberg, A kinematic notation for lower-pair mechanisms based on matrices, J. Appl. Mech. 22 (1955) 215–221. [2] J. Craig, Introduction to Robotics: Mechanics and Control, Addison-Wesley, Reading, MA, 1986. [3] R.W. Brockett, Robotic manipulators and the product of exponentials formula, in: Proc. Symp. Math. Theory Networks and Systems, Beer Sheba, Israel, 1993, pp. 120–129. [4] J.M. Selig, Geometrical Methods in Robotics, Springer-Verlag, New York, 1996. [5] D. Stewart, A platform with six degrees of freedom, Proc. Inst. Mech. Eng. Pt. 1 180 (1965–1966) 371–386. [6] J.-P. Merlet, Direct kinematics and assembly modes of parallel manipulators, Int. J. Robot. Res. 11 (2) (1992) 150– 162. [7] K.H. Hunt, E.J.F. Primrose, Assembly configurations of some in-parallel-actuated manipulators, Mechan. Mach. Theory 28 (1) (1993) 31–42. [8] K.H. Hunt, P.R. McAree, The octahedral manipulator: geometry and mobility, Int. J. Robot. Res. 17 (8) (1998) 868–885. [9] P.R. McAree, R.W. Daniel, A fast robust solution to the Stewart platform forward kinematics, J. Robot. Syst. 13 (7) (1996) 407–428. [10] W.H. Press, S.A. Teukolsky, W.T. Vetterling, B.P. Flannery, Numerical Recipes in C: The Art of Scientific Computing, Cambridge University Press, 1993. [11] R.A. Horn, C.R. Johnson, Matrix Analysis, Cambridge University Press, 1985. [12] G.H. Goulb, C.F. Van Loan, Matrix Computations, second ed., Johns Hopkins University Press, Baltimore, 1989. [13] T. Kailath, Linear Systems, Prentice-Hall, 1980.