and implemented in parallel on a board with 3 Motorolla DSP's. This board, part of the DMA machine, has been designed and built jointly by INRIA

Transcription

1 In Vision-based Vehicle Guidance, Springer, New York, 1992 edited by I. Masaki, Chapter 13, pages Obstacle Avoidance and Trajectory Planning for an Indoors Mobile Robot Using Stereo Vision and Delaunay Triangulation Michel Bua Olivier D. Faugeras Zhengyou Zhang INRIA Sophia-Antipolis BP Valbonne Cedex - FRANCE Abstract This article describes the work at INRIA on obstacle avoidance and trajectory planning for a mobile robot using stereovision. Our mobile robot is equipped with a trinocular vision system which is being put into hardware and will be capable of delivering 3D maps of the environment at rates between 1 and 5 Hz. Those 3D maps contain line segments extracted from the images and reconstructed in three dimensions. They are used for a variety of tasks including obstacle avoidance and trajectory planning. For those two tasks, we project on the ground oor the 3D line segments to obtain a twodimensional map, we simplify the map according to some simple geometric criteria, and use the remaining 2D segments to construct a tessellation, more precisely a triangulation, of the ground oor. This tessellation has several advantages: It is adapted to the structure of the environment since all stereo segments are edges of triangles in the tessellation, It can be eciently computed (the algorithm we use has a complexity o(n) if n is the number of segments used), It is dynamic, in the sense that segments can be added or subtracted from an existing triangulation eciently, It can be computed in parallel quite easily. We use this triangulation as a support for further processing. We rst determine free space, simply by marking those triangles which are empty, again a very simple processing, and then use the graph formed by those triangles to generate collision free trajectories. When new sensory data is acquired the ground oor map is easily updated using the nice computational properties of the Delaunay triangulation and the process is iterated. We show an example in which our robot navigates freely in a real indoors environment using this system. 1 Introduction Our mobile robot can use a vision machine which has been designed and built within an European Esprit project, project P940, also called DMA, for Depth and Motion Analysis. Without entering into the details of this machine, it is sucient to say here that it can process three images acquired from a trinocular stereo rig and compute the position and orientation in three dimensions of a set of line segments. These line segments are produced by polygonal approximations of edges in the three images. The stereo algorithm that matches the polygonal chains has been described in [AL87]

2 and implemented in parallel on a board with 3 Motorolla DSP's. This board, part of the DMA machine, has been designed and built jointly by INRIA and MATRA. Even though at the time of this writing the full machine is not yet available, all boards that implement the various vision modules have been built and tested. The rst prototype of the machine has been assembled at ELSAG, an Italian, Genova based company, and is being tested. At the time of the conference, the testing should be completed and boards in the process of being duplicated so that the machine is available at INRIA in Sophia-Antipolis to drive our mobile robot. The throughput time is between 1 and 5 Hz, depending upon the exact hardware conguration. This means that, in the best case, we will reconstruct a three-dimensional wireframe representation of the environment ve times a second. Having put things in perspective, what we want to describe in this article is a piece of work that is built on top of the DMA machine and plans to use its real time capabilities. The task is to build local representations of the robot environment that can be used to dynamically map free space, plan and update trajectories. We use the word trajectory and not itinerary to mark the dierence in time scale and in planning complexity. The representation we are going to describe is local and does not carry any semantics. It is a pure volumetric representation of the free space around the robot, as measured by the stereo system over the last few seconds. It is therefore a combination of, let us say, no more than 25 three-dimensional wireframes, which is used to support tracking of an itinerary which has been set up as a goal by another, slower process. The representation is thus local, both in space and in time. The use of this local trajectory planning is to allow this process to gather information about the actual state of the environment and in particular about events that could not be foreseen, such as the approach of a moving obstacle. This local representation is, on one hand passed over to the higher-level, slower process and, on the other hand used immediately, in a reactive fashion, to cope with possible collisions and to perform the actual robot control operations that lead to a satisfying pattern of motions. 2 What do we do with the 3D wireframes We assume that for the kinds of time scales we are considering, the ground on which the robot is moving can be considered as at. This local plane is known through calibration and used as a support for our representation. We project on the ground all 3D line segments obtained from stereo which lie between the ground and a plane parallel to it at a height equal to that of the robot. We have thus reduced a three-dimensional wireframe representation to a two-dimensional one. To help guide the reader's understanding, we show in gure 1 an image of a part of a typical scene that the robot has to cope with, and in gure 2 the projection on the ground oor of the 3D line segments reconstructed from stereo. We notice that this representation is probably not immediately useful, being still quite complex geometrically. A further item of interest is that in the process of reconstructing 3D line segments, we also compute a measure of their uncertainty. The exact measure depends on what kind of representation we are using for the line segments [AF89, ZF90a] but it always represents the amount of condence we have in a specic segment. This condence is a function of the reliability of the various elements in the processing chain such as, edge detection, polygonal approximation, calibration of the trinocular stereo rig, as well as of the geometry of the scene (segments which are far are less reliable than segments which are close). It is represented by a symmetric weight matrix, also called sometimes a covariance matrix if a probabilistic interpretation is required, whose size is that of the representation, and whose diagonal elements are big if the corresponding parameters in the representation

