Vision-Motion Planning of a Mobile Robot considering Vision Uncertainty and Planning Cost

1 Introduction

There has been an increasing interest in autonomous mobile robot which recognizes an environment with vi­ sion and moves without guidance of human operators . A key to realize such a robot is the ability to gener­ ate a plan of vision and motion operations so that a robot may efficiently reach the destination. To design a planning algorithm for such a robot, we have to con­ sider the following two issues: limited computational re­ sources and uncertainty in visual data. These two, issues are closely related; planning based on uncertain data usually requires more computation than planning with­ out uncertainty because multiple possible outcomes of actions should be considered, and therefore , the limita­ tion of computational resources tends to be critical.

One of the useful tools for planning under uncertainty is statistical decision theories [Berger, 1985]. Several works applied statistical decision theory to vision and/or motion planning tasks (e.g. , [Hutchinson and Kak , 1989] [Cameron and Durrant-Whyte, 1990] [Dean et al., 1990] [Miura and Shirai, 1993]). One drawback of these ap­ proaches is that a large branching factor of a search tree, which is determined not only by the number of possible actions but also that of possible situations that arise due to the uncertainty of sensory data, makes a planning pro­ cess computationally expensive.

Regarding the limitation of computational resources, many works have recently been focusing on the concept of limited rationality [Russell and Wefald, 1991], in which the cost of planning is explicitly considered and the time for object-level planning is allocated so that the overall utility including both plan efficiency and planning cost is maximized. Some of examples are: flexible computa­ tion [Horvitz, 1990], decision-theoretic meta-level control of (object-level) reasoning [Russell and Wefald, 1991], and expectation-driven iterative refinement (EDIR) us­ ing anytime algorithms [Boddy and Dean, 1989] .

This paper is concerned with a vision-motion plan­ ning of a mobile robot considering vision uncertainty and planning cost. Fig. 1 shows an example problem. Our mobile robot is going to the destination while avoiding obstacles. There is a route which passes the narrow space (we call it the gate) between the board and the partition; however the passability of the gate is initially unknown due to the uncertainty of visual data. The detour passing through the hallway is known to be passable, although it is longer. The robot estimates the gate width with stereo vision to determine the passability. The planner deter­ mines a set of observation points which efficiently navi­ gates the robot to the destination. For this problem, we propose a planning method combines a decision-theoretic approach with consideration of planning cost .

2 Basic Planning Strategy

2.1 Plan Representation

An action is composed of a movement to the next obser­ vation point and an observation at that point. A state is represented by the current estimate of the gate width and the current robot position . Due to the uncertainty in observation results, the robot cannot determine the gate width deterministically but obtains its probabilistic distribution.

After an observation, the robot classifies the state of the gate into one of the three states (see Fig. 2) : if the robot width is smaller than the minimum value of the probabilistic distribution of the gate width, the gate is passable; if the robot width is larger than the maxi­ mum value of the probabilistic distribution, the gate is impassable; otherwise the passability is unknown.

Since the actual state after an observation depends on the observation result and cannot be determined be­ forehand , a subplan is generated for each possible state, which is predicted using the uncertainty model of vision . Fig. 3 shows an example plan for the problem shown in Fig. 1. Such a plan is represented by an AND/OR tree; an OR node corresponds to selection of an action; an AND node corresponds to a possible state. Th.e qual­ ity of a plan is measured in terms of its execution cost , which is the expectation of the total execution time for movement and observation.

The leaves of an AND/OR tree are either t erminal node or open node. At a terminal node, the passability is decided without uncertainty and, thus, the final ac­ tion (i.e., passing the gate or taking the detour) is also decided. At an open node, since the passability is un­ known, the final action has not been decided yet . A plan candidate is refined by expanding (making subplans for) one of its open nodes . In expansion of an open node, the possible range of the gate width is discretized with some granularity, and a subplan is generated for each discretized state.

2.2 Computational Trade-offs to be Considered

The planning method is designed to deal with the fol­ lowing two computational trade-offs:

search depth vs. plan quality: This trade-off has been investigated by several researchers (e.g. , DTA · by Russell and Wefald [1991]) . We consider this trade-off in generating a multi-step plan.

granularity vs. plan quality: A finer granularity for discretization improves plan quality, but it increases the planning cost. This trade-off has little been consid­ ered, although it is important in planning under uncer­ tainty.

The first trade-off is managed by formulating the plan­ ning process as an iterative refinement process [Boddy and Dean, 1989], and by appropriately limiting the iter­ ation . To cope with the second trade-off, we represent the relationship between granularity and the expectation of the reduction of a plan cost (we call this expectation a plan improvement) as a performance profile [Dean and Boddy, 1988] [Zilberstein, 1993], and then determine the best granularity by examining the performance profile and the cost of node expansion. With consideration of these trade-offs, the planner tries to minimize the total cost of plan generation and plan execution.

