The navigability variable is binary either a cell is navigable or not. Thus, we can invert the entire reasoning by substituting x i for x i : (4)

Transcription

1 A Multi-Resolution Pyramid for Outdoor Robot Terrain Perception Micael Montemerlo and Sebastian Trun AI Lab, Stanford University 353 Serra Mall Stanford, CA Abstract Tis paper addresses te problem of outdoor terrain modeling for te purposes of mobile robot navigation. We propose an approac in wic a robot acquires a set of terrain models at differing resolutions. Our approac addresses one of te major sortcomings of Bayesian reasoning wen applied to terrain modeling, namely artifacts tat arise from te limited spatial resolution of robot perception. Limited spatial resolution causes small obstacles to be detectable only at close range. Hence, a Bayes filter estimating te state of terrain segments must consider te ranges at wic tat terrain is observed. We develop a multi-resolution approac tat maintains multiple navigation maps, and derive rational arguments for te number of layers and teir resolutions. We sow tat our approac yields significantly better results in a practical robot system, capable of acquiring detailed 3-D maps in large-scale outdoor environments. Introduction Tis paper addresses te problem of robots navigating troug unknown terrain. Tis problem arises in great many robot applications, suc as planetary exploration (Matties et al. 1995) and te exploration of abandoned mines (Ferguson et al. 003). In suc cases, a robot must rely on its sensors to determine te location of obstacles, so as to safely navigate troug free space. In indoor environments, tis is commonly acieved by assuming te world is planar and estimating a -D cross-section of te environment (Borenstein & Koren. 1991; Burgard et al. 000; Simmons et al. 000; Yamauci et al. 1998). Outdoor environments require a 3-D analysis of te ground and possible obstacles tat migt protrude into navigable space (Hasimoto & Yuta 003; Matties & Grandjean 199). Of particular importance are negative obstacles, caracterized by te absence of supporting ground (e.g., oles) (Matties et al. 1998) as well as obstacles tat migt protrude into te robot s workspace from far above te ground (e.g., overangs) (Ferguson et al. 003). Wen modeling te navigability of terrain, one common approac is to extract a navigability assessment from sensor measurements (e.g., range scans), and to integrate tese assessment over time using Bayes filters. To perform tis integration, existing tecniques partition te workspace into a grid, similar to te well-known occupancy grid map algoritm (Moravec 1988). For eac grid cell, sensor measurements are integrated using Bayes rule to diminis te effect of sensor noise. Figure 1: Te terrain perception pyramid: terrain is modeled wit multiple maps at differing resolutions, eac sensitive to observations at different ranges. Real-world terrain sensors ave limited measurement resolution. For example, SICK laser range finders can only measure ranges wit 0.5 accuracy; similar limitations exist for stereo camera systems and sonar sensors. Limited resolution can cause two problems wit standard evidence grid algoritms: 1. First, te limited resolution may make it impossible to detect small obstacles at a distance. Obstacles like curbs or low-anging wires can usually only be detected at close range. Tis is at odds wit te ideal of Bayesian evidence integration in terrain grids: If evidence gatered at a distance systematically misses an obstacle, te grid may not cange quickly enoug wen te robot is finally near te grid cell. As a result, te terrain model may miss obstacles, compromising te safety of te veicle.. Second, limited sensor resolution makes it difficult to systematically find navigable terrain at a distance. As a result, a motion planner is eiter forced to make optimistic assumptions about te nature of terrain at a distance (wit te obvious expense of aving to replan wen negative obstacles are encountered), or must remain confined to nearby regions tat ave been completely imaged. Te first limitation is te critical one tat motivates our researc, altoug our approac solves bot of tese problems. Te key insigt in solving te first problem is to note tat Increasing map range Increasing map resolution

