# «The Robotics Institute Carnegie Mellon University Pittsburgh, PA 15213 May 13, 2005 Submitted in partial fulfillment of the requirements for the ...»

The intuition is as follows. RAISE states are not optimal, since f* ( X, R ) f ( X, R ). Expansion allows them to reroute the path through lower-cost states. The value f* ( X, R ) of a RAISE state is a lower bound on the cost of LOWER states it can re-route to (it cannot activate a state that is less costly than the current optimum). Therefore, if all LOWER states’ costs f ( X, R ) exceed the f* ( X, R ) value of the RAISE state, it is better to expand the RAISE state first to possibly find lower-cost LOWER states. Note that if two OPEN states share the same f* ( X, R ) value, they are prioritized by their key values. This avoids creating cycles in state backpointers.

A1.2 Definitions This section presents algorithmic details of ISE. The following two sections informally define several functions used in the ISE algorithm description, presented in section A1.3.

Table 1-1 lists the ISE application-specific functions that must be defined by the ISE user.

A1.3 BESTPCOST and BESTDPARMS Modes At the highest level, ISE operates in either of two modes, BESTPCOST and BESTDPARMS. The BESTPCOST mode finds the minimum cost path. The BESTDPARMS mode finds the “best” path below a user-designated upper bound in path cost, where better paths are defined using the application-specific better ( X, Y ) function in Table 1-1.

Both modes share the underlying search algorithm, composed principally of two functions APPENDIX: ISE ALGORITHM EXPAND_NEXT_STATE and INSERT_STATE_CHECK. The two modes and these functions are described in detail in the following paragraphs.

The BESTCOST mode finds the minimum cost path. The BESTCOST algorithm appears in Table 1-3. Within an upper bound on iterations, ISE continues to expand states (L4), checking at each expansion whether the start set containing the robot start state R has been affected (L5). In lines L6-L7, ISE scans the updated set ( R ) for states that meet the termination criteria. A state must be feasible, must have been processed off the OPEN list (not awaiting cost updates), and its cost must be lower than the least cost estimate on the OPEN list (unless the OPEN list is empty) and than the current optimal solution. The optimal plan begins from the lowest cost state of states that first meet these criteria. Given ISE prioritizes lowest-cost states for expansion, the first feasible state to be expanded also tends to be the start state of the optimal solution. ISE returns the optimal state, its path cost and whether or not the maximum number of iterations was exceeded.

The BESTDPARMS mode finds the solution with the “best” start state DPARMS that falls below a maximum path cost. In this mode, listed in Table 1-4, the objective function serves only to measure path costs against the maximum.

In lines L13-L16, ISE continues to expand states until the OPEN list is exhausted or the minimum path cost on the OPEN list exceeds the cost upper bound. This ensures that all possible solutions at or below the upper bound cost are processed. ISE evaluates all states deposited into set ( R ) as a result of the search. As in the BESTPCOST mode, the best state must be a feasible start state, must be CLOSED, and must cost less than any state remaining on the OPEN list. In contrast to BESTPCOST, the state need not be the least costly, but must fall beneath the cost upper bound. The

search may generate many states satisfying these criteria. The better function prioritizes the qualifying states. The search ends when no states remain within set ( R ) below the cost upper bound. The optimal start state is the “best” from among the qualifying states.

A1.4 State Expansion Both ISE modes depend fundamentally on the function EXPAND_NEXT_STATE, listed in detail in Table 1-5. This function processes states off the OPEN list and communicates their effects, in terms of cost updates and dominance, on neighboring states.

The function begins by removing the lowest cost state X from the OPEN list. It returns an invalid cost vector if the OPEN list is empty. If the state is downstream of a modification to APARMS, lines L26-L30 designate the backwards (downstream) neighbors of X, whose paths contain X, for deletion and recycles them onto the OPEN list for future processing. This allows ISE to delete the old graph structure, created under the old APARMS, in preparation for creating graph nodes under the modified arc costs.

If h ( X ) k ( X ), then X is a RAISE state, and may not be optimal. In lines L31-L34, ISE checks all the forward neighbors of X to determine whether they might provide an alternate, cheaper path to the goal, with correspondingly lower h ( X ). If so, the state’s deletion status is unmarked, the path is re-directed, the state cost is re-computed to be the cost to the goal through Y, and the state is marked as a reduced RAISE state (L34).

