Global minimum-time trajectory planning of mechanical ...

INT. J. CONTROL, 1998, VO L. 71 , No. 4, 63 1- 652

Global minimum-time trajectory planning of mechanical manipulators using interval analysis

AURELIO PIAZZI t a nd ANTONIO VISIOLI t

The paper addresses the global minimum-time trajectory planning of an m-joint mechanical manipulator. Using a joint space scheme with given intermediate points to be interpolated by piecewise cubic polynomials, a novel bisecting-plane algorithm is proposed to schedule the times between adjacent knots under velocity, acceleration, and jerk constrain ts . This algorithm, which is proved to be globally convergent with certa inty within an arbitrary precision, uses an interval procedure (a subroutine adopting tools and ideas of interva l analysis) in proving that a local minimum is actually a global one. A wo rked example for the six-joint case is exposed, and computati onal results of a C + + implementation are included.

1. Introduction

The mecha nical manipula to r is a highly no n-linea r dynamic system which has to be subjected to (non-linear) co nstraints in order to obta in a co rrect a nd safe o perating functi oning. For this reaso n, making an optima l control fo r a mecha nica l ma nipula tor is a very difficult task. With va rio us simplifying ass umptions, only suboptimal solutions have appeared in the bterature. An incomplete list of relevant works includes Shin a nd McK ay (1 986), Ta n a nd Pot ( 1988) , Bo brow ( 1988) , Wu ( 1994), a nd a survey, with many additional references, was published by Shiller a nd Dubowski (1988). An a lterna ti ve simpler approach, where the ma nipula to r d ynamics is not ta ken into account, is to di vide the problem into two steps: optimal trajectory planning, followed by feedback trajectory tracking. A sequence of pa th po ints is usua lly specified in terms of a desired positio n and o rienta tio n o f the too l frame. Each of these pa th points is then mapped into a set of jo int a ngles/offsets (kno ts) by a pplicatio n of the inverse kinema tics. These kno ts a re then interpo la ted with smooth functio ns to be optimized subject to co nstra ints suita bly chosen fo r a specific manipula tor applica tion. The resulting joint trajecto ry fo rms the input to the robot's feedback control system.

In this context, Lin el al. ( 1983) proposed cubic polynomial functions (splines) fo r a trajectory pla nning where the tota l trave lling time is minimized under constra ints on joint velocities, accelera tio ns, a nd jerks. Cubic splines a re widely used fo r in terpo la tio n since they prevent the la rge oscilla tions of the trajecto ry which can result with higher-o rder polynomia ls. The trajectory smoothness was ass ured by imposing, fo r the piecewise cubic po lyno mia ls, continuity of positio ns, velocities, and accelera tions. They provide a n effici ent optimiza tion so lver based on the iterati ve genera tio n of fl exible po lyhedrons in the sea rch space, which can o nly determine

Received I I December 1997. Revised 5 May 1998 . t Dipartimento di Ingegneria dell ' lnformazione, University of Parma, Italy. e-mail :

aure [email protected]. it t Dipartimento di Elett ro nica per I'A utomazione, University of Brescia, Italy. e-mail :

visioli @bsing. ing. unibs. it

0020·7179/98 51 2.00 © 1998 Taylor & Francis Ltd .

632 A. Piazzi and A. Visioli

\ \ \

l

Figure I. The robot Doghi (by lcomatic) situa ted in the Robotics Labora tory of the University of Brescia.

loca l solutions to the constrained optimization problem. The aim of achieving a true global solution in optimal trajectory planning was pursued by Simon (1993) who proposed, in a similar context, a stochastic optimization method based on a neural network, in order to obtain a minimum-jerk joint trajectory.

Using the spline joint space scheme of Lin et at. (1983), the authors (Piazzi and Visioli 1997) proposed a deterministic global optimization approach based on an interval a lgorithm to obtain a minimum-time spline trajectory subject to constraints on joint accelerations and jerks . Jerk constraints are primarily due to the fact that joint position errors increase when the jerk increases, as asserted by Kyriakopoulos and Sa ridi s (1988), and to limit excessive wear on the robot and the excitation of resonances so that the robot life-span is extended (Craig 1989). Maintaining the aim of pursuing a globally optimal planning, thi s paper significantly extends our previous work by taking into account constraints on joint velocities, and proposes a novel algorithmic method of cutting planes in the search domain. From the viewpoint of computation time, this method leads to a significant improvement over the previous algorithm, since the interval procedure is applied to a few (n - 1) dimensional fea sibility problems instead of to a complete n-dimensional problem. Precisely, interval analysis is used to reveal if a local minimum, determined, for example, with a local

Mechanical manipulators planned by interval analysis 633

gradient procedure, is actually a global minimum. The techniques of interval analysis, which is a generalization of the 'standard ' real analysis over the arithmetic of real intervals, have recently been applied to control theory and motion planning (Walter and Jaulin 1994, Jaulin and Walter 1996, Piazzi and Marro 1996, Malan et al. 1997). An introduction to these techniques can be found in the book by Moore (1979) (the founder of interval analysis), and applications to general purpose global optimization are covered by Ratschek and Rokne (1988) and Hansen (1992).

The paper is organized as follows . In section 2, the problem of the global minimum-time trajectory planning is formalized. It is explained how to transform the pertinent semi-infinite optimization problem (equation (3)) into a finite one (equations (6)- (11)) by means of an elementary quantifier elimination technique. Section 3 gives the relevant properties and the bisecting-plane (BP) algorithm, which is proved to be globally convergent with certainty (Theorem I) , whilst the interval procedure Unfeasibility to be used by the BP algorithm is described in section 4. An illustrative example is presented in section 5. Perspectives and concluding remarks are included in the last section.

1.1. Notation

