An Uncertainty Model of Stereo Vision and its Application to Vision-Motion Planning of Robot

1 Introduction

There has been an increasing interest in autonomous mo­ bile robot which recognizes an environment with vision and moves without guidance of human operators. Fig.1 illustrates a typical situation, in which the objective of the robot is to reach the goal point at the minimum cost (in the minimum time) . The observed positions of ob­ stacles are uncertain because of uncertainties of visual recognition. There may be two behaviors of the robot: one is to approach the obstacles and to observe again in order obtain more accurate information for further plan­ ning; the other is to take a detour immediately without further observations. To decide which behavior is better, it is necessary to consider a trade-off between the cost of visual recognition and the effect of information obtained

·This work is supported in part by Grant-In-Aid for Sci­ entific Research from the Ministry of Education, Japanese Government .

·

by vision. The trade-off must be formulated on the basis of a model of vision which describes the required time for visual processing and uncertainty of obtained infor­ mation .

There are many researches on a uncertainty model of stereo vision. Moravec [Moravec, 1983] used a model in which uncertainty of an estimated position is inversely proportional to the depth from the viewpoint. Matthies et al. [Matthies and Shafer, 1987] proposed to use three­ dimensional Gaussian distributions to model the uncer­ tainty of the position, and showed the model performs better than former models . Ayache et al. [Ayache and Faugeras, 1989] described a method of building and up­ dating 3-D representation of the environment; uncertain­ ties caused by various factors such as the quantization error or the calibration error is modeled by probabilistic distributions, and such uncertainties are propagated via relationships between geometric entities. They also pro­ posed a method of fusing uncertain information by Ex­ tended Kalman Filter. Kriegman et al. [Kriegman et al. , 1989] proposed a similar approach in uncertainty model­ ing for a mobile robot . These uncertainty modelings are not related to planning. Moreover, these researches deal with only statistical errors in feature extraction and do not treat ambiguities such as false matching of features in the left and the right images . Since the uncertainty caused by false matchings is much larger in non-trivial indoor environments, an uncertainty model of stereo vi­ sion should consider such an uncertainty.

This paper describes a model of stereo vision and its application to a vision-motion planning for a mo­ bile robot. Uncertainty (including ambiguity) of a feature position caused by the quantization error and false matchings of features is represented by a probabilistic distribution. An efficient solving strategy is also de­ scribed which employs a pruning method based on the lower bound of the total cost calculated by the assump­ tion of perfect sensor information. An experimental re­ sult for a real world problem is presented.

2 Formulation of Vision-Motion Planning under Uncertainty

This section briefly describes the formulation of vision­ motion planning that we have proposed in [Miura and Shirai, 1992a]. Uncertainty of a recognition result is rep­ resented by a probabilistic distribution. If there are mul­ tiple properties in the environment, uncertainty of infor­ mation is represented by a multivariate distribution. A sequence of sensor data on a property are integrated us­ ing Bayes' theorem.

The planner minimizes the expectation of the total cost for reaching the destination since a plan is gener­ ated based on probabilistic information. Here, we derive a recurrence formula which calculates the optimal next observation point Zi+l and the next- optimal observa­ tion condition Oi+l from the current position :z:; and the current information i; . Let Pob1d( iob1d; i;, Zi+ 1, o;+l) de­ note the probability that information iob1d is obtained by observation Oi+l at Zi+l with the current information i;. Uncertainty of motion from :z:; to Zi+l is also included in this probability distribution. In addition, let fuse( it, i2) be a function which calculates the fusion result of i 1 and i 2 . It can be predicted that information fuse(i;, iob·d) is obtained with probability Pob·d( iob·d i i;, Zi+l, Oi+l) af­ ter observation Oi+t at Zi+l·

Since Bayes' theorem is used for information fusion, a fusion result is considered to include information ob­ tained by all of the past sensing operations. Therefore, an optimal plan depends only on the current position and the current information, whatever the history of the past vision and motion operations is. Consequently, the minimum cost at :z:; with information i; becomes the minimum of the sum of the following costs:

1. the cost of motion to the next observation point Zi+l j
2. the cost of the next observation o;+l;
3. the expectation of the minimum cost from :z:i+1 to the goal point .

Therefore, the following recurrence formula (equation (1)) is derived. If a robot can reach the goal point with­ out further observations, a recursive computation termi­ nates because the cost to the goal point can be computed directly.

