Page 327 - Sensing, Intelligence, Motion : How Robots and Humans Move in an Unstructured World

302 MOTION PLANNING FOR THREE-DIMENSIONAL ARM MANIPULATORS

algorithm’s convergence. This kind of extension will not, however, necessar-
ily 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.GotoStep 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 = 1andset local dir1 according toRule 1.Goto
Step 2.
(d) A Type II + obstacle is encountered. Set j = j + 1 and define H j =
curr loc;set F = 1andset local dir1 according toRule 2.Goto
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 = down-
ward or upward if the obstacle is of Type III + (or III − ); go to Step 3.