- Email: [email protected]
A dynamic 3D environmental model with real-time access functions for use in autonomous mobile robots
ELSEVIER
Robotics and Autonomous Systems 14 (1995) 119-131
Robotics and Autonomous Systems
A dynamic 3D environmental model with real-time access functions for use in autonomous mobile robots Gunter
Magin, Achim RuB, Darius Burschka,
Georg F~irber *
Lehrstuhl fiir Prozeflrechner, Technische Universitiit Miinchen, 80290 Munich, Germany
Abstract
A model of the environment is a mandatory requirement for the autonomy of a mobile robot. In this paper, we present a framework for the prediction of expected sensor images on feature level, which is based on a-priori knowledge about the geometry of the environment. Our algorithm is capable to serve all distance and image measuring sensors typically found on mobile robots in real time. Fast prediction is achieved by using concepts from database technology and computer graphics. By incorporating differences between the expected and sensed image, the model can be adapted to changes and uncertainties in the environment. So the predicted features can be attributed with uncertainties to support and enhance the match process. Keywords: Sensor-specific environment modelling; Spatial indexing; z-buffer projection; Computer graphics; Real-
time feature prediction
I. I n t r o d u c t i o n
1.1. Motivation
Autonomy of a mobile robot can be defined as the ability to handle irregular situations without external help. To recognize such situations a robot must know what it should regard as irregular and how to distinguish regular from irregular. This knowledge must be on board the robot. In case of a detected difference between reality and assumptions, the preplanned behavior must be adapted. Irregular situations concerning the ex-
* Corresponding author. E-mail: [email protected]; Fax: +49 89 2105-3555.
ternal world can be detected by comparing the perceived sensor images against synthetic images based on an internal representation of the environment and interpreting the differences. An unexpected obstacle in the robot's path is an example for such an irregular situation. A proper autonomous reaction would be to bypass the obstacle, if possible, or to choose another way to the destination. Planning a mission also needs knowledge of the environment, e.g. docking points, usable paths between source and destination, known obstacle positions, etc. The execution of a mission must be supervised for possible collisions, so the robot must verify that the expected free motion space is actually free. For navigation purposes it is essential to know the current position at any time,
0921-8890/95/$09.50 © 1995 Elsevier Science B.V. All rights reserved SSDI 0921-8890(94)00024-7
120
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
either in absolute world coordinates or relative to nearby objects in the scene. To control docking to or from an object in the world, knowledge of the docking partner's geometry and the relative position is necessary. All these requirements can be fulfilled if environment information can be provided based on an on-board environment representation. So an environmental model is a mandatory building-block for the autonomy of a mobile robot. To make even more use of an environmental model, maintenance is also an issue, especially in a dynamic environment. It is most desirable in such environments to have an accurate and upto-date model at hand. Therefore any chance to compare the internal representation with the real world must be used, which means, when a robot passes a scene, it should verify the specific part of the internal representation immediately and parallel to the execution of the mission. In case of permanent differences, possible options are: - stop the mission and check out the difference thoroughly to update the environment representation in a very careful way, the mission, but keep in mind to come back and analyze the scene later, continue the mission and mark the scene as uncertain in the environment representation, - ignore the detected change.
-continue -
The better options require an update of the environment representation.
1.2. Approach Considering these issues about autonomy, our goal was to support model verification and actualization during normal mission execution. This is a challenging real-time problem, as the sensor data rate cannot be reduced, and the robot should be able to cruise with a task-specific speed, not effected by the internal prediction and update processing. Therefore a new approach was chosen, which is fast and robust, capable of handling complex man-made environments by using the standard sensor classes for mobile robots. Our approach is based on prediction of sensory images (Fig. 1). Interpreting an acquired sensor image without knowing what is supposed to be seen is much more difficult than verifying a given hypothesis, for example the assumption that everything in the environment is as expected. So a prediction of synthetic images based on an internal environment representation makes preprocessing, abstraction and interpretation of sensory data faster, more robust and more reliable. An example for the predicted image and the raw data is shown in Fig. 2 and Fig. 3 for a video sensor in a manufacturing environment. Prediction takes place on a feature level of abstraction. This abstraction level is situated between raw sensor data and a map with the 3D-objects in the environment. Features can be easily extracted and abstracted from raw sensor data by fast preprocessing algorithms. Examples are natural landmarks like edges, corners, planes, points
f J
retrieval of local part of environment
~
imagerendering ~
I
::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
environment representation time threshold
filtering postprocessing
iiresults '
Fig. 1. Information flow.
'~
prePen~o7ed
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
/
Fig. 2. Predicted sensor image from estimated viewpoint.
with high reflection, or sensor-specific artificial landmarks. On the other hand, storing and predicting the environment on the feature level is much more efficient than on the level of raw sensory data. So comparing synthetic and real images on a feature abstraction level is an optimal compromise for both sensor and environmental model. Correspondences between the features in both images are established in the match process, where three classes of results are possible: features visible in both images, features only visible in the synthetic image, and features only
Fig. 3. Real sensor image from actual viewpoint.
121
visible in the real image. The match results are then used either to update the internal representation continuously or for localization of the mobile system. As an alternative to model based prediction, prediction can be based on the temporal context by tracking the features' positions in the image space and extrapolating the positions to the next frame. Our approach is characterized by the following facts: we use a pure 3D-approach where environment information is stored in a symbolic way. Nevertheless we are able to deliver synthetic sensor images in real time for a broad range of image and distance sensor systems, like video cameras with various sensor parameters [23], 2Dand 3D-laser-range cameras (e.g. [16,17]) with variable resolution, microwave-radar systems ([6,26]), or laser-scanners [30]. The model information can be delivered in a sensor- and taskspecific form, e.g. for absolute and relative localization purposes, motion space surveillance, object identification, or path planning. Our approach is also applicable to dynamic environments by integrating changes in the world. All inquiries for synthetic images and updates are handled within task-specific real-time limits. 1.3. O t h e r w o r k
In robotics many task-specific environmental models are used: [10,2] use occupancy grids for mapping the environment with ultrasonic sensors and performing navigation tasks. Other models emphasize their specific task, like object recognition [24], path-planning [18], navigation [12] and manipulation [29]. There are also specialized environmental models for specific sensor systems, like ultrasonic [10], video [24], laser range sensors [4,12,13,]. All these models have in common that they are very well adapted for the specific single need, but cannot serve as a general purpose data base for geometric environment information, where a 3D sensor-independent representation is necessary. In [9] the kinematic model of a moving object is used for the prediction of features. As in our approach, real-time preprocessing becomes possi-
122
G. Magin et al. /Robotics and Autonomous Systems 14 (1995) 119-131
ble and image interpretation feasible. However, the approach is limited to simply structured environments, like roads and convex rooms. Furthermore, no effort is made to model the world in a general way, which limits the applicability of this approach. Environmental models for real-time applications, ranging from 20 ms to some seconds are presented in [9] and [19]. Most groups do not emphasize the real-time aspect. Complexity of the modelled environment is usually low to medium [10]. However, environments for production and service scenarios are much more complex. Maintenance of dynamic environments is a very important issue for autonomy. Only some approaches ([1,19,28]) support continuous updating of the model.
2. Modelling of the environment
2.1. Object modelling The environment of the mobile robot is modelled in three dimensions by several layers describing the geometry of the world's objects and sensor-specific features. An object can be modelled in a variety of ways [3]. Here it is considered solid and it is described by a surface boundary representation. The geometry of an object is approximated by convex polygons which are attributed with surface normals and material characteristics (e.g. reflection factor, sensor-specific visibility and transparency). This geometric description of the environment is common to all sensor systems. For each sensor system a separate layer exists with the 3D positions of the features and their spatial and temporal uncertainties. The uncertainties are dynamically modified by the matching results between predicted and real images to support environments, where changes can occur. Modelling of complex manufacturing environments for the geometric layer is done by using a CAD-Modeller, which is part of a simulation system for automated manufacturing. The sensor-specific feature layers are built interac-
tively with a visual feature editor, because automated generation of features from CAD data is generally not feasible in our experience. Reconstructing the environment by a series of sensor images which were taken during a teach-in drive [1] seems to be a good solution to overcome the high effort of model generation by hand. However, the feasibility of this method for complexly structured environments is regarded as an open question.
2.2. Index structures A typical manufacturing environment consists of hundreds of objects whose geometry is described by a total number of more than 50.000 polygons. However, the area overlooked by the sensor systems at a given point of view contains only about 5% of the total number of polygons. Generating the synthetic sensor image can be significantly accelerated if only locally relevant polygons are fed into the following image rendering process (see Fig. 1). As an efficient access to the stored environment information is crucial for the real-time capability, some effort must be spent on the organization of the geometric information. As the robot position is the key index for most retrieval inquiries, the contents of the environmental model is stored in a spatial index structure. Spatial geometric searching is similar to non-spatial multi-key searching although the used index structures must be suitable for non-zero sized spatial objects. Access structures for extended spatial objects are surveyed in [25]. They are classified by position and orientation of the separating hyperplanes and the way of assigning objects to subspaces resulting from subdivision. A new structure is suggested in that paper, the spatial-k-dimensional tree (skD-tree), with k = 2 or 3 for indexing non-zero sized objects. In an s2D-tree the environment is hierarchically divided in rectangular subspaces (Fig. 4), where the objects are attached by their center of gravity. The node structure in the resulting tree contains information about the position of the separating hyperplane and about the extension of objects being cut by the hyperplane into the other
123
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131 memory efficiency
restricted main memory resources require efficient use. Three types of two-dimensional spatial indexing schemes based on dynamic hashing techniques and search trees were implemented and evaluated. They can be characterized how subspaces are defined and how objects covering more than one subspace are handled:
,__n ...................................................
.....................r"~r~-''i ~.tt
.......
=
!
!i
:
I
i
- Equidistant Grid File [10]: The 2D space is
Fig. 4. Example for subdivision by spatial 2D-tree.
uniformly subdivided in quadratic regions. Objects are assigned to all overlapping subspaces. - PLOP-Hashing (Piecewise Linear Order Preserving Hashing) [21,20]: The positions of the hy-
subspace. A subspace is divided either along its longest axis, or with alternating axis. For more details, see [25]. Spatial-k D-trees were designed for geographic information systems. The application in mobile robots is different, because in most cases the query region for the data base access is not a rectangle or a cubeid but a circular or spherical segment, and the data base is main memory resident. So the requirements for indexing schemes used in autonomous mobile robots are:
perplanes adapt to the spatial distribution of the objects. Objects are assigned to that subspace that includes its Centroid. - Spatial-kD-tree [25]: The positions of the hyperplanes of the hierarchical subdivision adapt to the spatial distribution of the objects. Objects are assigned to that subspace that includes its centroid. To evaluate these schemes measurements were carried out with an environmental model of a complexly structured manufacturing environment with about 10.000 polygons. The query region for model access was formed by a circular segment with a radius of 10 m and an angle of 60 ° (see Fig. 5). For dynamically sized query regions, which are
selectivity
local information should be retrieved as specific as possible, real-time access
local information must be retrieved as fast as possible, and
MeanAccessTime [msl
MeanNumberof Elements 2500 ~
Memoryused [kBytel
Equidistant Grid File
,
,
20
2000
/
/
/
/
200
/
lOO _ ~ . ~ _
10
Spatial2D-Tree
_/
PLOPHashing
1500 lO00 100
1000
10000 nos
0 100
1000
10000
o 100
1000
nos
Fig. 5. Selectivity,access time and memoryusage over number of subspaces (nos).
10000
nos
124
G. Magin et al. /Robotics and Autonomous Systems 14 (1995) 119-131
=========================================================================================
i ili ii ii! jiI iii!li i li liiiili!i ""l
-
11111 Fig. 6. Horizontal "span" for fast computation of depth in the z-buffer.
crowave radar systems which cover the environment only in a plane parallel to the ground, the intersection test between query region (spherical segment) and subspace (rectangular parallel-epiped) can be realized as two intersection tests between circular segment and rectangle. As a result access time is only doubled and overall the prediction of the sensor image is accelerated due to enhanced selectivity. An efficient realization of the algorithm that determines the subspaces overlapped by the query region is one main aspect besides tuning parameters for the index mechanism (minimum subspace area, maximum objects per subspace).
3. Prediction of sensor images necessary in a multi-sensor robot, an implementation of the spatial-2D-tree outperformes the other implementations: the s2d-tree achieves best selectivity, which means that almost no elements outside the visibility area of the sensor are delivered by the access functions. S2d lacks in access time, but this is more than compensated in the image rendering stage by the better selectivity. So s2d is considered best choice for complexly structured environments for an overall minimization of the image generation time. Memory usage for all three methods stays within bounds, which makes a memory resident implementation possible. The selectivity of the spatial-k-dimensionaltree becomes even better if k = 3 instead of k = 2, so that the search space is divided into three dimensions. As a drawback the access time increases compared to k = 2 . In case of sensor systems like 2D-laser range scanners or mi-
Table 1 Supported sensor systems Physical sensor Video camera 94 GHz microwaveradar Laser range camera Laser scanner
Generation of sensor-specific synthetic images from the polygons and features in the current sensor cone is done in the following steps: (1) projection into the image plane by coordinate transformation of polygons and features, (2) hidden surface elimination for all polygons, (3) check for features' visibility and partial occlusion (4) determination of regions of interest. These steps are termed the projection pipeline. 3.1. Sensor types a n d sensor m o d e l s
A sensor model is necessary for all the steps in the projection pipeline. It is derived from properties of the physical sensor and from the task-
Virtual sensor Free motion space Localization for natural landmarks Initial and continuous localization Obstacle detection and free motion space supervision High precision surveyingwith artificial landmarks 3D distance measurement Localization High precision relative localization for docking
Feature type Free floor space Edges Line segments Distance to closest obstacle Position of artif, landmarks 3D-depth image Vertical planes Artificial landmarks
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
specific virtual sensor [15]. We support the following models (Table 1): The model of the sensor system specifies how a synthetic sensor-image is generated from the internal representation, and what types of features are required. There is a trade-off between quality of the predicted sensor-images and the computation time needed. Experiments proved that for our application field, modelling the sensors as ideal is sufficient, because the objective is to predict visible features and define regions of interest rather than produce photorealistic images. In case of a video sensor system a simple pinhole camera model with radial distortions is used [22]. Resolution is 512 X 512. Range imaging sensors are also modelled as ideal systems, meaning that multiple reflections and ray expansion is not considered. The 3D-Imaging-MicrowaveRadar [7] measure,; the distance to obstacles at discrete angles with a scaleable angular resolution up to 1°. The range of sight is up to 75 m, the azimuth angle of sight is 360 ° and elevation angle is - 2 5 ° to +25 °. For localization purposes a single 360 ° scan of the sensor is used. The prediction is performed with an internal resolution of 3600 x 1. For generating 3D-depth images a resolution of 1280 x 2,00 is used. The 3D-LaserRange-Camera [13,14] gives a distance profile in the elevation range of - 5 5 ° to + 5 ° and from - 3 0 ° to + 30 ° in azimuth direction with a resolution of 321 × 41 lines at a maximum range 10 m. 3.2. Projection algorithms
The information extracted by the access function describes all obstacles in a local query region
z
=--,
:=:=:=:==
(see Fig. 11). Most of them are not visible to a sensor because of occlusion with other obstacles. The match (see Fig. 1) can be simplified if only visible features are compared with the preprocessed sensor image. Therefore we believe that it is necessary to spend the additional time for the projection of features into image space and for removal of hidden elements. Hidden surface elimination can be done with three different algorithm classes: list oriented (not applicable in our case), object space oriented and image space oriented methods. Methods operating in object space have a high precision: rays through the sensor focus and the border of each polygon are checked in 3D for intersection with all other polygons. Image space algorithms handle occlusions after projection and rasterization. In our application we found computation by object space algorithms 10 times slower compared to image space algorithms. As it was not possible to keep within real-time limits for the prediction of synthetic sensor images with object space algorithms, an image space method based on the z-buffer algorithm was implemented [11]. Inaccuracies resulting from the z-buffer algorithm's discretization are compensated by higher resolution. Every sensor's resolution is limited by hardware or physics. This results in a discretisation of the image coordinates. The z-buffer algorithm operates on these coordinates. It considers for each image point (pixel) of a polygon the distance to that point in 3D, from which the projection started. This distance value is entered at the specific pixel position in the z-buffer, if it is less than the actual value (see Fig. 6). This method has the advantage that every polygon is tested only once and it is not necessary
g
~
I
Planar Projection Plane
125
Equidistant Angular Resolution
Fig. 7. Adaptation of the planar z-buffer algorithm for non-planar projection surfaces.
126
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
Fig. 8. Regions of interest. to test its visibility against the other polygons• The main disadvantage arises from the fact that for each pixel of a projected polygon the distance must be determined• However, the computation effort for each pixel can be reduced significantly considering the polygons are plane: the polygon's surface is divided into horizontal "spans" (see Fig. 6). Each span consists of consecutive horizontal pixels. The spans are limited by edge lines• The distance value of each pixel in a span can be calculated by a cumulative addition of a constant to the distance of the preceding point• This constant depends on the span's length and the distance of the limiting pixels. The cumulative addition is supported by most graphic processors•
The z-buffer algorithm for planar projection planes (e.g. a ideal CCD-camera without radial distortions) can be modified for other projection surfaces in two different ways. The first alternative is the application of a lookup-table to transform the different mapping of the individual pixels in the image (see Fig. 7). This is applicable to any type of projection surface. In the other method a spherical projection surface is considered in the z-buffer algorithm itself• This method takes advantage of the equidistant angular resolution to simplify the computation of the distance• The distance k (see Fig. 9) of a point P in a plane E can be computed by division of the plane's distance from the origin d by the scalar product of the normal vector of the plane n E with the vector of the ray Uray. At the same time the reflection angle of the ray can be estimated by the scalar product n E* Uray already computed for the distance k. The reflection angle is important to estimate the intensity of the echo in the direction of the ray. The most significant echoes can be found at locations where the ray and plane are approximately perpendicular. Sensor data preprocessing and feature extraction is accelerated by using regions of interest defined by the predicted sensor image (Fig. 8). In such a region the corresponding real feature is
cos O * sin ~ y x
];my
0 * cos q ~ ]
~ cos sin •
]
k
ortgm plane E:
n
.......
hE*X- d = 0
nE - normalized normal vector o f plane E d - distance o f the plane from the origin x - point o f the plane E distance k -
for consecutive rays (equidistant angle resolution A~): cos(~ + A ~ ) = cos • * cos A ~ - sin • * sin A ~ sin(~ + A ~ ) = sin • * cos A ~ + cos • * sin A ~ sensor ray: k*vray = x k - distance o f P from the origin V.ray - normalized vector o f the ray x -point o f the ray line
dj
Fig. 9. Estimation of the distance for a non-planar projection plane.
127
G. Magin et aL / Robotics and Autonomous Systems 14 (1995) 119-131
. . . . . . . . . . /. ~ , / yl
/ / / i ~~
Reduced Query Regi~
Feature
Original Query Region ~ ] ~ ~
Mobile ,X0 Robot
iXl' ~- X
Fig. 1O. Dimensioning regions of interest. Fig. 11. Control of query region. very likely to be found. The size of these regions is determined dynamically by the spatial uncertainty of the current position and orientation (x 0, Y0, 3') of the mobile robot, the spatial uncertainty of the position of the feature (xl, yl) and the relative position between robot and feature. Fig. 10 visualizes the basic idea for a vertical line feature assuming uncorrelated gaussian distribution of the spatial uncertainties. The main effect of the regions of interest is that local procedures can be used instead of global procedures 'when matching synthetic and real sensor-image. Therefore feature properties delivered in the synthetic sensor image are used to make establishing correspondences more robust. These properties are either based on a-priori knowledge of material characteristics or on results of previous match processes, or even both.
3.3. Real-time control The typical prediction time interval is 0.25-0.5 seconds. The amount of computation time ttota I with the z-buffer-algorithm is estimated as follows: ttotal = taccess + npolygon " /polygon + n pixel- polygon " tpixel - polygon + nfeatur e " tfeature -I- npixel_feature • tpixel_feature . After a data base query (tac.... ) the number of retrieved polygons and f e a t u r e s (npolygon , nfeature) , together with statistical knowledge about the mean size of the polygons and features (npixel_polygon, npixel_polygon) give a rough figure of
ttota I. If the expected computation time is above the real-time limit, the data base query is repeated with a reduced query region (Fig. 11). There is now a good chance to meet the realtime requirements, but there is no guarantee in critical moments. Therefore a new prediction of ttota I is done after modelling transformation, clipping and rasterization. The error of the estimated ttota I is now less than a few percent. The resolution of the predicted sensor image is then reduced according to the linear relation between the resolution (number of pixels) and /total" Fig. 12 shows the combined effect of the two methods to meet given real-time requirements.
3. 4. History-based prediction It seems to be unnatural to spend computation time to go through the procedure of model access, projection, etc. for every single sensor frame,
Distribution Function 1,0 0,8 -
Controlled f It
Predicti
0,6-
I, ~ ~
/
UncontTlded Predi~
0,40,2-
J
i Real-time Linfit
0 0
0~
1,0
1,5
2,0 Time
Maximum Time Fig. 12. Real-time guarantee.
128
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
feature. To find the best match out of those, properties of the features are considered again. Once the correspondence is established, feature history is updated. In case no correspondence for a feature could be established, the old history is extrapolated. After 3 successive frames without correspondence, this feature is considered lost. With this, short-term disturbances can be eliminated. After a couple of frames, features disappear over the image borders. In case the number of tracked features becomes too small, a new model inquiry or another full frame processing phase is required.
Pt-
Qt-3 •
Qt_I-~Q t
Fig. 13. Feature movement.
as most of the predicted feature positions differ only slightly from the results of the previous frame. This is because the features do not move far between successive frames. We track the features to record a history of movement, and then predict where each individual feature will show up in the next frame. Feature history consists of its current position, speed and acceleration in the image plane. The initial position is determined by a model inquiry. Establishing correspondences between features in different frames is a crucial task in the tracking process. Similar to the model based approach a region of interest is established around the predicted position in the sensor image, in which feature extraction takes place. The size of this window must be large enough to ensure capturing of a feature even when movement starts to change (beginning of a curve after a long straight movement). On the other hand, it must be as small as possible to reduce the number of correspondence candidates and preprocessing load. Only the features found in this window are checked for a correspondence with the predicted
4. Acquisition of the environment representation In our approach, the environmental model reads a description of the initial contents. This description can be obtained in three different ways: In case of a manufacturing environment, CAD-data of the environment exists, which can be used after transformation into our description. In a second method, portions of the CAD-data can be used to generate a description of elements like load stands, tool machines, robots, etc. Other elements can be created from scaled geometric primitives, e.g. cubes, pyramids, etc. A macro language combines these elements with their global positions in the "world" resulting in our description for the environment. A third method uses a very rough description with geometric primitives as a-priori knowledge. In an exploration phase this model is enhanced by incorporating sensory perceptions.
5. Update of the environment representation Pi-1 Pi-2 Vi-
Pi_4vJ
vi-1 .
Pi vi 6 .
° ° ° ° Vi_l
@2 Fig. 14. Feature's speed & acceleration.
The concept of detecting changes in the environment is based on the evaluation of differences between predicted and detected features. Minor changes in the environment can be caused by varying illumination for instance. This type of change can be incorporated by modifying attributes for spatial and temporal uncertainty of the environmental model's elements, which also
129
G. Magin et aL /Robotics and Autonomous Systems 14 (1995) 119-131
512 x 512, f = 9 m m
360 ° - Scan
,/
1.0 0.8 0.6 0.4
/
1.0 0.8
0.6 0.4 0.2
0.2
0
0 0
0.1
0.2
0.3
0
[s]
Overall prediction times for a microwave radar (distribution function)
0.1
0.2
0.3
0.4
[s]
Overall prediction times for a C C D -
camera (distribution function)
Fig. 15. Access times in an environment with ca. 7000 polygons on a DEC-Alpha 3000/600 computer.
serves as a filter for transient disturbances of the sensor systems. Detected minor changes can result in major changes in the environmental model, such as insertion and deletion of objects and features. This process is limited by use of short term memories, so only robust and reliable information is entered into long term memory and used for the prediction process. New objects can be detected by an obstacle detection sensor. After identifying the obstacle with an object recognition algorithm [24] the apriori model of the recognized object can be included into the environmental model. Pose and position have been determined during object recognition. Similar functions for deletion and position modificatiion of objects in the environment representation are implemented.
6. Results and future work The quality and accuracy of our environmental model, contents and algorithms, have been proofed to be sufficient in many experiments [26]. The overall time for predicting sensor images is always under the time for sensor data preprocessing. Quantitative results for continuous localization of a moving platform equipped with a 3D laser range camera, a vision system, and a high resolution 3D microwave radar system in a manufacturing environment and other in-door environ-
ments prove the real-time capability and show the effect of the accuracy of the sensor models used in respect to execution time and accuracy of the localization process (see Fig. 15). Future work will emphasize on sensor based environment mapping and model improvement, as the survey of environments performed manually is tedious and error prone. We will also extend the applicability of the environmental model for applications in the manipulation domain and for planning purposes.
Acknowledgement The work present in this paper was supported by the Deutsche Forschungsgemeinschaft as part of an interdisciplinary research project on "Information Processing in Autonomous Mobile Robots" (SFB 331).
References [1] N. Ayache and O.D. Faugeras, Maintaining representations of the environment of a mobile robot, IEEE Transactions on Robotics and Automation 5 (6) (1989). [2] J. Borenstein and Y. Koren, Noise rejection for ultrasonic sensors in mobile robot applications, Proc. 1992 IEEE Int. Conf. on Robotics and Automation, Nice, France (1992). [3] P.J. Besl and R.C. Jain, Three-dimensional object recognition, Autonomous Mobile Robots 1 (1991) 241-311.
130
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
[4] M. Buchberger, K.W. J6rg and E. von Puttkamer, Laserradar and sonar based world modelling and motion control for fast obstacle avoidance of the autonomous mobile robot MOBOT-IV, Proc. IEEE Int. Conf. Robotics and Automation, Atlanta, GA (1993). [5] J.L. Crowley, World modelling and position estimation for a mobile robot using ultrasonic ranging, IEEE Int. Conf. on Robotics and Automation (1989) 674-680. [6] J. Detlefsen, M. Rozmann and M. Lange, Contributions of a microwave radar sensor to a multisensor system used for autonomous vehicles, in: G. Schmidt, P. Levi, R. Dillmann and U. Rembold, eds., Autonomous Mobile Systems (Springer, 1991) 93-102. [7] J. Detlefsen and M. Rozmann, Evaluation of 94 GHz radar sensor images obtained from industrial environments, Proc. 1992 Asia-Pacific Mcirowave Conference, Adelaide, (1992) Vol. 2, 687-690. [8] D. Dickmanns, R. Behringer and V. v. Holt, Road and relative ego-state recognition, Proc. Conf. on Intelligent Vehicles '92, Detroit (1992). [9] E.D. Dickmanns, Active vision through prediction-error minimization, Active Perception and Robot Vtsion, NATO ASI Series F, (Springer Verlag, 1992) Vol. 83, 71-90. [10] A. Elfes, Using occupancy grids for mobile robot perception and navigation, IEEE Computer (1989) 46-57. [11] J. Foley, A. van Dam, S. Feiner and J. Hughes, Computer Graphics - Principles and Practice (Addison-Wesley, Reading, MA 1990). [12] E. Freund, F. Dierks and R. Lichtenecker, Laserscannerbasierte freie navigation autonomer fahrzeuge, 8. Fachgespriich: Autonome Mobile Systeme, U. Rembold, R. Dillmann and P. Levi, eds., Universit~it Karlsruhe, (November 1992) 34-47. [13] C. Fr6hlich, F. Freyberger and G. Schmidt, A three-dimenisional laser range camera for sensing the environment of a mobile robot, Proc. Eurosensors IV: Sensoren Technologie und Anwendung, K.H. H~irdtl, ed. (Karlsruhe 1990) 453-458. [14] C. Fr6hlich, and G. Schmidt, Laser range scanner supporting 3-D range and 2-D grey level images for tunnel surface inspection, ISPRS Symposium, Ottawa (June 1994). [15] Ch. Fr6hlich, F. Freyberger, G. Karl and G. Schmidt, Multisensor system for an autonomous mobile robot, in: G. Schmidt, P. Levi, R. Dillmann and U. Rembold, eds., Autonomous Mobile Systems (Springer, 1991) 61-76. [16] J. Horn and J. Rug, Localization system for a mobile robot based on a 3D-laser-range-camera and an environmental model, Proc. Int. Conf. on Intelligent Vehicles, Paris (1994). [17] J. Horn and G.K. Schmidt, Continuous localization of a mobile robot based on 3D-laser-range-data, predicted sensor images and dead-reckoning, Robotics and Autonomous Systems, Special issue "Research on Autonomous Mobile Systems in Germany", (1995) this issue. [18] P. Kampmann, Ein topologisch strukturiertes Weltmodell als Kern eines Verfahrens zur L6sung von Navigation-
[19]
[20]
[21]
[22]
[23]
[24]
[25] [26]
[27]
[28]
[29]
[30]
saufgaben bei mobilen Robotern, Ph.D. Thesis, 1991, Technical University Munich, Department of Automatic Control Engineering. A. Kosaka and A.C. Kak, Fast vision-guided mobile robot navigation using model-based reasoning and prediction of uncertainties, CVGIP: Image Understanding 56 (3) (1992) 271-329. H. Kriegel and B. Seeger, Multidimensional order preserving linear hashing with partial extensions, Proc. Int. Conf. on Data Base Theory (1986) 203-220. S. Kriegel and B. Seeger, Plop-hashing: A grid file without directory, IEEE Proc. 4th Int. Conf. on Data Engineering (1988) 369-376. R. Lenz, Linsenfehlerkorrigierende Eichung von Halbleiterkameras mit Standardobjektiven fiir hochgenaue 3D-Messungen in Echtzeit, lnformatik Fachberichte (Springer Verlag, 1987) Bd. 149, 212-216. R. Lenz, Digitale Kamera mit CCD-Fl~ichensensor und programmierbarer Aufl6sung bis zu 2994×2320 Bildpunkten pro Farbkanal, Informatik Fachberichte 219, 11, DAGM-Symposium (1989) 411-415. O. Munkelt, P. Levi, B. Radig, M. Rozmann and J. Detlefsen, Integration eines hochaufl6senden Radarsensors in ein videobasiertes Objekterkennungssystem, 9. Fachgespriich: Autonome Mobile Systeme, G. Schmidt, ed., Miinchen (Oct. 1993) 299-311. B.C. Ooi, Efficient Query Processing in Geographic Information Systems (Springer Verlag, 1990). A. RuB, S. Lanser, O. Munkelt and M. Rozmann, Kontinuierliche Lokalisation mit Video- und Radarsensorik unter Nutzung eines geometrisch-topologischen Umgebungsmodells, 9. Fachgespriich: "Autonome Mobile Systeme", G. Schmidt, (ed)., Miinchen (Oct. 1993) 313-327. J. Schweiger, Facilitating teamwork of autonomous systems with a distributed real-time knowledge base, IEEE Proc. Int. Conf. on Robotics and Automation (1994). R.C. Smith and P. Cheeseman, On the representation and estimation of spatial uncertainty, Internat. Journal of Robotics Research 5 (4) (1986) 56-68. R. Stetter, Rechnergestiitzte Simulationswerkzeuge zur Effizienzsteigerung des Industrierobotereinsatzes (Springer Verlag, 1993). A. Welling and J. Milberg, Integrated sensor system for manipulation tasks with a mobile robot, Proc. Int. Conf. on Intelligent Autonomous Systems, Pittsburgh, PA, (1993) 509-518.
Gunter Magin has received a diploma in electrical engineering in 1989 from the Technisehe Universit~it Miinehen. After working on data acquisition instruments, he is now a research assistant at the Technische Universtiit Miinchen and working in the joint research project (SFB331) "Information Processing in Autonomous Mobile Robots". His research interests are sensor-based environment mapping and mobile robot navigation.
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131 Achim RuB was a research assistant at the Technische Universit/it Miinchen in SFB331. Before this he has designed microcomputer components for local area networks. He is currently involved with the development of epifluorescence microscope equipment.
Darius Burschka received the gradu-
ated engineer degree in electrical engineering from the Technische Universit~it Miinchen, Germany, in 1992. He is presently working at the Technische Universit~it Miinchen as a research assistant in the joint research project (SFB331). His research interests include sensor-based exploration of a robot's environment and the line based stereo-vision.
131
Georg Fiirber, 1940, studied electrical engineering at the Technische Universit~it Miinchen. He received the Ph.D. (Dr.-Ing.) in 1967, and founded PCS Computer Systems in 1970. Since 1973 he is professor for process control computers at the Technische Universit~it Miinchen.
Robotics and Autonomous Systems 14 (1995) 119-131
Robotics and Autonomous Systems
A dynamic 3D environmental model with real-time access functions for use in autonomous mobile robots Gunter
Magin, Achim RuB, Darius Burschka,
Georg F~irber *
Lehrstuhl fiir Prozeflrechner, Technische Universitiit Miinchen, 80290 Munich, Germany
Abstract
A model of the environment is a mandatory requirement for the autonomy of a mobile robot. In this paper, we present a framework for the prediction of expected sensor images on feature level, which is based on a-priori knowledge about the geometry of the environment. Our algorithm is capable to serve all distance and image measuring sensors typically found on mobile robots in real time. Fast prediction is achieved by using concepts from database technology and computer graphics. By incorporating differences between the expected and sensed image, the model can be adapted to changes and uncertainties in the environment. So the predicted features can be attributed with uncertainties to support and enhance the match process. Keywords: Sensor-specific environment modelling; Spatial indexing; z-buffer projection; Computer graphics; Real-
time feature prediction
I. I n t r o d u c t i o n
1.1. Motivation
Autonomy of a mobile robot can be defined as the ability to handle irregular situations without external help. To recognize such situations a robot must know what it should regard as irregular and how to distinguish regular from irregular. This knowledge must be on board the robot. In case of a detected difference between reality and assumptions, the preplanned behavior must be adapted. Irregular situations concerning the ex-
* Corresponding author. E-mail: [email protected]; Fax: +49 89 2105-3555.
ternal world can be detected by comparing the perceived sensor images against synthetic images based on an internal representation of the environment and interpreting the differences. An unexpected obstacle in the robot's path is an example for such an irregular situation. A proper autonomous reaction would be to bypass the obstacle, if possible, or to choose another way to the destination. Planning a mission also needs knowledge of the environment, e.g. docking points, usable paths between source and destination, known obstacle positions, etc. The execution of a mission must be supervised for possible collisions, so the robot must verify that the expected free motion space is actually free. For navigation purposes it is essential to know the current position at any time,
0921-8890/95/$09.50 © 1995 Elsevier Science B.V. All rights reserved SSDI 0921-8890(94)00024-7
120
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
either in absolute world coordinates or relative to nearby objects in the scene. To control docking to or from an object in the world, knowledge of the docking partner's geometry and the relative position is necessary. All these requirements can be fulfilled if environment information can be provided based on an on-board environment representation. So an environmental model is a mandatory building-block for the autonomy of a mobile robot. To make even more use of an environmental model, maintenance is also an issue, especially in a dynamic environment. It is most desirable in such environments to have an accurate and upto-date model at hand. Therefore any chance to compare the internal representation with the real world must be used, which means, when a robot passes a scene, it should verify the specific part of the internal representation immediately and parallel to the execution of the mission. In case of permanent differences, possible options are: - stop the mission and check out the difference thoroughly to update the environment representation in a very careful way, the mission, but keep in mind to come back and analyze the scene later, continue the mission and mark the scene as uncertain in the environment representation, - ignore the detected change.
-continue -
The better options require an update of the environment representation.
1.2. Approach Considering these issues about autonomy, our goal was to support model verification and actualization during normal mission execution. This is a challenging real-time problem, as the sensor data rate cannot be reduced, and the robot should be able to cruise with a task-specific speed, not effected by the internal prediction and update processing. Therefore a new approach was chosen, which is fast and robust, capable of handling complex man-made environments by using the standard sensor classes for mobile robots. Our approach is based on prediction of sensory images (Fig. 1). Interpreting an acquired sensor image without knowing what is supposed to be seen is much more difficult than verifying a given hypothesis, for example the assumption that everything in the environment is as expected. So a prediction of synthetic images based on an internal environment representation makes preprocessing, abstraction and interpretation of sensory data faster, more robust and more reliable. An example for the predicted image and the raw data is shown in Fig. 2 and Fig. 3 for a video sensor in a manufacturing environment. Prediction takes place on a feature level of abstraction. This abstraction level is situated between raw sensor data and a map with the 3D-objects in the environment. Features can be easily extracted and abstracted from raw sensor data by fast preprocessing algorithms. Examples are natural landmarks like edges, corners, planes, points
f J
retrieval of local part of environment
~
imagerendering ~
I
::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
environment representation time threshold
filtering postprocessing
iiresults '
Fig. 1. Information flow.
'~
prePen~o7ed
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
/
Fig. 2. Predicted sensor image from estimated viewpoint.
with high reflection, or sensor-specific artificial landmarks. On the other hand, storing and predicting the environment on the feature level is much more efficient than on the level of raw sensory data. So comparing synthetic and real images on a feature abstraction level is an optimal compromise for both sensor and environmental model. Correspondences between the features in both images are established in the match process, where three classes of results are possible: features visible in both images, features only visible in the synthetic image, and features only
Fig. 3. Real sensor image from actual viewpoint.
121
visible in the real image. The match results are then used either to update the internal representation continuously or for localization of the mobile system. As an alternative to model based prediction, prediction can be based on the temporal context by tracking the features' positions in the image space and extrapolating the positions to the next frame. Our approach is characterized by the following facts: we use a pure 3D-approach where environment information is stored in a symbolic way. Nevertheless we are able to deliver synthetic sensor images in real time for a broad range of image and distance sensor systems, like video cameras with various sensor parameters [23], 2Dand 3D-laser-range cameras (e.g. [16,17]) with variable resolution, microwave-radar systems ([6,26]), or laser-scanners [30]. The model information can be delivered in a sensor- and taskspecific form, e.g. for absolute and relative localization purposes, motion space surveillance, object identification, or path planning. Our approach is also applicable to dynamic environments by integrating changes in the world. All inquiries for synthetic images and updates are handled within task-specific real-time limits. 1.3. O t h e r w o r k
In robotics many task-specific environmental models are used: [10,2] use occupancy grids for mapping the environment with ultrasonic sensors and performing navigation tasks. Other models emphasize their specific task, like object recognition [24], path-planning [18], navigation [12] and manipulation [29]. There are also specialized environmental models for specific sensor systems, like ultrasonic [10], video [24], laser range sensors [4,12,13,]. All these models have in common that they are very well adapted for the specific single need, but cannot serve as a general purpose data base for geometric environment information, where a 3D sensor-independent representation is necessary. In [9] the kinematic model of a moving object is used for the prediction of features. As in our approach, real-time preprocessing becomes possi-
122
G. Magin et al. /Robotics and Autonomous Systems 14 (1995) 119-131
ble and image interpretation feasible. However, the approach is limited to simply structured environments, like roads and convex rooms. Furthermore, no effort is made to model the world in a general way, which limits the applicability of this approach. Environmental models for real-time applications, ranging from 20 ms to some seconds are presented in [9] and [19]. Most groups do not emphasize the real-time aspect. Complexity of the modelled environment is usually low to medium [10]. However, environments for production and service scenarios are much more complex. Maintenance of dynamic environments is a very important issue for autonomy. Only some approaches ([1,19,28]) support continuous updating of the model.
2. Modelling of the environment
2.1. Object modelling The environment of the mobile robot is modelled in three dimensions by several layers describing the geometry of the world's objects and sensor-specific features. An object can be modelled in a variety of ways [3]. Here it is considered solid and it is described by a surface boundary representation. The geometry of an object is approximated by convex polygons which are attributed with surface normals and material characteristics (e.g. reflection factor, sensor-specific visibility and transparency). This geometric description of the environment is common to all sensor systems. For each sensor system a separate layer exists with the 3D positions of the features and their spatial and temporal uncertainties. The uncertainties are dynamically modified by the matching results between predicted and real images to support environments, where changes can occur. Modelling of complex manufacturing environments for the geometric layer is done by using a CAD-Modeller, which is part of a simulation system for automated manufacturing. The sensor-specific feature layers are built interac-
tively with a visual feature editor, because automated generation of features from CAD data is generally not feasible in our experience. Reconstructing the environment by a series of sensor images which were taken during a teach-in drive [1] seems to be a good solution to overcome the high effort of model generation by hand. However, the feasibility of this method for complexly structured environments is regarded as an open question.
2.2. Index structures A typical manufacturing environment consists of hundreds of objects whose geometry is described by a total number of more than 50.000 polygons. However, the area overlooked by the sensor systems at a given point of view contains only about 5% of the total number of polygons. Generating the synthetic sensor image can be significantly accelerated if only locally relevant polygons are fed into the following image rendering process (see Fig. 1). As an efficient access to the stored environment information is crucial for the real-time capability, some effort must be spent on the organization of the geometric information. As the robot position is the key index for most retrieval inquiries, the contents of the environmental model is stored in a spatial index structure. Spatial geometric searching is similar to non-spatial multi-key searching although the used index structures must be suitable for non-zero sized spatial objects. Access structures for extended spatial objects are surveyed in [25]. They are classified by position and orientation of the separating hyperplanes and the way of assigning objects to subspaces resulting from subdivision. A new structure is suggested in that paper, the spatial-k-dimensional tree (skD-tree), with k = 2 or 3 for indexing non-zero sized objects. In an s2D-tree the environment is hierarchically divided in rectangular subspaces (Fig. 4), where the objects are attached by their center of gravity. The node structure in the resulting tree contains information about the position of the separating hyperplane and about the extension of objects being cut by the hyperplane into the other
123
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131 memory efficiency
restricted main memory resources require efficient use. Three types of two-dimensional spatial indexing schemes based on dynamic hashing techniques and search trees were implemented and evaluated. They can be characterized how subspaces are defined and how objects covering more than one subspace are handled:
,__n ...................................................
.....................r"~r~-''i ~.tt
.......
=
!
!i
:
I
i
- Equidistant Grid File [10]: The 2D space is
Fig. 4. Example for subdivision by spatial 2D-tree.
uniformly subdivided in quadratic regions. Objects are assigned to all overlapping subspaces. - PLOP-Hashing (Piecewise Linear Order Preserving Hashing) [21,20]: The positions of the hy-
subspace. A subspace is divided either along its longest axis, or with alternating axis. For more details, see [25]. Spatial-k D-trees were designed for geographic information systems. The application in mobile robots is different, because in most cases the query region for the data base access is not a rectangle or a cubeid but a circular or spherical segment, and the data base is main memory resident. So the requirements for indexing schemes used in autonomous mobile robots are:
perplanes adapt to the spatial distribution of the objects. Objects are assigned to that subspace that includes its Centroid. - Spatial-kD-tree [25]: The positions of the hyperplanes of the hierarchical subdivision adapt to the spatial distribution of the objects. Objects are assigned to that subspace that includes its centroid. To evaluate these schemes measurements were carried out with an environmental model of a complexly structured manufacturing environment with about 10.000 polygons. The query region for model access was formed by a circular segment with a radius of 10 m and an angle of 60 ° (see Fig. 5). For dynamically sized query regions, which are
selectivity
local information should be retrieved as specific as possible, real-time access
local information must be retrieved as fast as possible, and
MeanAccessTime [msl
MeanNumberof Elements 2500 ~
Memoryused [kBytel
Equidistant Grid File
,
,
20
2000
/
/
/
/
200
/
lOO _ ~ . ~ _
10
Spatial2D-Tree
_/
PLOPHashing
1500 lO00 100
1000
10000 nos
0 100
1000
10000
o 100
1000
nos
Fig. 5. Selectivity,access time and memoryusage over number of subspaces (nos).
10000
nos
124
G. Magin et al. /Robotics and Autonomous Systems 14 (1995) 119-131
=========================================================================================
i ili ii ii! jiI iii!li i li liiiili!i ""l
-
11111 Fig. 6. Horizontal "span" for fast computation of depth in the z-buffer.
crowave radar systems which cover the environment only in a plane parallel to the ground, the intersection test between query region (spherical segment) and subspace (rectangular parallel-epiped) can be realized as two intersection tests between circular segment and rectangle. As a result access time is only doubled and overall the prediction of the sensor image is accelerated due to enhanced selectivity. An efficient realization of the algorithm that determines the subspaces overlapped by the query region is one main aspect besides tuning parameters for the index mechanism (minimum subspace area, maximum objects per subspace).
3. Prediction of sensor images necessary in a multi-sensor robot, an implementation of the spatial-2D-tree outperformes the other implementations: the s2d-tree achieves best selectivity, which means that almost no elements outside the visibility area of the sensor are delivered by the access functions. S2d lacks in access time, but this is more than compensated in the image rendering stage by the better selectivity. So s2d is considered best choice for complexly structured environments for an overall minimization of the image generation time. Memory usage for all three methods stays within bounds, which makes a memory resident implementation possible. The selectivity of the spatial-k-dimensionaltree becomes even better if k = 3 instead of k = 2, so that the search space is divided into three dimensions. As a drawback the access time increases compared to k = 2 . In case of sensor systems like 2D-laser range scanners or mi-
Table 1 Supported sensor systems Physical sensor Video camera 94 GHz microwaveradar Laser range camera Laser scanner
Generation of sensor-specific synthetic images from the polygons and features in the current sensor cone is done in the following steps: (1) projection into the image plane by coordinate transformation of polygons and features, (2) hidden surface elimination for all polygons, (3) check for features' visibility and partial occlusion (4) determination of regions of interest. These steps are termed the projection pipeline. 3.1. Sensor types a n d sensor m o d e l s
A sensor model is necessary for all the steps in the projection pipeline. It is derived from properties of the physical sensor and from the task-
Virtual sensor Free motion space Localization for natural landmarks Initial and continuous localization Obstacle detection and free motion space supervision High precision surveyingwith artificial landmarks 3D distance measurement Localization High precision relative localization for docking
Feature type Free floor space Edges Line segments Distance to closest obstacle Position of artif, landmarks 3D-depth image Vertical planes Artificial landmarks
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
specific virtual sensor [15]. We support the following models (Table 1): The model of the sensor system specifies how a synthetic sensor-image is generated from the internal representation, and what types of features are required. There is a trade-off between quality of the predicted sensor-images and the computation time needed. Experiments proved that for our application field, modelling the sensors as ideal is sufficient, because the objective is to predict visible features and define regions of interest rather than produce photorealistic images. In case of a video sensor system a simple pinhole camera model with radial distortions is used [22]. Resolution is 512 X 512. Range imaging sensors are also modelled as ideal systems, meaning that multiple reflections and ray expansion is not considered. The 3D-Imaging-MicrowaveRadar [7] measure,; the distance to obstacles at discrete angles with a scaleable angular resolution up to 1°. The range of sight is up to 75 m, the azimuth angle of sight is 360 ° and elevation angle is - 2 5 ° to +25 °. For localization purposes a single 360 ° scan of the sensor is used. The prediction is performed with an internal resolution of 3600 x 1. For generating 3D-depth images a resolution of 1280 x 2,00 is used. The 3D-LaserRange-Camera [13,14] gives a distance profile in the elevation range of - 5 5 ° to + 5 ° and from - 3 0 ° to + 30 ° in azimuth direction with a resolution of 321 × 41 lines at a maximum range 10 m. 3.2. Projection algorithms
The information extracted by the access function describes all obstacles in a local query region
z
=--,
:=:=:=:==
(see Fig. 11). Most of them are not visible to a sensor because of occlusion with other obstacles. The match (see Fig. 1) can be simplified if only visible features are compared with the preprocessed sensor image. Therefore we believe that it is necessary to spend the additional time for the projection of features into image space and for removal of hidden elements. Hidden surface elimination can be done with three different algorithm classes: list oriented (not applicable in our case), object space oriented and image space oriented methods. Methods operating in object space have a high precision: rays through the sensor focus and the border of each polygon are checked in 3D for intersection with all other polygons. Image space algorithms handle occlusions after projection and rasterization. In our application we found computation by object space algorithms 10 times slower compared to image space algorithms. As it was not possible to keep within real-time limits for the prediction of synthetic sensor images with object space algorithms, an image space method based on the z-buffer algorithm was implemented [11]. Inaccuracies resulting from the z-buffer algorithm's discretization are compensated by higher resolution. Every sensor's resolution is limited by hardware or physics. This results in a discretisation of the image coordinates. The z-buffer algorithm operates on these coordinates. It considers for each image point (pixel) of a polygon the distance to that point in 3D, from which the projection started. This distance value is entered at the specific pixel position in the z-buffer, if it is less than the actual value (see Fig. 6). This method has the advantage that every polygon is tested only once and it is not necessary
g
~
I
Planar Projection Plane
125
Equidistant Angular Resolution
Fig. 7. Adaptation of the planar z-buffer algorithm for non-planar projection surfaces.
126
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
Fig. 8. Regions of interest. to test its visibility against the other polygons• The main disadvantage arises from the fact that for each pixel of a projected polygon the distance must be determined• However, the computation effort for each pixel can be reduced significantly considering the polygons are plane: the polygon's surface is divided into horizontal "spans" (see Fig. 6). Each span consists of consecutive horizontal pixels. The spans are limited by edge lines• The distance value of each pixel in a span can be calculated by a cumulative addition of a constant to the distance of the preceding point• This constant depends on the span's length and the distance of the limiting pixels. The cumulative addition is supported by most graphic processors•
The z-buffer algorithm for planar projection planes (e.g. a ideal CCD-camera without radial distortions) can be modified for other projection surfaces in two different ways. The first alternative is the application of a lookup-table to transform the different mapping of the individual pixels in the image (see Fig. 7). This is applicable to any type of projection surface. In the other method a spherical projection surface is considered in the z-buffer algorithm itself• This method takes advantage of the equidistant angular resolution to simplify the computation of the distance• The distance k (see Fig. 9) of a point P in a plane E can be computed by division of the plane's distance from the origin d by the scalar product of the normal vector of the plane n E with the vector of the ray Uray. At the same time the reflection angle of the ray can be estimated by the scalar product n E* Uray already computed for the distance k. The reflection angle is important to estimate the intensity of the echo in the direction of the ray. The most significant echoes can be found at locations where the ray and plane are approximately perpendicular. Sensor data preprocessing and feature extraction is accelerated by using regions of interest defined by the predicted sensor image (Fig. 8). In such a region the corresponding real feature is
cos O * sin ~ y x
];my
0 * cos q ~ ]
~ cos sin •
]
k
ortgm plane E:
n
.......
hE*X- d = 0
nE - normalized normal vector o f plane E d - distance o f the plane from the origin x - point o f the plane E distance k -
for consecutive rays (equidistant angle resolution A~): cos(~ + A ~ ) = cos • * cos A ~ - sin • * sin A ~ sin(~ + A ~ ) = sin • * cos A ~ + cos • * sin A ~ sensor ray: k*vray = x k - distance o f P from the origin V.ray - normalized vector o f the ray x -point o f the ray line
dj
Fig. 9. Estimation of the distance for a non-planar projection plane.
127
G. Magin et aL / Robotics and Autonomous Systems 14 (1995) 119-131
. . . . . . . . . . /. ~ , / yl
/ / / i ~~
Reduced Query Regi~
Feature
Original Query Region ~ ] ~ ~
Mobile ,X0 Robot
iXl' ~- X
Fig. 1O. Dimensioning regions of interest. Fig. 11. Control of query region. very likely to be found. The size of these regions is determined dynamically by the spatial uncertainty of the current position and orientation (x 0, Y0, 3') of the mobile robot, the spatial uncertainty of the position of the feature (xl, yl) and the relative position between robot and feature. Fig. 10 visualizes the basic idea for a vertical line feature assuming uncorrelated gaussian distribution of the spatial uncertainties. The main effect of the regions of interest is that local procedures can be used instead of global procedures 'when matching synthetic and real sensor-image. Therefore feature properties delivered in the synthetic sensor image are used to make establishing correspondences more robust. These properties are either based on a-priori knowledge of material characteristics or on results of previous match processes, or even both.
3.3. Real-time control The typical prediction time interval is 0.25-0.5 seconds. The amount of computation time ttota I with the z-buffer-algorithm is estimated as follows: ttotal = taccess + npolygon " /polygon + n pixel- polygon " tpixel - polygon + nfeatur e " tfeature -I- npixel_feature • tpixel_feature . After a data base query (tac.... ) the number of retrieved polygons and f e a t u r e s (npolygon , nfeature) , together with statistical knowledge about the mean size of the polygons and features (npixel_polygon, npixel_polygon) give a rough figure of
ttota I. If the expected computation time is above the real-time limit, the data base query is repeated with a reduced query region (Fig. 11). There is now a good chance to meet the realtime requirements, but there is no guarantee in critical moments. Therefore a new prediction of ttota I is done after modelling transformation, clipping and rasterization. The error of the estimated ttota I is now less than a few percent. The resolution of the predicted sensor image is then reduced according to the linear relation between the resolution (number of pixels) and /total" Fig. 12 shows the combined effect of the two methods to meet given real-time requirements.
3. 4. History-based prediction It seems to be unnatural to spend computation time to go through the procedure of model access, projection, etc. for every single sensor frame,
Distribution Function 1,0 0,8 -
Controlled f It
Predicti
0,6-
I, ~ ~
/
UncontTlded Predi~
0,40,2-
J
i Real-time Linfit
0 0
0~
1,0
1,5
2,0 Time
Maximum Time Fig. 12. Real-time guarantee.
128
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
feature. To find the best match out of those, properties of the features are considered again. Once the correspondence is established, feature history is updated. In case no correspondence for a feature could be established, the old history is extrapolated. After 3 successive frames without correspondence, this feature is considered lost. With this, short-term disturbances can be eliminated. After a couple of frames, features disappear over the image borders. In case the number of tracked features becomes too small, a new model inquiry or another full frame processing phase is required.
Pt-
Qt-3 •
Qt_I-~Q t
Fig. 13. Feature movement.
as most of the predicted feature positions differ only slightly from the results of the previous frame. This is because the features do not move far between successive frames. We track the features to record a history of movement, and then predict where each individual feature will show up in the next frame. Feature history consists of its current position, speed and acceleration in the image plane. The initial position is determined by a model inquiry. Establishing correspondences between features in different frames is a crucial task in the tracking process. Similar to the model based approach a region of interest is established around the predicted position in the sensor image, in which feature extraction takes place. The size of this window must be large enough to ensure capturing of a feature even when movement starts to change (beginning of a curve after a long straight movement). On the other hand, it must be as small as possible to reduce the number of correspondence candidates and preprocessing load. Only the features found in this window are checked for a correspondence with the predicted
4. Acquisition of the environment representation In our approach, the environmental model reads a description of the initial contents. This description can be obtained in three different ways: In case of a manufacturing environment, CAD-data of the environment exists, which can be used after transformation into our description. In a second method, portions of the CAD-data can be used to generate a description of elements like load stands, tool machines, robots, etc. Other elements can be created from scaled geometric primitives, e.g. cubes, pyramids, etc. A macro language combines these elements with their global positions in the "world" resulting in our description for the environment. A third method uses a very rough description with geometric primitives as a-priori knowledge. In an exploration phase this model is enhanced by incorporating sensory perceptions.
5. Update of the environment representation Pi-1 Pi-2 Vi-
Pi_4vJ
vi-1 .
Pi vi 6 .
° ° ° ° Vi_l
@2 Fig. 14. Feature's speed & acceleration.
The concept of detecting changes in the environment is based on the evaluation of differences between predicted and detected features. Minor changes in the environment can be caused by varying illumination for instance. This type of change can be incorporated by modifying attributes for spatial and temporal uncertainty of the environmental model's elements, which also
129
G. Magin et aL /Robotics and Autonomous Systems 14 (1995) 119-131
512 x 512, f = 9 m m
360 ° - Scan
,/
1.0 0.8 0.6 0.4
/
1.0 0.8
0.6 0.4 0.2
0.2
0
0 0
0.1
0.2
0.3
0
[s]
Overall prediction times for a microwave radar (distribution function)
0.1
0.2
0.3
0.4
[s]
Overall prediction times for a C C D -
camera (distribution function)
Fig. 15. Access times in an environment with ca. 7000 polygons on a DEC-Alpha 3000/600 computer.
serves as a filter for transient disturbances of the sensor systems. Detected minor changes can result in major changes in the environmental model, such as insertion and deletion of objects and features. This process is limited by use of short term memories, so only robust and reliable information is entered into long term memory and used for the prediction process. New objects can be detected by an obstacle detection sensor. After identifying the obstacle with an object recognition algorithm [24] the apriori model of the recognized object can be included into the environmental model. Pose and position have been determined during object recognition. Similar functions for deletion and position modificatiion of objects in the environment representation are implemented.
6. Results and future work The quality and accuracy of our environmental model, contents and algorithms, have been proofed to be sufficient in many experiments [26]. The overall time for predicting sensor images is always under the time for sensor data preprocessing. Quantitative results for continuous localization of a moving platform equipped with a 3D laser range camera, a vision system, and a high resolution 3D microwave radar system in a manufacturing environment and other in-door environ-
ments prove the real-time capability and show the effect of the accuracy of the sensor models used in respect to execution time and accuracy of the localization process (see Fig. 15). Future work will emphasize on sensor based environment mapping and model improvement, as the survey of environments performed manually is tedious and error prone. We will also extend the applicability of the environmental model for applications in the manipulation domain and for planning purposes.
Acknowledgement The work present in this paper was supported by the Deutsche Forschungsgemeinschaft as part of an interdisciplinary research project on "Information Processing in Autonomous Mobile Robots" (SFB 331).
References [1] N. Ayache and O.D. Faugeras, Maintaining representations of the environment of a mobile robot, IEEE Transactions on Robotics and Automation 5 (6) (1989). [2] J. Borenstein and Y. Koren, Noise rejection for ultrasonic sensors in mobile robot applications, Proc. 1992 IEEE Int. Conf. on Robotics and Automation, Nice, France (1992). [3] P.J. Besl and R.C. Jain, Three-dimensional object recognition, Autonomous Mobile Robots 1 (1991) 241-311.
130
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131
[4] M. Buchberger, K.W. J6rg and E. von Puttkamer, Laserradar and sonar based world modelling and motion control for fast obstacle avoidance of the autonomous mobile robot MOBOT-IV, Proc. IEEE Int. Conf. Robotics and Automation, Atlanta, GA (1993). [5] J.L. Crowley, World modelling and position estimation for a mobile robot using ultrasonic ranging, IEEE Int. Conf. on Robotics and Automation (1989) 674-680. [6] J. Detlefsen, M. Rozmann and M. Lange, Contributions of a microwave radar sensor to a multisensor system used for autonomous vehicles, in: G. Schmidt, P. Levi, R. Dillmann and U. Rembold, eds., Autonomous Mobile Systems (Springer, 1991) 93-102. [7] J. Detlefsen and M. Rozmann, Evaluation of 94 GHz radar sensor images obtained from industrial environments, Proc. 1992 Asia-Pacific Mcirowave Conference, Adelaide, (1992) Vol. 2, 687-690. [8] D. Dickmanns, R. Behringer and V. v. Holt, Road and relative ego-state recognition, Proc. Conf. on Intelligent Vehicles '92, Detroit (1992). [9] E.D. Dickmanns, Active vision through prediction-error minimization, Active Perception and Robot Vtsion, NATO ASI Series F, (Springer Verlag, 1992) Vol. 83, 71-90. [10] A. Elfes, Using occupancy grids for mobile robot perception and navigation, IEEE Computer (1989) 46-57. [11] J. Foley, A. van Dam, S. Feiner and J. Hughes, Computer Graphics - Principles and Practice (Addison-Wesley, Reading, MA 1990). [12] E. Freund, F. Dierks and R. Lichtenecker, Laserscannerbasierte freie navigation autonomer fahrzeuge, 8. Fachgespriich: Autonome Mobile Systeme, U. Rembold, R. Dillmann and P. Levi, eds., Universit~it Karlsruhe, (November 1992) 34-47. [13] C. Fr6hlich, F. Freyberger and G. Schmidt, A three-dimenisional laser range camera for sensing the environment of a mobile robot, Proc. Eurosensors IV: Sensoren Technologie und Anwendung, K.H. H~irdtl, ed. (Karlsruhe 1990) 453-458. [14] C. Fr6hlich, and G. Schmidt, Laser range scanner supporting 3-D range and 2-D grey level images for tunnel surface inspection, ISPRS Symposium, Ottawa (June 1994). [15] Ch. Fr6hlich, F. Freyberger, G. Karl and G. Schmidt, Multisensor system for an autonomous mobile robot, in: G. Schmidt, P. Levi, R. Dillmann and U. Rembold, eds., Autonomous Mobile Systems (Springer, 1991) 61-76. [16] J. Horn and J. Rug, Localization system for a mobile robot based on a 3D-laser-range-camera and an environmental model, Proc. Int. Conf. on Intelligent Vehicles, Paris (1994). [17] J. Horn and G.K. Schmidt, Continuous localization of a mobile robot based on 3D-laser-range-data, predicted sensor images and dead-reckoning, Robotics and Autonomous Systems, Special issue "Research on Autonomous Mobile Systems in Germany", (1995) this issue. [18] P. Kampmann, Ein topologisch strukturiertes Weltmodell als Kern eines Verfahrens zur L6sung von Navigation-
[19]
[20]
[21]
[22]
[23]
[24]
[25] [26]
[27]
[28]
[29]
[30]
saufgaben bei mobilen Robotern, Ph.D. Thesis, 1991, Technical University Munich, Department of Automatic Control Engineering. A. Kosaka and A.C. Kak, Fast vision-guided mobile robot navigation using model-based reasoning and prediction of uncertainties, CVGIP: Image Understanding 56 (3) (1992) 271-329. H. Kriegel and B. Seeger, Multidimensional order preserving linear hashing with partial extensions, Proc. Int. Conf. on Data Base Theory (1986) 203-220. S. Kriegel and B. Seeger, Plop-hashing: A grid file without directory, IEEE Proc. 4th Int. Conf. on Data Engineering (1988) 369-376. R. Lenz, Linsenfehlerkorrigierende Eichung von Halbleiterkameras mit Standardobjektiven fiir hochgenaue 3D-Messungen in Echtzeit, lnformatik Fachberichte (Springer Verlag, 1987) Bd. 149, 212-216. R. Lenz, Digitale Kamera mit CCD-Fl~ichensensor und programmierbarer Aufl6sung bis zu 2994×2320 Bildpunkten pro Farbkanal, Informatik Fachberichte 219, 11, DAGM-Symposium (1989) 411-415. O. Munkelt, P. Levi, B. Radig, M. Rozmann and J. Detlefsen, Integration eines hochaufl6senden Radarsensors in ein videobasiertes Objekterkennungssystem, 9. Fachgespriich: Autonome Mobile Systeme, G. Schmidt, ed., Miinchen (Oct. 1993) 299-311. B.C. Ooi, Efficient Query Processing in Geographic Information Systems (Springer Verlag, 1990). A. RuB, S. Lanser, O. Munkelt and M. Rozmann, Kontinuierliche Lokalisation mit Video- und Radarsensorik unter Nutzung eines geometrisch-topologischen Umgebungsmodells, 9. Fachgespriich: "Autonome Mobile Systeme", G. Schmidt, (ed)., Miinchen (Oct. 1993) 313-327. J. Schweiger, Facilitating teamwork of autonomous systems with a distributed real-time knowledge base, IEEE Proc. Int. Conf. on Robotics and Automation (1994). R.C. Smith and P. Cheeseman, On the representation and estimation of spatial uncertainty, Internat. Journal of Robotics Research 5 (4) (1986) 56-68. R. Stetter, Rechnergestiitzte Simulationswerkzeuge zur Effizienzsteigerung des Industrierobotereinsatzes (Springer Verlag, 1993). A. Welling and J. Milberg, Integrated sensor system for manipulation tasks with a mobile robot, Proc. Int. Conf. on Intelligent Autonomous Systems, Pittsburgh, PA, (1993) 509-518.
Gunter Magin has received a diploma in electrical engineering in 1989 from the Technisehe Universit~it Miinehen. After working on data acquisition instruments, he is now a research assistant at the Technische Universtiit Miinchen and working in the joint research project (SFB331) "Information Processing in Autonomous Mobile Robots". His research interests are sensor-based environment mapping and mobile robot navigation.
G. Magin et al. / Robotics and Autonomous Systems 14 (1995) 119-131 Achim RuB was a research assistant at the Technische Universit/it Miinchen in SFB331. Before this he has designed microcomputer components for local area networks. He is currently involved with the development of epifluorescence microscope equipment.
Darius Burschka received the gradu-
ated engineer degree in electrical engineering from the Technische Universit~it Miinchen, Germany, in 1992. He is presently working at the Technische Universit~it Miinchen as a research assistant in the joint research project (SFB331). His research interests include sensor-based exploration of a robot's environment and the line based stereo-vision.
131
Georg Fiirber, 1940, studied electrical engineering at the Technische Universit~it Miinchen. He received the Ph.D. (Dr.-Ing.) in 1967, and founded PCS Computer Systems in 1970. Since 1973 he is professor for process control computers at the Technische Universit~it Miinchen.