3 Formulation as Iterative Refinement

3.1 Easily-Obtainable Feasible Plan

In an iterative refinement framework , the planner searches the space of feasible plans (executable plans) for the final plan. This formulation entails an easily­ obtainable feasible plan for any open node. There are two such feasible plans. One of them is to take the detour from the current position; this feasible plan is usually costly. Thus we use the other feasible plan:

The robot moves from the current position to the position just before the gate 1 . If the gate is passable, the robot passes it; if not, the robot takes the detour from that position.

1 At this position, the robot is assumed to be able to measure the gate width without uncertainty; this position is called the zero-uncertainty point, indicated as x*.

Each plan candidate has the temporary cost ctemp which is obtained by temporarily assigning the above fea~ sible subplan to all of its open nodes.

3.2 Selection of Open Node to Expand

In the planning process, the most promising open node is select~d and expanded. Before expansion, an unknown state _1s treate~ as one open node and has a feasible plan w1th 1t (see F1g. 4(a)) . The expansion of the open node consists of searching for the best action for each dis­ cretized state and assigning the feasible subplan to newly generated open nodes (see Fig. 4(b)).

The open node to be expanded is determined as fol­ lows. Suppose we can predict the maximum plan im­ provement (i.e., cost reduction) D..C of a plan candidate which will be obtained by expanding its best open node 2 : Then, the merit of expanding the node is given by sub­ ~racting the expansion cost cexp from the predicted plan Improvement D.C.

Let c:emp, D..C;, and c:"'P denote the temporary cost the predicted plan improvement , and the expansion cost of a plan candidate PC; , respectively. Then, we define the new cost crew as:

Let Cp p· be the cost of the incumbent (the best feasi­ ble ~lan among t~ose which have been obtained so far) . Durmg the planmng, the plan candidates are kept whose new costs are less than Cpp·. Among the candidates the plan candidate PC,. which is to be expanded next is determined as:

The planner iteratively selects PC,. and expands its best open node. This planning process has the anytime [Dean and Boddy, 1988] property. The iteration stops when c;~·w is larger than Cpp· .

2 The method to predict the plan improvement will be de­ scribed in Section 4.

3.3 Consideration of Meta-Planning Cost

The above algorithm will expand a node as long as c~ew is less t_han. Cp p· even if their difference is very s~all. Expans10n m such a case, however, may be useless if the meta-planning cost is high.

Thus, we slightly modify the termination condition. That is, if_ the ~ifference is less than the meta-planning cost, the 1teratwn process stops and the best feasible plan is returned as the final plan. The cost of meta­ planning is, at present, considered to be constant.

3.4 Deciding Only the Next Action

When the objective of the planner is not to generate a whol~ pla~ but to select the best next action (in a dynam1c envuonment, for example) , the algorithm is al­ tered as follows.

Let Cpp;._ be the cost of the best feasible plan which starts with action A . If the smallest value of cnew of plan candidates which start with actions other th~n A is larger than Cpp;._ , the planning process terminates and returns A as the best next action. This strategy is similar to that of DTA* [Russell and Wefald, 1991].

4 Determining the Best Granularity

We have mentioned that the granularity in discretizing the ranges of random variables (the gate width in our case) directly affects both the plan improvement and the ~xpansion cost . !he selection of the best granularity 1s, therefore, cruc1al to managing the trade-off between planning cost and plan efficiency.

This paper proposes to represent the relationship be­ tween the granularity and the predicted plan improve­ ment as a performance profile, and to calculate the best granularity and the merit of expansion simultaneously.

Currently we equally divide the range of a variable· thus, the granularity is specified with the number of divi~ sions. Let PI ( n) denote the predicted plan improvement for granularity n .

The best granularity n· is given by

where C·"'P(n) is the COSt of expansion with granularity n , which is defined as follows :

where Ncand is the number of action candidates· cexam is the cost required for examining one action ca~didate . Fi~. 5 illustrates the determination of the best granu­ lanty. Once the best granularity n· is determined the merit of expansion is calculated as PI(n*)- c·"'P(~*).

If there is at least one OR node between the root node and a~ open node, the probability of reaching the open node 1s less than one. In this case, we multiply PI(n) which is originally generated for the case that the reach~ ~ng probability is one, by the current reaching probabil­ Ity.

Every time an open node is generated (by expansion of its parent), the best granularity for the node is de­ termined. We include the cost of determining the best granularity in the cost of expanding the parent node.

5 Deriving Performance Profile

