executing six dof robot motions in complex ... - CiteSeerX

Planning/executing six d.o.f. robot motions in complex environments C. Bellier, C. Laugier, E. Mazer, J. Troccaz



IRIMAG-IMAG/LIFIA 46, Avenue Felix Viallet, 38031 Grenoble Cedex, France

1 Introduction

abstract This paper deals with the problem of planning safe trajectories for a manipulator robot moving in an environment including both static obstacles and objects (other robots, conveyors ...) whose positions may change during the interval of time separating two planning steps. In order to solve this problem for a general manipulator having six degrees of freedom, we have developed and implemented a method combining two dierent path planners: a local planner whose purpose is to take the manipulator away from the constrained initial and nal con gurations, and a global planner which is in charge of generating a safe trajectory for the rst three degrees of freedom of the arm. Both the local and the global planners are based on a fast iterative algorithm for computing the distance between pairs of objects. These planners have been implemented within a robot programming system (called ACT) including a solid modeler dedicated to robotics applications and a connection to a robot controller.

Planning safe trajectories for a manipulator robot operating within an assembly workcell requires to make use of an ecient algorithm having the main following characteristics: (1) the system operates in a known and structured environment, (2) the position and the orientation of some objects (other robots, conveyors ...) may change independently of the robot considered, (3) the planning algorithm must be able to deal with up to six degrees of freedom, and (4) the model of the task (world states and the associated con guration space models) have to be updated as fast as possible after each planning step. Since solving this problem for a robot operating in a full dynamic world is too complex (according to the current state of the art), we have studied a more restricted subproblem: how to plan safe trajectories for a six axes manipulator robot moving in an environment including both static obstacles and objects whose positions may changed during the interval of time separating two planning steps? In order to solve this problem, we have developed and implemented a method combining two dierent path planners: a local planner whose purpose is to take the manipulator

On leave at TIMB, Departement d'Informatique Medicale, Faculte de Medecine, Domaine de la Merci, 38700 La Tronche 

1

has been devised for the purpose of computing in an ecient way the ranges of legal con gurations to associate to each robot's link. The experimental results obtained by applying the method to a SCARA manipulator having four degrees of freedom and to a general six axes revolute robot are presented and discussed in the section 4.

away from the constrained initial and goal con gurations, and a global planner which is in charge of generating a safe trajectory for the rst three degrees of freedom of the arm (excluding wrist rotations). Both the local and the global planners are based on a fast iterative algorithm for computing the distance between pairs of objects. These planners have been implemented within a robot programming system (called ACT) including a solid modeler dedicated to robotics applications and a connection to a robot controller [9]. A large number of theoretical studies and of practical algorithms developed for the purpose of solving various instances of the general path planning problem can be found in the contemporary literature (see [8] and [7]). But very little studies have given rise to the implementation of practical systems capable of controlling a true robot. As previously mentioned, the method described in this paper has been developed as a component of a robot programming system. It has been devised for the purpose of providing the user with interactive software tools for programming manipulation tasks involving several sequences of collisionfree motions. Reasonable computation times ?typically, from a few seconds to a few minutes of elapsed time when dealing with intricate situations? have been obtained by combining several well chosen techniques: modeling the robot world using hierarchical models [3], computing distances between pairs of objects (link-obstacle) using a fast iterative algorithm [3] [5], computing ranges of valid con gurations using a discretized representation [8] [7], and constructing an approximate model of the con guration space by combining several 2n -trees [2] [4]. The section 2 presents the outline of our approach along with the used methods and models. The section 3 describes the method which

2 Outline of our approach

2.1 De nitions and notations

The purpose of this subsection is to brie y introduce the concept of con guration space (Cspace) along with the used notations. Let A be an articulated system made up of p (p  1) rigid elements Li (called links) moving in the Cartesian space W = R3. A con guration q of A is a minimal set of parameters ?i.e. a vector in a n-dimensional space, with n  p? which completely speci es the position and orientation in W of each link Li of A. n represents the number of degrees of freedom (d.o.f) of A; n is not equal to p (i.e. n < p) if A includes some coupled joints. Let A(q) be the location in W of A when A is in the con guration q. The set of the possible values for the con guration vector q is the con guration space CA associated to A, and the sets of con gurations q which generate a collision between A(q) and an obstacle in W are called the C-obstacles. Let Ii be the range of values that can be associated to the i-th joint of the robot; CA is generally characterized by the cartesian product I1  I2 


 In. The associated free-space is the dierence between CA and the set of Cobstacles. In the sequel, we will denote by sweep(Li+1; Li+2;


; Ln ) the swept volume produced by executing a full movement for the links Li+1; Li+2 ;


