CHAPTER 6

Motion Planning for Three-Dimensional Arm Manipulators

The robot is going to lose. Not by much. But when the final score is tallied, flesh and blood is going to beat the damn monster.

—Adam Smith, philosopher and economist, 1723–1790

6.1 INTRODUCTION

We are continuing developing SIM (Sensing–Intelligence–Motion) algorithms for robot arm manipulators. The cases considered in Chapter 5 all deal with arm manipulators whose end effectors (hands) move along a two-dimensional (2D) surface. Although applications do exist that can make use of those algorithms—for example, assembly of microelectronics on a printed circuit board is largely limited to a 2D operation—most robot arm manipulators live and work in three-dimensional (3D) space. From this standpoint, our primary objective in Chapter 5 should be seen as preparing the necessary theoretical background and elucidating the relevant issues, before proceeding to the 3D case. Sensor-based motion planning algorithms should be able to handle 3D space and 3D arm manipulators. Developing such strategies is the objective of this chapter. As before, the arm manipulators that we consider are simple open kinematic chains.

Is there a fundamental difference between motion planning for two-dimensional (2D) and 3D arm manipulators? The short answer is yes, but the question is not that simple. Recall a similar discussion about mobile robots in Chapter 3. From the standpoint of motion planning, mobile robots differ from arm manipulators: They have more or less compact bodies, kinematics plays no decisive role in their motion planning, and their workspace is much larger compared to their dimensions. For mobile robots the difference between the 2D and 3D cases is absolute and dramatic: Unequivocally, if the 2D case has a definite and finite solution to the planning problem, the 3D case has no finite solution in general.

The argument goes as follows. Imagine a bug moving in the two-dimensional plane, and imagine that on its way the bug encounters an object (an obstacle). Assume the bug's target location is somewhere on the other side of the obstacle. One way for it to continue its motion is to first pass around the obstacle. The bug has only two options: It can pass the obstacle from the left or it can pass it from the right, clockwise or counterclockwise. If neither option leads to success—let us assume it is a smart bug, with a reasonably good motion planning skills—the goal is not achievable. In this case, by slightly exaggerating the bug's stubbornness, we will note that eventually the bug will come back to the point on the obstacle where it started. It took only one simple going around, all in one direction, to explore the whole obstacle. That is the essence of the 2D case.

Imagine now a fly that is flying around a room—that is, in 3D space. Imagine that on its way the fly encounters a (3D) obstacle—say, a child's balloon hanging on a string. Now there is an infinite number of routes the fly can take to pass around the obstacle. The fly would need to make a great many loops around the obstacle in order to explore it completely. That's the fundamental difficulty of the 3D case; in theory it takes an infinitely long path to explore the whole obstacle, even if its dimensions and volume are finite and modest.¹ The point is that while in the 2D case a mobile robot has a theoretically guaranteed finite solution, no such solution can be guaranteed for a 3D mobile robot. The 3D sensor-based motion planning problem is in general intractable.

The situation is more complex, but also not as hopeless, for 3D arm manipulators. Try this little experiment. Fix your shoulder and try to move your hand around a long vertical pole. Unlike a fly that can make as many circles around the pole as it wishes, your hand will make about one circle around the pole and stop. What holds it from continuing moving in the same direction is the arm's kinematics and also the fact that the arm's base is “nailed down.” The length of your arm links is finite, the links themselves are rigid, and the joints that connect the links allow only so much motion. These are natural constraints on your arm movement. The same is so for robot arm manipulators. In other words, the kinematic constraints of an arm manipulator impose strong limitations on its motion.

This fact makes the problem of sensor-based motion planning for 3D arm manipulators manageable. The hope is that the arm kinematics can be effectively exploited to make the problem tractable. Furthermore, those same constraints promise a constructive test of target reachability, similar to those we designed above for mobile robots and 2D arm manipulators.

As noted by Brooks [102], the motion planning problem for a manipulator with revolute joints is inherently difficult because (a) the problem is nondecomposable, (b) there may be difficulties associated with rotations, (c) the space representation and hence the time execution of the algorithm are exponential in the number of robot's degrees of freedom of the objects involved, and (d) humans are especially poor at the task when much reorientation is needed, which makes it difficult to develop efficient heuristics. This is all true, and indeed more true for arms with revolute joints—but these difficulties have been formulated for the motion planning problem with complete in formation. Notice that these difficulties above did not prevent us from designing rather elegant sensor-based planning algorithms for 2D arms with revolute joints, even in the workspace with arbitrarily complex obstacles. The question now is how far we can go with the 3D case.

It was said before that this is a difficult area of motion control and algorithm design. As we will see in Chapter 7, human intuition is of little help in designing reasonable heuristics and even in assessing proposed algorithms. Doing research requires expertise from different areas, from topology to sensing technology. There are still many unclear issues. Much of the exciting research is still waiting to be done. Jumping to the end of this chapter, today there are still no provable algorithms for the 3D kinematics with solely revolute joints. While this type of kinematics is just one mechanism among others in today's robotics, it certainly rules the nature.

As outlined in Section 5.1.1, we use the notion of a separable arm [103], which is an arm naturally divided into the major linkage responsible for the arm's position planning (or gross motion), and the minor linkage (the hand) responsible for the orientation planning of the arm's end effector. As a rule, existing arm manipulators are separable. Owing to the fact that three degrees of freedom (DOF) is the minimum necessary for reaching an arbitrary point in 3D space, and another three DOF are needed to provide an arbitrary orientation for the tool—six DOF in total as a minimum—many 3D arm manipulators' major linkages include three links and three joints, and so do typical robot hands. Our motion planning algorithms operate on the major linkage—that is, on handling gross motion and making sure that the hand is brought into the vicinity of the target position. The remaining “fine tuning” for orientation is usually a simpler task and is assumed to be done outside of the planning algorithm. For all but very few unusual applications, this is a plausible assumption.

While studying topological characteristics of the robot configuration space for a few 3D kinematics types, we will show that obstacle images in these configuration spaces exhibit a distinct property that we call space monotonicity : For any point on the surface of the obstacle image, there exists a direction along which all the remaining points of the configuration space belong to the obstacle. Furthermore, the sequential connection of the arm links results in the property called space anisotropy of the configuration space, whereby the obstacle monotonicity presents itself differently along different space axes.

The space monotonicity property provides a basis for selecting directions of arm motion that are more promising than others for reaching the target position. By exploiting the properties of space monotonicity and anisotropy, we will produce motion planning algorithms with proven convergence. No explicit or implicit beforehand calculations of the workspace or configuration space will be ever needed. All the necessary calculations will be carried out in real time in the arm workspace, based on its sensing information. No exhaustive search will ever take place.

As before with 2D arms, motion planning algorithms that we will design for 3D arms will depend heavily on the underlying arm kinematics. Each kinematics type will require its own algorithm. The extent of algorithm specialization due to arm kinematics will be even more pronounced in the 3D case than in the 2D case. Let us emphasize again that this is not a problem of depth of algorithmic research but is instead a fundamental constraint in the relationship between kinematics and motion. The same is true, of course, in nature: The way a four-legged cat walks is very different from the way a two-legged human walks. Among four-legged, the gaits of cats and turtles differ markedly. One factor here is the optimization process carried out by the evolution. Even if a “one fits all” motion control procedure is feasible, it will likely be cumbersome and inefficient compared to algorithms that exploit specific kinematic peculiarities. We observed this in the 2D case (Section 5.8.4): While we found a way to use the same sensor-based motion planning algorithm for different kinematics types, we also noted the price in inefficiency that this universality carried. Here we will attempt both approaches.

This is not to say that the general approach to motion planning will be changing from one arm to another; as we have already seen, the overall SIM approach is remarkably the same independent of the robot kinematics, from a sturdy mobile robot to a long-limbed arm manipulator.

As before, let letters P and R refer to prismatic and revolute joints, respectively. We will also use the letter X to represent either a P or a R joint, X = [P, R]. A three-joint robot arm manipulator (or the major linkage of an arm), XXX, can therefore be one of eight basic kinematic linkages: PPP, RPP, PRP, RRP, PPR, RPR, PRR, and RRR. As noted in Ref. 111, each basic linkage can be implemented with different geometries, which produces 36 linkages with joint axes that are either perpendicular or parallel to one another. Among these, nine degenerate into linkages with only one or two DOF; seven are planar. By also eliminating equivalent linkages, the remaining 20 possible spatial linkages are further reduced to 12, some of which are of only theoretical interest.

The above sequence XXX is written in such an order that the first four linkages in it are XXP arms; in each of them the outermost joint is a P joint. Those four are among the five major 3D linkages (see Figure 6.1) that are commonly seen in industry [111–113] and that together cover practically all today's commercial and special-purpose robot arm manipulators. It turns out that these four XXP arms are better amenable to sensor-based motion planning than the fifth one (Figure 6.1e) and than the remaining four arms in the XXX sequence. It is XXP arms that will be studied in this chapter.

While formally these four linkages—PPP, RPP, PRP, and RRP—cover a half of the full group XXX, they represent four out of five, or 80%, of the linkages in Figure 6.1. Many, though not all, robot arm applications are based on XXP major linkages. Welding, assembly, and pick-and-place robot arms are especially common in this group, one reason being that a prismatic joint makes it easy to produce a straight-line motion and to expand the operating space. The so-called SCARA arm (Selective Compliance Assembly Robot Arm), whose major linkage is, in our notation, an RRP arm, is especially popular among assembly oriented industrial arm manipulators.

Figure 6.1 Five commonly used 3D robot arm linkages: (a) RRP, “polar coordinates” arm, with a spherical workspace; (b) PRP, “cylindrical coordinates” arm, with a cylindrical workspace; (c) PPP, Cartesian arm, with a “cubicle” workspace; (d) RRP, SCARA-type arm, with a cylindrical workspace; and (e) RRR, “articulate” arm, with a spherical workspace.

Recall our showing in Chapter 5 that RR, the arm with two revolute joints, is the most general among XX arms, X = [P, R]. We will similarly show that the arm RRP is the most general case among XXP arms. The algorithm that works for the RRP arm will work for other XXP arms.

While arm configurations shown in Figure 6.1 have nice straight-line links that are mutually perpendicular or parallel, our SIM approach does not require such properties unless specified explicitly.