2 te range at wic terrain is analyzed plays a systematic role in te robot s ability to detect obstacles. To make tis dependence explicit, we propose to maintain multiple terrain maps, eac sensitive to different, possibly overlapping sensor ranges. Tis is illustrated in Figure 1: Terrain observations are incorporated into te maps sensitive to te range at wic te observation was acquired. Eac map, tus, models te probability of obstacles detectable at a particular range. Wen combining maps for assessing te navigability of terrain, preference is given to sorter range maps; owever, all maps participate in motion planning. Te use of multiple maps raises te issue of teir resolution. As we will sow in tis paper, a variable resolution approac yields superior coverage to a fixed resolution approac. To derive an appropriate resolution, we will analyze te effect of measurement noise and limited resolution on te density of measurements, so tat te maximum resolution is cosen at eac level tat guarantees tat all grid cells are covered wit ig likeliood. Te result of tis analysis is a pyramid of terrain maps: Near te robot we ave a fine-grained map sufficient for maneuvering te robot in close proximity to obstacles, wereas te map far from te robot is coarse, facilitating te task of motion planning. In extensive experiments, we sow tat te resulting approac is superior to existing terrain modeling approaces, in tat it enances te robot s safety wile facilitating motion planning. Bayesian Tecniques for Navigability In tis section we present our core Bayesian tecnique for constructing navigability maps of unknown terrain. Te approac requires tat te robot be equipped wit a 3-D range finder. For a fixed resolution grid, tis approac is identical to a previously publised algoritm in (Ferguson et al. 003). It forms te basis of te pyramid described in subsequent sections. Suppose te robot acquires a 3-D range scan of its surrounding terrain. Eac measurement is mapped into a (x y z)-coordinate using te obvious coordinate transformation. Weter or not a location (x y) is navigable is determined by a simple geometric analysis; A location is navigable at eigt z if te z-values of all measurements in te vicinity of (x y) are eiter near eac oter (e.g., less tan 5cm deviation), or are above te ground plane at a distance tat exceeds te eigt of te robot. Eiter criterion is easily verified by a simple geometric analysis, as sown in (Ferguson et al. 003). Te analysis of navigability results in a probability distribution p(x i z t ) for eac nearby location x i, conditioned on te range scan z t (ere t denotes te time). Multiple measurements covering te same grid cell are ten integrated using Bayes rule: p(x i z 1, z,..., z T ) = p(z T x i, z 1,..., z T 1 ) p(x i z 1,..., z T 1 ) (1) p(z T z 1,..., z T 1 ) Te standard derivation of te algoritm assumes tat x i is a sufficient statistic of te past, ence we get = p(z T x i ) p(x i z 1,..., z T 1 ) () p(z T z 1,..., z T 1 ) and after anoter application of Bayes rule = p(x i z T ) p(z T ) p(x i z 1,..., z T 1 ) (3) p(z T z 1,..., z T 1 ) Te navigability variable is binary eiter a cell is navigable or not. Tus, we can invert te entire reasoning by substituting x i for x i : p( x i z 1, z,..., z T ) = p( x i z T ) p(z T ) p( x i z 1,..., z T 1 ) p( x i ) p(z T z 1,..., z T 1 ) If we now divide (3) by (4) and substitute p( x i ) = 1 p(x i ) for arbitrary conditioning variables, we essentially obtain te standard grid mapping algoritm in (Moravec 1988), but ere for navigability: p(x i z 1, z,..., z T ) 1 p(x i z 1, z,..., z T ) = p(x i z T ) 1 p(x i z T ) 1 Or in log-form, wit te recursion added out: (4) (5) p( x i z 1,..., z T 1 ) 1 p( x i z 1,..., z T 1 ) p(x i z 1, z,..., z T ) log = log 1 p(x i z 1, z,..., z T ) 1 + [ p(x i z t ) log 1 p(x t i z t ) + log 1 p(x ] i) Tis update equation wic simply adds or subtracts evidence for te navigability from eac grid cell is at te core of classical tecniques wit a single navigability grid (Ferguson et al. 003; Hasimoto & Yuta 003), as well as our new approac tat maintains a pyramid of grids at different resolutions. Te key problem of tis update lies in its implicit independence assumption. Consider a grid cell x i tat contains a nonnavigable curb tat can only be detected at close range. Measured from a distance, p(x i z t ) will be smaller tan te prior, ence p(x i z t ) 1 p(x i z t ) + log 1 p(x i) will be negative. Let tis value be called α. At close range, te robot can detect te obstacles; ence tis expression turns into a positive value. Let us call tis positive value β. Te posterior will only indicate occupancy if te number of close range readings is at least n α β, were n is te number of long-range readings. Oterwise, te obstacle may be missed. Clearly, for any value of α and β, te approac can fail wen te number of long-range readings is too large. Multi-Resolution Approaces Te central idea of our paper is to develop a pyramid of terrain models, in wic eac terrain model captures te occupancy from measurement data acquired at different ranges. Tese maps are defined troug a sequence of distances (6) (7) 0 = d 0 < d 1 < d <... < d N = max dist (8) tat partition te space of all distances at wic te robot can perceive, up to its maximum range. Eac interval [d n ; d n+1 ) defines a map, tat is, at any point in time, our approac only updates a grid cell in te n-t map if te actual distance of tis cell to te sensor (at te time of measurement) falls into