; Ln (i.e. for all qi+1 2 2

Ii+1; qi+2 2 Ii+2;


; qn 2 In); by the same time, the links L1; L2;


; Li are supposed to be locked at a given location characterized by a partial con guration vector (q1; q2;


; qi). We will denote by A(q1; q2;


; qi?1) the location in W of the links L1; L2;


; Li?1 when the con guration parameters q1; q2;


; qi?1 are xed to a set of given values ?in other words, the reference point of the link Li is set to a given location characterized by the vector (q1; q2;


; qi?1)?.

patch of the discretized surface). This preprocessing phase is automatically performed by the system. It is completed by a modeling process whose purpose is to generate a hierarchy of enclosing volumes, by grouping together the previous components in an appropriate manner (in order to minimize the lost volume). This is obviously very useful for improving the distance determination procedure: this way, an obstacle which is far from the robot can be quickly processed using a rough resolution of this hierarchical model.

2.2 Fast distance computation

2.3 Overview of the path planners

Both the local and the global planners make use of Euclidean distance computations for constructing the required con guration space models. Since such computations are performed at each planning step for a large number of pairs of elementary objects (components of the robot links and of the obstacles), it is of a major importance to reduce the associated computation time. This is why we have implemented a fast iterative algorithm which takes full advantage of the hierarchical structure of the world model. This algorithm is based on the method proposed in [5] and in [3] for computing the distance between pairs of convex shapes and polytopes1 . It has been implemented within the ACT system [9] in such a way that complex geometric shapes can be handled by the path planners (i.e. objects made up of polyhedra, surfaces, cylinders, cones, spheres,


). Since the distance computation algorithm operates upon convex shapes and polytopes, non convex objects have to be decomposed into convex components and polynomial surfaces have to be approximated using simple polytopes (a planar polytope is associated to each

As previously mentioned, our approach for planning trajectories for six d.o.f robots combines a local path planner with a global one.

The local path planner is directly related

to the local method described in [3]. It makes use of a local approximation of the con guration space, constructed at each planning step using a set of tangent hyperplans. The main idea of this method is to separate the task description from the constraints resulting from the obstacles. The task description is given under the form of a quadratic function to minimize. The constraints are obtained by considering the variation of the distance between the links of the robot and the obstacles. Thanks to the hierarchical model of the environment, the system considers only the pairs of \link/obstacle" which become closer at given step of the trajectory. Each active pair generates a linear constraint. Finally, the task function is optimized under the set of these linear constraints. One feature of this planner is its ability to cope with a large number of degrees of freedom. Another important feature of this planner (rel-

a polytope is de ned as the convex hull of a set of vertices 1

3

for any type of robot, but the intrinsic algorithmic complexity of the path planning problem makes it unpractical when n > 4 (n is the number of d.o.f of the robot). This is why the global planner is only used for planning the motions of the rst three links of the arm (i.e. wrist positioning), assuming for that purpose that the next links are temporarily locked at some xed locations.

atively to our problem) is its ability to process incompletely speci ed goals: for instance, a trajectory for the end eector can be only speci ed in terms of the trajectory of a reference point, leaving in this way the orientation of the end eector unspeci ed. Nevertheless, this method does not avoid classical drawbacks associated to local approaches: only close obstacles are considered at each planning step (i.e. for each elementary movement) and it is not complete (no solution can be found even if one exists). Moreover, the generated paths do not guarantee any global optimality.

The mixed approach. Because of the draw-

