- Email: [email protected]
Local Navigation using Traversability Maps
Local Navigation using Traversability Maps P. Nordin ∗ J. Nyg˚ ards ∗∗ ∗
Fluid and Mechatronic Systems, Link¨ oping University (e-mail: [email protected]) ∗∗ Swedish Defence Research Agency (e-mail: [email protected]) Abstract: In outdoor robotics it is important to be able to ascertain the traversability of the surrounding terrain. This paper presents a system where continuously generated traversability maps, useful for obstacle avoidance, are generated, stored and later reused to perform detailed local path planning. The detailed plan can be used as a temporary replacement for parts of a global plan that may lack knowledge about impassable obstacles or troublesome areas. The paper also describes an algorithm useful for on-line alignment and merging of previously stored traversability maps. Being able to align and merge maps is vital as the estimated global poses of multiple overlapping maps stored at different times may differ. Keywords: Obstacle avoidance, local navigation, traversability, path planning 1. INTRODUCTION Consider the application of a robotic mule. The mule follows a human, avoiding obstacles along the way. After arriving at the goal, the mule knows the way back and can be sent to collect more load. In figure 1 repeated runs from such an experiment are illustrated. During that experiment the route was “learned” as global GPS coordinates. However, during repeated runs, the navigation errors were so large that it was necessary to continuously estimate traversability and use obstacle avoidance routines to keep the robot on the road. This was mainly due to poor (consumer grade) GPS accuracy. From the results of this experiment it can be concluded that local obstacle avoidance on a traversability map in combination with a global path plan in GPS coordinates is enough to achieve a mule system. Navigation would be improved if the human path could be represented in relation to the local traversability maps instead of as global GPS coordinates. One contribution of this paper is to show how this can be achieved by matching the traversability maps used for obstacle avoidance. In the example above, as for all outdoor mobile robotic systems, one very important capability is to be able to determine automatically where it is possible to drive. To accomplish this a variety of different sensors can be used to take measurements of the surrounding environment. As the robot control system should avoid obstacles, these sensors are preferably non contact sensors such as ultrasonic sensors, laser scanners, radar or cameras. All of these sensors have one drawback in common though. The further away measurements are taken the lower the measurement resolution. This is an especially important aspect to consider in outdoor robotics as it determines how soon nearground obstacles can be detected. Examples of common near-ground obstacles in urban environments are the edge of the sidewalk, bushes and rocks. These kinds of obstacles are usually small but must be taken into account as they
Fig. 1. The overlaid maps from several outdoor mule runs. They are misaligned because of drift in the estimated position. The robot has navigated back and forth along a road six times. Trees in the woods, edges of sidewalks and some building are visible in the map. may hinder the robot’s movement. Aside from their small size they may also have the same colour and texture as the surrounding non-obstacle ground, which could fool camera based detection. Detection of obstacles can also fail due to occlusion. One such example is obstacles that are hidden behind the corner of a building. How can these distant, small and maybe hidden obstacles be detected and taken into account when planning the robot’s path? Some prior information regarding their existence must be available. For this reason maps that contain information about traversability can be generated. Traversability can describe different things like the existence of large obstacles, the slope of the ground and how uneven or slippery the ground is. Traversability information can either be tightly coupled with a specific robot platform or described more generally
for use with different robots. One set of maps can be used for planning by multiple mobile robots and depending on each robot’s properties different plans may result. During earlier work with traversability mapping in the preRunners project, Nordin et al. (2009, 2008), it became clear that pose uncertainties both when creating the maps and when trying to reuse them is a problem that can not be avoided. Even when using GPS (consumer grade) the pose uncertainties will be noticeable in the maps. One experimental set-up in the project was a two-vehicle configuration, one preRunner and one master. The preRunner would gather traversability data from further ahead and stream it back to the master, which could immediately use it to extend its own planning horizon. Because the streamed data was inserted using the difference between estimated vehicle poses, the received map information would soon become misaligned. Even though both vehicles used the same GPS system, their relative pose could have errors of several meters and degrees. It was concluded that it should be possible to match received data against own data and realign the received data before use. This is also necessary if older maps are to be reused, which would be beneficial in the mule scenario for more efficient path planning. The generated maps are saved together with the estimated global pose that the robot had when the map was started. On a later occasion the estimated global robot pose at the same place may differ by a few meters. Map segments covering the same area but generated at different times will not align properly if only their estimated pose is used to place them.
2. RELATED WORK The idea of using a traversability measure for determining where to drive is not new. Creating and using maps to aid robot navigation is very common. Usually, maps are created and used when solving the SLAM problem for robot navigation. There are many different methods and algorithms for doing this but they will not be covered here. See for example Durrant-Whyte and Bailey (2006) parts 1 and 2. This paper instead presents a system that generates and uses maps primarily for path planning and obstacle avoidance. These maps are used as a means to store traversability information about the environment. The maps are used by the robot to determine where it is possible to drive and not only as a means of localization. Some especially noteworthy references to projects where similar work has been done are: Thrun et al. (2006), the winners of the DARPA Grand Challenge used laser range finders to scan the ground in front of the vehicle and build a representation of the road so that obstacles could be avoided. They also used a camera for terrain evaluation and classification of the road into drivable and non-drivable regions. Andersen et al. (2006) used a 2D laser scanner to classify parts of the road ahead into roughness groups and finally traversable or non-traversable regions. Andersen (2006) adds, among other things, vision based traversability classification to the roads. The roughness groups found from the laser scans classified the ground closest to the robot.
A camera was then used to find similar-looking terrain further ahead. Lamon et al. (2006a,b) used rotating laser scanners to acquire 3D scans of the surrounding environment. The scan data was gathered into a so-called MLS map, an extension to elevation maps, Triebel et al. (2006), giving the possibility to deal with multiple surfaces and vertical structures. While the 3D-scan maps where not used directly for planning, the data was used in a local grid based cost map to indicate traversability in the area along the robot path. From this map, path plans were generated. Within the DARPA LAGR program, autonomous ground vehicle navigation systems were to be improved by the use of learning algorithms. The navigation systems should not repeatedly lead a robot into the same type of reoccurring obstacles. Within the program, Hadsell et al. (2007), used stereo cameras to label traversability on image data up to 12 meters in front of the robot. A training module was trained on this data and the results were then used to classify data up to 35 meters in front of the robot. Konolige et al. (2009) used stereo cameras to find the ground plane and obstacles. A learning algorithm would learn from this and use texture and color models to classify traversable paths. Visual odometry was used when registering the data into a map. Because of the good performance of the visual odometry algorithm, the traversability maps could later be reused without the need to localize the robot within the map. The fact that maps were reused gave a significant performance gain. Some of these references, and others, have in common that they use multiple control-candidate trajectories to simulate the robot’s movement within the generated maps. The trajectory that does not lead to a collision and performs best in some manner is selected for the real robot control. The map matching in this paper is inspired by the tracking results in (Karlsson and Nyg˚ ards (2002)) that builds upon the efficient tracker by Hager and Belhumeur (1998). A good presentation of the area can be found in (Baker and Matthews (2004)) 3. SYSTEM SUMMARY The system can be divided into three main parts, each described in more detail in the coming sections. Traversability Generation: When the maps are generated it is important to have an accurate estimation of the robot pose when each laser scan is taken. Currently, EKF based sensor fusion of odometry, IMU and (consumer grade) GPS is used. If the GPS signal becomes unavailable the estimated pose will drift over time. The fused IMU and odometry data will still be locally accurate though. For this reason map data is stored into smaller sub-maps of a predetermined size. Even if the global estimated pose becomes more inaccurate over time the data within each sub-map will be locally correct. The traversability maps are continuously generated and used directly for obstacle avoidance. When the robot is nearing the end of a map patch, a new one is started. Old maps are saved to disk and information regarding them is stored in a kd-tree for later reuse. The kd-tree offers efficient lookup of spatial
data and is useful when maps in a certain region are to be retrieved.
measured data. A desired pose should lie within the measurement coverage in a map.
Visual odometry has not been implemented in this system but based on the results from, among others, Konolige et al. (2009); Scaramuzza and Siegwart (2008). Such a system would further improve the accuracy of the pose estimate, especially when the GPS signal is poor.
The map with the best score is selected, aligned and merged before the process begins again, this time with a larger initial map.
Planning: A path plan consists of lines connected by waypoints. The robot should try to move along these lines in order to reach each waypoint. The path planning can be separated into three levels. At the highest level, a pre-specified path, operator commands or some other high-level automatic planner determines a global path plan. In the mule scenario it is assumed that a human operator has access to e.g. a road network map or a map of areas where detailed traversability maps exist. At the medium level, a segment from the global plan is used to find detailed sub-maps along the way to the waypoint. This is illustrated in figure 2. The maps are retrieved from storage and merged to create a consistent map covering the desired area. Within this map the planner creates a more detailed local plan, leading the robot between the start position and the goal waypoint.
Fig. 2. A global path from A to B or from A to C requires two different sets of maps to be retrieved. As the robot progresses through the local plan, individual line segments are selected as base candidates for a linefollowing algorithm, Kanayama and Fahroo (1997), that is used to control the robot. The low-level planner simulates the robot’s moment along this line and alternative candidates using the current up-to-date traversability map to evaluate safety. Matching: When building the consistent map for the medium level planner, the first step is to find candidate maps for matching. K-nearest neighbor search is performed in the kd-tree to find potential matching neighbors. The map patches often contain large areas where no measurements have been made, e.g. due to occlusion or simply because the robot was heading the other way. The map matching algorithm needs information-rich measurements in overlapping regions to be able to align maps successfully. Convex hulls are used to contain the measured areas in each map. The intersection between hulls gives an estimate of the area with overlapping measurements. Candidates are scored based on two criteria: • The higher the overlap ratio the better, (up to a point). High overlap improves the chance of obtaining a successful match. • Candidates are penalized depending on the range between the desired poses along the way and the centroid (computed using image moments) of the
4. TRAVERSABILITY MAPPING As stated in the introduction, traversability maps may contain many types of information. In the current implementation, only the geometry of the surroundings is used. An area classified as traversable is flat enough and horizontal enough to be safe to drive on. Non-traversable regions are primarily made up of steep slopes. This means that the sides of e.g. trees, houses and cars will be marked as not traversable. They are all vertical objects. The edge of a sidewalk will also be marked as not traversable but the sidewalk itself which is flat will still be marked as a traversable surface. It is not the absolute height from the ground plane that determines traversability but rather the slope of surfaces. To measure the geometric properties of the ground two forward-looking laser scanners are used as shown in figure 3. One measures close to the robot and one measures further ahead. Because of the decreasing resolution with range, the laser looking further ahead might miss small obstacles. The closer-looking laser ensures that these are added to the map. From each laser scan, line segments are extracted. These lines are then sampled into a 2 21 -D elevation map where each grid cell can contain multiple measured height values. At the same time, the high resolution between measurement points within a scan is taken advantage of to find steep slopes and edges that are directly marked as obstacles in the traversability map.
Fig. 3. The robot is scanning the traversability along a forest path. The two scans are illustrated with overlaid yellow lines. The resulting map is also shown. 5. PATH PLANNING There are many ways to create path plans in map data for autonomous robots. A simple method based on distance transform in binary images was selected for the medium
level planner, see Jarvis (1989). The method is sometimes referred to as a type of grass-fire algorithm. (1) With a threshold the traversability map is divided into obstacle and non-obstacle areas. Unmeasured areas are assumed to be traversable. (2) To compensate for the robot size all obstacles are dilated, (made larger). (3) A wave works its way out from the goal, setting the distance in every pixel it visits. Chamfer distance is used as an approximation to euclidean distance. The wave can not propagate through pixels marked as obstacles. (4) Once all pixels have been set, a pathfinder algorithm travels from the current robot pose in the map towards lower values until it reaches the goal position. From the resulting path, line segments connected by waypoints are extracted. This method has two downsides though. Visiting and setting the distance in every pixel takes quite some time, especially on larger maps. The resulting plan usually ends up close to obstacle borders. This leads to a robot that may want to drive unnecessarily close to danger. Another method that would generate safer paths is the use of Voronoi diagrams. This has been done by, among others, Garrido et al. (2006); Bhattacharya and Gavrilova (2007). 5.1 Obstacle Avoidance The line follower module takes one line at a time from the medium path plan as its base control candidate. Using predefined offsets, it adds multiple additional control candidates. By simulating the robot’s movement along the candidates in the current traversability map, scores are generated. Equation (1) describes how the score is calculated. α, β, γ, φ are weight factors. 1 1 Score = α · dtc + β · ss + γ · +φ· (1) od hd The dtc (distance to collision) will be the most important factor of a control candidate. If a candidate leads to collision and there are alternatives, it should not be chosen. However if all candidates lead to collision, the one that takes the robot the furthest is a good choice. The ss (safety sum) will be low if a candidate brings the robot close to many obstacles. The od (offset deviation) and hd (heading deviation) are given lower weights and are used to penalize safe candidates that bring the robot too far from the base line or in the wrong direction. Figure 4 shows the simulated trajectories from a simulation where the robot avoids driving into a tree to its left. The candidate with the best score gets an additional set of offsets, just like the base line. If only one of the original candidates is acceptable, e.g. when passing through a narrow passage, the winner can still be adjusted further with these extra candidates. 6. MATCHING FOR MAP REUSE As stated in the introduction, it must be possible to reuse the generated traversability maps. Different vehicles starting from different positions relative to the maps should be able to use them in order to figure out where
Fig. 4. Evaluated control candidates from a simulated environment with trees, edges of sidewalks and a wall. The red curves indicate collision with obstacles. they can drive. This means that the robots needs to know where they are located and where they want to go in the maps coordinate systems. If we assume that we have a working GPS this task might seem easy. The robot knows where it is and where it is supposed to go. It can also retrieve the estimated GPS-based coordinates and orientation for each stored map and create a complete map of the way to the goal. As explained earlier, the map poses may be inaccurate up to few meters and the orientation may also be off by a few degrees. These inaccuracies are often enough to result in a path plan that tries to drive the robot outside of the actual traversable path. The system must be capable of aligning overlapping areas of map segments and to use the match information to merge the two maps. To be able to reliably use any generated plans from the maps, it must also be capable of localizing the robot within the maps. The maps view of the global coordinate system may not be completely consistent with the current one. Localizing the robot within the map is also useful when a GPS signal is missing. Even if visual odometry enhanced pose estimation is used, the robot must still be able to determine where within the stored maps it is. 6.1 Scale Pyramid and Gradient Search Matching The technique used to create the traversability maps means that the obstacle markings in a map are usually thin steep edges on the actual obstacle objects. Obstacles may also look slightly different depending on the direction they are being observed from. The robot might also have completely missed some obstacles in one map but not in another. The method used to merge two overlapping map segments is inspired by earlier tracking results of affine image templates, Karlsson and Nyg˚ ards (2002). In the present case, however, a simpler motion model of translation and rotation is sufficient. The algorithm is basically a GaussNewton gradient search. Consider a template image T , then by a Taylor approximation a slightly perturbed image P perturbed by translation (dx, dy) and rotation dθ can be described by equation 2. P ≈ T + Jx dx + Jy dy + Jθ dθ + h.o.t. (2) with the Jacobian images dT dT dT Jx = , Jy = , Jθ = = −yJx + xJy dx dy dθ The Jacobian images (Jx , Jy ) are approximated by the use of Sobel filters. As it is assumed that the initial estimated pose of each map has GPS or visual odometry accuracy (a few meters), it
can further be assumed that overlapping areas in the maps are initially quit close, that is, it is assumed that the match process begins with almost aligned maps. This means that the gradient search will start in a nearly correct position. If it were to start in a completely wrong position it would most likely lead to a local minimum, and an incorrect alignment. Scale pyramids are used to perform the Newton gradient search at different levels. Four scales are used, each one halving the map resolution. Because of the often thin obstacle markings, sub-sampling could wash out the obstacles. Before sub-sampling, all obstacles are grown. The initial low resolution steps are used to find a rough match dealing primarily with aligning large areas of the map, for instance a road between trees or a road next to a wall. At each new scale in the pyramid, map accuracy is increased giving further minor adjustments until the final map with the original resolution has been aligned. Since the traversability images are mostly black and white and the information concentrated to the gradients, the growing and the scale pyramid helps to alleviate the problem with thin obstacle markings. As illustrated in figure 5, the otherwise thin obstacles will have more impact at a rougher pyramid level than at their original scales. Collecting the Jacobian images of pyramid level l into rows in a matrix M l and the transformation parameters into µl = [xl , y l , θl ]T a Gauss-Newton step at each pyramid level can be found as T T ∆µ = −(M l M l )−1 M l (T l − P l ) (3) µl = µl+1 + ∆µ
(4)
Fig. 6. Several smaller maps have been merged for path planning. A slight misalignment is visible to the left of the center. The resulting plan is plotted in orange.
Fig. 7. The global path is triangular, A to B to C. On the first run the robot drives into the dead end generated by the trees. On successive runs, the reused traversability map helps the robot to go around. 7.2 Outdoor Experiment
Fig. 5. An illustration of the initial misalignment before the matching of two sub-maps within their overlapping region begins. The original resolution (left). One level in the scale pyramid (right). 7. SIMULATIONS AND EXPERIMENTS The system implementation is based on the Player platform, Collett et al. (2005). Together with the 3D simulator Gazebo, MiL (Model in the loop) simulations with the real software implementation can be performed. When real experiments are desired the Gazebo simulator is replaced by the real robot and the real world. 7.1 Simulations Figure 6 shows an example from the simulation environment where multiple maps have been merged into one convenient map covering the entire way between two global waypoints. It also includes the resulting local path plan from the medium level planner. In another simulation the benefit of retrieving map information is clearly visible. Figure 7 shows how the robot, by reusing traversability maps, can avoid a dead end on successive runs.
To illustrate the localization function, an outdoor mule scenario experiment was conducted. The robot was initially given the same waypoint track as shown in figure 1. Using GPS aided pose estimation, the robot travelled back and forth twice, resulting in a stored map coverage from four runs. These maps were later reused when running the system without GPS-aided pose estimation. Figure 8 shows how the estimated pose quickly drifts away from the coordinate system of the original maps once the GPS is turned off. By matching the current up-to-date traversability map against the original maps, a transformation from the erroneous estimated pose into the original map coordinate system can be decided. In this experiment the matching algorithm was continuously activated manually at approximately 10-meter intervals. The GPS was initially activated to obtain the correct starting pose. 8. CONCLUDING REMARKS We have presented a system where traversability maps from previous runs can be reused. After aligning and merging these maps, the medium-level path planner is able to make the robot avoid dead ends or inappropriate areas on repeated traverses of a path. The ability to match maps can also be used when a robot needs to localize itself within a set of previously stored maps. When localization is used the estimated pose must not be allowed to drift too far, however. The matching algorithm may then end up in local minima, which results in an incorrect match. The allowed
Fig. 8. By matching traversability maps, transformations from estimated poses into the map coordinate system are decided. This means that the robot still knows where it is even after the GPS signal has been lost. initial misalignment is difficult to determine as the results of the matching algorithm are highly dependent on the appearance of the contents in the maps to be merged. The traversability mapping technique presented here works best in static or near-static environments. Maps where moving objects have been captured are not reliable as they contain phantom obstacles. Map matching may also fail if too many obstacles that are in one map are missing from the other that is about to be matched. As new maps are continuously generated, old maps could be updated or replaced by new information when mismatches are detected. No such capabilities are currently implemented though. While it is possible to some extent to use the maps for localization, this is in no way intended to replace regular SLAM techniques. REFERENCES Andersen, J.C. (2006). Mobile robot navigation. Ph.D. thesis, Technical University of Denmark. Andersen, J.C., Blas, M.R., Ravn, O., Andersen, N.A., and Blanke, M. (2006). Traversable terrain classification for outdoor autonomous robots using single 2D laser scans. Integrated Computer-Aided Engineering, 13(3), 223–232. Baker, S. and Matthews, I. (2004). Lucas-kanade 20 years on: A unifying framework. International Journal of Computer Vision, 56(1), 221 – 255. Bhattacharya, P. and Gavrilova, M. (2007). Voronoi diagram in optimal path planning. In 4th International Symposium on Voronoi Diagrams in Science and Engineering, 38–47. Pontypridd, Wales, UK. Collett, T.H.J., Macdonald, B.A., and Gerkey, B.P. (2005). Player 2.0: Toward a practical robot programming framework. In Proc. of the Australasian Conf. on Robotics and Automation (ACRA). Sydney, Australia. Durrant-Whyte, H. and Bailey, T. (2006). Simultaneous localization and mapping: part i. Robotics & Automation Magazine, IEEE, 13(2), 99–110.
Garrido, S., Moreno, L., Abderrahim, M., and Martin, F. (2006). Path planning for mobile robot navigation using Voronoi diagram and fast marching. In Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on, 2376–2381. Beijing, China. Hadsell, R., Sermanet, P., Ben, J., Erkan, A., Han, J., Muller, U., and LeCun, Y. (2007). Online learning for offroad robots: Spatial label propagation to learn longrange traversability. In Proceedings of Robotics: Science and Systems. Atlanta, GA, USA. Hager, G.D. and Belhumeur, P.N. (1998). Efficient region tracking with parametric models of geometry and illumination. IEEE Transactions on Pattern Analysis and Machine Intelligence, 20, 1025–1039. Jarvis, R. (1989). Collision-free path planning in timevarying environments. In International Workshop on Intelligent Robots and Systems ’89. The Autonomous Mobile Robots and Its Applications. IROS ’89. Proceedings., IEEE/RSJ, 99–106. Kanayama, Y. and Fahroo, F. (1997). A new line tracking method for nonholonomic vehicles. In IEEE International Conference on Robotics and Automation. Albuquerque, New Mexico. Karlsson, H. and Nyg˚ ards, J. (2002). Robust and efficient tracking in image sequences using a Kalman filter and an affine motion model. In IEEE/RSJ International Conference on Intelligent Robots and System IROS2002. Lausanne, Switzerland. Konolige, K., Agrawal, M., Blas, M.R., Bolles, R.C., Gerkey, B., Sol`a, J., and Sundaresan, A. (2009). Mapping, navigation, and learning for off-road traversal. Journal of Field Robotics, 26(1), 88–113. Lamon, P., Kolski, S., and Siegwart, R. (2006a). The SmartTer - a vehicle for fully autonomous navigation and mapping in outdoor environments. In Proceedings of CLAWAR. Brussels, Belgium. Lamon, P., Stachniss, C., Triebel, R., Pfaff, P., Plagemann, C., Grisetti, G., et al. (2006b). Mapping with an autonomous car. In Proceedings of the Workshop on Safe Navigation in Open and Dynamic Environments at the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Beijing, China. Nordin, P., Andersson, L., and Nyg˚ ards, J. (2008). Sensor data fusion for terrain exploration by collaborating unmanned ground vehicles. In Proceedings of the 11th International Conference on Information Fusion. Cologne, Germany. Nordin, P., Andersson, L., and Nyg˚ ards, J. (2009). Results of the TAIS/preRunners-project. In Fourth Swedish Workshop on Autonomous Robotics SWAR’09. V¨aster˚ as, Sweden. Scaramuzza, D. and Siegwart, R. (2008). Appearance guided monocular omnidirectional visual odometry for outdoor ground vehicles. IEEE Transactions on Robotics, 24(5), 1015 – 1026. Thrun, S., Montemerlo, M., Dahlkamp, H., Stavens, D., Aron, A., Diebel, J., et al. (2006). Winning the DARPA Grand Challenge. Journal of Field Robotics. Triebel, R., Pfaff, P., and Burgard, W. (2006). Multi-level surface maps for outdoor terrain mapping and loop closing. In Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on, 2276–2282. Beijing, China.
Fluid and Mechatronic Systems, Link¨ oping University (e-mail: [email protected]) ∗∗ Swedish Defence Research Agency (e-mail: [email protected]) Abstract: In outdoor robotics it is important to be able to ascertain the traversability of the surrounding terrain. This paper presents a system where continuously generated traversability maps, useful for obstacle avoidance, are generated, stored and later reused to perform detailed local path planning. The detailed plan can be used as a temporary replacement for parts of a global plan that may lack knowledge about impassable obstacles or troublesome areas. The paper also describes an algorithm useful for on-line alignment and merging of previously stored traversability maps. Being able to align and merge maps is vital as the estimated global poses of multiple overlapping maps stored at different times may differ. Keywords: Obstacle avoidance, local navigation, traversability, path planning 1. INTRODUCTION Consider the application of a robotic mule. The mule follows a human, avoiding obstacles along the way. After arriving at the goal, the mule knows the way back and can be sent to collect more load. In figure 1 repeated runs from such an experiment are illustrated. During that experiment the route was “learned” as global GPS coordinates. However, during repeated runs, the navigation errors were so large that it was necessary to continuously estimate traversability and use obstacle avoidance routines to keep the robot on the road. This was mainly due to poor (consumer grade) GPS accuracy. From the results of this experiment it can be concluded that local obstacle avoidance on a traversability map in combination with a global path plan in GPS coordinates is enough to achieve a mule system. Navigation would be improved if the human path could be represented in relation to the local traversability maps instead of as global GPS coordinates. One contribution of this paper is to show how this can be achieved by matching the traversability maps used for obstacle avoidance. In the example above, as for all outdoor mobile robotic systems, one very important capability is to be able to determine automatically where it is possible to drive. To accomplish this a variety of different sensors can be used to take measurements of the surrounding environment. As the robot control system should avoid obstacles, these sensors are preferably non contact sensors such as ultrasonic sensors, laser scanners, radar or cameras. All of these sensors have one drawback in common though. The further away measurements are taken the lower the measurement resolution. This is an especially important aspect to consider in outdoor robotics as it determines how soon nearground obstacles can be detected. Examples of common near-ground obstacles in urban environments are the edge of the sidewalk, bushes and rocks. These kinds of obstacles are usually small but must be taken into account as they
Fig. 1. The overlaid maps from several outdoor mule runs. They are misaligned because of drift in the estimated position. The robot has navigated back and forth along a road six times. Trees in the woods, edges of sidewalks and some building are visible in the map. may hinder the robot’s movement. Aside from their small size they may also have the same colour and texture as the surrounding non-obstacle ground, which could fool camera based detection. Detection of obstacles can also fail due to occlusion. One such example is obstacles that are hidden behind the corner of a building. How can these distant, small and maybe hidden obstacles be detected and taken into account when planning the robot’s path? Some prior information regarding their existence must be available. For this reason maps that contain information about traversability can be generated. Traversability can describe different things like the existence of large obstacles, the slope of the ground and how uneven or slippery the ground is. Traversability information can either be tightly coupled with a specific robot platform or described more generally
for use with different robots. One set of maps can be used for planning by multiple mobile robots and depending on each robot’s properties different plans may result. During earlier work with traversability mapping in the preRunners project, Nordin et al. (2009, 2008), it became clear that pose uncertainties both when creating the maps and when trying to reuse them is a problem that can not be avoided. Even when using GPS (consumer grade) the pose uncertainties will be noticeable in the maps. One experimental set-up in the project was a two-vehicle configuration, one preRunner and one master. The preRunner would gather traversability data from further ahead and stream it back to the master, which could immediately use it to extend its own planning horizon. Because the streamed data was inserted using the difference between estimated vehicle poses, the received map information would soon become misaligned. Even though both vehicles used the same GPS system, their relative pose could have errors of several meters and degrees. It was concluded that it should be possible to match received data against own data and realign the received data before use. This is also necessary if older maps are to be reused, which would be beneficial in the mule scenario for more efficient path planning. The generated maps are saved together with the estimated global pose that the robot had when the map was started. On a later occasion the estimated global robot pose at the same place may differ by a few meters. Map segments covering the same area but generated at different times will not align properly if only their estimated pose is used to place them.
2. RELATED WORK The idea of using a traversability measure for determining where to drive is not new. Creating and using maps to aid robot navigation is very common. Usually, maps are created and used when solving the SLAM problem for robot navigation. There are many different methods and algorithms for doing this but they will not be covered here. See for example Durrant-Whyte and Bailey (2006) parts 1 and 2. This paper instead presents a system that generates and uses maps primarily for path planning and obstacle avoidance. These maps are used as a means to store traversability information about the environment. The maps are used by the robot to determine where it is possible to drive and not only as a means of localization. Some especially noteworthy references to projects where similar work has been done are: Thrun et al. (2006), the winners of the DARPA Grand Challenge used laser range finders to scan the ground in front of the vehicle and build a representation of the road so that obstacles could be avoided. They also used a camera for terrain evaluation and classification of the road into drivable and non-drivable regions. Andersen et al. (2006) used a 2D laser scanner to classify parts of the road ahead into roughness groups and finally traversable or non-traversable regions. Andersen (2006) adds, among other things, vision based traversability classification to the roads. The roughness groups found from the laser scans classified the ground closest to the robot.
A camera was then used to find similar-looking terrain further ahead. Lamon et al. (2006a,b) used rotating laser scanners to acquire 3D scans of the surrounding environment. The scan data was gathered into a so-called MLS map, an extension to elevation maps, Triebel et al. (2006), giving the possibility to deal with multiple surfaces and vertical structures. While the 3D-scan maps where not used directly for planning, the data was used in a local grid based cost map to indicate traversability in the area along the robot path. From this map, path plans were generated. Within the DARPA LAGR program, autonomous ground vehicle navigation systems were to be improved by the use of learning algorithms. The navigation systems should not repeatedly lead a robot into the same type of reoccurring obstacles. Within the program, Hadsell et al. (2007), used stereo cameras to label traversability on image data up to 12 meters in front of the robot. A training module was trained on this data and the results were then used to classify data up to 35 meters in front of the robot. Konolige et al. (2009) used stereo cameras to find the ground plane and obstacles. A learning algorithm would learn from this and use texture and color models to classify traversable paths. Visual odometry was used when registering the data into a map. Because of the good performance of the visual odometry algorithm, the traversability maps could later be reused without the need to localize the robot within the map. The fact that maps were reused gave a significant performance gain. Some of these references, and others, have in common that they use multiple control-candidate trajectories to simulate the robot’s movement within the generated maps. The trajectory that does not lead to a collision and performs best in some manner is selected for the real robot control. The map matching in this paper is inspired by the tracking results in (Karlsson and Nyg˚ ards (2002)) that builds upon the efficient tracker by Hager and Belhumeur (1998). A good presentation of the area can be found in (Baker and Matthews (2004)) 3. SYSTEM SUMMARY The system can be divided into three main parts, each described in more detail in the coming sections. Traversability Generation: When the maps are generated it is important to have an accurate estimation of the robot pose when each laser scan is taken. Currently, EKF based sensor fusion of odometry, IMU and (consumer grade) GPS is used. If the GPS signal becomes unavailable the estimated pose will drift over time. The fused IMU and odometry data will still be locally accurate though. For this reason map data is stored into smaller sub-maps of a predetermined size. Even if the global estimated pose becomes more inaccurate over time the data within each sub-map will be locally correct. The traversability maps are continuously generated and used directly for obstacle avoidance. When the robot is nearing the end of a map patch, a new one is started. Old maps are saved to disk and information regarding them is stored in a kd-tree for later reuse. The kd-tree offers efficient lookup of spatial
data and is useful when maps in a certain region are to be retrieved.
measured data. A desired pose should lie within the measurement coverage in a map.
Visual odometry has not been implemented in this system but based on the results from, among others, Konolige et al. (2009); Scaramuzza and Siegwart (2008). Such a system would further improve the accuracy of the pose estimate, especially when the GPS signal is poor.
The map with the best score is selected, aligned and merged before the process begins again, this time with a larger initial map.
Planning: A path plan consists of lines connected by waypoints. The robot should try to move along these lines in order to reach each waypoint. The path planning can be separated into three levels. At the highest level, a pre-specified path, operator commands or some other high-level automatic planner determines a global path plan. In the mule scenario it is assumed that a human operator has access to e.g. a road network map or a map of areas where detailed traversability maps exist. At the medium level, a segment from the global plan is used to find detailed sub-maps along the way to the waypoint. This is illustrated in figure 2. The maps are retrieved from storage and merged to create a consistent map covering the desired area. Within this map the planner creates a more detailed local plan, leading the robot between the start position and the goal waypoint.
Fig. 2. A global path from A to B or from A to C requires two different sets of maps to be retrieved. As the robot progresses through the local plan, individual line segments are selected as base candidates for a linefollowing algorithm, Kanayama and Fahroo (1997), that is used to control the robot. The low-level planner simulates the robot’s moment along this line and alternative candidates using the current up-to-date traversability map to evaluate safety. Matching: When building the consistent map for the medium level planner, the first step is to find candidate maps for matching. K-nearest neighbor search is performed in the kd-tree to find potential matching neighbors. The map patches often contain large areas where no measurements have been made, e.g. due to occlusion or simply because the robot was heading the other way. The map matching algorithm needs information-rich measurements in overlapping regions to be able to align maps successfully. Convex hulls are used to contain the measured areas in each map. The intersection between hulls gives an estimate of the area with overlapping measurements. Candidates are scored based on two criteria: • The higher the overlap ratio the better, (up to a point). High overlap improves the chance of obtaining a successful match. • Candidates are penalized depending on the range between the desired poses along the way and the centroid (computed using image moments) of the
4. TRAVERSABILITY MAPPING As stated in the introduction, traversability maps may contain many types of information. In the current implementation, only the geometry of the surroundings is used. An area classified as traversable is flat enough and horizontal enough to be safe to drive on. Non-traversable regions are primarily made up of steep slopes. This means that the sides of e.g. trees, houses and cars will be marked as not traversable. They are all vertical objects. The edge of a sidewalk will also be marked as not traversable but the sidewalk itself which is flat will still be marked as a traversable surface. It is not the absolute height from the ground plane that determines traversability but rather the slope of surfaces. To measure the geometric properties of the ground two forward-looking laser scanners are used as shown in figure 3. One measures close to the robot and one measures further ahead. Because of the decreasing resolution with range, the laser looking further ahead might miss small obstacles. The closer-looking laser ensures that these are added to the map. From each laser scan, line segments are extracted. These lines are then sampled into a 2 21 -D elevation map where each grid cell can contain multiple measured height values. At the same time, the high resolution between measurement points within a scan is taken advantage of to find steep slopes and edges that are directly marked as obstacles in the traversability map.
Fig. 3. The robot is scanning the traversability along a forest path. The two scans are illustrated with overlaid yellow lines. The resulting map is also shown. 5. PATH PLANNING There are many ways to create path plans in map data for autonomous robots. A simple method based on distance transform in binary images was selected for the medium
level planner, see Jarvis (1989). The method is sometimes referred to as a type of grass-fire algorithm. (1) With a threshold the traversability map is divided into obstacle and non-obstacle areas. Unmeasured areas are assumed to be traversable. (2) To compensate for the robot size all obstacles are dilated, (made larger). (3) A wave works its way out from the goal, setting the distance in every pixel it visits. Chamfer distance is used as an approximation to euclidean distance. The wave can not propagate through pixels marked as obstacles. (4) Once all pixels have been set, a pathfinder algorithm travels from the current robot pose in the map towards lower values until it reaches the goal position. From the resulting path, line segments connected by waypoints are extracted. This method has two downsides though. Visiting and setting the distance in every pixel takes quite some time, especially on larger maps. The resulting plan usually ends up close to obstacle borders. This leads to a robot that may want to drive unnecessarily close to danger. Another method that would generate safer paths is the use of Voronoi diagrams. This has been done by, among others, Garrido et al. (2006); Bhattacharya and Gavrilova (2007). 5.1 Obstacle Avoidance The line follower module takes one line at a time from the medium path plan as its base control candidate. Using predefined offsets, it adds multiple additional control candidates. By simulating the robot’s movement along the candidates in the current traversability map, scores are generated. Equation (1) describes how the score is calculated. α, β, γ, φ are weight factors. 1 1 Score = α · dtc + β · ss + γ · +φ· (1) od hd The dtc (distance to collision) will be the most important factor of a control candidate. If a candidate leads to collision and there are alternatives, it should not be chosen. However if all candidates lead to collision, the one that takes the robot the furthest is a good choice. The ss (safety sum) will be low if a candidate brings the robot close to many obstacles. The od (offset deviation) and hd (heading deviation) are given lower weights and are used to penalize safe candidates that bring the robot too far from the base line or in the wrong direction. Figure 4 shows the simulated trajectories from a simulation where the robot avoids driving into a tree to its left. The candidate with the best score gets an additional set of offsets, just like the base line. If only one of the original candidates is acceptable, e.g. when passing through a narrow passage, the winner can still be adjusted further with these extra candidates. 6. MATCHING FOR MAP REUSE As stated in the introduction, it must be possible to reuse the generated traversability maps. Different vehicles starting from different positions relative to the maps should be able to use them in order to figure out where
Fig. 4. Evaluated control candidates from a simulated environment with trees, edges of sidewalks and a wall. The red curves indicate collision with obstacles. they can drive. This means that the robots needs to know where they are located and where they want to go in the maps coordinate systems. If we assume that we have a working GPS this task might seem easy. The robot knows where it is and where it is supposed to go. It can also retrieve the estimated GPS-based coordinates and orientation for each stored map and create a complete map of the way to the goal. As explained earlier, the map poses may be inaccurate up to few meters and the orientation may also be off by a few degrees. These inaccuracies are often enough to result in a path plan that tries to drive the robot outside of the actual traversable path. The system must be capable of aligning overlapping areas of map segments and to use the match information to merge the two maps. To be able to reliably use any generated plans from the maps, it must also be capable of localizing the robot within the maps. The maps view of the global coordinate system may not be completely consistent with the current one. Localizing the robot within the map is also useful when a GPS signal is missing. Even if visual odometry enhanced pose estimation is used, the robot must still be able to determine where within the stored maps it is. 6.1 Scale Pyramid and Gradient Search Matching The technique used to create the traversability maps means that the obstacle markings in a map are usually thin steep edges on the actual obstacle objects. Obstacles may also look slightly different depending on the direction they are being observed from. The robot might also have completely missed some obstacles in one map but not in another. The method used to merge two overlapping map segments is inspired by earlier tracking results of affine image templates, Karlsson and Nyg˚ ards (2002). In the present case, however, a simpler motion model of translation and rotation is sufficient. The algorithm is basically a GaussNewton gradient search. Consider a template image T , then by a Taylor approximation a slightly perturbed image P perturbed by translation (dx, dy) and rotation dθ can be described by equation 2. P ≈ T + Jx dx + Jy dy + Jθ dθ + h.o.t. (2) with the Jacobian images dT dT dT Jx = , Jy = , Jθ = = −yJx + xJy dx dy dθ The Jacobian images (Jx , Jy ) are approximated by the use of Sobel filters. As it is assumed that the initial estimated pose of each map has GPS or visual odometry accuracy (a few meters), it
can further be assumed that overlapping areas in the maps are initially quit close, that is, it is assumed that the match process begins with almost aligned maps. This means that the gradient search will start in a nearly correct position. If it were to start in a completely wrong position it would most likely lead to a local minimum, and an incorrect alignment. Scale pyramids are used to perform the Newton gradient search at different levels. Four scales are used, each one halving the map resolution. Because of the often thin obstacle markings, sub-sampling could wash out the obstacles. Before sub-sampling, all obstacles are grown. The initial low resolution steps are used to find a rough match dealing primarily with aligning large areas of the map, for instance a road between trees or a road next to a wall. At each new scale in the pyramid, map accuracy is increased giving further minor adjustments until the final map with the original resolution has been aligned. Since the traversability images are mostly black and white and the information concentrated to the gradients, the growing and the scale pyramid helps to alleviate the problem with thin obstacle markings. As illustrated in figure 5, the otherwise thin obstacles will have more impact at a rougher pyramid level than at their original scales. Collecting the Jacobian images of pyramid level l into rows in a matrix M l and the transformation parameters into µl = [xl , y l , θl ]T a Gauss-Newton step at each pyramid level can be found as T T ∆µ = −(M l M l )−1 M l (T l − P l ) (3) µl = µl+1 + ∆µ
(4)
Fig. 6. Several smaller maps have been merged for path planning. A slight misalignment is visible to the left of the center. The resulting plan is plotted in orange.
Fig. 7. The global path is triangular, A to B to C. On the first run the robot drives into the dead end generated by the trees. On successive runs, the reused traversability map helps the robot to go around. 7.2 Outdoor Experiment
Fig. 5. An illustration of the initial misalignment before the matching of two sub-maps within their overlapping region begins. The original resolution (left). One level in the scale pyramid (right). 7. SIMULATIONS AND EXPERIMENTS The system implementation is based on the Player platform, Collett et al. (2005). Together with the 3D simulator Gazebo, MiL (Model in the loop) simulations with the real software implementation can be performed. When real experiments are desired the Gazebo simulator is replaced by the real robot and the real world. 7.1 Simulations Figure 6 shows an example from the simulation environment where multiple maps have been merged into one convenient map covering the entire way between two global waypoints. It also includes the resulting local path plan from the medium level planner. In another simulation the benefit of retrieving map information is clearly visible. Figure 7 shows how the robot, by reusing traversability maps, can avoid a dead end on successive runs.
To illustrate the localization function, an outdoor mule scenario experiment was conducted. The robot was initially given the same waypoint track as shown in figure 1. Using GPS aided pose estimation, the robot travelled back and forth twice, resulting in a stored map coverage from four runs. These maps were later reused when running the system without GPS-aided pose estimation. Figure 8 shows how the estimated pose quickly drifts away from the coordinate system of the original maps once the GPS is turned off. By matching the current up-to-date traversability map against the original maps, a transformation from the erroneous estimated pose into the original map coordinate system can be decided. In this experiment the matching algorithm was continuously activated manually at approximately 10-meter intervals. The GPS was initially activated to obtain the correct starting pose. 8. CONCLUDING REMARKS We have presented a system where traversability maps from previous runs can be reused. After aligning and merging these maps, the medium-level path planner is able to make the robot avoid dead ends or inappropriate areas on repeated traverses of a path. The ability to match maps can also be used when a robot needs to localize itself within a set of previously stored maps. When localization is used the estimated pose must not be allowed to drift too far, however. The matching algorithm may then end up in local minima, which results in an incorrect match. The allowed
Fig. 8. By matching traversability maps, transformations from estimated poses into the map coordinate system are decided. This means that the robot still knows where it is even after the GPS signal has been lost. initial misalignment is difficult to determine as the results of the matching algorithm are highly dependent on the appearance of the contents in the maps to be merged. The traversability mapping technique presented here works best in static or near-static environments. Maps where moving objects have been captured are not reliable as they contain phantom obstacles. Map matching may also fail if too many obstacles that are in one map are missing from the other that is about to be matched. As new maps are continuously generated, old maps could be updated or replaced by new information when mismatches are detected. No such capabilities are currently implemented though. While it is possible to some extent to use the maps for localization, this is in no way intended to replace regular SLAM techniques. REFERENCES Andersen, J.C. (2006). Mobile robot navigation. Ph.D. thesis, Technical University of Denmark. Andersen, J.C., Blas, M.R., Ravn, O., Andersen, N.A., and Blanke, M. (2006). Traversable terrain classification for outdoor autonomous robots using single 2D laser scans. Integrated Computer-Aided Engineering, 13(3), 223–232. Baker, S. and Matthews, I. (2004). Lucas-kanade 20 years on: A unifying framework. International Journal of Computer Vision, 56(1), 221 – 255. Bhattacharya, P. and Gavrilova, M. (2007). Voronoi diagram in optimal path planning. In 4th International Symposium on Voronoi Diagrams in Science and Engineering, 38–47. Pontypridd, Wales, UK. Collett, T.H.J., Macdonald, B.A., and Gerkey, B.P. (2005). Player 2.0: Toward a practical robot programming framework. In Proc. of the Australasian Conf. on Robotics and Automation (ACRA). Sydney, Australia. Durrant-Whyte, H. and Bailey, T. (2006). Simultaneous localization and mapping: part i. Robotics & Automation Magazine, IEEE, 13(2), 99–110.
Garrido, S., Moreno, L., Abderrahim, M., and Martin, F. (2006). Path planning for mobile robot navigation using Voronoi diagram and fast marching. In Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on, 2376–2381. Beijing, China. Hadsell, R., Sermanet, P., Ben, J., Erkan, A., Han, J., Muller, U., and LeCun, Y. (2007). Online learning for offroad robots: Spatial label propagation to learn longrange traversability. In Proceedings of Robotics: Science and Systems. Atlanta, GA, USA. Hager, G.D. and Belhumeur, P.N. (1998). Efficient region tracking with parametric models of geometry and illumination. IEEE Transactions on Pattern Analysis and Machine Intelligence, 20, 1025–1039. Jarvis, R. (1989). Collision-free path planning in timevarying environments. In International Workshop on Intelligent Robots and Systems ’89. The Autonomous Mobile Robots and Its Applications. IROS ’89. Proceedings., IEEE/RSJ, 99–106. Kanayama, Y. and Fahroo, F. (1997). A new line tracking method for nonholonomic vehicles. In IEEE International Conference on Robotics and Automation. Albuquerque, New Mexico. Karlsson, H. and Nyg˚ ards, J. (2002). Robust and efficient tracking in image sequences using a Kalman filter and an affine motion model. In IEEE/RSJ International Conference on Intelligent Robots and System IROS2002. Lausanne, Switzerland. Konolige, K., Agrawal, M., Blas, M.R., Bolles, R.C., Gerkey, B., Sol`a, J., and Sundaresan, A. (2009). Mapping, navigation, and learning for off-road traversal. Journal of Field Robotics, 26(1), 88–113. Lamon, P., Kolski, S., and Siegwart, R. (2006a). The SmartTer - a vehicle for fully autonomous navigation and mapping in outdoor environments. In Proceedings of CLAWAR. Brussels, Belgium. Lamon, P., Stachniss, C., Triebel, R., Pfaff, P., Plagemann, C., Grisetti, G., et al. (2006b). Mapping with an autonomous car. In Proceedings of the Workshop on Safe Navigation in Open and Dynamic Environments at the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Beijing, China. Nordin, P., Andersson, L., and Nyg˚ ards, J. (2008). Sensor data fusion for terrain exploration by collaborating unmanned ground vehicles. In Proceedings of the 11th International Conference on Information Fusion. Cologne, Germany. Nordin, P., Andersson, L., and Nyg˚ ards, J. (2009). Results of the TAIS/preRunners-project. In Fourth Swedish Workshop on Autonomous Robotics SWAR’09. V¨aster˚ as, Sweden. Scaramuzza, D. and Siegwart, R. (2008). Appearance guided monocular omnidirectional visual odometry for outdoor ground vehicles. IEEE Transactions on Robotics, 24(5), 1015 – 1026. Thrun, S., Montemerlo, M., Dahlkamp, H., Stavens, D., Aron, A., Diebel, J., et al. (2006). Winning the DARPA Grand Challenge. Journal of Field Robotics. Triebel, R., Pfaff, P., and Burgard, W. (2006). Multi-level surface maps for outdoor terrain mapping and loop closing. In Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on, 2276–2282. Beijing, China.