We will first analyze the PPP arm (Section 6.2), an arm with three sliding (prismatic) joints (it is often called the Cartesian arm), and will develop a sensor-based motion planning strategy for it. Similar to the 2D Cartesian arm, the SIM algorithm for a 3D Cartesian arm turns out to be the easiest to visualize and to design. After mastering in this case the issues of 3D algorithmic machinery, in Section 6.3 we will turn our attention to the general case of an XXP linkage. Similar to the material in Section 5.8, some theory developed in Section 6.2.4 and Sections 6.3 to 6.3.6 is somewhat more complex than most of other sections.

As before, we assume that the arm manipulator has enough sensing to sense nearby obstacles at any point of its body. A good model of such sensing mechanism is a sensitive skin that covers the whole arm body, similar to the skin on the human body. Any other sensing mechanism will do as long as it guarantees not missing potential obstacles. Similar to the algorithm development for the 2D case, we will assume tactile sensing: As was shown in prior chapters, the algorithmic clarity that this assumption brings is helpful in the algorithm design. We have seen in Sections 3.6 and 5.2.5 that extending motion planning algorithms to more information-rich sensing is usually relatively straightforward. Regarding the issues of practical realization of such sensing, see Chapter 8.

6.2 THE CASE OF THE PPP (CARTESIAN) ARM

The model, definitions, and terminology that we will need are introduced in Section 6.2.1. The general idea of the motion planning approach is tackled in Section 6.2.2. Relevant analysis appears in Sections 6.2.3 and 6.2.4. We formulate, in particular, an important necessary and sufficient condition that ties the question of existence of paths in the 3D space of this arm to existence of paths in the projection 2D space (Theorem 6.2.1). This condition helps to lay a foundation for “growing” 3D path planning algorithms from their 2D counterparts. The corresponding existential connection between 3D and 2D algorithms is formulated in Theorem 6.2.2. The resulting path planning algorithm is formulated in Section 6.2.5, and examples of its performance appear in Section 6.2.6.

6.2.1 Model, Definitions, and Terminology

For the sake of completeness, some of the material in this section may repeat the material from other chapters.

• υ_i is the set of points reachable by point J_i, i = 1, 2, 3;
• V_i is the set of points (the volume) reachable by any point of link l_i. Hence,
• υ₁ is a single point, O;
• υ₂ is a unit line segment, Oa;
• υ₃ is a unit square, Oabc;
• V₁ is a cylinder whose (link) cross section is s₁ and whose length is 2l_{1 max};
• V₂ is a slab of length 2l_{2 max} formed by all possible motions of the front and rear ends of link l₂ within the joint limits of l₁ and l₂;
• V₃ is a “cubicle” of height 2l_{3 max} formed by all possible motions of the front and rear ends of link l₃ within the joint limits of l₁, l₂, and l₃.

Figure 6.3 (a) The effective workspace of the 3D Cartesian arm—the volume that can be reached by the arm endpoint—is limited by the cubicle abcodefg. (b) Since the rear end of every link may also encounter obstacles, the workspace that has to be protected by the planning algorithm is larger than the effective workspace, as shown.

The total volume V_W of W-space is hence V_W = V₁ ∪ V₂ ∪ V₃. Out of this, the set {l} = {l ∈ [0, l_max]}, where l_max = (l_{1 max}, l_{2 max}, l_{3 max}), represents points reachable by the arm end effector; {l} is a closed set.

An obstacle in W-space, called W-obstacle, presents a set of points, none of which can be reached by any point of the robot body. This may include some areas of W-space which are actually free of obstacles but still not reachable by the arm because of interference with obstacles. Such areas are called the shadows of the corresponding obstacles. A W-obstacle is thus the sum of volumes of the corresponding physical obstacle and the shadows it produces. The word “interference” refers here only to the cases where the arm can apply a force to the obstacle at the point of contact. For example, if link l₁ in Figure 6.2 happens to be sliding along an obstacle (which is not so in this example), it cannot apply any force onto the obstacle, the contact would not preclude the link from the intended motion, and so it would not constitute an interference. W-obstacles that correspond to the three physical obstacles—O₁, O₂, and O₃—of Figure 6.2 are shown in Figure 6.4.

Figure 6.4 The W-obstacles produced by obstacles shown in Figure 6.2 consist of the parts of physical obstacles that intersect W-space plus their corresponding shadows.

• M-plane is a plane that contains an M-line and the straight line perpendicular to both the M-line and link l₃ axis. M-plane is thus undetermined only if the M-line is collinear with l₃ axis. This special case will present no difficulty: Here motion planning is trivial and amounts to changing only values l₃; hence we will disregard this case.
• V-plane contains the M-line and is parallel to link l₃ axis.

For our Cartesian arm, the M-line, M-plane, and V-plane map in C-space into a straight line and two planes, respectively.

Figure 6.5 C-space and C-obstacles that correspond to W-space in Figures 6.2 and 6.4. Thicker dotted and solid lines show intersections between obstacles. Shown also are projections of the three obstacles on the plane l₁, l₂.

1. The C-point moves along the M-line or along an intersection curve between the M-plane and an obstacle and is about to leave M-plane at the cross-point. Define the normal vector m of the M-plane [97]. Then the local direction b is upward if b · m > 0 and downward if b · m ≤ 0.
2. The C-point moves along the M-line or along an intersection curve between the V-plane and an obstacle, and it is about to leave V-plane at the cross-point. Let be the vector of l₃ axis. Then, local direction b is left if and right if .
3. In a special case of motion along the M-line, the directions are = forward and = backward.

Consider the motion of a C-point in the M-plane. When, while moving along the M-line, the C-point encounters an obstacle, it may define on it a hit point, H. Here it has two choices for following the intersection curve between the M-plane and the obstacle surface: Looking from S toward T, the direction of turn at H is either left or right. We will see that sometimes the algorithm may replace the current local direction by its opposite. When, while moving along the intersection curve in the M-plane, the C-point encounters the M-line again at a certain point, it defines here the leave point, L. Similarly, when the C-point moves along a V-plane, the local directions are defined as “upward” and “downward,” where “upward” is associated with the positive and “downward”—with the negative direction of l₃ axis.

6.2.2 The Approach

Similar to other cases of sensor-based motion planning considered so far, conceptually we will treat the problem at hand as one of moving a point automaton in the corresponding C-space. (This does not mean at all, as we will see, that C-space needs to be computed explicitly.) Essential in this process will be sensing information about interaction between the arm and obstacles, if any. This information—namely, what link and what part (front or rear) of the link is currently in contact with an obstacle—is obviously available only in the workspace.

Our motion planning algorithm exploits some special topological characteristics of obstacles in C-space that are a function of the arm kinematics. Note that because links l₁, l₂, and l₃ are connected sequentially, the actual number of degrees of freedom available to them vary from link to link. For example, link l₁ has only one degree of freedom: If it encounters an obstacle at some value , it simply cannot proceed any further. This means that the corresponding C-obstacle occupies all the volume of C-space that lies between the value and one of the joint limits of joint J₁. This C-obstacle thus has a simple structure: It allows the algorithm to make motion planning decisions based on the simple fact of a local contact and without resorting to any global information about the obstacle in question.

A similar analysis will show that C-obstacles formed by interaction between link l₂ and obstacles always extend in C-space in the direction of one semi-axis of link l₂ and both semi-axes of link l₃; it will also show that C-obstacles formed by interaction between link l₃and obstacles present generalized cylindrical holes in C-space whose axes are parallel to the axis l₃. No such holes can appear, for example, along the axes l₁ or l₂. In other words, C-space exhibits an anisotropy property; some of its characteristics vary from one direction to the other. Furthermore, C-space possesses a certain property of monotonicity (see below), whose effect is that, no matter what the geometry of physical obstacles in W-space, no holes or cavities can appear in a C-obstacle.

From the standpoint of motion planning, the importance of these facts is in that the local information from the arm's contacts with obstacles allow one to infer some global characteristics of the corresponding C-obstacle that help avoid directions of motion leading to dead ends and thus avoid an exhaustive search.

Whereas the resulting path planning algorithm is used in the workspace, without computations of C-space, it can be conveniently sketched in terms of C-space, as follows. If the C-point meets no obstacles on its way, it will move along the M-line, and with no complications the robot will happily arrive at the target position T. If the C-point does encounter an obstacle, it will start moving along the intersection curve between the obstacle and one of the planes, M-plane or V-plane. The on-line computation of points along the intersection curve is easy: It uses the plane's equation and local information from the arm sensors.

If during this motion the C-point meets the M-line again at a point that satisfies some additional condition, it will resume its motion along the M-line. Otherwise, the C-point may arrive at an intersection between two obstacles, a position that corresponds to two links or both front and rear parts of the same link contacting obstacles. Here the C-point can choose either to move along the intersection curve between the plane and one of the obstacles, or move along the intersection curve between the two obstacles. The latter intersection curve may lead the C-point to a wall, a position that corresponds to one or more joint limits. In this case, depending on the information accumulated so far, the C-point will conclude (correctly) either that the target is not reachable or that the direction it had chosen to follow the intersection curve would lead to a dead end, in which case it will take a corrective action.

At any moment of the arm motion, the path of the C-point will be constrained to one of three types of curves, thus reducing the problem of three-dimensional motion planning to the much simpler linear planning:

• The M-line
• An intersection curve between a specially chosen plane and the surface of a C-obstacle
• An intersection curve between the surfaces of two C-obstacles

To ensure convergence, we will have to show that a finite combination of such path segments is sufficient for reaching the target position or concluding that the target cannot be reached. The resulting path presents a three-dimensional curve in C-space. No attempt will be made to reconstruct the whole or part of the space before or during the motion.

Since the path planning procedure is claimed to converge in finite time, this means that never, not even in the worst case, will the generated path amount to an exhaustive search.

An integral part of the algorithm is the basic procedure from the Bug family that we considered in Section 3.3 for two-dimensional motion planning for a point automaton. We will use, in particular, the Bug2 procedure, but any other convergent procedure can be used as well.

6.2.3 Topology of W-Obstacles and C-Obstacles