A search tree becomes an AND/OR tree; an OR node corresponds to selection of an operation; an AND node corresponds to prediction of possible observation results.

3 Modeling Uncertainties of Stereo Vision

The formulation mentioned above does not make any assumptions on a model of vision except that uncertain­ ties are represented by probabilistic distributions. This section describes a model of segment-based stereo vision .

3.1 Segment-Based Stereo Vision

In indoor scenes, there are many line segments that are components of artificial objects. Such segments are useful as primitive features for stereo matching be­ cause structural information is implicitly imposed as constraints [Medioni and Nevatia, 1985]. Especially, ver­ tical line segments are useful for a mobile robot to detect collision-free areas on the floor.

We here treat a stereo system in which two cameras are mounted in parallel with each other and with the floor . Thus, vertical segments in the three-dimensional space are projected as vertical segments onto the image plane. Vertical segments are extracted by horizontal differenti­ ation and by line fitting. For each segment in the left image, corresponding segments in the right image are detected based on the epipolar and the similarity con­ straint; a pair of segments can be matched if their verti­ cal positions overlap each other to a certain extent and they have similar directions and contrast values. From each matching of features , a three dimensional position is calculated by triangulation.

3.2 Model of Uncertainty Caused by Quantization Error

We here consider the uncertainty of the two-dimensional position of a vertical segment in a real space caused by the quantization error. The positional distribution of the segment depends on edges used by stereo matching. The horizontal position of a segment in the image is cal­ culated from edges in the vertically overlapping part of the segment . The distribution of the horizontal posi­ tion is calculated from the positional distributions of the edges by the least squares method. Assuming that the horizontal position of each edge is normally distributed, the horizontal position of each segment also follows a normal distribution. By linearizing the equation of im­ age projection, the position of a segment in a real space is represented by a two-dimensional normal distribution [Ayache and Faugeras, 1989] [Kriegman et al., 1989] . .

The distributions of other properties can also be com­ puted. For example, suppose that a robot is measuring the distance d between two vertical segments at z 1 and Zr (see Fig.2), and that p.1, 1-'r> I:r, and I:r are the means and the covariance matrices of zr and Zr. Since the equa­ tion d =II zr- Zr II is non-linear, the true distribution of the distance d which is obtained by propagating uncer­ tainties of two positions via the equation is not normally distributed. Thus, by linearizing the equation, a normal distribution is obtained as an approximation; the mean JJd and the variance u~ of which is given by:

where lr (Jr) is the Jacobian matrix from ~zr (~zr)to ~d. Fig.3 shows the change of the variance of the dis­ tance (u~) according to the change of the viewpoint. From the figure, we can see that the variance depends not only on the observation distance but also on the ob­ servation direction.

3.3 Model of Ambiguity Caused by False Matchings

In stereo, the matching pair of a line segment in the left image is easily found if there is only one match­ ing candidate in the right image. If there are multiple corresponding segments, extra criteria are needed to de­ cide the correct matching. Medioni et a!. [Medioni and Nevatia, 1985] used surface continuity constraint as a criterion. Ayache et al.[Ayache and Faverjon, 1987] also used such a constraint by introducing a neighborhood graph which stores adjacency of segments. It is, how­ ever, sometimes difficult to decide correct matchings in case that the disparity smoothness constraint is violated because of occluding boundary or orderings of segments differ from each other in the right and the left image because of narrow occluding objects[Dhond and Aggar­ wal, 1992]. In our approach, the possible positions of segments are all kept in the model, and the ambiguities are resolved by the subsequent observations if necessary.

Let /i be a line segment in the left image and 'R = { rj} be a set of possible corresponding segments. For each matching pair {li, rj ), a normal distribution of the two­ dimensional position of a point in the real space is calculated using the model of the quantization error . We model the positional uncertainty of line segment /i by a set of such normal distributions; the positional distribu­ tion of /i is represented by a weighted sum of the normal distributions; a weight for a distribution is calculated by an evaluation function based on both the similarity of segments (on directions and contrasts) and the ratio of the length of the vertically overlapping part to the length of the longer segment .

Let us consider Fig.4 as an example. Suppose a robot is passing the space at the center of the scene toward the bookshelf. By applying our stereo algorithm, three sets of ambiguous matchings were detected which must be considered to decide the passability of the space in front of the robot. Fig.5 shows the normal distributions calculated from these matchings. The position of the matched segment corresponding to each distribution is indicated in the right image in Fig.4.

