An Algorithm for Moving a Computer-Controlled Manipulator While Avoiding Obstacles

ABSTRACT

In programming a computer to control a manipulator, the problem of avoiding obstacles present in the environment must be faced . This paper presents a simple world model containing the manipulator as well as obstacles, and describes an algorithm for guiding the manipulator to a speci­ fied destination point while avoiding collisions with obstacles . The algorithm combines a number of heuristics with a method for stepwise optimization based on a linear programming technique. A brief description of the computer program AVOID and a sample of computer simulated tasks solved by it are presented.

I INTRODUCTION

One of the main problems in robotics is the problem of devising efficient computer methods to control a manipulator. A typical approach to this problem involves the use of information obtained from visual as well as other sensory feedback for specifying an internal world model in computer memory. Having this model and an appropriate algorithm, the computer would hopefully be able to direct the manipulator to perform tasks requiring

some intelligence and dexterity.

II WORLD MODEL

In the situation shown in Figure I, the "world" consists of a part of the Euclidean plane containing the planar manipulator and the obstacles 0 1 and 0 2 . The manipulator is assumed to be a group of four links 1 1 , 1 2 , 1 3 , 1 4 con­ nected by the revolute joints B 1 , B?' B 3 , B 4 ·. The first link .1 1 is connected to grouna at the origin by the first joint B 1 while the endpoint B 5 of the last link 1 4 contains the jaws J, or some other type of terminal device of negligible size and orientation. Thus, to describe a configuration, say,_cnf 1 , of the manipulat~r it will sufffce 2 to 3 sRec~fy the vector of relat~ve angles ~(~ .~ .~ , 'f ) . The position of each joint B. (i=2, ... ,5) in the plane is given by the formula ~

where the vector functions B. are combinations of trigonometriT f~nc3io~s. Th~ set of all possible values ~= ('f ,'f ,f ,'f ) forms a 4-dimensional s:ate space~. The class of obstacles under consideration is restricted to convex polygons. The ithobstacle has vertices Ai,l· Ai,2···· and edges ai,J,

Eugene Grechanovsky and I.Sh. Pinsker

~i. 2 , ... The position and shape of each obstacle ~s assumed to be comrletely specified.

III BASIC TASK

Assuming the path G (with the initial point ?I and final point GF) for the jaws is specified ~n advance, the task consists in moving the manipulator from the initial configuration to a final one while avoiding obstacles. This problem could be stated as follows: Given the initial configuration of the manipulator and the path G for the jaws, compute a continuous sequence of con­ figurations such that (i) The first configuration in the sequence is the initial configuration; (ii) The jaws in each configuration in the sequence lie on the curve G and in the final configuration co­ incide with the final point G ; (iii) No con­ figuration in the sequence in{ersects with any of the obstacles. It is presumed that the jaws inthe initial configuration are located at the point GI, and that neither the initial configuration nor tfie path G intersects with the obstacles.

Our aim is to devise a computationally ef­ ficient algorithm for planning manipulator motions, i.e. capable of using the model described above for obtaining a curve in the~-space such that the corresponding sequence of configurations should satisfy conditions (i)-(iii) listed above . The major difficulty arises from the fact that the manipulator moves in the plane containing obstacles whereas the state space ~ , in which the control variables f move, is of an entirely dif­

ferent nature.

IV PRELIMINARIES

First of all, the continuous decision curve in~-space and the corresponding continuous con­ figuration sequence are to be replaced with a finite, possibly small, number of points. This is to be done with care, however, since the existence of a finite sequence of permissible configurations (i.e. configurations free of intersections with obstacles) does not guarantee the existence of a continuous permissible sequence. To overcome this difficulty, the obstacles should be replaced with E-covers, that is, the polygons which contain the obstacles as subsets and have at least €.-wide free margins. This procedure has an additional advan­ tage of enabling the original obstacles to have any shape. For simplicity, the symbols cnf and~ will he used for the ith configuration in a discrete sequence and the correspondi2$ vector of angles respectively, while c~f and ~will denote the next,

(i+I)th, configuration and its angle vector. For computing a continuous sequence of configurations permissible with respect to obstacles, it will suffice to compute a finite configuration sequence in which all the configurations are pe~missible with respect toe-covers and each one is close enough to the next, i.e.

The choice of Sk depends on the value ofE. Now the manipulator motion reduces to a number of steRs each consisting in computing an increment d ~ for the equation

These increments enter into the linear version (I)

To simplify matters further, the path G for the jaws is assumed either to be a broken line with verticies c 1 , c 2 , ... ,GN+l' and segmen~s g 1 , g 2 , ... , gN or to have been approx1.mated with such a line. The jaws are permitted to move near­ by this line in such a way that the distance between the j-aws and the line should never exceed a tolerance constant e . Thus, the basic task has been reduced to seEuring the motion of the jaws along each of the N corridors CRL in succession.

V OVERVIEW OF THE METHOD

Now we outline the major ideas and techniques of the AVOID system, leaving details for Sections 6 and 7.

Supposing all Sk have been computed, the current configuration cnf is permissible, and the jaws J are inside the corridor CR., tQe problem reduces to computing increments l. d f , k= I, ·.. , 4, which on substituting in (3) imply that

1. inequalities (2) hold;
2. the jaws in configuration ~f are inside CRi;
3. the resulting configuration ~f is permissible;
4. the distance covered by the jaws is maximized.

Here the design constraints are temporarily omitted. Dropping condition 3 for a moment, the problem.may be viewed as a linear programming problem (LPP). Indeed, condition 4 can be trans­ formed into the form

where F is a scalar linear function (Sect.7), whilst condition I combined with 2 is just a linear constraint set (LCS). Supposing such an LPP could be easily solved (see Sect. 6)., we get d~ satisfying conditions I ,2,4. Computing f1.rst ~from (3) and then B. from (1), we obtain cnf and can check it for pefmissibility. If cnf is permissible, we replace cnf with c~f and proceed as before; otherwise, the LPP must be modified to render cnf permissible. By definition, cnf is not permissible if it collides with at least one obstacle. Such a collision occurs, when either I) joint B. cuts into edge a. of obstacle 0; or 2) link T.l.cuts off vertex A~. Collision of type I takes ~lace when joint B.Jin its motion tries to run through edge a .. Tol.prevent this, we can forbid the joint B~ to puncture the edge a. at the moment of collisiofr, thereby forcing it toJride the edge a .. Let the edge a. have equation