3 Figure 1: A typical indoors scene 101 Segments Figure 2: Projection on the ground oor of the 3D segments produced from a stereo view of the scene of gure 1

4 Figure 3: Ellipses of uncertainty for the midpoints of the 2D segments of gure 2 is uncertain, and small otherwise. From the 3D representation of the line segments and the corresponding measure of uncertainty it is possible to compute a representation for the projected segments and the corresponding uncertainty (see section 3). Uncertainties are conveniently represented by ellipses in the plane where they can be thought of as geometric upper bounds. For example, gure 3 shows the ellipses representing the uncertainties of the midpoints of the projected segments of gure 4. We note that segments which are far from the robot are more uncertain than those which are close. Having such a measure is important because it allows processes that operate on the representations to give more weight to reliable measurements than to others. An example of such a process is one that estimates the robot egomotion [AF89, ZFA88, ZF90a] and the obstacles motion [ZF90c, ZF90b]. In this article we use this information to help producing a simplied version of the 2D map of gure 2. The main reason for producing such a map is that of conciseness. Since our goal is to produce a map of free space, we are uninterested in redundant information such as too many, geometrically similar, segments in the same area that possibly arise from a highly textured pattern. Suppressing redundant information will allow us to keep only the information really relevant to the task at hand and will facilitate and speed up further processes. We now discuss in details how the 2D map is simplied. 3 Two-dimensional map simplication We describe how line segments are represented.

5 3.1 Representation of line segments A (2D or 3D) line segment is usually represented by its endpoints M 1 and M 2 and their covariance matrices 1 and 2. Since the endpoints of a segment are not reliable, we cannot directly use them in most cases. Considering the longitudinal (along the segment) position of a line segment is much less precise than the transverse one, one may use instead its supporting line. However, after this abstraction we lose completely the longitudinal information, which is useful in many cases such as matching and fusion. Further, the uncertainty of the supporting line does not reect that of the segment, i.e., the supporting line of a very uncertain segment may be less uncertain than the supporting line of a less uncertain segment. For those reasons, we have proposed a new representation for 3D line segments which is a trade-o between lines and segments [Zha90, ZF90a]. In our representation, the spherical coordinates and are used to represent the direction of a line segment, and the midpoint m to locate the segment. The length of the segment is denoted by l. If we denote M 2? M 1 by v = [x; y; z] t (the non-normalized direction vector), the unit direction vector by u = v=kvk, [; ] t by, we get = 8 < : arccos = arccos 2? arccos x p x 2 +y 2 if y 0 x p x 2 +y 2 otherwise (1) p z : x 2 +y 2 +z 2 The covariance matrix of is given, to the rst order approximation, v ; where is the Jacobian matrix of with respect to v, and v = Similarly, we can compute the covariance matrix u of the unit direction vector u. The most important part of our representation is the modelization of the uncertainty of the midpoint. The midpoint of a line segment is modeled as m = (M 1 + M 2 )=2 + nu; (3) where n is a random variable. In fact, equation 3 says that the midpoint of a segment may vary randomly around its position (M 1 +M 2 )=2 along the direction of the segment. The random variable n is modeled as Gaussian, zero mean and whose standard deviation n is some positive scalar. In our implementation, n is related to the length l of the segment, n = l (we choose = 0:2). This says that a long segment is much likely to be broken into smaller ones in other views. The covariance matrix of m is given by (see [ZF90a, Zha90]) m = ( )=4 + 2 n ( u + uu t ): (4) The uncertainty in the length of a segment is not modeled because it is not required in our algorithm. A 2D line segment can be viewed as the orthogonal projection of a 3D line segment on a plane. A 2 3 matrix T can be used to describe this projection, i.e., we can relate a 3D point m 3 with its projection m 2 (here subscripts are used to denote the dimension, which will be omitted if no ambiguity) by the following equation m 2 = T m 3 : (5)