Derivation of a performance profile (PP) is one of the im­ portant issues in anytime algorithm-based approaches. In some cases, PPs may be obtained from the struc­ tural analysis of the problem; in other cases, PPs may be obtained from experimental data. It is, however, usu­ ally difficult to obtain PPs for complex problems only with one of these methods . We, thus, derive a PP through structural and experimental analysis of the plan­ ning problem in the following steps:

1. Analyze the structure of the planning problem and extract important problem parameters which can reasonably characterize the PP.
2. Construct a generalized PP using the parameters obtained above; a generalized PP has coefficients to be estimated.
3. Calculate actual PPs for an ·enough number of prob­ lem parameter sets, and adjust the coefficients of the generalized PP so that the PP fits well to the actual data set .

5.1 Problem Analysis

The plan improvement is the difference of temporary costs of a plan candidate before and after expanding one of its open nodes . Let us calculate the plan improvement using an example situation shown in Fig. 6.

Suppose that the robot is initially at zo and the next observation point is z 1 . The open node under consider­ ation is the state that the gate's passability is unknown after the observation at z 1 . The feasible plan before ex­ pansion is to go from z 1 directly to the zero-uncertainty point z·, where the robot can measure the gate width without uncertainty. Expansion of this open node , i.e., selection of a second observation point z2 for each dis­ cretized state results in a new feasible plan.

Figure 6: An example situation.

Using the cost of movement between points defined in Fig. _6 (for example, D~ is the cost of movement from z 1 to z2), the plan improvement tl.C is described as (see Appendix A):

where Cv is the cost of one observation; Pz; ,ok (Pz;,nu> Pz; ,ud) is the probability that the gate's state is passable (impassable, un_known) after observation at z~ . We here suppose that z2 indicates the best second viewpoint for the ith divided state.

This equation represents the trade-off between the ef­ fect of visual information to be obtained by observation at z~ and the cost of observation including that of move­ ment to the observation point. The first term inside the parentheses is the effect of visual information, which is gained by deciding to take the detour at z~, instead of decision at z· , in case that the gate is impassable. The second term is the cost of the extra observation at z· in case that the passability is still unknown at z~. The third term is the extra movement cost to visit z~.

5.2 Designing Generalized Performance Profile

We examine equation (5) in order to design a generalized PP which can reasonably approximate the equation.

The actual values which depend on z~ (e.g., Pz;,ng) cannot be obtained before performing the search for z~; it may also be hard to reliably predict these values. Thus , we would like to predict the plan improvement using only parameters whose values are available.

We use an upper bound of plan improvement for constructing a generalized PP. To calculate the upper bound, we first employ the concept of the assumption of perfect sensor information [Miura and Shirai, 1993], which is based on the fact that a plan generated using certain information is always more efficient than plans generated using uncertain information.

Assuming that the observation result at z~ includes no uncertainty (i.e., Pz; ud -+ 0), a modified plan im-,, provement tl.C' is given by

Since D..C' still includes values depending on z~, we then find its maximu~ value. D..C' is maximized by letting z2-+ Zt (i.e., Di,-+ Db, D~-+ De, and D~-+ 0); finally, we obtain the following upper bound:

where Px 1 -x· ,ng is the probability that the gate is im­ passable after observations at z 1 and x·, and is equal to :L7=l Px;,ng under the assumption of perfect sensor information. We suppose that the actual plan improve­ ment D..C increases as !::!.Cub increases.

We also reasonably assume that the plan improvement is a monotonically increasing function of the granularity n and asymptotically approaches some limit value.

Based on the above examination, we eventually de­ cided to use the following generalized PP (function PI(n)) as an approximation of equation (5):

where k1 , k2 , and k3 are coefficients to be estimated from the experimental data.

5.3 Determining PP through Experiments

We found that the coefficients in PI ( n) varies as the initial position z 0 changes. Thus, we divided the work space of the robot, which is the triangle composed of zo, x·, and Zd (see Fig. 6), into several regions and generated a PP for each divided region. The size of one region is determined empirically.

In each region, its center is selected as z 0 . Then, we calculated actual PPs about a hundred problems with various z 1 's and gate widths. We limited n to one of {1, 3, 5, 7, 9, 11}.

In estimating the coefficients, we first fitted equation (8) to each PP and estimated K and k1 . Fig. 7 shows an example of actual PP and the fitted curve. Then, we examined the relationship between K and !::!.Cub in equation (9). Fig. 8 shows a log-scaled plot of these values. By fitting a line to the plotted data, we obtained k2 = 0.033 and k3 = 1.319 in this case. As for k1 , we adopted the mean of all k1 's because we could not find any strong correlation between k 1 and other problem parameters.

6 Simulation Results