where r 1 and r 2 are Euclidean coordinates of a point r. Coefficients a~ are chosen in such a way as to insure positive va~ues for a. (r) at exterior points of the polygon, a.(B.)) 0. JThen to express the forbidding restrictiJn,l.we require that the point B.·B.+dB. lie outside a.: ]. ]. ]. J

Using (4), equation (5) can be transformed into the form

and appended to the LCS. Clearly, a solution to the thus modified LPP is permissible if the detected collision of B. with a. is the only one at the moment. Subsequ~nt collisions of both types, if any, could be similarly dealt with one at a time.

The design constraints (which can be readily incorporated into the LCS) and the constraints resulting from conditions 1-2 above (put into the LCS earlier) are called permanent constraints, whilst the constraints (6) are temporal ones. In Sect. 6 it will be shown that as a result of the proper choice of a solving method, only minor mod­ ifications of a solution d~ for the LPP are required to account for the additional constraints (6).

As long as the joint B. keeps riding the edge a., the temporal constraint (6) remains in the LCS. Sooner or later, however, either B. will reach an end of the edge a. and slide off, or it will take off from the edg~. Either of these cases is accounted for by dropping the corresponding con­ straint, i.e. by deleting the inequality (6) from the LCS. Treatment of the take-off case requires no special care since it involves an immediate dropping of the constraint in question from the basis (see Sections 6 and 7). To account for sliding-off, a special checkup procedure has been set up in the AVOID system for signalling to the control structure the fact of B. being off edge a .· In either case the constraint ifi question is J eliminated, thereby cutting down on the number of constraints in the LCS. Type 2 constraints, if any, are treated similarly. ·

Summing up, it may be said that a major func­ tion of AVOID consists in generat ing and solving a series of LPPs, each with a slightly different constraint set. Some of the constraints, the temp­ oral ones, are inserted or deleted as a need arise, reflecting interaction of the manipulator with obstacles . Thus, the difficulty mentioned in Section 3 is overcome stepwise by converting the restrictions imposed by obstacles in the vicinity of the current point into state space restrictions where they can be easily dealt with. AVOID's ability to insert and delete these additional con­ straints at each step renders the system sufficiert:­ ly flexible.