backs of the local approach (non optimality, no guarantee to nd an existing solution) and of the global one (limited to three d.o.f), we have decided to develop a mixed approach taking advantage of the two methods. The local one is used to plan sub-paths involving a large number of d.o.f (i.e. in too constrained environments). In the current implementation, it is used for taking the manipulator away from the constrained initial and goal con gurations. The global method is used to plan optimal subpaths involving only the rst three d.o.f of the arm (i.e. wrist positioning), assuming for that purpose that the next links are temporarily locked at some xed locations given by the local method. Connecting previous sub-paths (local and global solutions) is performed thanks to the fact that incompletely speci ed goals can be processed by the local planner (for instance, generating new wrist locations without specifying any wrist orientations). Using this approach, wrist re-orientations may be performed in two ways: either the local planner generates a solution which includes the required wrist rotations ?this is generally possible for small reorientations?, or the global planner searches for an appropriate free-area ?using the swept volume? in which the wrist recon guration can take place.

The global path planner makes use of an

approximate representation of the robot con guration space. This representation is built using a recursive algorithm operating on an adapted discretization of the joint space (see [8] and [7]). As we will see further, the con guration space model is obtained by combining several 2n-trees respectively representing the set of C-obstacles which is associated to the static obstacles and several C-obstacles characterizing the obstacles which can move independently of the robot. This computation is done using fast boolean operators [4]. In our approach, each 2n -tree is constructed by recursively computing the ranges of legal con gurations associated to each link of the arm. This is done using an iterative algorithm based on the distance computation between pairs of objects of the type \link-obstacle" (see section 3). Finally, collision-free trajectories are generated using a classical A algorithm operating upon a graph representing the connectivity of the freespace ?a node in this graph represents the free cell (i.e. a connected set of safe con gurations) which is associated to a particular node of the previous 2n -tree?. This approach can be theoretically applied 4

2.4 Constructing the C-space model