## APPENDIX: ISE ALGORITHM

RAISE states do not necessarily hold the same dominance relationships. If the state is a RAISE state at L34, then L35 re-checks the state’s dominance relationship in the OPEN list, without re-inserting it. Then, for states that remain RAISE states, ISE re-creates the forward neighbor states that were previously dominated, and re-inserts them directly to the OPEN list for re-processing (L36-L39). Finally, if a RAISE state itself becomes dominated, ISE marks it for later deletion on line L40.Following the previous reduction operations, the state could be either a RAISE or LOWER state. If h ( X ) = k ( X ), then X is a LOWER state and by definition optimal. Lines L42-L49 search amongst the backwards neighbors of LOWER state X and propagate cost changes to them. Three conditions prompt ISE to re-insert these states to the OPEN list (L43). First, if the neighbor is a NEW state, it must be supplied with an initial cost. Second, if a path is directed from the neighbor to the state X, but the neighbor’s cost is mismatched, then its cost must be re-matched to reflect the optimal cost through X. Third, if the path from the neighbor does not go through X, and the neighbor’s cost is higher than if it were directed through X, its cost must be decreased to reflect a re-direction through X. In all three cases, if X is marked for deletion, so are the neighbors (L45). If not, the neighbor backpointers are directed through X (L47). Furthermore, in all cases the neighbor is re-evaluated for dominance and placed onto the OPEN list with its new, optimal cost (L48).

If the state is not a LOWER state, then it remains a RAISE state that may affect both is forwards and backwards neighbors. ISE searches the forward neighbors first (L51). If a forward neighbor does not direct a path through X and the path from X through the neighbor reduces the cost from X, and the neighbor is not awaiting processing (L52), then ISE re-inserts the neighbor into the OPEN list with its old cost (L53). For backwards neighbors (L55), three conditions prompt changes (L56 and L63). As with the backwards neighbors of LOWER states, if the backwards neigh

<

** APPENDIX: ISE ALGORITHM**

bor of the RAISE state is NEW, or if it directs a path through X but its cost does not match, its cost is updated and it is placed on the OPEN list after having its dominance relationships re-evaluated. Finally, if the path from the neighbor does not pass through X, and the neighbor’s cost can be lowered by going through X (L63), then the state X itself is re-entered onto the OPEN list, since X is not necessarily optimal. This action is required to avoid creating a closed loop in the backpointers.

Function EXPAND_NEXT_STATE returns the vector the estimated path cost key value of the highest-priority state on the OPEN list, as well as a Boolean value signifying whether or not a state contained in set ( R ) was modified before being placed back on the OPEN list.

A1.5 State Dominance Management The management of dominance and “better” relationships among states is a key component of the ISE algorithm.

Dominated states cannot legally be a part of any ISE solution. Similarly, during a BESTDPARMS search, states given lower priority according to the better ( X, Y ) function cannot be a part of a solution. The difficulty is that during re-planning, dominating and “better” states may become too costly to be optimal, in which case the states they dominated become candidates for re-consideration. In the original implementation of ISE, states dominated by other states were maintained in their IPARMS sets in anticipation of that case. In practice, this led to excessive memory consumption for storage, since a majority of dominated states remained dominated during re-planning. The solution was to implement a function that manages these relationships, and deletes the states that are dominated. Just as in initial search, states can be re-generated in the event that their dominators grow too expensive.

Function DOM_INSERT_STATE ( X, h ( X ), flag ) inserts a state X into the OPEN list after checking and asserting state dominance relationships between the state and other states in the OPEN list. A state X is not put on the OPEN list if it is dominated by other existing states, and states dominated by X are deleted from the list. If flag is set to TRUE, the state may be put on the OPEN list; if flag is set to FALSE, its dominance relationships are checked, but it is not inserted into the OPEN list.

At the outset, the new state is considered not to be dominated (L66). The function searches over all old states that might either dominate or be dominated by X (L67). If the new state X is resolution equivalent to another state, then the function determines which state the lesser state and can be deleted from memory. Lines L69-L74 check whether state X is either cheaper than the old state, or if equivalent in cost, whether the new state’s DPARMS values are preferred over the other’s. If proven inferior, the old state is deleted outright if it has never been processed, and marked as dominated and placed back on to the OPEN list if not. If the old state is not proven inferior, then the state X itself