VI THE DUAL METHOD IN LINEAR PROGRAMMING

Now we will present briefly a particular im­ plementation format for the dual method in linear programming as being most suitable for the LPP under consideration. There are two major features to the LPP to be solved. The first is the exist­ ence of a subproblem of the given LPP, solution to to which is readily available. Subproblem here means another LPP with the same objective function and fewer constraint s in the LCS. The second feature is the probable need for incorporating ad­ ditional (temporal) constraints into the LCS after the solution has been obtained, which entails mod­ ifying the solution to account for the newcomers. Both features are easily accomodated in the dual method format described below.

1 consid~r the following LPP: Find x=(x , ... ,x) which minimizes the linear objective function

and satisfies the constraints

where all f. are linear functions. (7)-(9) con­ stitute the 1 subproblem. We proceed in two stages. First, the subproblem (7)-(9) is solved, i.e. a point ~ 1 is found which minimizes (7) subject to the constraints (8)-(9). This is a fairly simple task since our case (see Section 7): (i) inequalities (8) hold; (ii) inequalities (9) which combine the design constraints and inequalities (2) have the form

Here k given by the formulas X are

for which constants hk are easily computed. Thus, the n-dimensional subproblem factorizes into n one­ dimensional ones (n=4). The solution ~ 1 for this subproblem is a vertex of the n-dimensional parallelepiped determined by inequalities (8),

(11). The constraints generating the solution ~ 1 are called basic. The second stage, involving tne entire problem (7)-(10), consists in checking one by one whether the solution ~ 1 found thus far sat­ isfies each of the remaining constraints (10). If it does, the process terminates, and ~ 1 is the sol ­ ution for the entire LPP; otherwise, tne first violated constraint enters the basi c constraint set at the expense of some other constraint which drops from the basis. The solution point shifts from ~ 1 to some ~ 2 with F(~ 1 )~F(~ 2 ). The process term­ inates when the ent1re lisE of constraints is run through without modifying the basis. It is seen now that a newly emerged constraint causes at most a slight modification in solution, which allows us to save on the bulk of initial computations.

VII STRUCTURE OF THE PLANNING ALGORITHM

In this Section the planning algorithm of the AVOID system is presented on a step-by-step basis. A simplified flow- chart for computing the motionof the jaws along a corridor CR 1 from its back side to the front side F is shown in Fig.2. More details are given in (3].

PRINCIPAL MATRIX

Box 1. Here the Principal Matrix PM for the LPP is computed for the current configuration cnf . PM has 5 columns and (7+t) rows, where t is the. number of temporal constraints in the LCS at the moment. Column I consists of constant terms of the corresponding inequalities;kcolumns from 2 to 5 conta~n.their ~o:ffi~ients in x .. ~ow 1 con~ains coeff1c1ents F 1n x of the obJeCt1ve funct1on F(J+dJ), where F(.)=O is the equation of the froUf side of the corridor,kand nonnegative variables x are obtained from dff by linear substitutions (12). Rows from 2 to 7 contain permanent con­ straints, rows below 7 contain temporal constraints if any. Coefficients in rows 2 to 5 are those from inequalities (11), whilst coefficients in rows 6-7 are determined by the fact of the jaws J moving inside the corridor, i.e., by the formula

or

Rows 8 and 9 feature coefficients for exemplary constraints for collisions of type 1 (for joint B 2 involved in the collision) and of type 2 (for linR l3) respRctively. The derivation of formulas for uk and v is straightforward.

• Box 2. The initial basis is chosen by means of the Jordan Elimination Procedure applied to the elements (2,2), (3,3), (4,4), (5,5) of PM in turn. The number of eliminations required for solving this subproblem (cf. Section 6) is 0 to 4.

Box 3. Now a solution for the Rntire LPP is obtained. The resulting values of x ~e to be used in formulas (12) for computing d'f' . The corresponding value of the objective function F occupies the space (1,1) in PM.

,. k Box 4. The new configuration c~f, i.e. angles ~ and positions of joints B., are computed by formulas (3) and (1). ~