6 For example, if we want to project a 3D point on the plane y = 0, then T = " #. We represent 2D line segments similarly to 3D line segments, that is, a 2D line segment is described by its angle with the axis x, its midpoint m, and its length l. Those parameters can be easily computed based on equation 5. We compute also the variance for and the covariance matrix m for m. For example, m 2 = T m 3 T t. The uncertainty on l is not modeled. 3.2 Simplication At this point we have a set of 2D line segments, which may contain redundant information. In the following, we describe how to intelligently merge geometrically similar segments in the same area, which are, for example, projections of some textures on a wall. The simplication is performed as follows. All segments are sorted by a bucketing technique which allows to easily access the neighbors of a segment. The segments are also sorted according to their orientation. Now for a segment not yet processed S, we compute a list of segments which are neighbors to it and another list of segments which have similar orientations to it. The intersection of the two lists are the rst candidates. If a segment S 0 among those candidates is similar enough to S, we then merge S and S 0 yielding a new segment ^S. We then compare ^S with the rest of the candidates: if a new similar segment S 00 is found, ^S will be updated. The above procedure is applied to every candidate and every unprocessed segment. The merging technique for 2D line segments is the adaptation of that for 3D line segments [ZF90a, Zha90]. Let, m and l be the parameters of a segment S with uncertainty measurements 2 and m, and 0, m 0 and l 0 that of another segment S 0 with uncertainty measurements 2 and 0 m 0. Those two segments can be merged if and only if they satisfy the following relations based on the Mahalanobis distance (? 0 ) 2 =( ) ; (6) (m? m 0 ) t (m + m 0 )?1 (m? m 0 ) m ; (7) where and m are thresholds. Looking up the 2 distribution table, we can choose = 3:84 for a probability of 95% with 1 degree of freedom and m = 5:99 for a probability of 95% with 2 degrees of freedom. The above conditions say that we merge only segments which are the same (in the probabilistic sense). Based on a minimum-variance estimator, we get the following parameters for the merged segment ^S: ^ = ( )=( ); (8) = 2 2 0=( ); (9) 2^ ^m = m 0 (m + m 0 )?1 m + m(m + m 0 )?1 m 0 ; (10) ^m = m(m + m 0 )?1 m 0: (11) They give the orientation and the position of the merged segment. Since the two segments are considered as two instances of a single segment, the union of them should be its better estimate. By projecting the endpoints of the two segments on the merged segment, we choose the farthest projections as the endpoints of the merged segment and the length can also be computed. The real midpoint M of the merged segment can be determined and can always be expressed as M = ^m + s^u; (12)

7 44 Segments Figure 4: Simplication of the set of segments of gure 2 where s is a scalar and ^u is the unit direction vector of the merged segment. The above equation can be interpreted as the addition of a biased noise on ^m, thus the covariance matrix of M is given by M = ^m + s 2 (^u + ^u^u t ): (13) For more details, the reader is referred to [ZF90a, Zha90]. 4 Constructing a volume representation of free space We now have a simplied two-dimensional representation of the environment in terms of line segments in which free space is not explicitly represented. In order to obtain such a representation, we use a special data structure that has been quite thoroughly studied in Computational Geometry [PM85] and that we have been using recently to represent 3D stereo data [FBL]. We use it here in two dimensions but the basic ideas are similar. We have no space here to go into the details of the Delaunay triangulation and we will only try to convey the main ideas and the philosophy of the technique. 4.1 Constructing a triangulation of the 2D maps The idea is to compute, as an intermediate representation, a triangulation of the endpoints of the 2D segments that has the characteristic of containing the segments as edges of the triangulation. A set of planar points is said to be triangulated if its points are joined by nonintersecting straight line segments so that every region internal to the convex hull is a triangle. There exist many dierent triangulations of a set of points but we have chosen the Delaunay because of two reasons