is deleted or marked dominated and placed on the OPEN list in the same manner (L75-L81). If X is deleted the function returns.

If new state X is not resolution equivalent to the old state, the function checks to see whether a dominance relationship exists between the two. If X dominates over the old state, and the new state has the same or lesser cost, then the old state is deleted or marked dominated and placed on the OPEN list (L83-L89). If the old state dominates over the new state, then the new state is processed accordingly and the function returns (L90-L97). If the new state has not been deleted or dominated, and the insertion flag is set, then the new state is inserted into the OPEN list.

Appendix 2: Progress Distance In temporal path planning, it is difficult to express spatio-temporal relationships in two-dimensional plots. Plots of position ignore temporal aspects. Plots of the individual spatial dimensions against time can be confusing. Progress distance captures important aspects of the spatial dimensions in a single parameter, and plots of progress distance versus time are useful tools for analyzing spatio-temporal plans.

To define the term, assume a plan intersects a fully-ordered sequence Γ of N goals, that the Euclidean distance between the start position S and the first goal G 1 is given by ρ ( S, G 1 ), and the distance between two adjacent goals

**G i and G i + 1 is given by ρ ( G i, G i + 1 ). The total minimum plan distance is then given by:**

During the course of execution, goals may be achieved by a robot and removed from the list of remaining goals. If the new start state is X and the first remaining unachieved goal is given by next ( Γ ), then the minimum remaining

**distance is given by:**

The progress distance is a measure of the progress of the robot along the minimum-distance trajectory through the remaining goals. According to this measure, progress can only be made by reducing the minimum distance remaining. Moving away from the next goal results in negative progress, and moving in a circle about the goal results in no progress. Figure A2-1 illustrates progress distance for a hypothetical spatio-temporal plan. The left side of the figure shows a sequence of four snapshots from a plan execution. The right side shows corresponding plots of progress distance versus time. Figure A2-1a shows the initial start position S 0 and a sequence of two goals G 1 and G 2. The total minimum distance for the plan is shown by the dotted lines. The plot in Figure A2-1e shows the position of S 0 at the

axis origin, and the distances to the goals on the vertical axis. In this plot, progress speed is indicated by the slope of a progress distance trajectory. The maximum robot speed is given by the dashed line - no trajectory can advance towards the goals more quickly than this line. In Figure A2-1b, the robot has made progress towards the goal at position S 1. Note the trajectory from S 0 to S 1 in the plot of progress distance in Figure A2-1e. Since the path does not advance directly towards the first goal, the progress trajectory slope is shallow. In Figure A2-1c, the path moves from position S 1 to S 2. In the corresponding progress distance plot in Figure A2-1f, the curve shows no net progress in this move. The reason is that positions S 1 and S 2 are equally distant from the next goal. The final path frame shows the robot at a position S 3, having completed the first goal. The progress curve in Figure A2-1f shows how the turns of the path affect progress over time. Though not shown in the figure, if a robot stops, it produces a constant interval in the progress distance plot.

Progress distance is useful because it is a measure of the directed progress to cover the distance between goals. It reflects the indirection a path might take to avoid obstacles, and also any stationary periods that would otherwise be invisible in a purely spatial plot. Note that progress distance cannot distinguish between stationary activities and mobile activities that stay a fixed distance from the next goal. However, taken together, the path plot and progress distance plot contain sufficient information to determine the spatio-temporal state of the robot. Progress distance plots are used extensively throughout this thesis to illustrate spatio-temporal plan behavior.

## APPENDIX: PROGRESS DISTANCE

Glossary of TermsLocal Path Planning Path planning that utilizes only rover-local data, and because of the fine spatial planning resolution, often considers the finite size of the rover, finite turning radii, and may consider vehicle dynamics.

Global Constraint A constraint whose violation depends on the entire state history.

Non-Monotonic Resource A resource whose level varies non-monotonically, for example the battery charge in a solar-powered vehicle.

Progress Distance See Appendix 2.

Resolution Optimal Optimal to the resolution imposed by the planning representation.