Collision-avoidance trajectory planning using tube concept: Analysis and simulation

Collision-Avoidance Trajectory Planning Using Tube Concept: Analysis and Simulation* Suk-Hwan Suh Department of Industrial Engineering POSTECH, Pohang, Korea 790-600

Albert B. Bishop Department of lndustrial and Systems Engineering The Ohio Stare University, Columbus, Ohio 43210 Received December 17, 1987; accepted August 2, 1988

The concept of a tube is introduced and is applied to the solving of the collision- avoidance, minimum-time trajectory planning problem. A collision-free space is represented by an articulated tube with parameters of reference points and path tolerances. For obstacle avoidance, the end effector is constrained to move inside of the tube. An algorithm which will find suboptimal solutions for optimizing both path and velocity history of the trajectory by the use of piecewise joint-space polynomials is presented. This algorithm exploits the robot arm dynamics in realistic environments where obstacles are present and the minimization of task time is desired with smooth motion. Experimental results show that as the path tolerance increases the new algorithm takes advantage of the spatial freedom to provide solutions superior to conventional approaches and to methods based on predefined paths.

*Part of the material in this article was presented at the 1987 IEEE International Conference on Systems, Man, and Cybernetics, Alexandria, VA October 1987. All correspondence regarding this article should be addressed to S. H. Suh.

Journal of Robotic Systems, 5(6), 497-525 (1988). @ 1988 by John Wiley & Sons, Inc. CCC 0741 -2223/88/060497-29$4.00

498 Journal of Robotic Systems-I 988

1. INTRODUCTION

For the efficient use of industrial robots, minimum-time trajectory planning in automated manufacturing systems is very important. Minimum-time tra- jectories can lead to the reduction of the cycle times of the task, and hence improve productivity. However, current commercial robots are often too slow to justify their use economically for many applications especially when the robot operates in crowded work environments. One way to combat this problem can be to increase the speed of the robot by fully utilizing its potential capability while avoiding collisions.

Minimum-time trajectory planning for industrial robots has long been known to be a complicated problem. The complexities arise mainly from the fact that the robot itself is a highly-coupled nonlinear system, but the physical (actuator limits, joint limits, etc.) and environmental (danger of collision) constraints imposed on the motion of the robots can be very strict. Further- more, the algorithm to be used should be implementable without requiring too much computation for online control.

Because of the inherent complexities of the problem, previous works in this area have limited the scope of the problem in various ways. Most commonly, they have assumed that the workspace of the robot is free of obstacles 1-5 or a collision-free path is precisely given by a task planner. Bobrow et al.6*7 and Shin et aL8 have developed an algorithm for finding optimal velocity history along a predefined path. By the proper selection of a collision-free path, their method can be used for an industrial environment where obstacles are present. However, the global optimality of their solution cannot be claimed unless all the possible paths are evaluated, which is computationally intractable. This argument is true regardless of the existence of obstacles in the workspace of the robot. The point is that there must be a mechanism for selecting the optimal path at the task-planning stage.

Typically, minimization of total distance travelled in either Cartesian space or joint space has been widely used as a criterion for selecting optimal paths. Although solution of this problem, called the minimum-distance path problem, has been attempted by many researchers*" general solutions under the existence of obstacles have not been found. Recognizing the difficulties of the spatial planning problem, many efforts have been made to find a collision-free path instead of the minimum-distance path.'"'' Nonetheless, it is hard to accept the minimum-distance path as an optimal path since the minimum- distance path is not necessarily the minimum-time path.

An alternative approach for solving the minimum-time trajectory problem with a collision-avoidance constraint presented in this article is based on

Suh and Bishop: Collision-Avoidance Trajectory 499

engineering insight. We have represented the feasible space (collision-free space) by an articulated tube based on the idea of a conventional path- planning method where a piecewise straight-line path is chosen. The tube is parameterized with a set of comer points and variable tube diameters, and the feasible path of the trajectory is constrained to be inside of the tube. By this scheme, the collision-avoidance problem can be considerably simplified. In addition, the performance of the optimal trajectory (i.e., time) is expected to be superior to previous methods which are based on a fixed path since the inclusion of the tube provides considerably more spatial freedom for the trajectory optimizer.

However, there still exists the problem of finding the optimal path in order to use Bobrow and Shin’s algorithm. Instead of pursuing true optimality for both path and velocity planning separately, our method finds a suboptimal solution by the use of joint-space polynomial as a trajectory primitive. In the polynomial trajectory, both position (thus path) and velocity are defined simultaneously, and hence the optimization for both path and velocity history can be made in a unified manner.

For the optimization of the polynomial trajectory, we developed a two-phase algorithm consisting of a branch-and-bound search followed by a gradient search based on the time scaling property and a IocaIization method for the construction of the polynomial trajectories. By imposing boundary conditions at every knot point so that the continuity of acceleration is ensured, the smoothness of motion can be achieved. Thus, the output of our trajectory optimizer can be used for the most realistic environment where obstacles exist but the minimization of the task execution time is desired with smooth motion (which has been the primary concern of this research).

Our secondary concern has been to investigate the effect of spatial freedom to the performance of the minimum-time trajectory. Intuitively, more spatial freedom will result in lower minimum times. The investigation will be made by comparing the minimum time from optimizing only the velocity history for a predefined path with minimum times from our algorithm under various sizes of the spatial freedom (tube diameters) with respect to the predefined path.

The remainder of this article is organized as follows. Section I1 introduces the tube concept and formulates the minimum time trajectory problem. Section I11 is dedicated to the development of an algorithm for solving the minimum-time trajectory problem. Fundamental properties involving the time scaling property, which is used as the basis for the development of the algorithm, are presented. Then, a two-phase algorithm consisting of a branch- and-bound search and a gradient method is presented. Section IV shows results of experiments based on the presented algorithm, and compares them with conventional and other existing methods. Section V concludes the article.

II. FORMULATION OF THE PROBLEM

In this section, the minimum-time trajectory problem is formulated. The main constraints included are: path constraint, actuator constraint, and motion

500 Journal of Robotic Systems-1 988