3 te interval [d n ; d n+1 ). In tis way, eac range scan leads to updates at multiple maps. Te definition of te values {d n } requires some analysis, in tat te issue of te grid resolution and te distance band to wic eac grid is tuned are closely intertwined. Our analysis will be carried out in two parts. First, we will derive a bound on te maximum resolution of eac grid cell, wic guarantees wit ig likeliood tat all grid cells are covered. Tis bound sets te resolution levels at different ranges d n ; as to be expected, te resolution decreases wit distance. Te actual ranges {d n } tat define te individual maps are ten determined by an argument tat ensures uniform spread of updates at all levels of te pyramid, wic implies tat all of te occupancy maps are similar in teir resulting confidence. Upper Bound for te Grid Resolution In tis section, we will derive an upper bound on te resolution of eac grid cell as a function of te distance to te robot, d. Tis bound is derived as a lower bound on te grid resolution. Te bound is driven by tree considerations: te effects of measurement noise, te vertical resolution, and te orizontal resolution of te range scanning device. Its rationale is tat for exploration, we would like to guarantee coverage of eac grid cell wit ig likeliood. Part of our analysis requires a flat ground. Tis is because te curvature in front of te robot is generally unknown a priori. Te bound assumes a range sensor wit vertical angular resolution of φ (e.g., 1 degree) and orizontal angular resolution ψ, wic is mounted at a fixed eigt on te robot. Let r α be te correct measurement of a range finder pointed at te vertical angle α, were α = 0 denotes te orizontal direction. On a flat surface, te exact range r α is obtained by te following equation r α = (9) sin α Sensor measurements are, of course, noisy. Te actual sensor measurement is a probabilistic function of r α. Let tis distribution be denoted by p(ˆr α r α ), were ˆr α denotes te actual measurement. For our analysis, let us assume tat te variance of tis distribution is given by σr. Let d be te orizontal distance of a grid cell to te robot. Te measurement noise causes noise in te distance at wic ground is detected. Te variance of tis distance d due to measurement noise is given by σ d = σ 1 + ( d ) (10) were σ is te variance of p(ˆr α r α ). To see tis, we note tat te measurement angle at distance d is (1) α = arctan d (11) Tus, range noise is projected onto surface noise in proportion to te cosine of tis angle: cos α = ( cos arctan ) d = ( d ) Figure : Lower bound on te grid cell size, for a range scanner wit φ = 0.5 vertical resolution, ψ = orizontal resolution, and measurement noise σ = 15cm. Since te variance is a quadratic function, te resulting variance on te ground is as stated in (10). For d, we ave σ σd, suggesting a lower bound for te grid cell size tat is asymptotically independent on te distance d. Tis is quite different for te effect of limited angular resolution. Let φ be te vertical angular resolution of te sensor. Equation (11) specified te angle at wic a point on te ground at distance d is detected. Tis angle is a function of d. Its dependence on d is caracterized by te derivative α d = d + (13) Tus, for any (small) angular resolution φ, we obtain d φ (d + ) (14) were d is te minimum vertical ground difference detectable by a sensor wit resolution φ. Tis value is of course only an approximation, since te function α is approximated using a linear function (first order Taylor expansion). However, for small φ, tis value gives us an accurate measure of te distance between ground detections due to sensor limitations. Tis value increases quadratically wit te distance d. Te orizontal resolution of te scanner ψ gives us te relation and ence d d = arctan ψ d = d arctan ψ (15) (16) wic grows linearly in d. Since resolution and noise effects are all worst-case additive, an appropriate lower bound for te grid cell size is δ(d) = σ + d arctan ψ 1 + ( + φ (d + ) d ) (17) wic consists of a constant, a linear, and a quadratic term. Figure illustrates te lower bound on te grid cell size, for a vertical resolution φ = 0.5, a orizontal resolution ψ =,