3.4 Strategy for Resolving Ambiguous Matchings

If there are ambiguous matchings, a trade-off is consid­ ered between the cost of resolving ambiguity and the ef­ fect of the disambiguated information. If resolving ambi­ guity is better, a robot searches for the next observation point in the area where all of false matchings are elim­ inated. Such an area is calculated as follows. In Fig.6, for example, two candidate positions are obtained for a feature by the first observation. Suppose T is the true position and F is the false one which is, of course, not yet known. If a false matching occurs again in the second observation, the false position will lie on the line L1T or R1T. Let F' be such a false position. IfF and F' are apart from each other so that they do not overlap, only T will remain and the matching ambiguity will be resolved.

Let 1-'F· 1-'F'· EF , and EF' denote the means and co­ variance matrices ofF and F' . The relative position ofF with respect to F' is represented by the mean 1-'F' - 1-'F and the covariance matrix EF + EF'. Whether the two distribution overlap or not can be checked by checking whether F belongs to this distribution of the relative po­ sition. The Mahalanobis distance of F with respect to this distribution,

has a x 2 distribution with two degrees of freedom. It is possible to check whether the two distributions can be fused with certain confidence by setting an appropriate threshold on dm [Ayache and Faugeras, 1989].

The following steps are employed to check whether a viewpoint is valid for disambiguation. Fig.7 shows the case where a viewpoint is on the right side of the line L1T. In this case, .four cases (F~ ....., F_h) are examined. Let us explain the case of F~. F~ lies on the line L2 T . For each possible position ofF~, the distribution ofF~ is first calculated considering the quantization error; then, the Mahalanobis distance is calculated between this dis­ tribution and the distribution of F . By repeating this calculation for all possible positions ofF~, the minimum value of the Mahalanobis distance for F~ is obtained. If the minimum value throughout the four cases is larger than some threshold, the distributions of F and F' do not overlap, and consequently the matching ambiguity is resolved. A set of valid viewpoints is the search area for the next observation point .

The front position is true.

The rear position is true.

Figure 7: Four cases for the check using the Mahalanobis distance.

4 Efficient Solving Strategy using the Uncertainty Model

In our formulation, the computational cost for solving a vision-motion planning problem depends on the combi­ nation of the number of possible observation points , that of possible targets to be observed, and that of possible observation results. An exhaustive search at each level of the search tree may cause combinatorial explosion. In order to reduce the computational cost , an efficient prun­ ing methods is required. In branch-and-bound method [Ibaraki, 1987], branches are pruned which do not gen­ erate better solutions than the current incumbent (the best solution among those that have been acquired so far) . Such a pruning is based on the estimate of the lower bound of a branch. This section describes a method of calculating the lower bound using the uncertainty model of stereo vision.

4.1 Assumption of Perfect Sensor Information

Let iu be information including uncertainty to be ob­ tained by a next observation o, and ip be information to be obtained by assuming that o can provide information without uncertainty. Generally, the cost of a solution sp based on ip is less than or equal to that of a solu­ tion Su based on iu. Therefore, Sp gives the lower bound of the cost of possible solutions. We call the cost of sp the lower bound under the assumption of perfect sensor information.

In order to employ the assumption of perfect sensor in­ formation, it is necessary to know what is perfect sensor information. If a property (e.g. the position of a fea­ ture) is to be sensed, perfect sensor information means that the variance of the distribution of the property is zero. Such information, however, is useless because the probability of obtaining each possibility of the perfect information is derived only from the obtained distribu­ tion.