8 It has some nice properties related to shape It can be implemented eciently Both reasons are well described in [FBL]. The shape property can be summarized as follows in two dimensions but it is true also in three dimensions: suppose we measure a number of points on the boundary of an object and assume that this boundary is piecewise smooth. Then, under some fairly weak assumptions, the Delaunay triangulation of the set of measured points contains a polygon that approximates the boundary shape well. If that polygon can be easily recovered from the triangulation, then we have a nice way of representing the shape of the object. How to do this is described in the next section. With respect to the implementation, there exist algorithms for computing the Delaunay triangulation of a set of n 2D points with an optimal time complexity of o(n log n). The basic idea is to use the divide and conquer strategy [PM85]. Even though this kind of algorithms is optimal in complexity, it has one main drawback, namely that they are not incremental: we have to wait until all data points have been collected before we can start triangulating them. This is in contrast with what is needed for our application where data are collected sequentially and must be processed on the y. Fortunately, there exist an incremental algorithm whose worst case time complexity is o(n 2 ) but whose average complexity is o(n). The algorithm is described in details in [FBL] but the basic idea is quite simple: suppose we have triangulated the rst n points, and that we have just measured the n + 1st. We exploit the fundamental property of the Delaunay triangulation which is the following. If ABC is a Delaunay triangle, consider the disk it denes. That disk does not contain any other data point. This property of the disks is necessary and sucient for a triangulation to be Delaunay. Coming back to our new data point, we only have to determine those disks of the existing triangulation it falls into (the corresponding triangles are edge connected) and retriangulate their data points together with the new point. A further property of the Delaunay triangulation which is very relevant to our application is related to the skeleton. Indeed, if we consider the centers of the Delaunay disks and join those centers whose triangles share an edge but do not cross the boundary of free space, we obtain a set of polygonal lines that are a subset of the Voronoi diagram of the set of measured points [FBL]. Voronoi diagrams have been heavily used in robotics as a support for trajectory planning and the fact that the Voronoi diagram of free space is present in our representation comes as a strong support for our approach. In our case we have more than points, namely segments. If we just triangulate their endpoints as shown in gure 5 we obtain the so-called unconstrained triangulation of the set of segments of gure 4. It can be seen that some stereo segments are not Delaunay edges. This is a problem because the next step described in the following section may produce a less accurate representation of free space than if all stereo segments are Delaunay edges. The problem can be eliminated by adding some more points on some of the stereo segments. The procedure is described in [FBL] and the results on the same scene are shown in gure 6 which is the so-called constrained Delaunay triangulation of the set of stereo segments. 4.2 Marking empty triangles Having constructed the constrained triangulation we now wish to identify those triangles which are part of free space. In order to do this, we exploit a very simple visibility property which is best explained by looking at gure 7. In this gure, the robot position is indicated by R 1 and a number

9 Problem! Figure 5: Unconstrained Delaunay triangulation of the set of segments of gure 2 The problem has been solved! Figure 6: Constrained Delaunay triangulation of the set of segments of gure 2

10 R 1 P 1 P 2 P 8 P 7 P9 P 3 P 4 P 6 P 5 Figure 7: How to mark empty triangles of segments (P 1 P 2 to P 8 P 9 ) are present and represented as thick lines. Only a subset (marked with black bullets) of those segments are visible from R 1. The other Delaunay edges are represented by thin lines. Since point P 5 has been seen by the robot in position R 1, it means that all triangles crossed by the optic ray R 1 P 5 are empty and should be marked as such. We have therefore a very simple way of constructing from the Delaunay triangulation a set of marked triangles which represent free space. 4.3 Taking into account several viewpoints Having discovered a portion of its environment which is free space, the robot can plan to move to a new position. Once it has moved to this position, it needs an estimate of its motion. This is provided by another process described elsewhere [AF89, ZF90b] which provides at the same time an estimate of the motion and a measure of its uncertainty. Using this information, the previous set of segments can be transformed in the current coordinate system and simplication can be attempted with the set of newly measured segments. This previous set of segments is actually the model of the environment that the robot builds. It is the concatenation of everything it has seen in the previous instants. How this model has been built has been described in section 3. The robot does not have innite memory in the sense that segments that have not been seen for a long time have their measures of uncertainty increased. When those measures reach a certain threshold, the corresponding segments are discarded. The model is triangulated and free space is explicitly represented in it by marked triangles. Segments in the model also know from which robot position they have been seen. When a new segment is fused with an old one, the old segment is erased from the triangulation and the new one inserted. This operation can be implemented quite eciently with the incremental algorithm that has been sketched in section 4.1. Once the new segments have been inserted into the model, free space can be recomputed using the triangulation and the various robot positions. In gure 8, the robot has moved from R 1 to R 2 and discovered segments P 5 P 6 and P 6 P 7. Applying again the visibility property allows to improve its local representation of the environment.