Fig. 9 shows examples of generated plan for a planning problem. From the initial observation result of the gate width, a complete plan, which is an AND/OR tree of observation points, was planned off-line. Observation points are limited only to grid points set on the work space of the robot.

In order to show that the planner can adaptively de­ termine the planning time (search depth and granular­ ity) according to its computational power , we performed simulations for the same problem with different com­ puting powers (cexam·s, see equation (4)). Notice that the higher the computing power is (the smaller cexam is) , the more precise and the less costly plan is gener­ ated. The difference of the costs is, however, rather small compared with that of the computing powers. This is be­ cause that the initial feasible plan is already a good plan in this case.

We then compared, in terms of the total of planning cost and execution cost, the proposed method with the fixed method, which uses a fixed granularity and a fixed search depth. Fig. 10 shows a comparison result. The proposed method gives the best performance.

We conducted this comparison for about forty differ­ ent problems; in about two-thirds cases, the proposed method outperformed others. In other cases, the dif­ ference between the best result and the result by the proposed method was less than one percent of the best result. Note that in order to obtain the best result with the fixed method, we had to adjust the granularity and search depth for each problem, while the proposed method always performed well without any such adjust­ ments.

7 Experiments using Real Robot

This section describes preliminary results for a real plan­ ning problem shown in Fig. 1. The gate width is esti­ mated by the two vertical segments on both sides; its probabilistic distribution is calculated using an uncer­ tainty model of stereo vision [Miura and Shirai, 1993].

(c) cex·m =J.le-2, Cost=189.0, n· =0

Figure 10: Comparison of the proposed method with fixed-granularity and fixed-search depth methods . Change of the total cost according to the change of the search depth is indicated for each granularity.

In the experiment, the plamier decides only the next action (see Section 3.4). Then the robot moves to the planned observation point and observes the gate. The planning and action operations are iteratively performed until the passability of the gate is determined.

Fig. 11 shows the result of a trial. From the initial im­ age (at lower-right in the figure) , the robot estimated the probabilistic distribution of the gate width; the probabil­ ity of the gate being passable was 0.53. Then the planner determined the next observation point as shown in the map. The robot moved to the next observation point (lower-left) and obtained the new image (upper-right). After this observation, the gate was determined to be passable; thus the robot moved forward and passed the gate (upper-left) .

8 Conclusions and Discussion

We have proposed a planning method for a vision-motion planning of a mobile robot under vision uncertainty and limited computational resources. We managed granular­ ity vs. plan quality and search depth vs. plan quality trade-offs by: (1) considering the relationship between granularity and plan improvement which is represented as a performance profile, and (2) formulating the process of generating a multi-step plan as an iterative refinement process. The proposed method is always comparable with the best of the methods which uses fixed granu­ larity and fixed search depth. We have also described a method to derive a performance profile through struc­ tural and experimental analysis of the planning problem.

In the performance profile (PP)-based planning, the quality of PP has the largest importance. If the parame­ ters (e.g., configuration of obstacles) of the current prob­ lem are largely different from those of problems used in derivation of PPs , the predicted pl'an improvement may not be accurate enough. In this paper, in order to in­ crease the accuracy, we divided into the problem space into several regions and obtained a PP for each region. Such a strategy might be practical if a PP which is ap­ plicable to a wide problem space is hard to find.

This paper has treated a relatively simple vision­ motion planning problem (a single-gate problem). A future work is to apply the proposed method to large planning problems which have many gates to observe. To solve such a large problem, it is necessary to decom­ pose the problem into a set of single-gate problems. We are now developing an efficient decomposition method.

Acknowledgments

This work is supported in part by Grant-in-Aid for Sci­ entific Research from Ministry of Education, Science, Sports, and Culture, Japanese Government and by the Okawa Foundation for Information and Telecommunica­ tions , Tokyo, Japan.

A Derivation of Plan Improvement ~C

Only the subplan for the open node at z 1 changes by expansion. The cost C1 of the subplan before expansion is given by

where Dgoal and Ddetour is the cost of movement from :~:· to the destination and from Zd to the des­ tination through the detour, respectively; Pz,-z· ,ok (Pz,-z·,ng) is the probability that the gate is pass­ able (impassable) at z· in case that the passability is unknown at Zl· The cost c2 after expansion is given by

where pi 1s the probability of the ith divided z, ,ud state; Pz·-z· ok (Pz·-z· ng) is similarly defined as ~ ' 2 ' Pz,-z· ,ok (Pz,-z· ,ng) · The best z~ is selected for each state si 0

The plan improvement is obtained as C1 - C2 . The following relations hold among probabilities:

After a series of calculation using the above relations , we eventually obtain equation (5).