In what follows, most of the analysis of obstacle characteristics is done in terms of C-space, although it applies to W-space as well. Comparing Figures 6.2 and 6.5, note that although physical obstacles occupy a relatively little part of the arm's workspace, their interference with the arm motion can reduce, often dramatically, the volume of points reachable by the arm end effector. The kinematic constraints are due to the arm joints, acting differently for different joint types, and to the fact that arm links are connected in series. As a result, the arm effectively has only one degree of freedom for control of motion of link l₁, two degrees of freedom for control of link l₂, and three degrees of freedom for control of link l₃. A simple example was mentioned above on how this can affect path planning: If during the arm motion along M-line the link l₁ hits an obstacle, then, clearly, the task cannot be accomplished.

The monotonicity property implies that C-obstacles, though not necessarily convex, have a very simple structure. This special topology of W- and C-obstacles will be factored into the algorithm; it allows us, based on a given local information about the arm interaction with the obstacle, to predict important properties of the (otherwise unknown) obstacle beyond the contact point. The monotonicity property can be expressed in terms more amenable to the path planning problem, as follows:

W-obstacle monotonicity affects differently different links and even different parts—front or rear—of the same link. This brings about more specialized notions of l_i-front and l_i-rear monotonicity for every link, i = 1, 2, 3 (see more below). By treating links' interaction with obstacles individually and by making use of the information on what specific part—front or rear—of a given link is currently in contact with obstacles, the path planning algorithm takes advantage of the obstacle monotonicity property. Because this information is not available in C-space, the following holds:

We will now consider some elemental planar interactions of arm links with obstacles, and we will show that if a path from start to target does exist, then a combination of elemental motions can produce such a path. Define the following:

• Type I obstacle corresponds to a W- or C-obstacle that results from the interaction of link l₁ with a physical obstacle.
• Type II obstacle corresponds to a W- or C-obstacle that results from the interaction of link l₂ with a physical obstacle.
• Type III obstacle corresponds to a W- or C-obstacle that results from the interaction of link l₃with a physical obstacle.

We will use subscripts “+“ and “−“ to further distinguish between obstacles that interact with the front and rear part of a link, respectively. For example, a Type III₊ obstacle refers to a C-obstacle produced by interaction of the front part of link l₃ with some physical obstacle.

In the next section we will analyze separately the interaction of each link with obstacles. Each time, three cases are considered: when an obstacle interacts with the front part, the rear part, or simultaneously with both parts of the link in question. We will also consider the interaction of a combination of links with obstacles, setting the foundation for the algorithm design.

As a result, a Type II₊ collision, as at point H in Figure 6.7, indicates that any motion directly upward or downward from H along the obstacle will necessarily bring the C-point to one of the side walls of the C-space cubicle. This suggests that a plane can be chosen such that the exploration of the intersection curve between this plane and the Type II₊ obstacle will produce a more promising outcome that will result either in a success or in the correct conclusion that the target cannot be reached. In the algorithm, the M-plane will be used, which offers some technical advantages. In general, all three arm joints will participate in the corresponding motion.

Figure 6.7 (a) W-space and (b) C-space with a Type II obstacle. (S, T) is the M-line; HabL is a part of the intersection curve between the obstacle O and M-plane.

For this case (front part of link l₂ interacting with an obstacle), the decision on which local direction, right or left, is to be taken at a hit point H in order to follow the intersection curve between an M-plane and a Type II₊ obstacle is made in the algorithm based on the following rule:

Rule 1:

If l_{1 H} > l_{1 T}, the current direction is “left.”

If l_{1 H} > l_{1 T}, the current direction is “right.”

If l_{1 H} > l_{1 T}, the target cannot be reached.

In terms of decision-making, this case is similar to the one above, except that the direction of obstacle monotonicity along l₂ axis reverses, and the choice of the current local direction at a hit point H obeys a slightly different rule:

Rule 2:

If l_{1 H} > l_{1 T}, the current direction is “right.”

If l_{1 H} < l_{1 T}, the current direction is “left.”

If l_{1 H} = l_{1 T}, the target cannot be reached.

Figure 6.8 C-space in the case when both front and rear parts of link l₂ interact with obstacles, producing a single obstacle that is a combination of a Type II₊ and Type II₋ obstacles.

This property results in a special “stalactite” shape of Type III₊ obstacles. A typical property of icicles and of beautiful natural stalactites that hang down from the ceilings of many caves is that their horizontal cross section is continuously reduced (in theory at least) from its top to its bottom. Each Type III₊ obstacle behaves in a similar fashion. It forms a “stalactite” that hangs down from the ceiling of the C-space cubicle, and its horizontal cross section can only decrease, with its maximum horizontal cross section being at the ceiling level, l₃ = l_{3 max}(see cubicle Oabcdefg and obstacle O₁, Figure 6.4). For any two horizontal cross sections of a Type III₊ obstacle, taken at levels and such that , the projection of the first cross section ( level) onto a horizontal plane contains no points that do not belong to the similar projection of the second cross section ( level). This behavior is the reflection of the monotonicity property.

Because of this topology of Type III₊ obstacles, the sufficient motion for maneuvering around any such obstacle—that is, motion sufficient to guarantee convergence—turns out to be motion along the intersection curves between the corresponding C-obstacle and either the M-plane or the V-plane (specifically, its part below M-plane), plus possibly some motion in the floor of the C-space cubicle (Figure 6.9).

Figure 6.9 C-space with a Type III obstacle. (a) Curve abc is the intersection curve between the obstacle and V-plane that would be followed by the algorithm. (b) Here the Type III obstacle intersects the floor of C-space. Curve aHbLc is the intersection curve between the obstacle and V-plane and C-space floor that would be followed by the algorithm.

The motion sufficient for maneuvering around a Type III₋ obstacle and for guaranteeing convergence is motion along the curves of intersection between the corresponding C-obstacle and either the M-plane or the V-plane (its part above M-plane), or the ceiling of the C-space cubicle.

Figure 6.10 C-space in the case when both front and rear parts of link l₃ interact with obstacles, producing a single obstacle that is a combination of a Type III₊ and Type III₋ obstacles.

As in the case of intersection between the Type III obstacle and the V-plane (see above), one of the two possible local directions is clearly preferable to the other. For example, when in Figure 6.11 the C-point reaches, at point b, the intersection curve between obstacles O₂ and O₃, it is clear from the monotonicity property that the C-point should choose the upward direction to follow the intersection curve. This is because the downward direction is known to lead to the base of the obstacle O₂ “stalagmite” and is thus less promising (though not necessarily hopeless) as the upward direction (see path segments bc and cL, Figure 6.11).

Figure 6.11 The path in C-space in the presence of obstacles O₂ and O₃ of Figure 6.2; SHabcLdeT is the actual path of the arm endpoint, and curve S′H′a′…T′ is its projection onto the plane (l₁, l₂).

Let us stress that in spite of seeming multiplicity of cases and described elemental strategies, their logic is the same: All elemental strategies force the C-point to move either along the M-line, or along the intersection curves between a C-obstacle and a plane (M-plane, V-plane, or C-space side planes), or along the intersection curves between two Type III C-obstacles. Depending on the real workspace obstacles, various combinations of such path segments may occur. We will show in the next section that if in a given scene there exists a path to the target position, a combination of elemental strategies is sufficient to produce one.

6.2.4 Connectivity of

A space or manifold is connected relative to two points in it if there is a path that connects both points and that lies fully in the space (manifold). For a given path l, the corresponding trajectory l(t) defines this path as a function of a scalar variable t; for example, t may indicate time. Denote the 2D Cartesian space formed by joint values l₁, l₂ as .

We intend to show here that for the 3D Cartesian arm the connectivity in can be deduced from the connectivity in _p. Such a relationship will mean that the problem of path planning for the 3D Cartesian arm can be reduced to that for a point automaton in the plane, and hence the planar strategies of Chapter 3 can be utilized here, likely with some modifications.

Define the conventional projection P_c(E) of a set of points E = {(l₁, l₂, l₃)} ⊆ onto space _p as . Thus, _c(S), _c(T), _c(M-line), and _c({O}) are, respectively, the projections of points S and T, the M-line, and C-obstacles onto _p. See, for example, projections _c of three obstacles, O₁, O₂, O₃ (Figure 6.12). It is easy to see that _c(O₁ ∩ O₂) = _c(O₁) ∩ _c(O₂).

Define the minimal projection P_m(E) of a set of points E = {(l₁, l₂, l₃)}, ⊆ onto space _p as _m(E) = {(l₁, l₂) | ∀ l₃, (l₁, l₂, l₃) ∈ E}. Thus, if a C-obstacle O stretches over the whole range of l₃ ∈ [0, l_{3 max}], and E contains all the points in O, then _m(E) is the intersection between the (l₁, l₂)-space and the maximum cylinder that can be inscribed into O and whose axis is parallel to l₃. Note that if a set E is a cylinder whose axis is parallel to the l₃ axis, then _c(E) = _m(E). Type I and Type II obstacles present such cylinders. In general, _m(S) = _m(T) = = .

If the hypothesis is not true, then . We have . Thus a contradiction.

The next statement provides a sufficient condition for the existence of a path in C-space:

Let l_p(t) = {l₁(t), l₂(t)} be a trajectory of P_c(C-point) between _c(S) and _c(T) in _p. From Lemma 6.2.1, in . Hence, for example, the path connects S and T in .

To find the necessary condition, we will use the notion of a minimal projection. The following statement asserts that a zero overlap between two sets in implies a zero overlap between their minimal projections in _p:

By definition, and . Thus, if _m(E) ∩ _m(O) = , then .

To use this lemma in the algorithm design, we need to describe minimal projections for different obstacle types. For any Type I or Type II obstacle O, _c(O) = _m(O). For a Type III obstacle we consider three cases, using, as an example, a Type III₊ obstacle; denote it O₊.

• O₊ intersects the floor F of . Because of the monotonicity property, _m(O₊) = O₊ ∩ F. In other words, the minimal projection of O₊ is exactly the intersection area of O₊ with the floor F.
• O₊ intersects with a Type III₋ obstacle, O₋. Then, B(_m(O₊ ∪ O₋)) = _c(B(O₊) ∩ B(O₋)), where B(O) refers to the boundary of O. That is, the boundary curve of the combined minimal projection of O₊ and O₋ is the conventional projection of the intersection curve between the boundary surfaces of O₊ and O₋.
• Neither of the above cases apply. Then _m(O₊) = .

A similar argument can be carried out for a Type III₋ obstacle.

We now turn to establishing a necessary and sufficient condition that ties the existence of paths in the plane _p with that in . This condition will provide a base for generalizing, in the next section, a planar path planning algorithm to the 3D space. Assume that points S and T lie outside of obstacles.