constraint. The path constraint is for the avoidance of collision with obstacles existing in the workspace of the robot. Collision-free space (feasible space) is represented as an articulated tube in which the travel of the end effector is constrained. The actuator constraint is due to the torque (or force) limit of the joint actuator. The motion constraint is for the smoothness of motion throughout the travelling time. Smoothness of motion is defined as motion for which the governing trajectory is everywhere continuous through its second derivatives. Other constraints considered are the limits in the angular position and velocity of the joints.

A. TubeConcept

In an industrial environment where the task is performed under the exis- tence of obstacles, a desired path is programmed by a task planner. A common method for obtaining a desired path is to program several points through which the end effector of the robot is to pass. These points, represent- ing the position and orientation of the end effector, are chosen in such a manner that the resulting path, consisting of piecewise straight lines by connecting these points, can avoid collisions with the existing obstacles. In reality, however, some deviations from this path may not cause any

collision. The magnitude of the allowable deviation would depend on the configuration and location of obstacles along the path. In other words, the desired path is just one possible path avoiding collision, and at best it is thought of as a reference path. In this context, the desired path is referred to as the reference path, and the comer points forming the desired path are referred to as reference points. In the sequel, collision free space is defined as feasible space.

The feasible space of the robot hand can be derived by the reference path and maximum deviations from it (path tolerances). Conceptually, the feasible space can be represented as a tube with axial symmetry around the reference path, and the cross sectional radius as path tolerance. For instance, the feasible space shown in Figure 1 can be represented as:

Epeme 1. Conceptual representation of feasible space.

Suh and Bishop: Collision-Avoidance Trajectory 50 1

where R i . i = I , . . . , N are reference points, and E,, j = 1,. . . , N - 1 are the path tolerances, and N denotes the number of reference points. Note that the configuration of the feasible space where two tubes are joined may have more complicated form than the way it is shown in Figure 1.

In general, the reference path defined in Cartesian space describes both the positional and rotational displacement of the robot hand along the path. For positional displacement, the reference path is simply a set of connecting straight lines describing the positional element of the end effector coordinate frame in terms of X, Y, 2 coordinates. Likewise, the rotational reference path can be defined by a set of straight lines in terms of screw angles along the rotational element of the path.