If the possible situation is classified into several situ­ ations according to the values of properties, and if the cost can be calculated for each situation, the assumption of perfect sensor information provides useful informa­ tion. For example, let us consider the situation depicted in Fig.l. Suppose the robot has obtained a probability distribution of the width of the space between objects. There are three possible situations: (1) the lower bound of the distribution is larger than the robot width and the space is passable; (2) the upper bound of the distri­ bution is smaller than the robot width and the space is impassable; {3~ otherwise, the passability is undecided.

Let N(wo, u0) be the current distribution of the width of the space between obstacles and 11~6 , be the uncer­ tainty of the next observation. In addition, let w1 and u? denote the mean and the variance of the distribu­ tion after observation which is the fusion result of the current distribution and the observed one; u? is given by u5u~6,/(u5 + u~6,); w1 follows a normal distribution N(/Jwp u~,) = N(wo, u6/(u5 + 11~6,)) [Miura and Shi­ rai, 1992b]. If the passability of the space is determined by comparing the robot width Wrobot with w1 ± 30"1, the probability Po that the space is passable, the probabil­ ity Px that the space is impassable, and the probability P t:. that the passability is undecided are given by the following (see Fig.8):

If the observation result includes no uncertainty (i.e. under the assumption of perfect sensor information), the probabilities are obtained by letting 11~6, = 0 in equation (5) . In such a case, the passability of the space is per­ fectly determined, that is, Pt:. becomes zero.

4.2 Calculating Lower Bounds

If a set of fixed situations with probabilities is obtained, the cost for each situation is calculated. Then, the lower bound is obtained as the expectation of the total cost . A better lower bound is obtained using the uncertainty model of vision. Again in the above example, once an observation point and a target of observation are de­ cided, the uncertainty 11~6 , is determined and therefore probabilities in equation (5) are calculated. In case of Po and Px, costs can be calculated because the situ­ ation is fixed . In case of Pt:., the lower bound can be obtained by applying the assumption of perfect sensor information recursively. If there are multiple targets of observation in the environment, the assumption is also recursively applied to the targets in turn . Fig.9 shows the lower bounds for possible observation points in front of the obstacles in Fig.l. If the lower bound of a point is higher than the incumbent value , the point can be eliminated from the candidates.

5 Experimental Result

Fig.lO shows the experimental environment . A robot is going to the goal point among obstacles. An optimal vision-motion plan is generated from a pair of images shown in Fig.4. The images are taken at the start po­ sition. In the experiment, we made the following as­ sumptions: only the positions of vertical segments in front of the robot are unknown, that is, potential routes (gaps) are given; the cost of taking a detour is given; time for one observation is constant; uncertainty of mo­ tion is negligible because it is much smaller than that of visual information .

From the first pair of images, three ambiguous match­ ings (see Fig.5) are detected which affect the plan gen­ eration. By considering both disambiguation of multiple matchings (described in 3.4) and the possibility of oc­ clusion, the search area for the next observation point is decided as shown in Fig.ll(a).

Fig.ll(b) shows the generated optimal plan. The solid arrow indicates the next move, and dashed arrows indi­ cate possible paths after the next observation. By the next observation, the ambiguities for three positions are resolved. Only for the case that A and C are true po­ sitions, the passability of the narrow space may be un­ decided, and the second observation point is recursively determined. Numbers attached to paths indicate the probabilities of taking the paths. Of course, we cannot predict which path the robot actually takes because ac­ tual behaviors depend on further observation results.

It takes about five minutes to generate the plan on a SPARCstation (33MHz). About 65% of the candi­ dates for the next observation points are eliminated by the pruning based on the lower bound. At deeper levels of the search, more than 90% of the candidates are eliminated in almost all cases. This result shows the effectiveness of the pruning.

• (a) An indoor scene.
• (b) Top view.
• (a) Search area for the next observation point. Symbols corresponds to those in Fig.5.
• (b) The generated optimal plan.

Figure 11: Planning result.

6 Conclusions and Discussion

This paper describes an uncertainty model of stereo vi­ sion and its application to a vision-motion planning for a mobile robot. In the model, the positional uncertainty of a feature is represented by a weighted sum of normal distributions; each distribution is calculated by a possi­ ble matching considering the quantization error; each weight is decided according to the plausibility of the matching. A strategy is also proposed which resolves matching ambiguities by selecting a viewpoint to avoid the ambiguities . The uncertainty model is used in the vision-motion planner, and an optimal plan is generated for a real world problem. An efficient solving strategy is also described which employs a pruning method based on the lower bounds calculated by the assumption of per­ fect sensor information. The pruning method effectively reduces the computational cost.

Currently, the positional uncertainty of a segment is modeled by the quantization error and the matching am­ biguity. It is a future work to consider other factors such as the contrast of an edge segment.

The vision system provides only positions of line seg­ ments . It is necessary to determine whether a region formed by the segments is an obstacle or a free space [Faugeras et al., 1990] . Ur.certainty of free spaces could be modeled by using uncertainties described in this pa­ per .