11 P 1 P 2 P 8 P 7 P9 P 3 P 4 R 2 P 6 P 5 Figure 8: Taking into account a second viewpoint 4.4 Parallelization There are two stages where parallelism can be introduced to speed up computation time. The Delaunay triangulation can be parallelized by splitting the environment into square buckets and computing independently the Delaunay triangulation of the segments within each bucket. There are of course side eects such as if a segment crosses two buckets then its point of intersection with their boundary must be added. The result is that the union of the Delaunay triangulations is not globally Delaunay, only locally. This is no real problem since the Delaunay property is used only for building the triangulation but not for the representation of free space. This parallelization is being implemented on the parallel machine CAPITAN, built by MATRA, which will be the host for the DMA machine. The other step that can be made parallel is the marking of the empty triangles: each optical ray can be traced independently for each robot position. We have not yet implemented that idea. 5 Results The constrained Delaunay triangulation (gure 6) of gure 4 can be used to compute free space. The result is shown in gure 9. The robot then plans a motion in the free space zone and acquires a new set of 3D data. This is used to estimate its displacement and the previous simplied 2D model is fused with the new data after projection on the ground plane. The result of this fusion is shown in gure 11 which is to be compared with gure 4. The robot displacement between the two views is 4.5 degrees and 15cms. The new representation of free space is shown in gure 12 which is to be compared with 9. Finally we show in gure 13 the result of integrating 13 views obtained while the robot was moving and in gure 14, the resulting border of free space. 6 Conclusions We have presented the current work at INRIA on obstacle avoidance and trajectory planning for a mobile robot using stereovision. In order to facilitate and speed up further processes, we project 3D line segments reconstructed by our trinocular system on the ground plane. We have detailed how the resulting 2D map is further simplied based on some geometric criteria. Performing the

12 Figure 9: The border of free space for the environment of gures 1 and Segments Figure 10: Before the fusion of two dierent view points

13 55 Segments Figure 11: After the fusion of two dierent view points Figure 12: The border of free space for gure 11

14 The robot first looked around, went forward, backward, turned again... Figure 13: The fusion of 13 scenes Delaunay triangulation on the remaining segments, we get a tessellation of the ground oor. The free space can then be determined by marking empty triangles, and is used to generate collision free trajectories. After the robot arrives in a new position, the stereo system builds a new set of 3D line segments. They are again projected on the ground plane, which are integrated into the existing 2D map. The Delaunay triangulation can be easily updated. We have also described that most part of our algorithms can be implemented in parallel. References [AF89] [AL87] [FBL] Nicholas Ayache and Olivier D. Faugeras. Maintaining Representations of the Environment of a Mobile Robot. IEEE transactions on Robotics and Automation, December also INRIA report 789. N. Ayache and F. Lustman. Fast and reliable passive trinocular stereovision. In Proceedings ICCV '87, London, pages , IEEE, June Olivier D. Faugeras, Jean-Daniel Boissonnat, and Elizabeth Lebras-Mehlman. Representing Stereo data with the Delaunay Triangulation. Articial Intelligence, (),. also INRIA Tech. Report 788. [PM85] F. Preparata and Shamos M. Computational Geometry. Springer-Verlag, New-York, [ZF90a] Zhengyou Zhang and Olivier D. Faugeras. Building a 3d world model with a mobile robot: 3d line segment representation and integration. In Proceedings of the 10th International Conference on Pattern Recognition, IEEE Computer Society, June to appear.

15 Figure 14: The border after 13 scenes

16 [ZF90b] Zhengyou Zhang and Olivier D. Faugeras. Motion Analysis of Two Stereo Views and its Applications. In Proceedings of the ISPRS Symposium on Close-Range Photogrammetry Meets Machine Vision, September to appear. [ZF90c] Zhengyou Zhang and Olivier D. Faugeras. Tracking and Motion Estimation in a Sequence of Stereo Frames. In Proceedings of the 9th European Conference on Articial Intelligence, ECAI, August to appear. [ZFA88] Z. Zhang, O.D. Faugeras, and N. Ayache. Analysis of a sequence of stereo scenes containing multiple moving objects using rigidity constraints. In Proc. the Second International Conference on Computer Vision, pages , IEEE, Tampa, Florida, December [Zha90] Zhengyou Zhang. Motion Analysis from a Sequence of Stereo Frames and its Applications. PhD thesis, University of Paris-Sud, Orsay, Paris, France, in English, to appear.