4 and a measurement noise variance σ = 15cm. Te top curve is our additive lower bound, composed of te noise bound (dased curve) tat asymptotes into a constant, te linear orizontal resolution bound (tin line), and te resolution bound (gray curve) tat grows quadratically. Witin a range of up to five meters, te total bound is surprisingly close to a linear function. Beyond tis range, it becomes quadratic. As noted above, our analysis is based on te assumption of a flat ground. Tis assumption is adopted to reflect tat te ground aead of te robot is unknown. It is pessimistic in te face of an upward slope, in wic case te density of measurements is larger tan expected (ence te bound could be reduced). It is optimistic on downward-sloped ground, in wic te resulting grid cells migt not always be covered by at least one sensor measurement. In suc situations, te robot will ave to reduce its exploration speed so as to compensate te effects of reduced visibility in suc terrain. Te Pyramid We ave just devised a lower bound on te grid cell size depending on te range d at wic measurements are integrated. In tis section, we will derive a rational argument for number and sape of te layers in our pyramid. In determining te number of layers in te pyramid, we trade off te size of an obstacle tat can be detected by te robot and te resolution and range at wic it can be detected. For a sensor wit vertical resolution π, te ability to determine an obstacle of eigt z depends on te distance d. Tis eigt is easily determined using te obvious geometric equation: z = arctan φ (18) d Tis implies tat te eigt of detectable obstacles depends linearly on te distance d, for a sensor wit fixed resolution: z = d arctan φ (19) Tis linear dependence suggests a linear decrease of grid resolution in te pyramid tis is at stark contract of an exponential structure of most pyramids in te field of computer vision. In particular, we propose an obstacle grid pyramid of resolutions d, γ d, γ d, 3γ d,..., for a linear scaling parameter γ. Wit suc a linear increase in resolution, te corresponding range for eac layer is determined by te bound in (0): d i = min δ(d) < i γ d (0) d Wile tis bound is generally quadratic, witin a measurement range of 5 meters te function is approximately linear. Tus, wit appropriate coice of te pyramid resolution γ, we obtain a pyramid tat scales linearly in resolution, and were eac layer focuses on an approximately linear distance of cells near te robot. In our experiments, we found tat a soft boundary works better tan a ard boundary on te association of ranges d to a layer in te pyramid. In particular, wen receiving range measurements tat provide information on navigability at te distance d, te information integrated into te grid defined for te range d i is gated by a factor of { exp 1 (d d i ) } [ p(x i z t ) σ log 1 p(x i z t ) + log 1 p(x ] i) Figure 3: Te robotic veicle is based on a Segway RMP, equipped wit a vertically oriented SICK laser range finder mounted on an Amtec pan/tilt unit. Te robot uses a igly tuned Inertial Measurement Unit and a GPS for localization. Tis exponential decay leads to a smoot implementation of our distance-based multi-resolution grid approac. Experimental Setup We recently developed a mobile robot system for 3-D mapping and navigation of outdoor environments. Our navigation algoritm is described elsewere (Likacev, Gordon, & Trun 003): In essence, tis approac finds a pat troug te map tat maximizes te progress to a target location wile minimizing te risks of encountering non-navigable space. Our robot, sown in Figure 3, is based on te Segway RMP mobile platform. Te RMP is a computer-controlled version of te commercial Segway HT scooter. Te robot is equipped wit a SICK laser range finder mounted on an Amtec pan-tilt unit. Te laser is swept back and fort in order to acquire 3-D scans of te robot s environment. Te robot also incorporates a sopisticated Inertial Measurement Unit (IMU) and GPS for localization. By projecting te endpoints of te laser scans into 3-D according to te estimated position of te robot and te angle of te pan-tilt, te robot can construct clouds of 3-D points describing te world around te robot. By integrating constraints from te IMU, GPS, and matces between laser scans, we are able to construct large-scale, globally consistent, 3-D maps of urban environments. A map of te center of Stanford campus over 600 meters wide is sown in Figure 4. Wile suc maps are necessary for planning global routes from one location to anoter, te robot must also monitor te immediate surroundings of te robot to ensure safe motion. Te navigation pyramid algoritm described in tis paper was implemented as te local navigation layer on te Segway. Experiments were performed to validate te two advantages of te pyramid algoritm: robustness to limited spatial resolution, and improved sensor coverage. Te first experiment is sown in Figures 5 to 7. Figure 5 sows te robot standing at a distance from a set of stairs. At tis distance te robot is not able to detect te stairs as an obstacle. Bot te standard evidence grid algoritm and te pyramid algoritm generate terrain maps like te one sown in Figure 5. (All levels of te pyramid were cosen to ave a fixed resolution for te purposes of comparison wit te stan-

5 Figure 5: Watcing te stairs from a distance, bot algoritms create a similar terrain map wit te stairs marked as free space. Figure 4: 3-D map of te center of te Stanford campus constructed by te Segway. Te map is over 600 meters across, and was constructed from over 10 km of travel. dard algoritm.) After standing still for several minutes, bot algoritms describe te stairs area as being empty wit ig probability. Subsequently, te robot was driven up to te stairs. At closer ranges, te stairs are detected as obstacles, but te standard evidence grid is unable to overcome te previous evidence describing te cells as free space. Te resulting evidence grid, sown in Figure 6 does not contain te steps and would ave resulted in a collision of te robot. Te pyramid algoritm, on te oter and (Figure 7, was able to detect te stairs. As te robot moved closer to te stairs, te obstacles were incorporated into te sorter range layers of te terrain map. Te second experiment, pictured in Figures 8 and 9, sows an example of te effect of multi-resolution pyramids on mapping performance. Figure 8 sows a fixed resolution terrain map generated wile te robot was approacing a gate. Te localization of obstacles in te map is quite good, but te sweeping pattern of te laser leaves large oles in te terrain map. Te abundance of oles makes it difficult to plan smoot local pats troug tis environment. Figure 9 sows te same scenario, except using a multi-resolution pyramid. Obstacles near te robot are precisely mapped, wile obstacles in te distance are stored at low resolution. Furtermore, most of te oles in te terrain map ave been filled. Conclusion Tis paper proposed a pyramid approac to acquiring terrain models wit mobile robots. Te model was motivated by a key flaw in flat grid approaces to modeling te navigability of terrain; namely, tat te ability of a sensor to detect obstacles varies wit range. By integrating information into a single flat map, failures to detect obstacles at a distance are treated as random noise, and not wat tey actually are: te systematic effect of limited sensor resolution. Our approac alleviates tis problem by devising a ierarcy of maps, eac tuned to a different sensor range (and ence a different sensor resolution). We derive a matematical bound tat provides a rational argument for determining te region covered by eac map, and te granularity of te grid cells in eac map. As a result, te map resolution decreases wit te Figure 6: After te robot approaces te stairs, te standard occupancy grid algoritm is unable to overcome te previous negative evidence and te curb is not detected. Figure 7: Te pyramid approac is able to detect te stairs in time to safely avoid tem. measurement range, wic as te nice side effect tat te resulting maps tend to ave muc iger coverage tan common single-map approaces. We ave demonstrated our approac using an actual outdoor mobile robot system. Our system is based on a robotic Segway scooter, and as successfully acquired large-scale models of areas 1km in size. Our experiments demonstrate tat our approac successfully identifies obstacles in situations were te

6 Figure 8: Model of te robot s surroundings using a fixed resolution grid. Te large number of oles in te map makes motion planning difficult. flat approac fails, and tat it indeed leads to improved coverage in te resulting map. Computationally, our approac is only marginally more expensive tan te single map approac, wic sould make our approac te metod of coice for outdoor mobile robot navigation. Acknowledgements Tis researc is sponsored by by DARPA s MARS Program (Contract number N C-6018), wic is gratefully acknowledged. References Borenstein, J., and Koren., Y Te vector field istogram fast obstacle avoidance for mobile robots. IEEE Journal of Robotics and Automation 7(3): Burgard, W.; Fox, D.; Moors, M.; Simmons, R.; and Trun, S Collaborative multi-robot exploration. In ICRA. San Francisco, CA: IEEE. Ferguson, D.; Morris, A.; Hänel, D.; Baker, C.; Omoundro, Z.; Reverte, C.; Tayer, S.; Wittaker, W.; Wittaker, W.; Burgard, W.; and Trun, S An autonomous robotic system for mapping abandoned mines. In Trun, S.; Saul, L.; and Scölkopf, B., eds., NIPS. MIT Press. Hasimoto, K., and Yuta, S Autonomous detection of untraversability of te pat on roug terrain for te remote controlled mobile robots. In Proceedings of te International Conference on Field and Service Robotics. Likacev, M.; Gordon, G.; and Trun, S ARA*: Anytime A* searc wit provable bounds on sub-optimality. In Trun, S.; Saul, L.; and Scölkopf, B., eds., NIPS. MIT Press. Matties, L., and Grandjean, P Stereo vision for planetary rovers: Stocastic modeling to near real-time implementation. International Journal of Computer Vision 8(1): Figure 9: Multi-resolution pyramid model of te robot s surroundings. Te majority of oles in te map are filled in. Te terrain map close to te robot is very ig resolution, wile te area far from te robot is very coarse. Matties, L.; Gat, E.; Harrison, R.; Wilcox, B.; Volpe, R.; and Litwin, T Mars microrover navigation: Performance evaluation and enancement. Autonomous Robots (4): Matties, L.; Litwin, T.; Owens, K.; Rankin, A.; Murpy, K.; Coorobs, D.; Gilsinn, J.; Hong, T.; Legowik, S.; Nasman, M.; and Yosimi, B Performance evaluation of ugv obstacle detection wit ccd/flir stereo vision and ladar. In Proceedings of te Joint Conference on te Science and Tecnology of Intelligent Systems. Moravec, H. P Sensor fusion in certainty grids for mobile robots. AI Magazine 9(): Simmons, R.; Apfelbaum, D.; Burgard, W.; Fox, D. an Moors, M.; Trun, S.; and Younes, H Coordination for multi-robot exploration and mapping. In Proceedings of te AAAI National Conference on Artificial Intelligence. Austin, TX: AAAI. Yamauci, B.; Langley, P.; Scultz, A.; Grefenstette, J.; and Adams, W Magellan: An integrated adaptive arcitecture for mobile robots. Tecnical Report 98-, Institute for te Study of Learning and Expertise (ISLE), Palo Alto, CA.