We denote by IRI and N the set of reals and of positive integers, respectively. The closure and the interior of a set I are denoted by clo(I) and int(I). A homogeneous polynomial is denoted by a capital letter with its total degree placed between parentheses as a superscript, e.g. P(IIl(h). Ilhll denotes the Euclidean norm of a vector h in IRI", whereas Ihl denotes the vector whose components are the absolute values of the components of h.

2. Minimum-time trajectory planning

Consider a mechanical manipulator with In revolute or prismatic joints and let qi, i = I , ... , m, denote the joint variables (as an example see the six-joint robot in figure I). These are packed together into the joint variable vector q = (ql , " " qm) T that spans in Q the joint-space work envelope. Given s interspaced location points of the tool frame Cartesian path, these are mapped, by the application of the inverse kinematics, into s point vectors of Q, formally denoted as qi = (qi , . . . , q;II)T , iE {0 , 2, 3 ... ,n - 2, n} , n:=s+ I. With this notation, q~ is the displacement of the kth joint at the ith knot. Hence, the sequences of displace-

. . . k . b {o 2 3 11 - 2 1I } (k I ) E h f ments at a gIven JOlllt are gIven y qk, qkl qkl . .. , qk , qk = , . .. ,m. ac 0

these sequences will be exactly interpolated by cubic polynomials which have to ensure an overall continuity of position (displacement) , velocity, and acceleration (Lin et al. 1983). In this context, assigning the joint velocity and acceleration for the first and last location (at knot ° and knot n, respectively) implies that two free displacement vectors q I and q"- I have to be inserted in the second and penultimate

. . . T locations (at knot I and knot n - I , respectively). By v' := (Vii, .. . , v;n ) and ai

:= (a\ , . .. , a;n) T, we denote the vectors of joint velocities and of accelerations at the ith knot, respectively . In defining the trajectory planning problem, velocity and acceleration vectors vo, v" and aO

, a" must be assigned . Denote by hi the elapsed time necessary for the ith spline qUt) to connect knot i-I to knot i for t E [0, hi] at the joint k (note that hi is independent of the considered joint k) . The vectorial function of the ith splines is defined as qi (I) := (qUt ), . . . , q;ll (t)) T. A convenient parametriza-

634 A. Piazzi and A . Visioli

tion of spline qHt) naturally incorporates continuity of positions and velocities (Craig 1989):

; ( ) ;- 1 ;- 1 [3 (; ;- 1) 1 (; 2 ;- 1)] 2 qk t = qk + Vk t + 2" qk - qk - -, Vk + Vk t h; 1;

+ [- 23(q~ - q~- I ) + ~(Vk + Vk- I )]t3 , tE [O ,h;] h; h;

The unknown parameters {vk, .. . , v;;- I, qk , qlrl} in each spline set can be determined by imposing the continuity of acceleration, i.e. solving the following system of n + 1 linear equations (k = 1, ... , m):

··1 (0) 0 qk = ak

ijk(h l ) = ij~(O)

(2)

It can easily be seen that once the h's have been fixed , the system in equations (2) d · . I ·" . d {o 2 11 - 2 n 0 11 0 II} (L· a mlts a unIque so utlOn lor any asslgne set qk , qb . .. , qk , qk , Vb Vb ab ak In

et at. 1983). The total travelling time required to perform the robotic task is evidently 2::;=1 hi, and the (globally) optimal trajectory planning problem with minimum-time criterion can be posed as follows:

subject to (i = 1, . .. ,n)

li:((t) I ::; jM "It E [O ,h; ]

lint)1 ::; aM "It E [0, hi]

l(dt)1 ::; VM "It E [O,h;]

(3)

where h := (hI , . .. , hn) T E IR/+n is the vector of spline times , and • ( ·1 .m)T ( I m)T d ( I m)T d h JM := lM ,· ·· , l M , aM := aM , · · · ,aM an VM:= VM , . .. ,VM enotet evec-tors of bound values for the jerk, acceleration, and velocity, respectively . The problem in equation (3), which is a semi-infinite optimization problem, can be transformed into a finite one by taking into account the following arguments. First of all, note that joint jerk i} ;(t) is independent of time and denote it by f (h) := (jl (h) , .. . , j~ (h)) T (whose components a re rational functions of vector argument h) . Hence, the first inequality Ii} ; (t) I ::; jM , "It E [0, hi] is simply substituted by

- jM ::; i(h) ::; jM

Also, joint acceleration functions qi(t) are affine functions of time, so that the semiinfinite inequality Iq; (t)1 ::; aM, "It E [O,h;] is equivalent to

- aM ::; a;- I (h) ::; aM

- aM::; a;(h) ::; aM


where ai (h) , a vector of rational functions , explicitly indicates its dependence on vector h. The last vector inequality constraint of equation (3) has to be pondered more carefully. By considering that qk( t) is a quadratic function of time, the semiinfinite inequality I q~( t) I :S v~ , Vt E [0, hi] can be equivalently substituted by the following set of inequalities :

where

v~+ (h):S v~ if aC I(h) > Oanda~ (h ) < 0

- v~ :S v~- (h ) if a~- I (h ) < Oandak(h) > 0

i+ ·i ak

(i- I (h) )