Boxes 5 and 6. Having obtained c~f, it is checked for collisions with the obstacles. If no collision is detected, control is transferred to Box 7; otherwise, a proper t emporal constraint is appended to the LCS (Box 6), and the process of solving the new LPP resumes (Box 3; see also Section 6).

• Box 7. After colliding with an obstacle, the joint keeps riding it, and the corresponding temporal constraint remains in the LCS for a number of steps. But after a while the joint either slides off the edge or takes off. To account for either event without detecting relevant inter­ section afresh at each step, the inheritable basis technique is introduced. If a constraint enters the basis, it is likely to remain there for the time being. Therefore, after computing c;f, all the necessary information for the collision (i.e., type of the collision, number i for B., etc.) is retained for future use. Entering this most-likely to-be-encountered constraint into the LCS at the next step prior to the main body of calculations (Box 1) saves the system a lot of trouble. Sooner or later, however, any temporal constraint will dr p from the basis and therewith from the LCS.
• Box 8. This stage of computation involves outputting configuration c~f to storage as the latest one in the sequence and installing it as the current configuration cnf.

Boxes 9 and 10. Clear without comments.

VIII SIMULATION RESULTS

To illustrate the performance of the compute~ implemented version of the AVOID system, some of the experiments run with it are displayed below. The chosen manipulator model has four 1inks, each of length 1. Bold lines are used for obstacles and configurations while € -covers ce =0.1) and corridors for the jaws are shown with thin ones.

TASK 1 (Fig.3). The coordinates of the three vertices of the only obstacle available are A 1 =(0.120, -0.120), A 2 =(1.620, -1.620), A 3 =(J.620, -0.120) wh~lst those of the6-cover are (-0.1 21, -0.020), (1.720, -1.861), (1.720,-0.020). The broken line for the jaws'· motion consists of a single segment with ini tial point G 1 =(2.975,l.OOO) and final point c 2 =(2.975, -1.810). It requires 7 steps of the system to solve the problem. The initial (no.l) and the final (No. 8) configuratiore are shown in Fig. 3. In the process of planning the second step the joint B 2 strikes on the edge 3 of the cover, which leads to the insertion of a temporal constraint of type 1. During the plan­ ning of the fourth step, the link 1 2 cuts off the vertex 3 while the joint B takes off the edge 3, which results in dropping fhe first temporal con­ straint. The second tempqral constraint remains in the basis until the end. The distance covered by the jaws for the sequence of 7 steps is 0.763, the mean step size being 0.109.

TASK 2 (Fig.4). The task site here features two obstacles 0 1 and 0 2 and the broken line of 4 segments for the jaws. This rather difficult task takes 36 steps to accomplish. After a number of interactions of the links with both obstacles, the jaws happily reached their destination point c 5 .

TASK 3 (Fig.5) has been solved in 38 steps.

IX CONCLUSIONS

Though the problem of planning manipulator motion in between obstacles has been taken up in a number of papers (see References) no comparative study of different methods for solving this pro­ blem has been attempted. Various facets of our approach may be found in [1]- [4] . Advantages of the AVOID system are as follows.

1. The method is very simple. All the con­ straints imposed on the manipulator, including those implied by obstacles, are imbedded into a set of linear inequalities and treated uniformly by the dual method, neatly fitting the problem format. The amount of computation involved is fairly moderate.
2. Having encountered an opstacle in its way, the system does not attempt any precarious movements in space trying to get clear of it. Instead, the trouble-maker is incorporated into the system for the time being, i.e. as long as it really matters. The moment the obstacle in question does not hinder the manipulator any more, it is dismissed for good. This device provides considerable flexibility for the system.
3. The concept of inheritable basis employed throughout the planning process helps further to reduce the amount of computation.

The described method may be extended to 3dimensional tasks, and to curves and obstacles more sophisticated than those used in the actual work done. It could be integrated into a completely autonomous robot system as an intermediate level of planning manipulator motions. Also, it could be used in a man-machine system for computing a path between points in space specified by a human operator.

As currently implemented, the AVOID system has no facilities for computing velocities and ac­ celerations. The task of modifying the system to enable it to compute dynamic characteristics will need further study.