Let R i = [Ry, R!lT, i = 1 , . . . , N , c j = [ e j , 4 I T , j = 1 , . . . , N- 1, where superscript p( r ) indicates the positional(rotationa1) part of the reference point and path tolerance. Then the feasible space for positional displacement F P is:

P

N- 1

F P = U F: i = l

where

and V is any point in X, Y, 2 space. Note that )(Li(A)- V(l indicates positional distance between V and Li(A) where Li(A) is reference position at A .

Similarly, the rotational feasible space can be represented as:

N- I

F ' = U F ; i = l

(4)

where

and W is any coordinate frame rotated with respect to the world coordinate frame. Note that 11 H;'( .) . Wll indicates rotational distance between W and H i ( * ) , and RotfSi, si] is the rotational displacement from R; to R i + l , where S i and si denote the screw axis and screw angle required to reorient R I into R ;+I.

502 Journal of Robotic Systems-1988

Figure 2. Approximation of end-effector geometry by sphere.

The rotational feasible space expressed in Eqs. (4) through (7) is theoretic- ally legitimate with a given set of rotational path tolerances. However, the actual sweeping volume corresponding to the expression will be a very complicated form since it is a function of both the geometry and position of the robot end-effector. This will make the problem of finding the tube parameters extremely difficult. A more promising approach is to treat the end effector as a sphere shown in Figure 2. By the approximation, the rotational problem can be eliminated with the compensation of the radius of the sphere when the positional tube parameters are found. The approximation is justified from the fact that the minimum-time solution is mostly affected by the positional joints (upper arm), but not much by the orientational joints (forearm). Based on this argument, we will concern mainly on the positional motion of the robot.

Using the tube concept, the collision-avoidance problem can be easily solved by checking if the path of robot hand stays inside of the tube in the case that the joint angles of the manipulator can be determined uniquely by the position of its hand. In general, however, checking the path of robot hand is not sufficient for collision-avoidance of the manipulator as the space occupied by the manipulator depends on its joint angles and the solution of the inverse kinematic equation is not unique.* To deal with the above problem, additional parameter by which the joint angles of the manipulator can be defined uniquely most be specified. For such purpose, the sign of the joint angles may be used. In other words, we need the sign of the joint angles at every tube segment as well as the tube parameters to represent the collision-free space. In what follows, methods to find the tube parameters? are described.

Practically, a task planner could find approximate values of the tube parameters simply by leading the robot arm around the workspace and

*This was brought to our attention by an anonymous referee. ?Henceforth, tube parameters will include the sign of joint angles.

Suh and Bishop: Collision-Avoidance Trajectory 503

measuring the free space. Alternatively, a more precise tube parameters can be found by off-line programming method in which a simulated robot in graphic terminal is used. Or, one may develop a spatial planning algorithm for finding the tube parameters automatically. In this article, we assume that the tube parameters are determined by moving the real (teach-in method) or simulated (off -line programming method) robot and measuring the maximum deviation avoiding collisions with obstacles by any part of the robot. Note that the above assumption is realistic and provides a means to determine the sign of the joint angles. In other words, in the process of moving the real or simulated robot in the free space and measuring the maximum deviation from the reference path, the task planner can also determine the sign of the joint angles of desired configuration at every tube segment.

B. Trajectory Primitive

I n the feasible space, there is an infinite number of paths that connect the starting and destination points. Obviously, the reference path adopted for the conventional trajectory scheme is one of them. In such a scheme, the robot stops at the end of each line segment of the path. The resulting motion will, therefore, not be smooth, and the task execution time will not be the shortest possible time because of these unnecessary stops. To remove the unnecessary stops, the corner points have been replaced by smooth Taylor’s bounded deviation algorithm adds sufficient knot points at the Cartesian midpoints so that the maximum deviation between the Cartesian and joint space straight line path is less than a predefined path tolerance.” Figure 3 compares the paths implied in these schemes. Notice that there exists a rule to form the path in each of the previous schemes. Unless they are shown to be the optimal path(s), the rules may over-constrain the path in the minimum- time trajectory planning problem. The only necessary constraint in the for- mation of a path is keeping the path inside of the tube.

Bounded deviation method Arc comer method

. . . --- +++ Potentially superior path

Figure 3. Comparison of paths adopted in previous work.

504 Journal of Robotic Systems-1 988

In this article, the trajectory function adopted is a set of joint-space polynomial functions. The path is defined in terms of joint angles based on the polynomial functions. The Cartesian path is obtained by a transform (direct kinematics), and is constrained to stay inside of the tube. Thus, there is no longer an explicit governing rule in the formation of the path. If the Cartesian path is out of the tube, the trajectory polynomial must be modified in some manner. To handle this problem, intermediate knot points can be introduced through which the trajectory function must pass. To ensure smooth motion, the trajectory polynomials are constrained to be continuous through the second derivatives for the entire interval. Thus, each additional knot point imposes four more boundary conditions: two positional boundary conditions, as each of the trajectory polynomials is required to pass through the knot point, and two boundary conditions for the continuity of velocity and ac- celeration." The robot rests at its starting point before it moves, and stops at its destination. Therefore, there are a total of 4M- 2 boundary conditions for M knot points including starting and destination points.

C. Problem Statement

Joint-space trajectory polynomials are characterized by three design parameters: the number of knot points (ME R') the location of the knot points (Oi E R", i = 1,. . , , M), and the transition time between the knot points (??: E R', i = 1, . . . , M - 1). Suppose a set of joint space trajectory polynomi- als, Ci , j ( - ) , i = 1,. . . , M- 1, j = 1,. . . , n, where n denotes the number of degrees-of-freedom of the robot, are obtained by choosing a set of the design parameters..Then Ci,j( t ) , Ci,j( t ) , CZj( t ) specify angular position, velocity, and acceleration of the j " joint at time t , where rE[O, Ti]. Note that once the design parameters are chosen, the trajectory functions which contain all the trajectory information are defined. Now, the problem to find the minimum-time trajectory can be stated as

finding the best set of the design parameters so that the sum of the Ti, i = 1, . . . , M- 1, is minimized while satisfying the path constraint, actuator constraint, boundary conditions for motion constraint, and other constraints due to physical limits in the angular position and velocity of the joints. Mathematical representation of the problem is summarized as follows:*

Objective function:

M-1

min Ti i = 1

*Here, we present a rather general model taking into account the orientation of the end effector. An approximate model formed by eliminating the orientational problem can be obtained by dropping out Eq. (15) and lumping the mass and inertia of the end-effector into the last link of the manipulator when the dynamics equation is developed in Eq. (17).

Suh and Bishop: Collision-Avoidance Trajectory 505

Boundary conditions:

C';(O) = 0 C L 4 ( TM-1) = 0 C!( 7,;) = C!+l(O) 1 ci(o) = ej cM- l (~M- l ) = eM c I ( T i ) = C , + ~ ( O ) C i (0 ) = 0 CL-l(TM-1) = 0 Ci(7-i) = C{+1(0)

where i = 1 , . . . , M - 2, j = 1, . . . , M - 1, and C;(t) = [ Ci,l( t ) , Note that time is reset at the beginning of every segment.

and

where

Di(t) = min Di.j(t)l 1 S j s N - I

and DZRp[ .I, DZR'[ . ] are direct kinematics for positional and rotational displacement of the end effector coordinate frame. Note that Di(t) and G;(t) are the positional and rotational deviations at time t in the i" segment of the trajectory function, Ci(t), and &(r ) is the positional distance in Cartesian space between C, ( t ) and the j" segment of the reference path (i.e., line segment connecting Ri and Ri+l ) . Note also that & ( t ) indicates the segment number of the reference path which contains the minimum-distanced point from Ci(t ) . It is worth mentioning that Eq. (11) is the sign constraint of the joint angles where sign[x] and Si are the sign of x and the sign vector of the joint angles in the i" tube segment.*

Actuator constraint:

*This constraint is due to the non-uniqueness relationship between Cartesian and joint spaces as discussed in Section IIA.

506 Journal of Robotic Systems-1988

where Y-[ - 3 , Y'[ . ] E R" are the bounds on the torque/force at joint actuators, and they are state (joint position and velocity) dependent, and T i ( t ) is from the following dynamic equation:

where I [ - 1 E R""" is the inertia matrix, h[ -1 E R" represents the centrifugal and Coriolis fprces, and g[ - 1 E R" represents the effect of gravity.

Joint position limit:

0- I Ci( t ) 5 0+ V i , t ,

where 0-, O + E R" are the angular position limits of joints.

Joint velocity limit:

9-5 C:( t ) I VI+ Vi , t ,

where VI-, VI+ E R" are the angular velocity limits of joints.

111. ALGORITHM DEVELOPMENT

The algorithm for solving this problem essentially involves the following three steps: a) generation of design parameters (M, ei, Ti), b) construction of the trajectory, and c) evaluation of the trajectory. However, the problem space prohibits a straightforward enumerations of the design parameters. The algorithm presented tries to limit the number of enumeration of the design parameters by the use of appropriate schemes for approximation.

A. Time Scaling Property

Definition 1 (Time scaling): Consider two functions in which time is an independent variable: C(t ) , t E [0, TI, and e(f), f~ [0, T / b ] , where b > 0. If the following conditions hold, e(-) is a time-scaled function of C(.), and defined to have time scaling property:

C( t) = C( bt)

C"(t) = b2C"(bt) , V t E [0, Tlb]. (20)

Property 1 (Uniform scaling): Consider two sets of functions satisfying the boundary conditions (9):

Suh and Bishop: Collision-Avoidance Trajectory 507

Then, the necessary and sufficient condition that c has the time scaling property with respect to C is bi = bi, Vi, j .

Property 2 (Dynamic scaling):** If the time scaling property holds, then the minimum time of C = {Ci,i( a ) , i = 1, . . . , M- 1, j = 1, . . . , n}, T *c due to the velocity and torque limits of the joint actuator is found as follows:

M-1

T % = Ti/b* i = l

where Ti is a trial transition time for segment i, and

b*=minbi, i = l , ..., M-1, i

bi = min bi,i(t), j = 1, . . . , n, t E [0, Ti], i.t

where g1 and a2 indicate the signs of C:,](t) and ~ ~ , ~ ( t ) - g i , j ( f ) , respectively, and gi , i ( f ) is the gravity term acting on the j th joint at time fin segment i . Note that the first(second) term of the brace in Eq. (22) is the maximum time scaling factor due to the velocity(torque) limit of joint j at-time t in segment i .

Property 3: The minimum time of any c = {C i , j ( - ) } which is uniformly time-scaled from C = {Ci,,( -)}, for i = 1, . . . , M- 1, j = 1, . . . , n, is the same as the minimum time of C.

Property 3 assures that the minimum-time for a given set of knot points is dependent only on the ratio of intersegment time intervals, not the individual magnitudes of the time intervals, which considerably reduces the complexity of finding T:. Nevertheless, there still exists a combinatorial problem in finding the optimal ratio of intersegment time intervals, and it becomes very diffcult to solve as the number of knot points increases. To solve this problem, the equal-time-interval, i.e., Ti = Ti = T, Vi, j , is assumed based on the intrinsic property of the polynomial trajectories.

Property 4 (Equal- time-internal): Suppose an optimal trajectory polynomial {C:(.))- is constructed by TT, a:, i = 1 , . . . , M*- 1, k = 1,. . . , W . Then almost the same trajectory can be constructed by f;, 6:, p = 1, . . . , I@ - 1, q = 1,. . . , A?', where f; = T* = xr<'TY/(A?' - l), Vp, and I@ = 1 + CE,' ai, where ai = T:/ p* and a. = 0. Note that f* and n;l* are chosen so that all the ai are integer. Finally, 6; = C*,(rf*), where p is the smallest integer satisfying 1f-1 ai + 1 - q z 0, and y = q - 1 -Cf=1 a;-1.

**This property is based on the constant torquelforce bounds, but it can be modified to be state (position and velocity in joint space) dependent. Refer to Ref. 22 for a detailed discussion on this.

508 Journal of Robotic Systems-1988

From Property 4, the trajectory problem is reduced to finding the optimal number and location of knot points which are equally spaced in time. For computational purposes, a transition time of 1 second per segment is used.

B. Polynomial Construction Scheme

A trajectory made up entirely of cubic polynomials for all M - 1 segments fits 4(M - 1 ) =.4M - 4 boundary conditions. Since there are 4M - 2 boundary conditions to fit, two additional boundary conditions must be considered in some fashion. The use of quartic polynomials for the first and last segments resulting in a 4/31.. . /3/4 trajectory is a scheme often adopted. In this scheme, trajectory functions are found by solving a matrix equation of dimension (4M- 2) x (4M - 2). Despite the existence of efficient algorithms for solving the matrix equation, e.g., see Refs. 19 and 21, the computational burden is considerably heavy, and numerical error becomes significant as M increases. In addition, the matrix equation must be resolved for any change in the location of knot points and/or the number of knot points since the entire set of trajectory functions is governed by the matrix equation. Moreover, the tra- jectory functions cannot be constructed unless the location and the number of the knot points are defined completely.

An alternative scheme is to construct quartic polynomials sequentially from the first segment to the last segment considering a fixed, limited number of knot points ahead. The scheme does not involve the solving of matrix equations; hence, the computation cost is lower. Furthermore, the per- formance of partially constructed trajectory functions can be evaluated. Thus, it is not necessary to construct the whole trajectory if the performance of a partial trajectory is not satisfactory, which saves computation time.

Let the quartic polynomial for the ith segment be*

Then the quartic polynomials for the first M - 3 segments is constructed by the following five boundary conditions:

It is worth noting that Eq. (26) is a pseudo boundary condition used only for *For notational convenience, the subscript j indicating the joint is dropped.

Suh and Bishop: Collision-Avoidance Trajectory 509

the construction of Ci( a ) . In other words, Ci( * ) may not actually pass through the knot point 8i+2 (but Citl(-) does) because we take the first half 01" the polynomial as Ci( .). For the last two segments, two quartics are constructed by ten boundary conditions. At 8 ~ 4 , three boundary conditions are from Eqs. (24) through (26) with i replaced by M - 2. At four boundary conditions are:

Finally, three boundary conditions at OM are:

CL-l(l) = 0, (34)

CL-l(l) = 0. (35)

C. Phase 1 Algorithm

Up to this point, we have described the schemes generating Ti, and construction of polynomials assuming that the knot points are given. In this subsection, we discuss the method for finding the optimal number and location of knot points. Basically, any point in the feasible space can be considered as a knot point. To consider the location of knot points schematically, the space needs to be discretized in some fashion.

If the entire space is discretized by m equally spaced points, including starting and destination points, the total number of possible paths using all possible ordered combinations of the rn points is 2"-2. Evaluating all these paths is computationally impractical for even moderate values of rn. Note that using smaller values of rn limits the potential quality of the solution. Instead of discretizing the entue space and evaluating all possible paths, the phase 1 algorithm partially discretizes the feasible space and finds suboptimal solutions. Specifically, the phase 1 algorithm restricts the location of knot points to the center of the feasible space; i.e., reference path, and finds the best solution by a branch-and-bound search technique.

1. Discretization of Reference Path

Let M be the total number of possible knot points equally spaced on the reference path as shown in Figure 4. Examples for possible paths are those which pass through: 1 - M, 1 - 4 - ( M - 1) - M, etc. Thus, there is a total of

51 0 Journal of Robotic Systems-1 988

Figure 4. Discretization of reference path.

2M-2 possible paths since points 1 and M are starting and destination points, respectively, through which all paths are required to pass. Note the assumption that the path is always constructed in the order of increasing knot point indexing number. This assumption is realistic since any paths containing backward movement are unlikely to be optimal paths. The problem statement of phase 1 is as follows:

Find a subset of the set of M equally spaced knot points on the reference path so that the trajectory functions constructed by the knot points selected provide a minimum-time trajectory while satisfying all the constraints.

2. Branch-and-Bound (BB) Search

The proposed BB search tries to limit the number of explicitly-enumerated nodes in the search tree by using fathoming rules. Fathoming rules are based upon feasibility and lower-bound estimation. Suppose a partial trajectory C = {C,( .), . . . , Ck-l( -)} is constructed by k + 1 knot points with the knot point index set Q = {q , , . . . , qk+l}, and let 0qk+2 be the next knot point to be considered. Then a quartic polynomial ck(') is constructed for the augmented segment, and evaluated based upon the following fathoming rules. If any of them are satisfied at any time t E [0, 11, then the augmented partial trajectory CA = {c,(.), . . . , c k - l ( ' ) , c k ( ' ) } is fathomed out.

Fathoming rule- 1 (FR- 1): FR-1 is due to the joint position limit:

c k ( t ) [@-, 0'1, vt E [o, 13. (36)

Fathoming rule-2 (FR-2): FR-2 is due to the path constraint:

where*

*Again, Eq. 41 can be ignored for the simplified model in which the end-effector is modelled as a sphere.

Suh and Bishop: Collision-Avoidance Trajectory 51 1

D(t) = min Di(t) 1 sis N- 1

Fathoming rule-3 (FR-3): FR-3 is due to the velocity and torque limits of the joint actuator:

LB( t ) > UB (42)

where UB is the lowest minimum time of the complete path found so far. The lower bound at time t in the augmented segment, LB(t) is found as follows:

where

B( t) = min{ b( t), min b i ) l s i s k - 1

(47)

and s1 and sz indicate the signs of [eqF+!,j- c k , j ( t ) ] and [flM,j - flqk+l,j], respectively. Note that v1 is the actual minmum time taken to reach the current position since the beginning of motion and 7j2, 7j3 are the lower-bound estimates of the minimum time to reach the destination point via t3qk+1 from the current position, v indicates the minimum number of segments to reach the destination point, eM, from Oqk+, .

3. Summary of Phase 1 Algorithm

The Phase 1 algorithm can be summarized by the following four steps:

51 2 Journal of Robotic Systems-1988

Step 1: Compute {O,, i = 1,. . . , M} based on the input M and {Ri, i = 1 , . . . , N}, and let the initial upper bound be 00 sec. Step 2: Select a partial path having the lowest lower-bound, and form augmented paths. Step 3: For each augmented path, construct a quartic polynomial for the augmented segment, and apply the fathoming rules. If the augmented path is a complete path and the minimum-time is lower than the current upper-bound, update the upper-bound. Step 4: If there are no remaining paths, stop. Otherwise, go to Step 2.

D. Phase 2 Algorithm

Phase 2 relaxes the restriction imposed in phase 1 that the knot points lie on the reference path and finds the minimum-time trajectory by adjusting the number and location of knot points. Phase 2 takes the phase 1 solution as an initial feasible solution and optimizes by modification and doubling processes.

1. Modification Process (MP)

The MP finds the optimal locations of the existing knot points by a gradient search method. Let 'I)=[&,. .. ,ern] be a matrix of dimension n x rn representing the locations of the rn knot points in joint space. Note that the minimum time of a, T*, can be found by applying the Property 2 after constructing trajectory polynomials by the method described in Section IIIB. Let 9 be the above procedure to find the minimum time; i.e., T* = $(a), and V*(@) be the gradient of Q, with respect to minimum time. Then, the MP can be described by the following three steps:

0 Step 1 (Initialization): Let Q,r be the initial location matrix, and TI be the

0 Step 2 (Termination): Find V*(Q,I). If V*(@I) is a null matrix, stop.

s Step 3 (Line search): Find A*ER' minimizing 9(@1+AV*(Q,,)). Let

minimum time of 0 1 .

Otherwise, go to Step 3.

TI = 9 ( @ I + A*V*(Q,I)), and @ I = @ I + A*V*(Q,r). GO to Step 2.

2. Finding the Gradient

Since 9 cannot be represented in a closed form but is only a symbolic transformation of a numerical procedure, the V* can be found only numeric- ally by discretizing the direction of the gradient. The direction of the gradient at each knot point is discretized by an increment along each axis. Since there are two directions for each axis, the total number of directions for n joints is 2n + 1 including zero (i.e., do not move it at all). For instance, the directions for two-link manipulator with an rn of 4 is shown in Figure 5. Since a different direction vector at each knot point yields a different gradient matrix, there are a total of (2n + l)m-2 possible gradient matrices for rn knot points, Now, the

Suh and Bishop: Collision-Avoidance Trajectory 51 3

joint2( angle)

+ s

- 8

+s

- 8

+s %

+ s

~ - jointl(angle)

Figure 5. Five directions for two-link manipulator.

problem to find the gradient is to find V*(aI) where V*(al) is the optimal solution to the problem to minimize 9(V(Q1) + @ I ) subject to V(@,) E R, where R is the set of (2n + l)m-2 possible gradient matrices. This problem can be solved by using very similar methodology to that used in the phase 1 algorithm with the only difference being in the computation of 7j3. Since the number of knot points (NKP) in the MP is fixed, Eq. (48) becomes:

~ = M - l - k (49)

3. Doubling Process (DP)

In the MP, the NKP is fixed and the optimization process involves only the location of the knot points. DP involves finding the optimal NKP. In searching for the optimal number of knot points, a change in NKP yields a new problem for finding an initial feasible solution due to the path constraint. Note that the MP must always start with a feasible solution. If the initial solution is in- feasible, the search has to start from scratch to find an initial feasible solution, which is very time consuming. One way to avoid this problem is using a previously found feasible solution as an initial feasible solution. We use Property 4.

Suppose a known feasible trajectory is formed by NKP of mr with locations of {&, i = 1,. . . , mr}, and the trajectory functions, {C,(r), i = 1,. . . , mr - 1, t E [0, 11). Then, almost the same trajectory can be formed using an NKP of mo with locations of {Qi, i = 1, . . . , mo}, where 62i-1 = Oi , i = 1,. . . , mr, e2, = Ci(1/2), i = 1,. . . , mr - 1, and mo = 2mr - 1. By this scheme, the new poly- nomial trajectory functions {Ci( - ) , i = I , . . . , mo - I}, are: C2i-1(r) = Ci(t ) , c2i(t) = C,(r + 1/2), where t E [0,1/2] and i = 1,. . . , mr - 1.

51 4 Journal of Robotic Systems-1988

4. Summary of Phase 2 Algorithm

The phase 2 algorithm is briefly summarized as follows:

Step 1: Read Phase 1 solution. Step 2: Perform MP. Step 3: Perform DP followed by MP. If the reduction of minimum time is less than a prescribed value, stop. Otherwise, repeat Step 3.

IV. SIMULATION AND ANALYSIS

The two-phase algorithm has been tested using a two-link robot (Fig. 6) simulated on an IBM 3081 computer. The geometrical data of the robot, actuator limits, and the values of the constant maximum velocity and ac- celeration associated with the conventional trapezodial velocity scheme are shown in Table I.** To compare the performance of the two-phase algorithm with the conventional trajectory scheme, five examples were chosen. Examples one through four were chosen for the basic motion of the robot arm on vertical, horizontal, and diagonai paths. Example five is for motion on a combination of paths. The computer results for the five example paths with various path tolerances are summarized in Table 11. Note that the tube parameters Ri, E (constant) are in meters, and the sign of & is kept to be minus (i.e., elbow-up configuration) in every tube segment, and T:, TT, and T: indicate the minimum time (in seconds) of the phase 1, phase 2, and conventional schemes, respectively. Also, note that the M value of 30 is used for the two-phase method.

Y I / /

Figure 6. Two-link robot.

**The values are taken from Ref. 6 with the following conversion factors: 1 lb = 0.454 Kg, 1 f t = 0.304 rn, 1 ft - Ib = 1.3558 N m.

Suh and Bishop: Collision-Avoidance Trajectory 51 5

Table 1. Data for two-link robot.

Description Value

Robot Geometry Length of link (m) Mass of link (kg)

1, = 0.30, I, = 0.30 m, = 14.6, m2= 14.6

Inertia of link (Kg + m2) z, = 0.1, z* = 0.1 Torque limit (N - m) T1€[-95,95], T2€[-27,27]

Conventional Scheme Constant vel. max. Constant acc. max.

i = 0.75 mlsec z = 1.2 mlsec’

The results show that even the phase 1 solutions yield times less than 50% of those obtained by conventional scheme. This significant reduction comes from the fact that the conventional method does not fully consider the dynamics of the robot and takes the most conservative maximum of the velocity and acceleration which can be realizable in any configuration of the robot arm throughout the movement. As a result, the robot is underutilized and the travelling time is longer than necessary.

The velocity profiles of example 2 for the conventional scheme and the phase 2 solutions are displayed in Figure 7. These experimental results also indicate that in general the phase 1 solution is not much different from the

Table II. Computer results of the five examples.

Example No. Ri T: T: T ;

Rl = (0.3, -0.5) 0.01 1.08 0.90 1 R2 = (0.3,0.5) 0.03 1.96 1.04 0.75

0.05 1.04 0.73

R, = (0.3,O.O) 0.01 0.44 0.40 2 R, = (0.6,0.3) 0.03 1 .oo 0.44 0.39

0.05 0.44 0.39

R, = (0.5, -0.3) 0.01 1.04 0.92 3 R2 = (0.1,0.5) 0.03 1.93 0.95 0.83

0.05 0.95 0.80

R, = (0.5,0.3) 0.01 0.91 0.80 4 R, = (0.1, -0.5) 0.03 1.93 0.91 0.77

0.05 0.88 0.72

R I = (0.0, -0.5) 0.01 1.91 1.52 5 R2 = (0.4, -0.2) 0.03 3.74 1.44 1.26

R, = (0.4,0.2) 0.05 1.44 1.03 R, = (0.0,0.5)

51 6 Journal of Robotic Systems-1 988

2.0

1 .o

0.0

'. .. '. '. '. . 4 sec 0.0 0.5 1 .o 1.5 2.0

-cli- c = 0.01(m), = 0.90 sec - c = 0.02(m), r; = 0.85 sec c = 0.03(m), fi = 0.75 sec Conventional scheme, T * = 1.958 sec - . - . -

Figure 7. Cartesian velocity magnitude plot.

phase 2 solution when the path tolerance is small. This seems to be obvious considering the center line of the feasible space approximately represents the entire feasible space so that the optimal location of knot points in phase 2 is close to the reference path. As the path tolerance increases, however, the difference between the phase 1 and phase 2 solutions become larger. Without exception, as the path tolerance increases, the minimum time decreases in both the phase 1 and phase 2 solutions. This can be explained by the fact that the bigger tolerance provides more room to exploit the dynamic effects of the robot to obtain for speedy motion.

In the literature, Bobrow6 presents a method to determine a mathematically proven time-optimal solution for a predefined path. The two examples presen- ted are solved by the algorithm developed herein by taking the predefined path as a reference path. Then, a comparison is made using various mag- nitudes of path tolerances. Bobrow's true minimum-time for the straight line path shown in Figure 8(a) turns out to be 0.72 seconds while the minimum time from the presented algorithm varies over the magnitude of path tolerances as shown in Table 111. With extremely tight tolerances, for instance 0.005 (m), the minimum-time from the proposed method appears to be considerably higher than the true minimum time. However, as the path tolerance increases, the proposed algorithm provides somewhat lower mini- mum times.

Note, however, that a true comparison cannot be made because of differences in the modelling of the robot notion. The true minimum time

Suh and Bishop: Collision-Avoidance Trajectory

X

Figure 8(a). Path example 1.

Y 0.6

0.3

0.0

51 7

Y

0.3 0.6

Figure 8(b). Cartesian paths.

518 Journal of Robotic Systems-1 988

20 - I -&& r=0.005(m)

I

e=O.OlO(m) = 0.050 (m) 15 ' I -

1

I

t / T* 0.0 0.25 0.50 0.75 1 .oo

Figure H(c). Scale factor plots.

trajectory involves sudden changes of torque at the switching points and high nonzero acceleration at both the start and end of the motion, while in the presented model the acceleration at start and end are zero and the path and its first two derivatives are everywhere continuous. Thus, there is a cost of smoothness which must be considered in some manner."

Sec

-95

Figure H(d). Torque plots for joint 1.

*In a particular experiment,2') the cost of smoothness is estimated at 26% of the true minimum-time indicating 0.91 seconds of smoothened true minimum-time.

Suh and Bishop: Collisiondvoidance Trajectory 51 9

Sec

-21 1

Figure S(e) . Torque plots for joint 2.

The path of the end-effector with different path tolerances is seen in Figure 8(b). According to Figure 8(b), the robot seems to take advantage of the system dynamics for speedy motion by traversing away from the minimum- distance or reference path, as path tolerance increases. The b(t ) curve (Fig. 8(c)), which is a plot of the maximum time-scaling value at time I , supports this finding. A b(t) value of 1 indicates that the torque is fully utilized, and a b(t ) value greater than 1 indicates that the torque capacity is not fully utilized at time t. A b(t) value less than 1 is not feasible because the torque required at time t is not realizable from the joint actuator.

As the path tolerance increases, the shape of the curve approaches “U” shape, indicating that at least one of the joints at its torque bound most of the time, the major exceptions being at the start and end of the trajectory. However, the b( t ) curve for the tight-tolerance cases shows an increasing “W’ shape, indicating that neither of the joints is actuator-limited much of the time. This can also be observed in the torque plot in Figures 8(d) and 8(e). In fact, Bobrow6 and Shin et a1.’ proved that a necessary condition for an optimal

Table III. Minimum-time comparison for path in Figure 8(a).

Path tolerance (m) Minimum time (sec)

0.005 0.010 0.020 0.030 0.040 0.050

1.18 1 .oo 0.86 0.84 0.83 0.83

520 Journal of Robotic Systems-I 988

trajectory for a predefined path is that at least one of the joint actuators is always at its torque bound.

It is, however, not appropriate to use their optimality criterion for our problem, where the path is not predefined but given as a tube and motion smoothness is required. It is hard to realize the optimality criterion if the continuity of acceleration is imposed at the switching point where the dis- continuity of acceleration occurs. There must be a singular zone where none of the actuators is at the torque bound even for a predefined path. Nonethe- less, the ctoser to a “U” shape with the bottom of the “U” at or near one, the better the torque utilization. Note that some portion of underutilization around both ends of the motion is unavoidable in any case due to the boundary condition for zero acceleration at both ends.

The minimum time with the bang-bang scheme for the path shown in Figure 9(a) is 0.70 second* with two switchings in the phase plane. Results of the present algorithm can be seen in Table IV. If the path tolerance is bigger than the 0.05 (m), the minimum time from the presented method is lower than from the bang-bang scheme, and if there are no obstacles, a minimum time of 0.42 sec. is realized. The Cartesian path plot with different path tolerances is seen in Figure 9(b). Plots illustrating the behavior of robot motion under obstacle-free environments are displayed in Figures 9(c)-(e). From the plots, the existence of the singular intervals mentioned above can be confirmed.

Y

Figure 9(a). Path example 2.

*Again, if the cost of smoothness estimated in Ref. 20 is used, it becomes 0.88 seconds.

Suh and Bishop: Collision-Avoidance Trajectory

S - e=0.005 (m) -ct ~ = 0 . 0 3 0 (m)

Q = 0.070 (m)

52 1

I 1 i

0.3 0.6

- - S

Figure 9(b). Cartesian paths with path constraint.

D

-0.3

-0.6

Figure 9(c). Cartesian path without path constraint ( E = a).

522

20

15

10

Journal of Robotic Systems-1 988

-

.’

*.

Joint I (N * m) Joint 2 (N . m)

=Joint 1 - =Joint 2

27

0

-27

Table IV. Minimum-time comparison for path in Figure 9(a).

Path tolerance (m) Minimum time (sec)

0.005 0.010 0.020 0.030 0.040 0.050 0.060 0.070

m

0.98 0.95 0.89 0.85 0.82 0.77 0.68 0.62 0.42

Suh and Bishop: Coflision-Avoidance Trajectory 523

V. CONCLUDING REMARKS

In this article, we have presented a new scheme for robot trajectory planning and developed an algorithm to determine a practically useful tra- jectory. The presented method can be readily applicable to any robotic manipulator by representing the feasible space as an articulated tube. A simple way to incorporate it into the teach-in method is to include the maximum deviation when the intermediate points are determined. Alternately, it can be incorporated into an off-line CAD programming. In the latter case, any currently available CAD program can be easily modified to generate more accurate tube parameters. By any method, the maximum deviation can be determined without much extra effort, as the task is a matter of measurement (teach-in-method), or distance computation (off-line CAD approach).

Inclusion of tolerances in path planning provides much flexibility in the planning of robot trajectories. First, it provides a spatial freedom for the robot to optimize their motions using the dynamics. Through numerous examples, compared with the conventional method the new scheme results in the time savings of over 50%. In addition, the spatial freedom can be used as a buffered zone allowing some discrepancy due to non-linear relationship between Car- tesian space and joint space. In other words, the joint-space trajectory can be used as a means of collision-free trajectory planning. Note that taking joint- space trajectory as a nominal trajectory is attractive as it relieves computing burden (such as direct and inverse kinematic computation which are required for Cartesian nominal trajectory) for the robot controller. By utilizing the relieved computing power, an accurate path tracking can be achieved with a high sampling rate. Further, the joint-space polynomials require little memory space as only the coefficients of the polynomials are to be stored. Therefore, the new trajectory planning method can be applied in the most realistic environment where obstacles exist but the minimization of the task execution time is desired with smooth motion. Note that although the performance index used in our algorithm can be easily extended to the general performance index, such as energy consumption or both the time and energy.

As far as the optimality is concerned, our trajectory optimizer finds sub- optimal solutions for both path and velocity history. However, its performance turns out to be superior to the conventional trajectory scheme where neither is optimized and the bang-bang scheme where only the velocity history is optimized. Note that compared with the conventional scheme taking trapezoidal velocity profile, the time saving even by phase 1 solutions turned out to be over 50%, and compared with the bang-bang scheme, the new scheme provides even lower minimum-times as the path tolerance increases. This convinces us of the significance of the spatial freedom for the trajectory optimizer. However, it should be pointed out that the performance of the trajectory optimizer could be largely dependent on the tube parameters being chosen. Thus, to achieve the global optimality, an efficient algorithm for finding the optimal tube parameters has to be developed. This is a matter of our future inquiry.

524 Journal of Robotic Systems-1988

It is worth pointing out that some inferiorities (even with compensation for the smoothness of motion) of the presented scheme to the bang-bang scheme under tight path tolerances seems to be involved with our trajectory modelling method, namely the use of the joint-space polynomial as a trajectory primitive. In the polynomial trajectory, the position (thus path), velocity and acceleration are defined simultaneously. This could be both a strong and weak point. The strong point is that both path and velocity history can be optimized concur- rently, and hence can be used as a means for a unifying approach as ours. The weak point is that the velocity history is dependent on the path. Thus, the tight path tolerance limits the feasible space for velocity optimization. Note, however, that for a task requiring very high accuracy in the robot path, a fine motion planning is applied, and in such a situation, there are many more important and compelling requirements to be met than minimum time.

Our final remark concerns the algorithmic issues of the presented method. A heuristic used for the phase 1 algorithm, limiting the location of knot points on the reference path, is proven to be attractive by experiment especially when the path tolerance is small. This agrees with the common notion that the center-line path of the tube well represents the feasible space having small tolerance. Since the computation time for the phase 1 algorithm is low (a few seconds or even tens of seconds), it can be used when a suboptimal solution with lower computing cost is needed. Practical application of the phase 1 algorithm showed that an M value of between 20 and 30 was quite satis- factory. It is also observed that each of the three fathoming rules, particularly fathoming rule 3, reduced the computation time significantly for both the phase 1 and the modification process of phase 2. The cpu time for phase 2 varies widely between approximately 30 and 200 seconds in the IBM 3081, the main frame used. Note that the cpu time for phase 2 includes that of phase 1.

The authors would like to thank anonymous reviewers for their constructive comments on the first draft of this article, and University of Michigan and Pohang Institute of Science and Technology for the support in preparation of this article.

References

1. M. E. Kahn and B. Roth, “The near-minimum-time control of open-loop arti- culated kinematic chains,” ASME, J . Dynam., Syst., Measur., and Conn., 164-172 (September 1971).

2. B. K. Kim and K. G. Shin, “Minimum-time path planning for robot arms and their dynamics,” IEEE Trans. Syst., Man, and Cybem., SMC-15 213-223, (MarcWApril 1985).

3. C. Lin, P. Chang, and J. Y. S. Luh, “Formulation and optimization of cubic polynolinial joint trajectories for mechanical manipulators,” ZEEE Tram. Automat. Conn., AC-28(12), (December 1983).

4. J. Y. S. Luh and C. Lm, “Optimal path planning for mechanical manipulators,” ASME J . Dynam. Syst., Measw., and Conn., 102 142-151, (June 1981).

5. K. G. Shin and N. D. Mckay, “Near-minimum time geometric paths for robotic manipulators,” B E E Trans. Automat. Conn., AC-31(6), (June 1986).

Suh and Bishop: Collision-Avoidance Trajectory 52 5

6.

7.

8.

9.

10.

11.

12.

13.

14.

15.

16. 17.

18. 19.

20.

21.

22.

J. E. Bobrbw, “Optimal control of robotic manipulators,” Ph.D. Dissertation, UCLA, 1982. J. E. Bobrow, S. Dubosky, and J. S. Gibson, “On the optimal control of robotic manipulators with actuator constraints,” Proc. American Contr. Conf., San Fran- cisco, June 22-24, 1983, pp. 782-787. K. G. Shin and N. D. Mckay, “Minimum-time control of a robotic manipulator with geometric path constraints,” ZEEE Trans. Automat. Contr., AC-20 531- 541, (June 1985). T. Lozano-Perez, “Spatial planning: a configuration space approach,” IEEE Trans. Comput C-32 108-120, (1983). J. Y. S. Luh and C. E. Campbell, ‘‘Minimum distance collision-free path planning for industrial robots with a prismatic joint,” IEEE Trans. Automat. Contr.,

W. E. Red, “Minimum distances for robot task simulation,” Robotica, 1 231-238, (1 983). R. A. Brooks and T. Lozano-Perez, “A subdivision algorithm in configuration space for findpath with rotation,” IEEE Trans. Syst., Man, and Cybern.,

0. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” Proc. Znt. Conf. Robot., and Automat., 50G505, St. Louis, (March 1985). T. Lozano-Perez, “An algorithm for planning collision-free paths among poly- hedral obstacles,” Comm. ACM 22(10) 681-698, (1981). T. Lozano-Perez, “Automatic planning of manipulator transfer movement,” IEEE Trans. Syst., Man, and Cybern., l l(10) 681-698, (1981). R. P. Paul, Robot manipulator, The MIT Press, 1982. R. H. Taylor, “Planning and execution of straight line manipulator trajectories,” ZBM J. Res. and Deoelop., 23(4) 424-436, (July 1979). M, Brady et al., Robot motion: Planning and Confrol, The MIT Press, 1982. C. F. Gerald and 0. W. Patrick, Applied Numerical Analysis, Addison Wesley, 1984. S. H. Suh, “Development of an algorithm for a minimum-time trajectory planning problem under practical considerations,” Ph.D. Dissertation, The Ohio State University, 1986. H. H. Ahlberg et al., The Theory of Splines and Their Applicah’ons, Academic Press, New York, 1967. J. M. Hollerbach, “Dynamic scaling of manipulator trajectories,” ASME J . Dynam., Syst., Measw., and Control. 106 102-106, (March 1984).

AC-29 675-680, (1984).

SMC-15(2) 224-233, (M~ch/April 1985).