For each qij in (Discretization(Ci ? Ci), Return (Con g(q1; q2;


; qi?1; qij ; Li+1)).

An important step of the global path planning process is to construct an explicit representation of the robot con guration space Crobot. As previously mentioned, our system makes use of the method described in [8] and [7] for constructing an approximate model of Crobot. This method consists in recursively computing the ranges of con guration parameters which generate a collision with an obstacle in W , using the hierarchical model of the world and an adapted discretization of the joint space. In the current version of the system, the shape approximation level to consider and the discretization parameter associated to each d.o.f of the robot are speci ed by the user. The implemented method operates recursively among the n d.o.f of the robot, using the following algorithm to deal with the i-th link Li :

The ranges of qi's values generating true collisions (by considering the solid components of Li ) and potential collisions (by considering the swept volumes) are computed using the function \Range" which is described in the section 3. The components which are considered for this computation are extracted from the hierarchical world model according to the chosen shape approximation parameter.

Remark: This algorithm can also be applied

for robots having coupled joints, assuming for that purpose that closed kinematics chains, like parallelograms, are represented by open kinematics chains after having arbitrarily removed one of the coupled joints. In this case, all the links associated to a coupled joint have to be considered together at step 2 of the algorithm.

Con g(q1; q2;


; qi?1; Li)

2.5 Constructing and searching the free-space model

1. /* Set Li to its initial location */ - Compute A(q1; q2;


; qi?1) using the previously computed con guration parameters q1; q2;


; qi?1. - Determine the initial con guration parameters qi (see section 3).

As previously mentioned, our system makes use of a 2n -tree representation for characterizing the part of Crobot which does not include any C-obstacle. Since the global planner is only used for planning the movements of the rst three links of the arm, we will make use of a 23-tree representation (an octree) for modeling the associated con guration space [1]. It has been shown in [4] that such a treelike structure can be easily constructed, modi ed and searched using simple and fast recursive algorithms. For instance, combining several octrees can be done using some fast boolean operators. This property is very useful for solving our problem, because it permits us to quickly update the con guration space model at each planning step. This is why, we

2. Compute the collision ranges Ci = Range(qi; Li); /* The computed values Ci 2 C-obstacles */ 3. If (i < n) then /* Li is not the last link of the arm */ Compute the potential collisions Ci Ci = Range(qi; sweep(Li+1; Li+2;


; Ln)); 5

3 Computing collision ranges

have de ned Crobot as the combination of several octrees: an octree representing the set of C-obstacles which is associated to the set of static obstacles, and several octrees representing the C-obstacles which are associated to the mobile obstacles (other robots, conveyors, ...). Then, simple updating operations on the environment (adding, deleting or changing the position/orientation of an object) can be executed by only modifying the involved octrees (before combining them together).

Two dierent algorithms (aimed at respectively processing prismatic and revolute joints) have been developed for computing the ranges of legal con gurations to associate to a given robot joint. Both algorithms make use of the fast distance computation procedure (see section 2.2). They operate upon several hierarchical world models: one associated to the set of static obstacles, one associated to each mobile obstacle, and one associated to each robot link.

Searching for a safe-path in the previous representation requires to make use of an explicit characterization of the connectivity of the freespace. Since two adjacent regions in Crobot are generally represented by nodes located in different places in the tree structure [4] [1], it is necessary to construct a connectivity graph linking together the nodes representing adjacent free regions of Crobot. This graph is called the free-space model Cfree. It is built using a straightforward algorithm consisting in recursively determining the free neighbors of the current node (there are six possible searching directions for an octree) [4]. Then, searching for a safe-path in the Cfree consists in searching the connectivity graph. This is done using a classical A algorithm and a cost function based on a particular distance in Crobot [7]. The algorithm used to compute a \good" connected set of con gurations (i.e. as \short" and as \smooth" as possible), in the ordered set of nodes representing the free-path generated using the A algorithm, is described in [1]. When no safe path exists at the current level of representation ?the initial and goal con gurations belong to two non connected components of Cfree?, the system provides the user with a simple failure diagnosis [1].

3.1 Dealing with prismatic joints

Let A be a convex component of the considered robot link, B be a convex component belonging to an obstacle, and ~u be the unit vector associated to the joint axis. In the sequel, we will refer by P (X; u~ ) the plane de ned as follows: P (X; u~ ) is tangent to the convex shape X , ~u is the normal vector associated to P (X; u~ ), and ~u is oriented towards the exterior of X . Such a plane can be easily obtained using our distance computation procedure (by determining the closest points on X from a plane P (~u) located on the boundary of W ). Thanks to the convexity property, any range of con gurations of A which generate a collision between A and B is an interval of the type [qsup; qinf ]. The con guration qsup (resp. qinf ) can be easily determined by \moving" A along the ?~u (resp. +~u) direction, starting from a point which generates no collision between A and B . A straightforward way to determine this starting point is to superpose two particular planes: the relation \A above B " can be obtained by placing the plane P (A; ?~u) onto the plane P (B; ~u); the relation \A under B "can be characterized in the same way using the planes P (A; ~u) and P (B; ?~u). Then, the following al6

gorithm can be used to compute qsup:

from the fact that the iterative movements applied to A have as consequence to move each point of A along an arc of circle whose length depends on both the rotation angle and the gyration radius (see below). Since no simple geometric property can be used for de ning an initial collision-free con guration of A in the general case, we have decided to heuristically select this con guration when no obvious solution exists (see gure 2). The applied heuristic consists in exploring the discretized orientation domain of A, until a collision-free con guration has been found. In the particular case where the axis of rotation
is outside A and outside B , we compute the planes, tangent respectively to A and B which are at the minimum distance from
and we put them in opposition ( gure 2). Let [qsup; qinf ] be the collision range associated to the pair (A; B ). The following algorithm can be used to compute qsup :

1. Determine q such that the relation \A(q) above B" holds; d = d(A(q); B ); 2. While (d  ) do q Translate(A(q); ?~u; d); d = d(A(q); B ); If (d > d) then Return (no collision) else d = d ; 3. Return (q);

1. If Slice(P (A; ~u); P (A; ?~u))\ Slice(P (B; ~u); P (B; ?~u)) = ; then Return (no collision); 2. Determine q such that d(A(q); B ) > ; d = d(A(q); B );  = 0; 3. While (d  ) do  = Angle(A; B; d);  =  + ; If (  2) /* A can rotate freely */ then Return (no collision); q Rotate(A(q); ~u; +); d = d(A(q); B ); 4. Return (q);

Mobile A

Mobile A

This algorithm is applied for each pair (A; B ) belonging to the involved pairs of objects (linkobstacle) (see gure 1). The resulting collision range (which is possibly made up of several non connected intervals) is de ned as the union of the collision ranges associated to the processed pairs (A; B ). P(A,-u) -u

u

+u

+u

P(A,-u) = P(B,u)

-u

Fixed B

Fixed B

P(B,u)

Position in the environment

Initialization: "A above B"

ITERATION: dT