First, we prove the necessity. Let l(t), t ∈ [0, 1], be a trajectory in . From Lemma 6.2.3, _m(l(t)) ∩ _m({O}) = . Hence, the path _m(l(t)) connects _c(S) and _c(T) in _p.

To show the sufficiency, let l_p(t), t ∈ [0, 1], be a trajectory in _p and let l_p(·) be the corresponding path. Then presents a manifold in . Define and let E^c be the complement of E in . We need to show that E^c consists of one connected component. Assume that this is not true. For any t_* ∈ [0, 1], since l_p(t_*) ∩ _m({O} = , there exists l_3* such that point (l_p(t_*), l_3*) ∈ E^c. The only possibility for E^c to consist of two or more disconnected components is when there exists t_* and a set , such that (l_p(t_*), l_3*) ∈ E while and . However, this cannot happen because of the monotonicity property of obstacles. Hence E^c must be connected, and since points S and T lie outside of obstacles, then S, T ∈ E^c. Q.E.D.

1. A wall is met; this corresponds to _c(C-point) encountering an obstacle. Algorithm A_p now has to decide whether _c(C-point) will move along _c(M-line), or turn left or right to go around the obstacle. Accordingly, the C-point will choose to reverse its local direction along the M-line, or to turn left or right to go around the wall. In the latter case we choose a path along the intersection curve between the wall and the M-plane, which combines two advantages: (i) While not true in the worst case, due to obstacle monotonicity the M-plane typically contains one of the shortest paths around the obstacle; and (ii) after passing around the obstacle, the C-point will meet the M-line exactly at the point of its intersection with the obstacle (point L, Figure 6.7), and so the path will be simpler. In general, all three joints participate in this motion.
2. A Type III₊ or III₋ obstacle is met. The C-point cannot proceed along the M-line any longer. The local objective of the arm here is to maneuver around the obstacle so as to meet the M-line again at a point that is closer to T than the encounter point. Among various ways to pass around the obstacle, we choose here motion in the V-plane. The intersection curve between the Type III obstacle and the V-plane is a simple planar curve. It follows from the monotonicity property of Type III obstacles that when the front (rear) part of link l₃ hits an obstacle, then any motion upward (accordingly, downward) along the obstacle will necessarily bring the C-point to the ceiling (floor) of the C-space. Therefore, a local contact information is sufficient here for a global planning inference—that the local direction downward (upward) along the intersection curve between the V-plane and the obstacle is a promising direction. In the example in Figure 6.9a, the resulting motion produces the curve abc.

1. The M-line is encountered, as at point L₁, Figure 6.12; in _p this means _c(M-line) is encountered. At this point, algorithm A_p will decide whether _c(C-point) should start moving along the _c(M-line) or continue moving along the intersection curve between the M-plane and the obstacle boundary. Accordingly, the C-point will either resume its motion along the M-line, as in Motion I, or will keep moving along the M-plane/obstacle intersection curve.
2. A Type III₊ or III₋ obstacle is met. In _p, the _c(C-point) keeps moving in the same local direction along the obstacle boundary (path segments bcL and b′L′, Figure 6.11). As for C-point, there are two possible directions for it to follow the intersection curve between the Type III obstacle and the wall. Since the Type III₊ (or III₋) monotonicity property is preserved here, the only promising local direction for passing around the Type III₊ (III₋) obstacle is downward (upward). This possibility corresponds to Motion IV below.

Figure 6.12 The path in C-space in the presence of obstacles O₁, O₂ and O₃ of Figure 6.2. Trajectory SH₁aL₁bH₂cdL₂eT is the actual path of the arm endpoint; is its projection onto the plane (l₁, l₂).

1. The M-line is met. C-point resumes its motion along the M-line as in Motion I; see path segments bc, cT and b′c′, c′T′, Figure 6.9a.
2. A wall is met. This corresponds to the _m(C-point) encountering an obstacle. According to the algorithm A_p, _c(C-point) will either reverse its local direction to move along _c(M-line) or will make a turn to follow the obstacle. Accordingly, the C-point will either (a) reverse its local direction to follow the intersection curve between the V-plane and the (Type III) obstacle or (b) try to go around the union of the wall and the obstacle. For the latter motion we choose a path along the intersection curve between the wall and the Type III obstacle.
3. Another Type III₊ or III₋ obstacle is met. Since the _m projection of both Type III obstacles onto _p is not zero, this corresponds to the _c(C-point) encountering an obstacle, which presents the _m projection of the intersection curve between both obstacles. According to A_p algorithm, the _c(C-point) will either (a) reverse its local direction to move along _c(M-line) or (b) make a turn to follow the obstacle. Accordingly, the C-point will either (a) reverse its local direction to follow the intersection curve between V-plane and the Type III obstacle or (b) try to go around the union of two Type III obstacles. For the latter motion, we choose a path along the intersection curve between the two Type III obstacles in the local direction left or right decided by A_p.

1. The C-point returns to the M-plane. Then C-point resumes its motion along the intersection curve between the M-plane and the wall, similar to Motion II.
2. The V-plane is encountered (see point L, Figure 6.11). In _p this means that _c(M-line) is encountered. At this point, algorithm A_p will decide whether the _c(C-point) should start moving along the _c(M-line) or should continue moving along the obstacle boundary. Accordingly, the C-point will either (a) continue moving along the intersection curve between the Type III obstacle and the wall or (b) move along the intersection curve between V-plane and the Type III obstacle in the only possible local direction, as in Motion III.
3. Another Type III obstacle is encountered. Then there will be a nonzero projection of the intersection curve between two Type III obstacles onto _p; the _c(C-point) will continue following the obstacle boundary. Accordingly, the C-point will follow the intersection curve between the two Type III obstacles in the only possible local direction, see Motion V.

1. The V-plane is encountered (point L₂, Figure 6.12). In _p this means that _c(M-line) is encountered. At this point, algorithm A_p will decide whether the _c(C-point) should start moving along the _c(M-line) or should continue moving along the obstacle boundary in one of the two possible directions. Accordingly, the C-point will either (a) move along the intersection curve between the V-plane and the Type III obstacle that is known to lead to the M-plane (as in Motion III.3 above) or (b) keep moving along the intersection curve between two Type III obstacles.
2. A wall is encountered. In _p this corresponds to continuous motion of the _c(C-point) along the obstacle boundary. Accordingly, the C-point starts moving along the intersection curve between the newly encountered wall and one of the two Type III obstacles—the one that is known to lead to the M-line (as in Motion IV).

To summarize, the above analysis shows that the five motions that exhaust all distinct possible motions in can be mapped uniquely into two categories of possible motions in _p—along the _c(M-line) and along _m({O})—that constitute the trajectory of the _c(C-point) in _p under algorithm A_p. Furthermore, we have shown how, based on additional information on obstacle types that appear in , any decision by algorithm A_p in _p can be transformed uniquely into the corresponding decision in . This results in a path in that has the same convergence characteristics as its counterpart in _p. Hence we have the following theorem:

6.2.5 Algorithm

Theorem 6.2.2 states that an algorithm for sensor motion planning for the 2D Cartesian robot arm can be extended to a 3D algorithm, while preserving the algorithm's convergence. This kind of extension will not, however, necessarily produce efficient paths, since it may or may not utilize all the information available in the arm (3D) workspace. For example, the obstacle monotonicity property suggests that some directions for passing around an obstacle are more promising than others. Following the analysis in Section 6.2.4, a more efficient algorithm, which preserves the convergence properties discussed in Section 6.2.4 and also takes into account the said additional information, is presented below. The algorithm is based on a variation of the Bug2 procedure (see Section 3.3.2).

The following notation is used in the algorithm's procedure:

• Flag F is used to check whether or not both local directions have been tried for a given curve.
• Variables curr_dir and curr_loc, respectively, store the current local direction and current robot position.
• Function dist(x, y) gives the Cartesian distance between points x and y.
• Rules 1 and 2 are as defined in Section 6.2.3.
• When the C-point encounters a curve on the surface of an obstacle that it has to follow, the directions right and left are defined similar to those for the M-plane above.
• In order to choose the local direction upward or downward, function AboveMLine(curr_loc) is used to determine if the current location is above or below the M-line.
• Variable local_dir1 refers to the local directions forward, backward, left and right.
• Variable local_dir2 refers to the local directions upward and downward.

The algorithm proceeds as follows.

Step 0 Initialization. Set j = 0. Start at S. Go to Step 1.

Step 1 Motion Along the M-Line. Move along the M-line toward T until one of the following occurs:

(a) T is reached. The procedure terminates.

(b) A wall or a Type I obstacle is encountered. T cannot be reached—the procedure terminates.

(c) A Type II₊ obstacle is encountered. Set j = j + 1 and define H_j = curr_loc; set F = 1 and set local_dir 1 according to Rule 1. Go to Step 2.

(d) A Type II₊ obstacle is encountered. Set j = j + 1 and define H_j = curr_loc; set F = 1 and set local_dir1 according to Rule 2. Go to Step 2.

(e) A Type III obstacle is encountered. If l_{3 S} = l_{3 T}, T cannot be reached—the procedure terminates. Otherwise, set local_dir2 = downward or upward if the obstacle is of Type III₊ (or III₋); go to Step 3.

Step 2 Motion Along the Intersection Curve Between a Type II Obstacle and an M-Plane. Move along the intersection curve until one of the following occurs:

(a) The M-line is encountered. If dist(_c(curr_loc), _c(T)) > dist(_c(H_j), _c(T)), go⁵ to Step 2. Otherwise, define L_j = curr_loc; go to Step 1.

(b) A wall or a Type I obstacle is encountered. T cannot be reached—the procedure terminates.

(c) Another Type II obstacle is encountered. T cannot be reached—the procedure terminates.

(d) A Type III obstacle is encountered. Set local_dir2 = downward or upward if the obstacle is of Type III₊ (or III₋). Go to Step 4.

Step 3 Motion Along the Intersection Curve Between a Type III Obstacle and a V-Plane. Move along the intersection curve until one of the following occurs:

(a) The M-line is met. Go to Step 1.

(b) A wall or a Type I obstacle is encountered. T cannot be reached—the procedure terminates.

(c) A Type II₊ obstacle is encountered. Set j = j + 1 and define H_j = curr_loc; set F = 1 and set local_dir1 according to Rule 1. Go to Step 4.

(d) A Type II₋ obstacle is encountered. Set j = j + 1 and define H_j = curr_loc; set F = 1 and set local_dir1 according to Rule 2. Go to Step 4.

(e) Another Type III obstacle is encountered. Set j = j + 1 and define H_j = curr_loc; set F = 2 and set local_dir1 = right. Go to Step 5.

Step 4 Motion along the intersection curve between a Type II obstacle and a Type III obstacle. Move along the intersection curve until one of the following occurs:

(a) A wall or a Type I obstacle is encountered. Target T cannot be reached—the procedure terminates.

(b) The M-plane is encountered. Go to Step 2.

(c) The V-plane is encountered. If dist(_c(curr_loc), _c(T)) > dist(_c(H_j), _c(T)), then go to Step 2. Otherwise, define L_j = curr_loc; go to Step 3.

(d) Another Type II obstacle is encountered. Target T cannot be reached—the procedure terminates.

(e) Another Type III obstacle is encountered. Go to Step 5.

(f) The V-plane is encountered. If dist(_c(curr_loc), _C(T)) > dist(_c(H_j), _c(T)), then repeat Step 4. Otherwise, define L_j = curr_loc; go to Step 3.

Step 5 Motion Along the Intersection Curve Between Two Type III Obstacles. Move along the intersection curve until one of the following occurs:

(a) A wall or a Type I obstacle is encountered. F = F − 1. If F = 0, T cannot be reached—the procedure terminates. Otherwise, set local_dir1 to its opposite; retrace to H_j; repeat Step 5.

(b) The V-plane is encountered. If dist(_c(curr_loc), _c(T)) > dist(_c(H_j), _c(T)), then repeat Step 5. Otherwise, define L_j = curr_loc; if AboveMPlane(curr_loc) = true, then follow the intersection curve between the V-plane and the Type III₋ obstacle. Otherwise, follow the intersection curve between the V-plane and the Type III₊ obstacle. Go to Step 3.

(c) A Type II obstacle is encountered. If AboveMPlane(curr_loc) = true, then follow the intersection curve between the Type II obstacle and the Type III₋ obstacle. Otherwise, follow the intersection curve between the Type II obstacle and the Type III₊ obstacle; go to Step 4.

6.2.6 Examples

Two examples considered here demonstrate performance of the motion planning algorithm presented in the previous section. Both examples make use of examples considered above in the course of the algorithm construction. To simplify the visualization of algorithm performance and not to overcrowd pictures with the arm links and joints, only the resulting paths are presented in Figures 6.11 and 6.12. Since for the Cartesian arm the C-space presentation of a path is the same as the path of the arm end effector in W-space, the paths are shown in C-space only.

6.3 THREE-LINK XXP ARM MANIPULATORS

In Section 6.2 we studied the problem of sensor-based motion planning for a specific type, PPP, of a three-dimensional three-link arm manipulator. The arm is one case of kinematics from the complete class XXX of arms, where each joint X is either P or R, prismatic or revolute. All three joints of arm PPP are of prismatic (sliding) type. The theory and the algorithm that we developed fits well this specific kinematic linkage, taking into account its various topological peculiarities—but it applies solely to this type. We now want to attempt a more universal strategy, for the whole group XXP of 3D manipulators, where X is, again, either P or R. As mentioned above, while this group covers only a half of the exhaustive list of XXX arms, it accounts for most of the arm types used in industry (see Figure 6.1).

As before, we specify the robot arm configuration in workspace (W-space, denoted also by ) by its joint variable vector j = (j₁, j₂, j₃), where j_i is either linear extension l_i for a prismatic joint, or an angle θ_i for a revolute joint, i = 1, 2, 3. The space formed by the joint variable vector is the arm's joint space or J-space, denoted also by . Clearly, is 3D.

Define free J-space as the set of points in J-space that correspond to the collision-free robot arm configurations. We will show that free J-space of any XXP arm has a 2D subspace, called its deformation retract, that preserves the connectivity of the free J-space. This will allow us to reduce the problem's dimensionality. We will further show that a connectivity graph can be defined in this 2D subspace such that the existing algorithms for moving a point robot in a 2D metric space (Chapter 3) can be “lifted” into the 3D J-space to solve the motion planning problem for XXP robot arms.

In particular, in Section 6.3.1 we define the arm configuration L_j = L(j) as an image of a continuous mapping from J-space to 3D Cartesian space ℜ³, which is the connected compact set of points the arm would occupy in 3D space when its joints take the value j. A real-world obstacle is the interior of a connected compact point set in ℜ³. Joint space obstacles are thus defined as sets of points in joint space whose corresponding arm configurations have nonempty intersections with real-world obstacles. The task is to generate a continuous collision-free motion between two given start and target configurations, denoted L_s and L_t.

Analysis of a J-space in Section 6.3.2 will show that a J-space exhibits distinct topological characteristics that allow one to predict global characteristics of obstacles in based on the arm local contacts with (that is, sensing of) obstacles in the workspace. Furthermore, similar to the Cartesian arm case in Section 6.2, for all XXP arms the obstacles in exhibit a property called the monotonicity property, as follows: For any point on the surface of the obstacle image, there exists one or more directions along which all the remaining points of belong to the obstacle. The geometric representation of this property will differ from arm to arm, but it will be there and topologically will be the same property. These topological properties bring about an important result formulated in Section 6.3.3: The free J-space, _f, is topologically equivalent to a generalized cylinder. This result will be essential for building our motion planning algorithm.

Deformation retracts of and _f of _f, respectively, are defined in Section 6.3.4. By definition, _f is a 2D surface that preserves the connectivity of _f. That is to say, for any two points j_s, j_t ∈ _f, if there exists a path p_J ⊂ _f connecting j_s and j_t, then there must exist a path p_D ⊂ _f connecting j_s and j_t, and p_D is topologically equivalent to p_J in _f. Thus the dimensionality of the planning problem can be reduced.

When one or two X joints in XXP are revolute joints, X = R, is somewhat less representative of , only because the mapping from to is not unique. That is, it may happen that L(j) = L(j′) for j ≠ j′. Let S_J = {j ∈ |L(j) = L_s} and T_J = {j ∈ |L(j) = L_t}. The task in J-space is to find a path between any pair of points j_s ∈ S_J and j_t ∈ T_J. We define in Section 6.3.5 a configuration space or C-space, denoted by , as the quotient space of over an equivalent relation that identifies all J-space points that correspond to the same robot arm configuration. It is then shown that and _f, the quotient spaces of and _f over the same equivalent relation, are, respectively, deformation retracts of and _f. Therefore, the connectivity between two given robot configurations in can be determined in _f.

A connectivity graph will be then defined in Section 6.3.6, and it will be shown that preserves the connectivity of _f and _f. We will conclude that the workspace information available to the 3D robot is sufficient for it to identify and search the graph, and therefore the problem of 3D arm motion planning can be reduced to a graph search—something akin to the maze-searching problem in Chapter 3. Finally, in Section 6.3.7 we will develop a systematic approach, which, given a 2D algorithm, builds its 3D counterpart that preserves convergence.

The following notation is used throughout this section:

• X, Y ⊂ ℜ³ are point sets.
• ∂X denotes the boundary of X.
• X ≅ Y means X is homeomorphic to Y.
• includes the closure of .
• For convenience, define the closure of and .
• It is obvious that .

6.3.1 Robot Arm Representation Spaces

To recap some notations introduced earlier, a three-joint XXP robot arm manipulator is an open kinematic chain consisting of three links, L_i, and three joints, J_i, i = 1, 2, 3; J_i also denotes the center point of joint J_i, defined as the intersection point between the axes of joints J_i−1 and J_i. Joints J₁ and J₂ can be of either prismatic (sliding) or revolute type, while joint J₃ is of prismatic type. Joint J₁ is attached to the base O and is the origin of the fixed reference system. Figures 6.1a–d depict XXP arm configurations. Let p denote the arm end point; θ_i, a revolute joint variable, l_i, a prismatic joint variable, and j_i, either one of them, a revolute or a prismatic joint variable; i = 1, 2, 3. Figure 6.13 depicts the so-called SCARA type arm manipulator, which is of RRP type; it is arm (d) in Figure 6.1. We will later learn that from the standpoint of sensor-based motion planning the RRP arm presents the most general case among the XXP kinematic linkages.

Figure 6.13 An RRP robot arm manipulator: p is the arm end point; J_i and L_i are, respectively, the ith joint and link, i = 1, 2, 3; θ₁, θ₂, and l₃ are the joint variables.

As before, assume the robot arm has enough sensing to (a) detect a contact with an approaching obstacle at any point of the robot body and (b) identify the location of that point(s) of contact on that body. The act of maneuvering around an obstacle refers to a motion during which the arm is in constant contact with the obstacle.

Without loss of generality, assume for simplicity a unit-length limit for joints, l_i ∈ I¹ and . Points at infinity are included for convenience. The joint space is defined as , where _i = I¹ if the ith joint is prismatic, and if the ith joint is revolute. In all combinations of cases, ≅ I³. Thus, by including points at infinity in _i, it is possible to treat all XXP arms largely within the same analytical framework.

We assume that L_i has a finite volume; thus is bounded.

Arm links L₁ and L₂ can be of arbitrary shape and dimensions. Link L₃ is assumed to present a generalized cylinder —that is, a rigid body characterized by a straight-line axis coinciding with the corresponding joint axis, such that the link cross section in the plane perpendicular to the axis does not change along the axis. There are no restrictions on the shape of the cross section itself, except the physical-world assumption that it presents a simple closed curve—it can be, for example, a circle (then the link is a common cylinder), a rectangle, an oval, or any nonconvex curve.

We distinguish between the front end and rear end of link L₃. The front end coincides with the arm endpoint p (see Figure 6.13). The opposite end of link L₃ is its rear end. Similarly, the front (rear) part of link L₃ is the part of variable length between joint J₃ and the front (rear) end of link L₃. Formally, we have the following definition:

The purpose of distinguishing between the front and rear parts of a prismatic (sliding) link as follows: When the front (respectively, rear) part of link L₃ approaches an obstacle, the only reasonable local direction for maneuvering around the obstacle is by decreasing (respectively, increasing) the joint variable l₃. This makes it easy to decide on the direction of motion based on the local contact only. Since the point set is L₃((j₁, j₂, 0)) ∩ L₃((j₁, j₂, 1) ⊂ L₃((j₁, j₂, l₃)) for any l₃ ∈ I, and it is independent of the value of joint variable l₃, the set is considered a part of L₂. These definitions are easy to follow on the example of the PPP (Cartesian) arm that we considered in great detail in Section 6.2: See the arm's effective workspace in Figure 6.3a and its complete workspace in Figure 6.3b.

The robot workspace may contain obstacles. We define an obstacle as a rigid body of an arbitrary shape. Each obstacle is of finite volume (in 2D of finite area), and its surface is of finite area. Since the arm workspace is of finite volume (area), these assumptions imply that the number of obstacles present in the workspace must be finite. Being rigid bodies, obstacles cannot intersect. Formally, we have the following definition:

We use notation to represent a general obstacle, where M is the number of obstacles in .

Lemma 6.3.1 follows from Definition 6.3.1.

The robot arm can simultaneously touch more than one obstacle in the workspace. In this case the obstacles being touched effectively present one obstacle for the arm. They will present a single obstacle in the joint space.

Let j* ∈ O_J. By Definition 6.3.5, there exists a point x ∈ L such that y = x(j*) ∈ O. Since O is an open set (Definition 6.3.3), there must exist an ∈ > 0 such that the neighborhood U(y, ∈) ⊂ O. On the other hand, since x(j) is a continuous function⁶ from to , there exists a δ > 0 such that for all j ∈ U(j*, δ), we have x(j) ∈ U(y, ∈) ⊂ O; thus, U(j*, δ) ⊂ O_J, and O_J is an open set.

The free J-space is . Theorem 6.3.1 gives rise to this corollary:

Being a closed set, . Thus, a collision-free path can pass through ∂_f.

When the arm kinematics contains a revolute joint, due to the 2π repetition in the joint position it may happen that L(j) = L(j′) for j ≠ j′. For an RR arm, for example, given two robot arm configurations, L_s and L_t, in , L(j_sk1,_k2) = L_{s0, 0} = L_s for j_sk1,_k2 = (2 k₁ π + θ_{1 s}, 2 k₂ π + θ_{2 s}) ∈ , k₁, k₂ = 0, ±1, ±2, …. Similarly, L(j_tk3,_k4) = L_{t0, 0} = L_t for j_tk3,_k4 = (2 k₃π + θ_1t, 2 k₄π + θ_2t) ∈ , k₃, k₄ = 0, ±1, ±2, …. This relationship reflects the simple fact that in every link comes to exactly the same position with the periodicity 2π. In physical space this is the same position, but in J-space these are different points.⁷ The result is the tiling of space by tiles of size 2π. Figure 6.14 illustrates this situation in the plane. We can therefore state the motion planning task in J-space as follows:

The motion planning problem has thus been reduced to one of moving a point in J-space. Consider the two-dimensional RR arm shown in Figure 6.14a; shown also is an obstacle O₁ in the robot workspace, along with the arm starting and target positions, S and T. Because of obstacle O₁, no motion from position S to position T is possible in the “usual” sector of angles [0, 2π]. In J-space (Figure 6.14b), this motion would correspond to the straight line between points s_{0, 0} and t_{0, 0} in the square [0, 2π]; obstacle O₁ appears as multiple vertical columns with the periodicity (0, 2π).

However, if no path can be found between a specific pair of positions j_s and j_t in J-space, it does not mean that no paths between S and T exist. There may be paths between other pairs, such as between positions j_{s0, 0} and j_{t1, 0} (Figure 6.14b). On the other hand, finding a collision-free path by considering all pairs of j_s and j_t drawn from _s and _t, respectively, is not practical because _s and _t are likely to contain an infinite number of points. To simplify the problem further, in Section 6.3.5 we will introduce the notion of configuration space.

Figure 6.14 The L_s and L_t configurations (positions S and T) in robot workspace in part (a) produce an infinite number of S-T pairs of points in the corresponding J-space, part (b); vertical shaded columns in J-space are J-obstacles that correspond to obstacle O₁ in the robot workspace.

6.3.2 Monotonicity of Joint Space

Let L_i(j) ⊂ ℜ³ be the set of points that link L_i occupies when the manipulator joint value vector is j ∈ , i = 1, 2, 3 (Definition 6.3.1). Define joint space obstacles resulting from the interaction of L_i with obstacle O as Type i obstacles. For link L₃, let L₃₊ (j) and L₃₋ (j) ⊂ ℜ³ be, respectively, the set of points that the front part and rear part of link L₃ occupy when the joint value vector is j ∈ (Definition 6.3.2). Define Type 3₊ and Type 3₋ J-obstacles respectively resulting from the interaction of L₃₊ and L₃₋ with an obstacle O. More precisely:

Note that O_J3 = O_j3+ ∪ O_J3− and O_J = O_J1 ∪ O_j2 ∪ O_J3. We will also need notation for the intersection of Type 3₊ and Type 3₋ obstacles: O_J3+.

We now show that the underlying kinematics of the XXP robot arm results in a special topological properties of J-space, which is best described by the notion of J-space monotonicity:

Consider Figure 6.13. If j ∈ O_J1, then L₁(j) ∩ O ≠ . Since L₁(j) is independent of l₃, then L₁ (j′) ∩ O = for all j′ = (j₁, j₂, ). Similarly, if j ∈ O_J2, then L₂(j) ∩ O ≠ . Since L₂(j) is independent of l₃, then L₂(j′) ∩ O ≠ for j′ = (j₁, j₂, ) with any ∈ I.

Using again an example in Figure 6.13, if j ∈ O_J3, then L₃(j) ∩ O ≠ . Because of the linearity and the (generalized cylinder) shape of link L₃, L₃(j′) ∩ O ≠ for all j′ = (j₁, j₂, ) and > l₃. A similar argument can be made for the second half of the lemma.

Let us call the planes {l₃ = 0} and {l₃ = 1} the floor and ceiling of the joint space. A corollary of Lemma 6.3.3 is that if O₃₊ ≠ , then its intersection with the ceiling is not empty. Similarly, if O₃₋ ≠ , then its intersection with the floor is nonempty. We are now ready to state the following theorem, whose proof follows from Lemmas 6.3.2 and 6.3.3.

This statement applies to all configurations of XXP arms. Depending on the specific configuration, though, J-space monotonicity may or may not be limited to the l₃ direction. In fact, for the Cartesian arm of Figure 6.15 the monotonicity property appears along all three axes: Namely, the three physical obstacles O₁, O₂, and O₃ shown in Figure 6.15a produce the robot workspace shown in Figure 6.15b and produce the configuration space shown in Figure 6.15c. Notice that the Type 3 obstacles O₁ and O₂ exhibit the monotonicity property only along the axis l₃, whereas the Type 2 obstacle O₃ exhibits the monotonicity property along two axes, l₁ and l₂. A Type 1 obstacle (not shown in the figure) exhibits the monotonicity property along all three axes (see Figure 6.6 and the related text).

6.3.3 Connectivity of _f

We will now show that for XXP arms the connectivity of _f can be deduced from the connectivity of some planar projections of _f. From the robotics standpoint, this is a powerful result: It means that the problem of path planning for a three-joint XXP arm can be reduced to the path planning for a point robot in the plane, and hence the planar strategies such as those described in Chapters 3 and 5 can be utilized, with proper modifications, for 3D planning.

Let _p be the floor {l₃ = 0}. Clearly, . Since the third coordinate of a point in _f is constant zero, we omit it for convenience.

Figure 6.15 The physical obstacles O₁, O₂, and O₃ shown in figure (a) will be effectively perceived by the arm as those in figure (b), and they will produce the rather crowded C-space shown in (c).

Thus, for a joint space obstacle O_J, given start and target configurations j_s, j_t and any path p_J between j_s and j_t in , P_c(O_J), P_c(j_s), P_c(j_t), and P_c(p_J) are respectively the conventional projections of O_J, j_s, j_t, and p_J onto _p. See, for example, the conventional projections of three obstacles, O₁, O₂, and O₃, Figure 6.15b. It is easy to see that for any nonempty sets E₁, E₂ ⊂ , we have P_c(E₁ ∩ E₂) = P_c(E₁) ∩ P_c(E₂).

The minimal projection of a single point is empty. Hence P_m({j_s}) = P_m({j_t}) = . If E ⊂ is homeomorphic to a sphere and stretches over the whole range of l₃ ∈ I, then P_m(E) is the intersection between _p and the maximum cylinder that can be inscribed into O_J and whose axis is parallel to l₃. If a set E ⊂ presents a cylinder whose axis is parallel to the axis l₃, then P_c(E) = P_m(E).

In general, O_J is not homeomorphic to a sphere and may be composed of many components. We extend the notion of a cylinder as follows:

Type 1 and Type 2 obstacles present such generalized cylinders. It is easy to see that for any is a generalized cylinder, and .

We will now consider the relationship between a path p_J in and its projection in _p.

If , then we have . Thus a contradiction.

The next statement provides a sufficient condition for the existence of a path in joint space:

Let p_Ip(t) = {(j₁(t), j₂(t))} be a path between P_c(j_s) and P_c(j_t) in _p avoiding obstacle P_c(O_J). From Lemma 6.3.4, in . Hence, for example, the path connects positions j_s and j_t in and avoids obstacle O_J.

To find the necessary condition, we use the notion of a minimal projection. The next statement asserts that a zero overlap between two sets in implies a zero overlap between their minimal projections in _p:

By definition, , and E. Thus, if P_m(E₁) ∩ P_m(E₂) ≠ , then .

To use this lemma for designing a sensor-based motion planning algorithm, we need to describe minimal projections for different obstacle types. For a Type 1 or Type 2 obstacle O, we have P_c(O) = P_m(O). For a Type 3 obstacle, we consider three events that cover all possible cases, using as an example a Type 3₊ obstacle; denote it O₊.

• O₊ intersects the floor _f. Because of the monotonicity property, P_m(O₊) = O₊ ∩ _f. In other words, the minimal projection of O₊ is exactly the intersection area of O₊ with the floor _f.
• O₊ intersects with a Type 3₋ obstacle, O₋. Then, P_m(O₊ ∪ O₋) = P_c(∂ O₊ ∩ ∂ O₋). That is, the combined minimal projection of O₊ and O₋ is the conventional projection of the intersection curve between O₊ and O₋.
• Neither of the above cases apply. Then P_m(O₊) = .

A similar argument can be carried out for a Type 3₋ obstacle.

Define and . It is easy to see that _pf = P_c(_f). Therefore, .

Define . Clearly, and . By Theorem 6.3.2, each component of can be deformed either to the floor {l₃ = 0} or to the ceiling {l₃ = 1} and thus does not affect the topology of _f. Thus, and, by definition, presents a generalized cylinder in .

From the motion planning standpoint, Theorem 6.3.3 indicates that the third dimension, l₃, of _f is not easier to handle than the other two because _f possesses the monotonicity property along l₃ axis. It also implies that as a direct result of the monotonicity property of joint space obstacles, the connectivity of _f can be decided via an analysis of 2D surfaces.

We now turn to establishing a necessary and sufficient condition that ties the existence of paths in the plane _p with that in 3D joint space . This condition will provide a base for generalizing planar motion planning algorithms to 3D space. Assume that points (arm positions) j_s and j_t lie outside of obstacles.

To prove the necessary condition, let p_J(t), t ∈ [0, 1], be a path in _f. From Lemma 6.3.6, P_m(p_j(t)) ∩ P_m(O_j) = . Hence the path P_m(p_j(t)) connects P_c(j_s) and P_c(j_t) in _pf.

To show the sufficiency, let p_Jp(t), t ∈ [0, 1], be a path in _pf. Then presents a “wall” in . Define and let E⁻¹ be the complement of E in . We need to show that E⁻¹ consists of one connected component. Assume that this is not true. For any t_* ∈ [0, 1], since , there exists l_3* such that point (p_Jp(t_*), l_3*) ∈ E⁻¹. The only possibility for E⁻¹ to consist of two or more disconnected components is when there exists t_* and a set , such that (p_Jp(t_*), l_3*) ∈ E⁻¹ while and . However, this cannot happen because of the monotonicity property of obstacles. Hence E⁻¹ must be connected.

6.3.4 Retraction of _f

Theorem 6.3.4 indicates that the connectivity of space _f can indeed be captured via a space of lower dimension, _pf. However, space _pf cannot be used for motion planning because, by definition, it may happen that _pf ∩ O_J ≠ ; that is, some portion of _pf is not obstacle-free. In this section we define a 2D space _f ⊂ _f that is entirely obstacle-free and, like _pf, captures the connectivity of _f.

In other words, set ⊂ χ is a deformation retract of χ if χ can be continuously deformed into A. We show below that _f is a deformation retract of _f. Let _p, _pf, and be as defined in the previous section; then we have the following lemma.

Lemma 6.3.7. _p is a deformation retract of , and _pf is a deformation retract of .

Define r(j₁, j₂, l₃) = (j₁, j₂, 0). It follows from Lemma 6.3.2 that r is a retraction. Since for Type 1 and 2 obstacles , then, if contains only Type 1 and Type 2 obstacles—that is, _f = − (O_J1 ∪ O_J2)—it follows that and _pf is a deformation retract of _f. In the general case, all obstacle types (including Type 3) can be present, and _pf is not necessarily a deformation retract of _f.

₁ and ₂, are respectively, the obstacle-free portion of ∂ O_J3− and _p. It is easy to see that _f ≅ _pf. Since (Theorem 6.3.3) and _pf is a deformation retract of (Lemma 6.3.7), then _f is a deformation retract of _f.

Let denote the 2D space obtained by patching all the “holes” in _f so that ≅ _p. It is obvious that is a deformation retract of .

From Theorem 6.3.5, _f is a deformation retract of _f. Let r be the retraction as in Ref. 57, II.4; then p′ = r(p) must be an equivalent path in _f.

On the other hand, if and are not connected in _f, then by definition and are not connected in _f either. Hence the connectivity of and can be determined completely by studying _f, which is simpler than _f because the dimensionality of _f is lower than that of _f. Furthermore, a path between two given points j_s = (j_1s, j_2s, l_3s), j_t = (j_1t, j_2t, l_3t) ∈ _f can be obtained by finding the path between the two points _f. Because of the monotonicity property (Theorem 6.3.2), and always exist and they can be respectively connected within _f with j_s and j_t via vertical line segments. Hence the following statement:

6.3.5 Configuration Space and Its Retract

Our motion planning problem has thus been reduced to one of moving a point in a 2D subset of J-space, . However, as pointed out in Section 6.3.1, the joint space is not very representative when revolute joints are involved, because the mapping from to workspace is not unique. Instead, we define configuration space :

By definition, . Define equivalence relations _i in _i such that if and only if . Define and the project f_i : _i → _i given by f_i(j) = _ij. Apparently, _i ≅ S¹ with length υ_i = 2π if _i = , and _i ≅ I¹ with length υ_i = 1 if _i = I¹. Because f is the product of f_i's, f_i's are both open and closed, and the product topology and the quotient topology on ₁ × ₂ × ₃ are the same (see Ref. 57, Proposition A.3.1); therefore, ≅ ₁ × ₂ × ₃ is of finite volume υ₁ · υ₂ · υ_n.

For an RR arm, for example, ≅ S¹ × S¹ with area 2π · 2π = 4π²; for an RRP arm, ≅ S¹ × S¹ × I¹ with volume 2π · 2π · 1 = 4π².

For c ∈ , we define , where j ∈ f⁻¹(c), to be the area the robot arm occupies in when its joint vector is j.

The proof for the following theorem and its corollary are analogous to those for Theorem 6.3.1.

The configuration space obstacle O_C may have more than one component. For convenience, we may call each component an obstacle.

If there exists a motion between L_s and L_t in , then there must be a path p_J ⊂ _f between two points j_s, j_t ∈ _f such that L(j_s) = L_s and L(j_t) = L_t. Then, p_C = f(p_J) ⊂ _f is a path between _s = f(j_s) and c_t = f(j_t). The other half of the theorem follows directly from the definition of _f.

We have thus reduced the motion planning problem in the arm workspace to the one of moving a point from start to target position in C-space.

The following characteristics of the C-space topology of XXP arms are direct results of Theorem 6.3.7:

• For a PPP arm, ≅ I¹ × I¹ × I¹, the unit cube.
• For a PRP or RPP arm, ≅ S¹ × I¹ × I¹, a pipe.
• For an RRP arm, ≅ S¹ × S¹ × I¹, a solid torus.

Figure 6.16 shows the C-space of an RRP arm, which can be viewed either as a cube with its front and back, left and right sides pairwise identified, or as a solid torus.

The obstacle monotonicity property is preserved in configuration space. This is simply because the equivalent relation that defines and _f from and _f has no effect on the third joint axis, l₃. Thus we have the following statement:

As with the subset _f, _p ⊂ can be defined as the set {l₃ = 0}; O_C1, O_C2, O_C3, O_C3+, O_{C3_}, P_c, P_m, _f, _pf, and can be defined accordingly.

Figure 6.16 Two views of C-space of an RRP arm manipulator: (a) As a unit cube with its front and back, left and right sides pairwise identified; and (b) as a solid torus.

The proof is analogous to that of Theorem 6.3.5. Let denote the 2D space obtained by patching all the “holes” in _f so that ≅ _p. It is obvious that ≅ _p ≅ is a deformation retract of . We obtain the following statement parallel to Theorem 6.3.6.

A path between two given points j_s = (j_1s, j_2s, l_3s), j_t = (j_1t, j_2t, l_3t) ∈ _f can be obtained by finding the path between the two points , . Because of the monotonicity property (Theorem 6.3.10), and always exist and can be respectively connected within _f with j_s and j_t via vertical line segments. Hence the following statement:

6.3.6 Connectivity Graph

At this point we have reduced the problem of motion planning for an XXP arm in 3D space to the study of a 2D subspace that is homeomorphic to a common torus.

Consider the problem of moving a point on a torus whose surface is populated with obstacles, each bounded by simple closed curves. The torus can be represented as a square with its opposite sides identified in pairs (see Figure 6.17a). Note that the four corners are identified as a single point. Without loss of generality, let the start and target points S and T be respectively in the center and the corners of the square. This produces four straight lines connecting S and T, each connecting the center of the square with one of its corners. We call each line a generic path and denote it by g_i, i = 1, 2, 3, 4.

Define a connectivity graph on the torus by the obstacle-free portions of any three of the four generic paths and the obstacle boundary curves. We have the following statement:

Without loss of generality, let g₁, g₂, and g₃ be the complete set of generic paths, as shown in Figure 6.17a, where the torus is represented as a unit square with its opposite sides identified.

The generic paths g₁, g₂, and g₃ cut the unit square into three triangular pieces. Rearrange the placements of the three pieces by identifying the opposite sides of the square in pairs along edges a and b, respectively (see Figures 6.17b and 6.17c). We thus obtain a six-gon (hexagon), with three pairs of S and T as its vertices and generic paths g₁, g₂, and g₃ as its edges. The hexagon presentation is called the generic form of a torus.

Figure 6.17 Paths g₁, g₂, and g₃ constitute a complete set of generic paths. A hexagon is obtained by (a) cutting the square along g₁, g₂, and g₃, (b) pasting along b, and (c) pasting along a. (d) The resulting hexagon.

Now consider the effects of the above operation on obstacles (see Figure 6.18a). Obstacle boundaries and the generic paths partition our hexagon into occupied areas (shaded areas in Figure 6.18b) and free areas (numbered I, II, III, IV and V in Figure 6.18b). Each free area is not necessarily simple, but it must be homeomorphic to a disc, possibly with one or more smaller discs removed (e.g., area I of Figure 6.18b has the disc that corresponds to obstacle O₂ removed). The free area's inner boundaries are formed by obstacle boundaries; its outer boundary consists of segments of obstacle boundaries and segments of the generic paths.

Any arbitrary obstacle-free path p that connects points S and T consists of one or more segments, p₁, p₂, …, p_n, in the hexagon. Let x_i, y_i be the end points of segment p_i, where x₁ = S, x_i+1 = y_i for i = 1, 2, … , n − 1, and y_n = T. Since p is obstacle-free, x_i and y_i must be on the outer boundary of the free area that contains p_i. Therefore, x_i and y_i can be connected by a path segment of the outer boundary of the free area. The path that connects S and T and consists of segments of the generic paths and segments of obstacle boundaries is therefore entirely on the connectivity graph .

Figure 6.18 Illustration for Theorem 6.3.13. Shown are two obstacles O₁, O₂ (shaded areas) and path p (thicker line). The torus is represented, respectively, as (a) a unit square with its opposite sides a and b identified in pairs and (b) as a hexagon, with generic paths as its sides. Segments p₁, p₂ and p₃ in (b) are connected; they together correspond to the path p in (a).

Figure 6.18a presents a torus shown as a unit square, with its opposite sides a and b identified in pairs. O₁ and O₂ are two obstacles. Note that the three pieces of obstacle O₁ in the figure are actually connected. Segments g₁, g₂ and g₃ are (any) three of the four generic paths.

For an XXP arm, we now define generic paths and the connectivity graph in , which is homeomorphic to a torus.

In other words, _ab contains both a and b and is parallel to the l₃ axis. The degenerate case where is parallel to the l₃ axis is simple to handle and is not considered.

For the RRP arm, which is the most general case among XXP arms, consists of four components _i, i = 1, 2, 3, 4. Each _i represents a class of vertical planes in and can be determined by the first two coordinates of a pair of points drawn respectively from and . If and are the robot's start and target configurations, the four components of the vertical surface can be represented as follows:

where sign() takes the values +1 or −1 depending on its argument. Each of the components of V-surface determines a generic path, as follows:

Since is homeomorphic to a torus, any three of the four generic paths can be used to form a connectivity graph. Without loss of generality, let and denote . A connectivity graph can be defined as . If there exists a path in _f, then at least one such path can be found in the connectivity graph .

Now we give a physical interpretation of the connectivity graph ; consists of the following curves:

• ∂_p—the boundary curve of the floor, identified by the fact that the third link of the robot reaches its lower joint limit (l₃ = 0) and, simultaneously, one or both of the other two links reach their joint limits.
• _p ∩ ∂(O_C1 ∪ O_C2)—the intersection curve between the floor and the Type 1 or Type 2 obstacle boundary, identified by the fact that the third link of the robot reaches its lower joint limit (l₃ = 0) and simultaneously, one or both of the other two links contact some obstacles.
• ∂ O_C3− ∩ ∂ —the intersection curve between the Type 3₋ obstacle boundary and C-space boundary, identified by the fact that the robot's third link touches obstacles with its rear part and, simultaneously, one or more links reach their joint limits; this case includes the intersection curve between a Type 3₋ obstacle boundary and the ceiling.
• ∂ O_C3− ∩ ∂(O_C1 ∪ O_C2)—the intersection curve between the Type 3₋ obstacle boundary and the Type 1 or Type 2 obstacle boundary, identified by the fact that the third link of the robot touches obstacles with its rear part and that, simultaneously, one or both of the other two links contact some obstacles.
• ∂ O_C3− ∩ ∂ O_C3+—the intersection curve between the Type 3₊ obstacle boundary and the Type 3₋ obstacle boundaries, identified by the fact that both front and rear parts of the third link contact obstacles.
• g ∩ _pf —the obstacle-free portion of the generic path, identified by the fact that the robot is on the V-surface and that the third joint reaches its lower limit (l₃ = 0).
• ∩ ∂ O_C3− —the intersection curve between V-surface and the Type 3₋ obstacle boundaries, identified by the fact that the robot is on V-surface and simultaneously touches the obstacle with the rear part of its third link.

Note that the sensing capability of the robot arm manipulator allows it to easily identify the fact of being at any of the curves above.

6.3.7 Lifting 2D Algorithms into 3D

We have reduced the problem of motion planning for an XXP arm to the search on a connectivity graph. The search itself can be done using any existing graph search algorithm. The efficiency of a graph search algorithm is often in general—and, as we discussed above, in the Piano Mover's approach in particular—measured by the total number of nodes visited and the number of times each node is visited, regardless of the number of times that each edge is visited. In robotics, however, what is important is the total number of edge traverses, because physically each edge presents a part of the path whose length the robot may have to pass. For this reason, typical graph algorithms—for example, the width-first search algorithm [114]—would be inefficient from the standpoint of robot motion planning. On the other hand, depth-first search algorithms may work better; Tarry's rule [42] and Fraenkel's rule [45], which we discussed in Section 2.9.1, are two versions of such search algorithms. More efficient algorithms can be obtained by taking into account specifics of the connectivity graph topology [59].

In this section we intend to show how a 2D motion planning algorithm—we will call it A_p —can be carried out in _f. We assume that A_p operates according to our usual model—that is, using local information from the robot tactile sensors, the paths it produces are confined to generic paths, obstacle boundaries, and space boundaries, if any—and that it guarantees convergence.⁸ As before, it is also assumed that all decisions as to what directions the robot will follow along the generic path or along an obstacle boundary are made at the corresponding intersection points.

Without loss of generality, side walls of the C-space, if any, are simply treated below as Type I and Type II obstacles, the C-space ceiling is treated as a Type III₊ obstacle, and the C-space floor is treated as a Type III₋ obstacle.

Since the robot start and target positions are not necessarily in _f, our first step is to bring the robot to _f. This is easily achieved by moving from j_s downward until a Type III₋ obstacle is encountered; that is, the link L₃ of the robot either reaches its joint limit or touches an obstacle with its rear end. Then, the robot switches to A_p, which searches for point directly above or below j_t, with the following identification of path elements:

• Generic path—the intersection curve of and ∂ O₃₋.
• Obstacle boundary—the intersection curve of ∂ O₃₋ and ∂(O₁ ∪ O₂ ∪ O₃₊).

If A_p terminates without reaching , then the target j_t is not reachable. On the other hand, if is reached, then the robot moves directly toward j_t. Along this path segment the robot will either reach j_t or encounter an obstacle, in which case the target is not reachable. This shows how a motion planning algorithm for a compact 2D surface can be “lifted” into 3D to solve the motion planning problem of an XXP arm.

6.3.8 Step Planning

Similar to 2D algorithms in Chapter 5, realization of the above 3D motion planning algorithms requires a complementary lower-level control piece for step calculation. The required procedure for step calculation is similar to the one sketched in Section 5.2.3 for a 2D arm, except here the tangent to the C-obstacle at the local point of contact is three-dimensional. Since, according to the motion planning algorithm, the direction of motion is always unique—the curve along which the arm moves is either an M-line, or an intersection curve between an obstacle and one of the planes M-plane or V-plane, or an intersection curve between obstacles—the tangent to the C-obstacle at the contact point is unique as well. More detail on the step calculation procedure can be found in Refs. 106 and 115.

6.3.9 Discussion

As demonstrated in this section, the kinematic constraints of any XXP arm major linkage result in a certain property—called monotonicity—of the arm joint space and configuration space (C-space or _f). The essence of the monotonicity property is that for any point on the surface of a C-space obstacle, there exists at least one direction in C-space that corresponds to one of the joint axes, such that no other points in space along this direction can be reached by the arm. The monotonicity property allows the arm to infer some global information about obstacles based on local sensory data. It thus becomes an important component in sensor-based motion planning algorithms. We concluded that motion planning for a three-dimensional XXP arm can be done on a 2D compact surface, _f, which presents a deformation retract of the free configuration space _f.

We have further shown that any convergent 2D motion planning algorithm for moving a point on a compact surface (torus, in particular) can be “lifted” into 3D for motion planning for three-joint XXP robot arms. The strategy is based on the monotonicity properties of C-space.

Given the arm's start and target points j_s, j_t ∈ _f and the notions “above” and “below” as defined in this section, the general motion planning strategy for an XXP arm can be summarized as consisting of these three steps:

1. Move from j_s to , where ∈ _f is directly above or below j_s;
2. find a path between and within _f, where ∈ _f is directly above or below j_t; and
3. move from to j_t.

Because of the monotonicity property, motion in Steps 1 and 3 can be achieved via straight line segments. In reality, Step 2 does not have to be limited to the plane: It can be “lifted” into 3D by modifying the 2D algorithm respectively, thus resulting in local optimization and shorter paths. With the presented theory, and with various specific algorithms presented in this and previous chapters, one should have no difficulty constructing one's own sensor-based motion planning algorithms for specific XXP arm manipulators.

6.4 OTHER XXX ARMS

One question about motion planning for 3D arm manipulators that still remains unanswered in this chapter is, How can one carry out sensor-based motion planning for XXR arm manipulators—that is, arms whose third joint is of revolute type? At this time, no algorithms with a solid theoretical foundation and with guaranteed convergence can be offered for this group. This exciting area of research, of much theoretical as well as practical importance, still awaits for its courageous explorers.

In engineering terms, one kinematic linkage from the XXR group, namely RRR, is of much importance among industrial robot manipulators. On a better side, the RRR linkage is only one out of the five 3D linkages shown in Figure 6.1, Section 6.1, which together comprise the overwhelming majority of robot arm manipulators that one finds in practice. Still, RRR is a popular arm, and knowing how to do sensor-based motion planning for it would be of much interest. Judging by our analysis of the RR arm in Chapter 5, it is also likely the most difficult arm for sensor-based motion planning.

Conceptually, the difficulty with the RRR arm, and to some extent with other XXR arms, is of the same kind that we discussed in the Section 6.1, when describing a fly moving around an object in three-dimensional space. The fly has an infinite number of ways to go around the object. Theoretically, it may need to try all those ways in order to guarantee getting “on the other side” of the object.

We have shown in this chapter that, thanks to special properties of monotonicity of the corresponding configuration space, no infinite motion will ever be necessary for any XXP arm manipulator in order to guarantee convergence. No matter how complex are the 3D objects in the arm's workspace, the motion planning algorithm guarantees a finite (and usually quick) path solution. No such properties have been found so far for the XXR arm manipulators. On the other hand, similar to XXP arms considered in this chapter, motion planning for XXR arms seems to be reducible to motion along curves that are similar to curves we have used for XXP algorithms (such as an intersection curve between an obstacle and an M-plane or V-plane, etc.). Even in the worst case, this would require a search of a relatively small graph.

Provided that the arm has the right whole-body sensing, in practice one can handle an XXR arm by using the motion planning schemes developed in this chapter for XXP arms, perhaps with some heuristic modifications. Some such attempts, including physical experiments with industrial RRR arm manipulators, are in described Chapter 8 (see also Ref. 115).