vk (h) := q k i- I) i ( ) hi ak (h - ak h (4)

(5)

The introduced v~+ (h ), vC (h) are the global maximum and minimum of the velocity over [0, hi ], respectively, provided that the endpoint accelerations a~- I (h) , a~(h) have discordant signs. Note that f(h) , ai(h) , vi(h), and vi+(h) := ( v~+ (h ), . .. , v;;; (h»T , vi- (h) := (v\- (h) , ... , v~~(h ) )T are given as explicit vector functions of spline times; they are well-defined vector rational functions over ~+I/ , to be determined by solving the tridiagonal linear systems in equation (2) . Finally, we obtain this equivalent finite optimization problem:

subject to

-aM :S a'(h) :S aM i = I , .. . , n - 1

(6)

(7)

(8)

(9)

vi+(h) :S VM if ai- I(h) > Oandai(h) < 0 i = I , . .. , n (10)

- VM :S vi- (h) if ai- I (h) < Oandai(h) > 0 i = I , . . . , n (11 )

Remark 1: All the vector inequalities in equations (7- 11) have to be interpreted componentwise. In particular, the conditioned inequality in equation (10), for a given i, is equivalent to


Note that if a~- I (h)aUh) :2: 0 the corresponding conditioned inequality has to be considered satisfied regardless of the actual value of vit(h). Analogous considerations can apply to the constraints in equation (II).

Remark 2: The novelty of the above optimization problem with respect to standard finite optimization is the presence of the 'conditional' constraints in equations (10) and (II) which depend on logical connectives. From a geometrical viewpoint this novelty is a formal one because the boundary of the feasible set in ~+" is given again by piecewise algebraic surfaces as in the case of standard finite optimization .

The complete data set of the optimization problem in equation (6)- (11) can be displayed as follows:

qO, va , aO (displacement, velocity, and acceleration vectors at the knot 0)

2 3 ,, - 2 ( f d· I q ,q , . . . , q vectors 0 ISP acements at the intermediate knots)

q" , v", a" (displacement, velocity, and acceleration vectors at the knot n)

v M , aM , jM (vectors of absolute bounds for velocity, acceleration , and jerk)

( 12)

The actual values in the data set (12) are arbitrary with the sole compatibility requirement given by Ivol :::; VM , Iv"l :::; VM and lao l :::; aM , lan l :::; aM . In our framework, solving the minimum-time trajectory problem implies finding a global minimizer h* = (h~ , . .. , h;, r corresponding to the global minimum T * = 2:7=1 h7 of problem in equations (6)- (11).

3. The bisecting-plane algorithm

The following definition introduces the feasible set F of equations (6- 11) which is not empty for any given data set (\ 2) .

Definition 1: The feasible set in ~+n is denoted by F := {h E ~+n such that h satisfies equations (7)-(11)}.

Assumption 1: Assume that the values of the initial and final velocity and acceleration are fixed to zero (vo = v" = 0, aO = a" = 0).

By virtue of Assumption I , a strong result regarding the structure of F is given by the following property.

Property 1 (conical parametrization of F): Denote by £ := {h E ~+Il : Ilhll = I} the set of all the unit-vectors in ~+n . Then there exists a scalar function r : £ ~ ~+ , e ~ r( e) such that F = {h E ~+11 : h = Ar( e )e , A :2: 1, e E £} .

Proof: For notational simplicity the proof is provided with m = I for the general m-joint case which is immediately attainable. In this case, all the vectors of equations (7- 11) are indeed scalar functions , so that F becomes

F ={h E ~+11 : lai(h)1 :::; aM , Ivi(h) 1 :::; vM , i = I , ... , n - 1;

1/(h)1 :::;iM , / + (h) :::; VM if {d- I(h) > OA ai(h) < O},

/ - (h) :2: -VM if{d- I (h) < 0 A ai(h) > O}, i = I , ... , n; } (13)

M echanical manipulators planned by interval analysis 637

By scrutiny of equations (I) and (2) the structures of functions / (h), ai (h), and / (h) can be deduced:

· d p (3 I1 - 3) (h) + ... + d p (3 I1 - 3) (h) I(h) I II S IS

V = h . D (311 - 3) (h) 1+ 1

( 14)

· d Q (311 - 3) (h) + . . . + d Q (3n- 3) (h) I (h) I II S IS

a = h2 D (3 n- 3) (h) . H I

( 15)

· d R (3n- 3) (h) + .. . + d R (3n- 3) (h) ·1 (h) _ I II S IS

J - htD (3n- 3)(h ) ( 16)

where D (3 n- 3)(h ) is the determinant of the coefficient matrix of equation (2) and p ;i

n-

3) (h), ... , R ;; n- 3) (h ) are other homogeneous polynomials, all with degree

equal to 3(n - 1) . Moreover, taking into account the definitions in equations (4) and (5) and equations (14) and (15), we deduce

. F (12n- 6) (h· d) H(h) ' (17)

v = hiD (3n- 3) (h )E (9n- 3) (h; d)

. H (12n- 6) (h· d) 1- (h) ,

v = hiD (3n- 3) (h )G (9n-3 )(h ; d) (18)

where d =(dl , . . . , d s ) and E (9n- 3)(h ; d ), F (12n- 6)(h ; d ), d 9n- 3)(h;d), and H (l2n- 6) (h; d) are homogeneous h-polynomials with coefficients depending on d. For every e E E define f(e) := b E ~+ : ,e E F} and

r (e) := min b } I'Er(e)

( 19)

Function r (e) is well defined because F is a closed set and f (e) is not empty for any given e. Indeed, by virtue of the properties of homogeneous polynomials, we infer that

and

. 1 . v' h e) = - Vi ( e ) , , . 1 .

i h e) = 3i(e) , (20)

(21 )

So that for a sufficiently high " surely , e belongs to F. Define the set F e as

F e := {h E ~+n : h = Ar(e)e, A ?: l , e E E} (22 )

First verify that F ~ F e, i.e. for any h E F then h E F e. Define e := h/ llhll and A := Ilhll / r(e). Clearly the following identity holds: h = Ar(e)e. Since r(e) :s Ilhll by equation (19) , we imply A?: 1. The proof is completed by showing that F e ~ F. Consider hE F e> i.e. h = Ar(e)e with A ?: 1 and e E E. By virtue of equation (19), the vector r(e)e belongs to F. The derived equations (20) and (21) imply that

and

1/(Ar(e)e)l:S 1/(r(e)e)l ,

VA?: 1,


I v+ (>\r(e)e) I S Iv+(r(e)e)l,

This means Ar(e)e E F , VA ~ 1. o

Remark 3: The conical parametrization of F provided does not imply that F is convex, but that F is certainly simply connected.

Given a positive parameter T and the associated hyperplane hi + ... + h" = T , con-sider the intersection of this hyperpla ne with ~+": .

T: = {h E ~+" : hi + ... + h" = T}

T is the (open) convex polyhedron defined by vertexes (T, 0, ... , O)T , . . . , (O,O, .. . ,T)T. A strong result which characterizes T as a special separating set in the search domain ~+" is the following .

Property 2: If the convex polyhedron T is not f easible, i. e. T n F = 0, then the negative half space hi + ... + h" < T is not f easible either. Formally , it holds that {h E ~+" : hi + ... + hll < T} n F = 0.

Proof: This property is an immediate consequence of Property I. Indeed suppose the absurdity that there exists a feasible point hE {h E ~+Il : hi + . . . + hll < T}. 1?efine e := h/ll hll an~ f = 1;1 + . . . + ';/1 < T. By virtue of Property I there exists A ~ I such that h = Ar(e)e. Determine the intersection of the line ray h = Ar(e)e with the hyperpla ne hi + ... + hll = T: Ar(e)e l + ... + Ar(e)e/l = T. It follows that A = T/( r(e)e l + ... + r(e )e,, ). Since ~ = f/( r(e)e l + ... + r(e)e,, ), it follows that A > i Therefore A > I , which in turn implies that Ar(e)e E T is feasible. This is the contradiction which ends the proof. 0

The special role played by the convex polyhedron T , as expressed by Property 2, is enforced considering that T is also a level curve of the objective function E;~ I hi' These facts suggest devising the following bisecting-plane (BP) algorithm to solve the problem in equations (6)- (11).

Input of the BP algorithm

(I) the data set (12);

(2) the precision parameter E: (> 0);

(3) the threshold parameter 'TJ (> 0) to be used by procedure Unfeasibility .

Output of the BP algorithm

(I) T+,T- respectively upper a nd lower bound of T* satisfying T+ - T- S E:;

(2) hf = (hfl , ... , hJ,,) T E F approximate globa l minimizer satisfying ",11 J + L.Ji= 1 1/; = T ;

The BP algorithm

(I) T - := 0.

(2) Determine a feasible point hf E F.

(3) To improve the current feasible point hf apply a local procedure to find (if possible) a better local minimizer hf E F and T+ := E;~ I/Yi '

(4) T:= T+ - E: .

Mechanical manipulators planned by interval analysis

(5) Apply procedure UnfeasibilitY(Tf , T , hI ' answer).

(6) If answer = 'Critical' apply procedure Criticalness(Tf, T, hI ' answer).

(7) If answer =' Unfeasible' then T - := T and terminate.

(8) T+ := L;~ I hk

(9) If T + - T - ::; E. then terminate.

(10) T: = (T+ + T- )/ 2.

(11) Apply procedure Unfeasibility (Tf , T , hI ' answer).

(12) If answer = 'Critical ' apply procedure Criticalness(Tf , T , hI ' answer).

(13) If answer = 'Unfeasible' then T - := T and and go to 9.

(14) Go to 3.

(IS) End.

639

At Step 2, this algorithm requires a feasible point of F to be found. The simplest method to accomplish this is to choose any point h E 1R/+11

• If h is not feasible , then scale this point h with a sufficiently high factor A to obtain hI = Ah. By virtue of Property I , hI has to be feasible . A local optimization procedure applied to equations (6)- (11) at Step 3 is used to improve the current upper bound of T * . To speed up the BP algorithm it is opportune to use a good local optimizer (such as, for example, the Ma tlab optimizer for constrained optimization (Grace 1994)) . The core of the algorithm is the procedure Unfeasibility which requires tha t positive real values are already assigned to its first two arguments. The aim of this procedure is to prove tha t the convex polyhedron T is completely unfeasible; in this case the output is given by answer =' Unfeasible'. If it is not possible to prove that T n F = 0 then two cases emerge:

(I) a feasible point hI has been found : then the procedure stops with answer = 'Feasible';

(2) no conclusion can be obtained (critical case): then the procedure has to halt with answer = 'Critical' .

The critical case of procedure Unfeasibility can appear when T is completely unfeasible but very close to the feasibility region F , or when T is unfeasible with the exception of isolated feasible points or of very tiny feasible regions embedded in T. In order to guarantee the overall global convergence of the BP algorithm, procedure Unfeasibility has to satisfy these properties (cf. subsection 4.2):

Property 3: For any given unfeasible convex polyhedron T , i.e. T n F = 0, there exists a sufficiently small Tf > 0 for which procedure Unfeasibility( Tf , T , hI , answer) converges within a finite number of steps with answer = 'Unfeasible' .

Property 4: For any given convex polyhedron T containing a non-empty feasible subset T n F , which is robust as an (n - I ) dimensional set, i. e. c1o(int(T n F )) = T n F (Luenberger, p . 369), there exists a sufficiently small Tf > 0 for which procedure UnfeasibilitY(Tf, T , hI , answer) converges within a finite number of steps with answer = 'Feasible' .

The next section shows how to construct a procedure satisfying Properties 3 and 4 by using interval analysis techniques coupled with an exhaustive global sea rch upon T.


The presentation of procedure Criticalness completes the exposition of the BP algorithm.

Procedure Criticalness

(I) p := 2.

(2) For i = I , .. . ,p - I

(a) ~:= T + i/ p· (c/2); rJ: = rJ/ 2. (b) Apply procedure Unfeasibility (rJ , ( , hf ' answer). (c) If answer = ' Unfeasible' then T := ~ and return. (d) If answer = 'Feasible' then return. (e) end of i-loop.

(3) p := p + I and go to 2.

(4) End.

The aim of procedure Criticalness lies in resolving with certainty the critical case which may emerge at Steps 5 and II of the BP algorithm.

Theorem 1: For any positive values of c and rJ, the BP algorithm converges with certainty and solves the optimization problem in equations (6)- (11) in accordance with the given output definition .

Proof: First we prove the correct behaviour of procedure Criticalness to be used at Steps 6 and 12 whenever answer = 'Critical' (critical case). This procedure has to converge in a finite number of itera tions at Step 2(c) with answer = ' Unfeasible' and ~ E (T, T + c/2) or at Step 2(d) with answer = 'Feasible' and a new, better feasible point hf' Introduce the set

I: = {~ E (T, T + c/2) : T n F = 0 or clo(inl(T n F )) = T n F} (23 )

Since the feasible set F is defined through rational function inequalities (7)--(11), the set I is given by the same interval (T, T + c/2) with the exception of a finite number of values:

(24)

for some u E N. Indeed a value ( E (T , T + c/2) does not belong to I if and only if it corresponds to a local tangency of the boundary of F , given by a piecewise algebraic surface, with the hyperplane h, + ... + h" = T. Therefore, the cyclic iterations of (values generated by procedure Criticalness necessarily determines C E I for some p* and i* positive integers. Thus, the same values C are generated indefinitely many times in correspondence with the sequences ik = k i* and P k = kp*, k E N with values of the threshold parameter rJ becoming smaller and smaller. By virtue of Properties 3 and 4, this proves that procedure Criticalness halts at Step 2(c) or 2(d) within a finite number of iterations.

The global convergence of the BP algorithm is now proved . The key result to be used is Property 2. Indeed this result demonstrates that if procedure UnfeasibilitY( rJ , T, hf , answer) converges with answer = ' Unfeasible', then T is a lower bound of T*, i.e . T ~ T*. This validates the assignments of Steps 7 and 13. In particular, if at Step 7 answer =' Unfeasible', then we have found a lower bound T- which is equal to T+ - c, or belongs to (T+ - C, T+ - c/2) in cases where the previous step was performed by procedure Criticalness. Hence inequality

Mechanical manipulators planned by interval analysis 64 1

T + - T - :::; c holds and the algorithm halts with the requested output. On the o ther hand, if the algori thm does not ha lt a t Step 7, it necessarily does ha lt at Step 9. This comes from the bisection mechanism issued at Steps 10, 13, and 14 which genera tes a sequence of converging lower and upper bounds T - and T +. 0

4. The interval procedure

The BP algorithm presented in the prevIous section provides correct resul ts rega rdless of the algorithmic method used to realize the procedure UnfeasibilitY(TJ , T , hi' answer). While it is conceivable to adopt genera l global optimization methods such as, for example, constrained Lipschitz optimiza tion (Thach and Tuy 1987) it is more viable to solve the posed unfeasibility problem over T by using in terva l analysis technjques. F irst a brief introduction to interval ana lysis is given, and then the devised interval procedure fo llows.

4.1. Interval analysis

Let I := {[a, b] : a, b E 1R1 , a :::; b} denote the set of rea l interva ls. The interval a rithmet ic defined over I is given by:

[a, b] + [c, d] := [a + c, b + d] (25 )

[a ,b]- [c , d] := [a - d ,b - c] (26)

[a,b] · [c , d]:= [min {ac ,ad ,bc ,bd} , max {ac ,ad,bc,bd} ] (27)

[a, bl/[c, d] := [a , b] . [1/ d, 1/ c] if o ~ [c, d] (28)

Regarding the algebra ic structure, the mathematica l systems {I , +} and {I , .} are commuta ti ve semigroups contai njng the 'un its' [0, 0] and [I , I ], respectively. It is noteworthy to cite the subdistributivity property, if A , B, e E l , then

A(B +C)~AB + AC

and the monotonic inclusion, if A, B , C, D E I with A ~ C and B ~ D then

A * B ~C* D

(29)

(30)

with * denoting any of the arithmetic opera tors +, -,. and / (the additional hypothesis for the case / is Ort. D). Let X E I" be a multidimensional interval (box) of 1R1", i.e. X = [x"xilx ... x[x;;-,x~]. Denote by I (X):={ Y E I" : Y ~ X} and w(X) := maxi=J, .. ,,,{xi - xi} respectively the set of a ll subboxes of X and the 'width ', o r the measure, of X . Consider f : X ---- 1R1, a rea l scala r funct ionf(x) defined over the box X; for any Y E I (X) define by f( Y) := {f(x) : X E Y} the range off over Y .

Definition 2: A function F: I (X) ---- I is an inclusion function of f (x) if

( I) f ( Y) ~ F ( Y ) \f Y E I (X );

(2) lim ..,( Y) --.o w(F( Y)) = O.

The concept of inclusion function is central in the development of interva l ana lys is and the use of it characterizes the so-called ' in terva l a lgorithms', especia ll y in deterministic global optimization (Ratschek and Rokne 1988, Hansen 1992). There a re a


large variety of inclusion functions: natural interval extensions, standard centred forms , meanvalue forms, Taylor forms, Cornelius- Lohner forms, etc. (Ratschek and Rokne 1984). The simplest inclusion function is the natural interval extension, which can be obtained by substituting, the requested box argument Y in the expression of a given f(x) , and then performing the necessary interval computations. For example, given f(x) = (1 + 3x2

) sin (7x) , its natural interval extension F(Y) with Y = [-1 , 2] can be computed as follows: F( Y ) = (I + 3[-1 , 2f) sin (7[-1 , 2]) = (I + 3[0, 4]) sin ([-7, 14]) = (1 + [0 , 12])[-1 , I] = [1 , 13][-1 , I] = [~13 , 13] . In the following, scalar functions are denoted by lower-case letters, whereas inclusion functions are denoted by corresponding upper-case letters (e.g. f(x) and F(X)).

4.2. The procedure Unfeasibility

The following assumption is considered to guarantee that the problem in equations (6)-(11) admits a global solution (it avoids degenerate situations such as qO = qn = q; = constant, i = 2, ... , n - 2 for which obviously there is no solution to equations (6)-(11)) .

Assumption 2: A sufficiently small v > 0 is known such that

~~n := {h E ~n : h; ~ v, i = I , ... , n} :2 F (31 )

To simplify the subsequent development, write all the constraints in equations (7)(11) by introducing the truth-value functions b; : ~+n --+ {'true' , 'false'} , h --+ b;(h) defined as follows:

b;(h):= {- c;::; c;(h) /\ c;(h) ::; c; } , i= I , . .. ,(3n-2)m

b;(h) := {y ;(h)z; (h) ~ O} V {y; (h) > 0 /\ z;(h) < 0 /\ cui(h) ::; c;}

V {y;(h) < 0/\ z;(h) > 0/\ - c; ::; cd,(h)}

i= (3n-2)m+ 1, . . . , (4n-2)m

(32)

(33)

where the introduced functions can be unambiguously identified from equations (7)-(11). By virtue of Assumption 2, the convex polyhedron T can be substituted by T v:

Tv := {h E ~n : hI + ... + hn = T, h; ~ v, i = 1, . .. ,n} (34)

The procedure Unfeasibility aims to prove one of these statements:

(I) For any hE T v there exists an integer ih (which depends on h) such that b;Jh) = 'false'; output argument answer := 'Unfeasible' .

(2) There exists hr E Tv such that b;(hf ) = ' true', i = 1, . .. , (4n - 2)m; output arguments answer := 'Feasible' and hf.

If it is not possible to establish (1) or (2), then

(3) the output argument answer is set to 'Criticaf (critical case).

The following notation is introduced . Denote a vector point in ~+(n- I ) by - T · h := (hI> . . . , hn _ l ) and mtroduce:


The above functions with domain in ~+(n- I ) are simply obtained by the corresponding constraints functions evaluated over the hyperplane hI + ... + hn = T. The multidimensional interval to be processed by the interval procedure is given by

A v := [v , T - (n - I )vr- I ~ ~n- I

A generic box H ~ ~II- I can explicitly be denoted by H := [hI , hi] x ... X [h;;-_ I, h:_ I]. Let the projection of H onto the hyperplane hI + ... + hn = T be denoted by

P {H) ,~ {h E ~ , b ~ (h" ... ,h"_,,T -~h,r, h E H} (35)

This projection has a useful property.

Property 5: Assume H ~ A v; the following statements hold

II - I

P(H) ~ T v if and only if T - L hi 2 v i= 1

n- I

P(H) n T v = 0 if and only if T - L hi < v i= 1

n- I

P(H) ~ T v if and only ifT - Lhi > 0 i= 1

Proof of the above property is omitted because of its immediate derivability.

Procedure Unfeasibility

(I) Insert A v into the initialized stack S ; set crit := O.

(2) If the stack S is empty then

(a) If crit = 0 then answer := ' Unfeasible' else answer := ' Critical'.

(36)

(37)

(38)

(39)


(b) Return.

(3) From S unstack into H.

(4) If T - I:;:i hi < v then go to 2 (box H is rejected) .

(5) If T- I:;:i hi < V/PI then go to 11.

(6) hf := (hi , . . . , h;;_I> T - I:::i hj) T; count := O. (7) For i = 1, . .. , (3n - 2)m do

(a) If bi(hf ) = ' true' then count := count + I and go to (c) . (b) If C\(H ; T) n [- Ci , cil = 0 then go to 2. (c) end of i-loop.

(8) For i = (3n - 2)m + 1, . .. , (4n - 2)m do

(a) If bi(hf ) = ' true' then count:= count + 1 and go to (d). (b) Denote by [y - ,/], [z - ,z+], [c;;, cn [cd, cdl the interval values of

Yi (H;T) , Z i(H; T) , ClI;(H ;T) , Cd;(H ;T) , respectively. (c) If {y- > 0 /\ z+ < 0 /\ Ci < c;; } V {y+ < 0 /\ z- > 0 /\ cd < -c;} then go

to 2. (d) end of i-loop.

(9) If count = (4n - 2)m then answer:= 'Feasible' and return.

(10) If w(H ) < Tj then crit := I and go to 2.

(II) Bisect, thus obtaining boxes VI and V 2, the box H on its maximum dimen-sion.

(12) H t-- VI and push V2 into S .

(13) Go to 4 .

(14) End.

The variable arguments to be passed to the procedure are Tj , the threshold parameter, and T , the positive constant associated with the hyperplane hI + ... , hn = T.

The arguments to be returned are answer, according to Statements (1) , (2) and (3) , and hf E :F for the case answer = 'Feasible'.

Remark 4: At Step 5, PI is any fixed integer such that PI ~ 2. This parameter implicitly defines the outer approximation of T v by means of the set union of all generated P (H) for which it has to hold P(H) ~ T (cf. statement (39) of Property 5). In other words, all the components of h E P(H) have to be strictly positive in order to correctly process P(H).

Theorem 2: The procedure Unfeasibility always converges. It satisfies Statements (1)- (3) and Properties 3 and 4.

Proof: First it is proved that procedure Unfeasibility always converges, satisfying Statements (1 )-(3). The procedure is based on an exhaustive search over the projection P (Av) which contains T v by virtue of equation (36). At Step 4, the current box H is rejected if H satisfies inequality (38): P (H ) is completely external to convex polyhedron T v. At Step 5 it is necessary to bisect H further when condition T - I::: i hi < v / PI is satisfied; otherwise the processed projection P (H ) would have points not belonging to T v, or even with a negative last component. Obviously the mechanism issued at Steps 6, 7(a), 8(a) and 9 guarantees that when answer is set to 'Feasible' a feasible point in T v has been found (hf). If the state-


ment at Step 7(b) or Step 8(c) is true, then by virtue of the definition of inclusion functions , for any h E P (H) '* bi(h) = 'false' '* h ~ F. This dictates that H is discarded and the next instruction is at Step 2. When the stack S is empty and crit = 0, it means that all the area T v has been examined and proved to be unfeasible: then answer := ' Unfeasible' and the procedure terminates . If the interval procedure does not converge at Step 2 with crit = 0 so that answer = 'Unfeasible' or at Step 9 with answer = 'Feasible', then it necessarily converges at Step 2 with crit = 1 so that answer = 'Critical'. This can be ascertained by examination of the exhaustive procedure implemented: boxes are inserted and extracted at the top of the stack S according to a 'depth-first' strategy. Indeed if the procedure does not terminate at Step 2 with crit = 0 or at Step 9, the given starting box is bisected over and over again until the current box H is arbitrarily small (cf. Step 11). Hence, condition w(H ) < TJ is eventually verified for any given positive value of threshold TJ and variable crit is set to 1. Subsequently, by virtue of the exhaustive search over A v , if the condition at step 9 is never verified , the procedure necessarily clears out all the boxes of stack S and terminates with answer = 'Critical ' .

Now it will be proved that the exposed procedure satisfies Property 3. Suppose that T be unfeasible. Then for any h E T there exists an integer i(h) which depends on h such that bi(h) (h) = 'false' . Now introduce the function 8: T --+ ~+ , h --+ 8(h) defined as follows: If i(h) E {I , .. . , (3n - 2)m}

then 8(h) := ICi(h) (h) I - Ci(h)

else (it holds i(h) E { (3n - 2)m + I, . . . , (4n - 2)m} ) If {Yi(h) (h) > 0 1\ Z;(h) (h) < 0 1\ CUI(h) (h) > Ci(h)}

then 8(h) := min {Y;(h) (h) , - z;(h) (h) , CUI(h) (h) - C;(h)}

else 8(h) := min{ - Y;(h)(h), Z;(h) (h) , -C;(h) - Cdl(h) (h)}

endif endif.

Define 8* := minhET 8(h) which is a positive value. Hence for any h E T there exists a violated constraint, with the appropriate index i, such that

or c·(h) < -c· - 8* 1 · _ 1

or y;(h) ~ 8* 1\ z;(h) :::; - 8* 1\ C UI (h) ~ c; + 8*

or

y;(h) :::; -8* 1\ z;(h) ~ 8* 1\ - c; - 8* ~ cd,(h)

By considering the continuity over ~+n of all the functions involved in the constraints, we infer there exists a small .;, > 0 and a constraint with index i such that for any box H ~ A v with w(H) < .;, 1\ P(H ) ~ T, one of the following statements is satisfied:


( 40)

or

(41 )

or

min {y;(h)} 2 8*/ 2 A max {z;(h)} ::; -8*/ 2 A min {cu(h)} 2 c;+8*/2 (42 ) hEP {H) hEP {H) hEP {H) , .

or

max {y;(h)} ::;-8*/2 A min {z;(h)} 28*/2 A-C; -8*/22 max {cd(h)} (43) hEP {H) hEP {H) hEP(H) ,

Moreover, by virtue of the limit property of inclusion functions , there a lways exists a small 6 > 0 such that for any box H ~ A I.' with w(H) < 6 , we simultaneously have (see notation introduced at Step 8(b) and [c- , c+] := C;(H; T))

min {c;(h)} - c- < 8* / 2 (44) hEP(H)

c+ - max {c;(h)} < 8*/2 hEP {H)

min {y;(h)} - y- < 8* / 2 hEP {H)

/ - max {Yi(h)} < 8* / 2 hEP{H)

min {z;(h)} - z- < 8* / 2 hEP(H)

z+ - max {zi(h)} < 8* / 2 hEP {H)

min {cu(h)} - c;; < 8*/2 hEP(H) ,

cd - max {Cd (h)} < 8* / 2 hEP {H) ,

Therefore inequa lities (40) and (44) imply that

or, considering inequa lities (41 ) and (45) ,

c+ < -Ci

or, considering inequa lities (42), (46) , (49) , and (50),

or, fro m inequalities (43), (47), (48), and (5 1),

y+ < 0 A z- > 0 A cd < -Ci

( 45)

( 46)

( 47)

( 48)

(49)

(50)

(51 )

(52)

(53)

(54)

(55)

This permits the concl usion that by choosi ng a positive 'T/ < min {~1 , 6}, every sufficiently sma ll box generated by the procedure is eventua ll y discarded by the acti vation of conditions (52) or (53) at Step 7(b), o r conditions (54) o r (55) at Step 8(c). Therefore, the exhaustive na ture of the procedure implies that the stack S finally


becomes empty within a finite number of iterations without modifying the zero value of the ' flag ' variable crit. This concludes the proof of Property 3.

Finally, Property 4 is verified. Procedure Unfeasibility discards boxes with projection on T when they are proved to be unfeasible (cf. Steps 7(b) and 8(c)) or when they have widths smaller than TJ , (cf. Step 10). This guarantees convergence within a finite number of iterations. Suppose that T n F is not empty and is as robust as an (n - I )-dimensional set. Hence, for a sufficiently small TJ the procedure necessarily generates a box H such that P (H ) ~ T n F. Indeed for any box H with P (H) I); Tn F it must happen that: (i) it is discarded by proving that P(H ) is not feasib le, or (ii) it is discarded by setting crit = I , or (iii) it is bisected over and over until all the regions of H are discarded through (i) and (ii) or a subbox of H is found with its projection completely contained in T n F . As a consequence

(

11 - 1 ) P(H) ~ Tn F =} hI := hi, .. . ,h-;;_I, T - L hi E F

1= 1

so that the condition at Step lOis verified and the procedure terminates with answer = ' Feasible'. o Remark 5: To find a value for v to be used in the procedure Unfeasibility, in practice we can choose any sufficiently small positive value compatible with computation precision, for example v = E:.

5. An illustrative example

The previous bisecting-plane algorithm has been implemented in C ++ language exploiting the PROFIL/BIAS libraries made by Kriuppel (1993 a , b) and using the Matlab optimization toolbox as local optimizer (Grace 1994). As an illustrative example we considered the case of a trajectory composed of five splines of a six degrees-of-freedom robot manipulator, like the one shown in figure 1, in which the maximum limits of velocity, acceleration , and jerk are the same for each joint and fixed as vt, = 50 degrees/s, at, = 50 degrees/s2 and Jt, = 60 degreesjs3, k = 1, ... , 6. The starting, intermediate, and final joint positions are indicated in table 1. We set the precision parameter E = 0.0 I and the other parameters TJ and v to 0.00 I and 0.0 I, respectively. The BP algorithm has been initialized with the starting feasible point 11;-. = (15 , IS , IS , IS , 15)T. The optimal travelling time obtained is 12.53s, with corresponding spline times as listed in table 2. The plots of the velocities,

Knot (degrees) Jo int

number 0 2 3 4 5

I 40 120 -30 10 2 75 - 10 - 120 - 40 3 10 Extra 40 70 Extra 170 4 - 30 kno t 40 10 knot -80 5 100 120 180 10 6 100 10 50 - 5

Table I . The data knots of the example .


2 3 4 5

Spline time (s) 0.80 2.10 4.62 4.45 0.56

Table 2. The optimal spline times.

2 3 4 5

SpLine time (s) 0.83 2.00 4.72 7.37 0.40

Table 3. The spline times obtained with the Matlab optimizer.

accelerations, and jerks of the six joints are shown in figures 2- 7. Note how the constraints are active for joint I Uerk in the first spline and velocity in the third), joint 5 (velocity in the fourth spline, jerk in the last one) and joint 6 Uerk in the first spline). It is worth stressing that the Matlab optimizer, starting from the feasible point (15 , 15, 15 , 15, 15)T, converges to the feasible local minimum with a total time 15.32 s, which is not the required global minimum (see the corresponding spline times in table 3). Even starting from better initial points, such as hi = (4, 4 , 4,4, 4)T, the Matlab optimizer converges to the same result. This definite improvement (of 18 %) of the global minimum over the local minimum emphasizes the potential importance

JOINT 1 60 ~~----,-------,--------,-------,-------,--------,-,

-20

-40 \ ;'

\ I . I

. ' . 1/ ..

I I

- velocity [deg/sl - acceleration [deg/sA21

jerk [deg/sA31

_60L-------L-------L-------L-------L-------L-------~

o 2 4 6 8 10 12 time [sl

Figure 2. The optimal trajectory of joint I for the example.


JOINT 2 60 r-------r-------r-------r-------r-------r-------~

40

20

o

-20

-40

\ : . I \ I

I I

I

I

I I

I

- velocity [deg/s) - . - acceleration [deg/sI\2)

jerk [deg/sI\3)

I , ,/

I I

I

_60 L-------L-------L-------L-------L-------L-------~

o 2 4 6 8 10 12 time [s)

Figure 3. The optimal trajectory of joint 2 for the example.

JOINT 3 60r-------.-------,--------r-------,-------,--------~

40

20

I I

0 '

-20

- 40

I

I : ' I , ,

, , , , , ,

--,

- velocity [deg/s) _ . - acceleration [deg/sI\2)

jerk [deg/sI\3)

I 'd

I I

I

_60 L-------L-------L-------L-------L-------L-------~

o 2 4 6 time [s)

8 10


12

649


JOINT 4 60 .-------.-------.--------.-------.-------,,-------,-,

40

20

o

-20

- 40

.\ 1\

I \

I \ \ \

\ \ \

\ \

\ \

\ \

\ \

\ \- -

- velocity [deg/s] - - acceleration [deglsA2]

jerk [deg/sA3]

\ \

-60 L-------~------~--------~------~------~--------~~

o 2 4 6 8 10 12 time [s]


JOINT 5 60 .-------,-------,--------,-------.--------,-------,-,

40

20

o

-40 - velocity [deg/s] - - acceleration [deg/sA2]

jerk [deg/sA3]

-60 L-------~------~--------~------~------~L-------~~

o 2 4 6 8 10 12 time [s]



JOINT6 60 .-------.--------.--------.--------,-------,--------,-,

40

20

i i

i

,

I

, ,

, ,

- velocity [deg/s] - - acceleration [deg/sA2]

jerk [deg/sA3]

-\ \

\ \

\

-60 ~~----~------~--------~------~------~--------~~ o 2 4 6 8 10 12

time [5]


651

in an automated industrial environment and in space robotics of searching for the global minimum time.

6. Conclusions

A global optimizer, using an interval procedure, was devised for the optimal minimum-time trajectory generation of an m-joint manipulator. The joint space scheme adopted employs cubic splines to ensure continuity of displacements, velocities, and accelerations under constraints on velocities, accelerations, and jerks. The key result, that has led to proof of the global convergence of the proposed algorithmic method of cutting planes in the unfeasible domain, is the conical parametrization of the feasible set F (Property I) . This was derived under the assumption of zero velocities and accelerations in the first and last knot. If we assume as a conjecture that F is connected in the general case (i.e. arbitrary given velocities and accelerations are assigned in the extreme knots) , then the BP algorithm would provide a global minimum-time solution again. This conjecture is an open question which needs further investigation.

Even though the minimum-time problem addressed is evidently NP-hard from the computational complexity viewpoint, the proposed BP algorithm can be of practical use in the planning of mechanical manipulators in highly automated environments.

652 Mechanical manipulators planned by interval analysis

Acknowledgments

Partial support for this research has been provided by MURST scientific research funds and by the ASI (Italian Space Agency) .

References

BOBROW, 1. , 1988, Optimal robot path planning using the minimum time criterion, IEEE Journal of Robotics and Automation, 4, 174-185.

CRAIG, 1., 1989, Introduction to Robotics: Mechanics and Control (New York: AddisonWesley).

GRACE, A. , 1994, Matlab Optimization Toolbox- User's Guide (Natick, MA: The Mathworks Inc.).

HANSEN, E ., 1992, Global Optimizat ion Using l!uerval Analysis (New York: Marcel Dekker). lAULIN, L. , and WALTER, E. , 1996, Guaranteed tuning with application to robust control a nd

motion planning. Automatica, 32, 1217- 1221. KNUPPEL, 0. , 1993a, BIAS- basic interval arithmetic subroutines. Technical Report, Tech

nische Universitiit Hamburg, Hamburg, Germany. KNUPPEL , 0. , 1993b, PROFIL- programmer's runtime optimization fast interval library .

Technical Report, Technische Universitiit Hamburg, Hamburg, Germany. KYRIAKOPOULOS, K. , and SARIDIS, G., 1988, Minimum jerk path generation. Proceedings of

the IEEE International Conference on Robotics and Automation, Philadelphia , PA, pp. 364-369.

LIN, c. , CHANG, P. , and LUH, 1., 1983, Formulation and optimization of cubic polynomial joint trajectories for industrial robots. IEEE Transactions on Automatic Control, 23, 1066-1074.

LUENBERGER, D. , 1989, Linear and Non-linear Programming (Reading, MA: AddisonWesley) .

MALAN, S. , MILANESE, M ., and TARAGNA, M. , 1997, Robust analysis and design of control systems using interval arithmetic. AUlomatica, 33, 1363- 1372.

MOORE, R. , 1979, Methods and Applications of Interval Analysis (philadelphia, PA: SIAM Press).

PIAZZI , A. , and MARRO, G. , 1996, Robust stability using interval analysis. International Journal of Systems Science, 27, 1381- 1390.

PIAZZI , A ., and VISIOLI, A., 1997, A global optimization approach to trajectory planning for industrial robots. Proceedings of the 10th IEEE-RSJ International Conference on Intelligent Robots and Systems, Grenoble, pp. 1553- 1559.

RATSCHEK, H ., and ROKNE, 1., 1984, Computer Methods for the Range of Functions (Chichester: Ellis Horwood).

RATSCHEK, H., and R OKNE, 1. , 1988, New Computer M ethods for Global Optimization (Chichester: Ellis Horwood).

SHILLER, Z ., and DUBOWSKI , S. , 1988, Robot path planning with obstacles, actuator, gripper, and payload constraints . International Journal of Robotics Research, 8,3- 18 .

SHIN, K. , and McKAY, N. D. , 1986, A dynamic programming approach to trajectory planning of robotic manipulators. IEEE Transactions on Automatic Control, 31, 491- 500.

SIMON, D. , 1993, The application of neural networks to optimal robot trajectory planning. Robotics and Autonomous Systems, 11 , 23- 34.

TAN, H. , and POT, R ., 1988, Minimum time trajectory planner for the discrete dynamic robot model with dynamic constraints. IEEE Journal of Robotics and Automation, 4, 174-185.

THACH, P. , and Tuy, H ., 1987, Global optimization under Lipschitzian constraints. Japan Journal of Applied Mathematics, 4, 205- 217.

WALTER, E., and lAULIN, L. , 1994, Guaranteed characterization of sta bility domains via set inversion. IEEE Trans. Automatic Control, 39, 886-889.

Wu, C., 1994, A numerical approach for time-optimal path-planning of kinematically redundant manipulators. Robotica, 12, 401-410.

Global minimum-time trajectory planning of mechanical ...

Documents

Transcript of Global minimum-time trajectory planning of mechanical ...