Solving a Path Planning Problem in a Partially Known Environment using a Swarm Algorithm

by   Eshwaran Vijaya Kumar, et al.

This paper proposes a path planning strategy for an Autonomous Ground Vehicle (AGV) navigating in a partially known environment. Global path planning is performed by first using a spatial database of the region to be traversed containing selected attributes such as height data and soil information from a suitable spatial database. The database is processed using a biomimetic swarm algorithm that is inspired by the nest building strategies followed by termites. Local path planning is performed online utilizing information regarding contingencies that affect the safe navigation of the AGV from various sensors. The simulation discussed has been implemented on the open source Player-Stage-Gazebo platform.



page 2

page 4

page 5


Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform

Path planning is a key component in motion planning for autonomous vehic...

To Explore or Not to Explore: Regret-Based LTL Planning in Partially-Known Environments

In this paper, we investigate the optimal robot path planning problem fo...

On the Use of the Observability Gramian for Partially Observed Robotic Path Planning Problems

Optimizing measures of the observability Gramian as a surrogate for the ...

Toward Crowd-Sensitive Path Planning

If a robot can predict crowds in parts of its environment that are inacc...

Appendix for the Motion Primitives-based Path Planning for Fast and Agile Exploration Method

This manuscript presents enhancements on our motion-primitives explorati...

A-star path planning simulation for UAS Traffic Management (UTM) application

This paper presents a Robot Operating System and Gazebo application to c...

Quality Amplification of Error Prone Navigation for Swarms of Micro Aerial Vehicles (with Detailed Simulations)

We present an error tolerant path planning algorithm for Micro Aerial Ve...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

The design of a complete planner that computes a collision free path for a non holonomic robot in any partially known environment especially outdoor terrains is constrained by a number of factors. These include the number of degrees of freedom imposed by the robot’s geometry, the presence or absence of obstacles and sensor inaccuracies 

[1] which cause an exponential increase in computational complexity of the problem [2]. One strategy to reduce the complexity of the problem is to formulate assumptions that simplify the environment [3, 4, 5]. Another strategy is to design planners that satisfy weaker forms of completeness such as the probablistic path planner [6] and the randomized path planner [7]. An auxiliary strategy presented here is to reduce the computational processing requirments by the use of a distributed algorithm based on the principle of swarm intelligence. Swarm intelligence involves the design of algorithms based on the interactions between social insects such as termites and ants which leads to emergent intelligent behaviour [8].

Approaches to designing planners for partially or dynamic environments differ in the way the environment is modelled. Topological maps utilize graphs where nodes represent landmark information and edges represent connections between them [9]. Planners based on topological maps include [10]. The main problem of utilizing such approaches in outdoor terrains is that of misidentification of similar landmarks. Metric maps capture geometric properties of the environment. Planners utilizing metric information include the D* algorithm [11, 12], potential field planners [13] and wave-front based planners [14, 15]. For large areas, although metric maps provide finer resolutions allowing for more detailed planning to take place, processing requirements increase drastically.

Ii The Swarm Algorithm

The generation of a desired collision free path involves two steps namely, (i) Global Path planning and (ii) Local Path planning. Here, global path planning is performed by first generating a spatial database of the region with selected features such as soil and gradient information. Gradient information is generated from Digital Elevation Models (DEM) which are digital representations of ground surface topography [16]. Elevation data having 30-90 metre resolution is freely available from the Shuttle Radar Topography Mission (SRTM) [17]. Other sources of DEMs include [18]. Figure 1 depicts the grey scale height map which is used as an input to generate the terrain. The corresponding terrain rendered in 3D is shown in Figure 2. Every pixel in a height map has a value varying from 0-255. A black spot corresponds to minimum elevation and a white spot corresponds to maximum elevation. In our simulation, this data is stored in a Postgres database table. This is mainly done to facilitate integration with soil information. To optimize space requirements, the data is sub-sampled so that every cell in the table is an average of the height map values of a group of 8 pixels. This height information is used to compute the gradient at each point/cell, in different directions and is simply the difference in the height values between two neighbouring points. The Pioneer2DX robot can travel safely on terrin bounded by a 25% limit on grade (an angle of approximately 15 deg. with the horizontal). Once the gradient information is computed, the data is mapped to a range of values ranging from 1 to 9 as shown in Table I below.

Cell Height Difference Gradient Goodness Value
255 - 193 1
192 - 130 2
126 - 67 3
66 - 1 4
0 5
-1 - -66 4
-67 - -129 3
-130 - -192 2
-192  -  -255 1
TABLE I: Generation of Gradient-based Rank Values from Height Difference
Fig. 1: Grey scale representation of the height map.
Fig. 2: The height map rendered in 3D.

Soil information of the region is obtained and classified into five categories that place bounds on robot manouverability. Soil information of the region is obtained from the GRASS POSTGIS database in the form of a vector map where the various soil types are classified by a parameter called

cat value. This information is parsed and classified into five different categories namely, Gravel, Sand, Clay, Silt and Rock, in order of suitability for vehicle traversal. To facilitate computation these categories are assigned values 5, 4, 3, 2, 1, respectively to indicate the “goodness” for traversability. Table II records this map formally.

Soil Category Soil Goodness Value
Gravel 5
Sand 4
Clay 3
Silt 3
Rock 1
TABLE II: Generation of Soil Goodness Values

Once the destination and start points are selected on the map, an arc joining the two is generated and a wide swathe of grid cells around the region are selected for processing. A biomimetic swarm algorithm inspired by nest building strategies in termites is used to process the data. The algorithm has already been proven successful in optimizing site selection in Geographical Information Systems (GIS) [19]. A rank map is first generated where a cell is defined to be good or bad by generating a single rank value by adding the Gradient Goodness Values and Soil Goodness Values discussed above. The expression for computing rank is given as equation 1.


This is a very simple way of computing the rank. More formal and complex Multi-Criteria Decision Making (MCDM) strategies like the Analytical Hierarchical Process (AHP) can be used. Once the rank has been obtained we now use a swarm algorithm inspired by the nest building behaviour of termites to perform the global path planning.

The algorithm is summarized as follows: A swarm of agents endowed with simplistic rules that govern their behaviour and local interaction is deployed randomly on the map to determine those cells that satisfy the constraints imposed by the vehicle for safe navigation. Each agent’s interaction rules lead to two behaviours, namely, pellet dropping and nest building. In the pellet dropping stage, agents utilize permanent pheromones in the form of pellets to mark cells of a suitable rank. Once the number of pellets reaches a certain maximum limit, agents use the cell as a focal point to move to the next stage, namely nest building. In the nest building mode, agents forage in the local neighborhood of the suitable cell looking for other cells which satisfy the navigation criteria. If all the cells in the local vicinity of the cell with maximum number of pellets satisfy the navigation criteria, a nest of these cells is then created. When two nests created by different agents come in contact, they are merged together to create large contiguous areas suitable for navigation. Figure 5 illustrates the rank map, where the cells are ranked and declared as part of a nest, as indicated by the shaded patterns. The flow chart of the swarm algorithm is included in Figures 3 and 4.

Fig. 3: Flowchart of the Swarm Algorithm
Fig. 4: Flowchart of the Nest Building Subroutine

Grid maps have a major limitation in that the path produced can become suboptimal due to unnecessary increase in length. This is caused by wastage in space as cells are marked occupied by obstacles even if only a small portion of the cell is occupied by an obstacle. One possible solution is to increase the grid resolution which however leads to increase in processing requirements. Such a restriction is partially removed by generating two grid maps where each cell is four times the Robot size and the maps are separated by an offset which is equivalent to one fourth of the cell size. The resultant map consists of overlapping grid cells that are suitably ranked and nested, as can be seen in Figure 6. Both maps are continously checked during the robot’s navigation.

Fig. 5: Grid for Path Planning.
Fig. 6: Two Overlapping Grids.
Fig. 7: Grid indicating different soil categories.
Fig. 8: Ranked cells indicating both soil and height data combined. Outlined regions are nests (navigable regions) detected by the swarm algorithm.

Iii Local Path Planning

In a partially known environment, real time obstacle avoidance is performed by utilizing sensory information regarding contingencies that affect the safe navigation of the robot. A key factor in designing a obstacle avoidance algorithm is minimising the generation of non-optimal local paths. Various obstacle avoidance techniques include the edge detection [20] and certainty grid technique [21] and the Vector Field Histogram (VFH) Technique [22, 23, 24]. The VFH algorithm is one of the most efficient local path planning algorithms. It generates a two dimensional cartesian histogram grid from the robot’s ranging sensors and utilizes the same to create a one dimensional polar grid around the immediate position of the robot. Contiguous sectors with a polar obstacle density below threshold are selected based on the proximity to target direction. This is then utilized by the robot to change steering towards the selected candidate direction. The processed swathe of cells around the arc generated by the swarm algorithm is then utilized to compute a suitable local path.

The VFH algorithm utilizes data from distance ranging sensors like laser or sonar sensors to perform obstacle avoidance. A robot moving across cross-country terrain will have to take into account vagaries of the terrain soil composition. Therefore, there arises a need to embed additional sensors that will sample the soil in a particular region to determine whether it is suitable for robot traversibility. The digital cone penetrometer  [25, 26] has been developed primarily to enable rapid assessment of the in-situ strength of the soil. The instrument is used to manually grade the soil and categorize it based on its attributes. The data obtained is matched with the attributes of the various soil types and the region is categorized accordingly. This soil information is concurrently used along with the obstacle avoidance by the VFH algorithm to aid local path planning.

Iv Implementation Details and Simulation Results

The algorithm discussed has been implemented on the open source Player-Stage-Gazebo platform [27]. A simulated pioneer2DX robot equipped with SICK LMS 200 laser sensor is utilized to test the algorithm on a terrain generated on the Gazebo 3D simulation tool. DEM data from the United States Geographic Services (USGS) was obtained for a region for which free soil information was available in the GRASS GIS project. This DEM data was converted into a height map which was used as input to Gazebo. The effectiveness of the VFH driver in short term path planning was tested out by introducing new obstacles in the form of simulated crates whose presence was not communicated to the grid data fed to the swarm algorithm. A digital cone penetrometer has been simulated as well. Updated soil composition data is fed to the Player client program which parses the soil information into one of the five different categories and suitably moves the robot into safer terrain.

The entire simulation was run on an Intel Core 2 Duo machine with 256 Mb video RAM and 2 GB RAM. Although the swarm algorithm is designed to run with several agents, the number of agents were limited to 10 because Gazebo’s processing power requirements placed constraints on the amount of memory available for other processes to run. Sample soil data is included in Figure 7. Figure 8 has the combined rank due to soil goodness and heigh information. The set of cells with a dark outline indicate the nest. Figure 9 has the initial global path marked on it and the local variations consequent to obstacle avoidance (shown as a dotted line). Snapshots of the Player video are given in Figure 10.

Fig. 9: Global path and local path.


The authors would like to thank Director, CAIR for supporting this work.


  • [1] J.  Laumond, “Robot Motion Planning and Control”, Lecture Notes in Control and Information Sciences, 229, Springer-Verlag New York, Inc., Secaucus, NJ, pp. 305-343, 1998.
  • [2] J.F.  Canny, “The Complexity of Robot Path Planning”, MIT Press, Cambridge, USA, 1988.
  • [3] F.  van  der  Stappen, D.  Halperin, and M.H.  Overmars, “The complexity of the free space for a robot moving amidst fat obstacles”, Journal of Computational Geometry: Theory and Applications, Vol. 3, pp. 353-373, 1993.
  • [4] F.  van  der  Stappen, “Motion Planning amidst Fat Obstacles”, Ph.D. Thesis, Department of Computer Science, Utrecht University, Utrecht, Netherlands, 1994.
  • [5] J.T.  Schwartz and M.  Sharir, “Efficient motion planning algorithms in environments of bounded local complexity”, Report 164, Department of Computer Science, Courant Institute of Mathematical Sciences, New York Univ., New york, 1985.
  • [6] P.  Svestka and M.H.  Overmars, “Motion planning for car-like robots using a probabilistic learning approach”, International Journal of Robotics Research, Vol. 16, pp. 119-143, 1995.
  • [7]

    J.  Barraquand and J.C.  Latomber, “Robot motion planning: A distributed representation approach”,

    International Journal of Robotics Research, Vol. 10, pp. 628-649, 1991.
  • [8] E.  Bonabeau, M.  Dorigo and G.  Theraulaz, “Swarm Intelligence: From Natural to Artificial Systems”, Oxford University Press, 1999.
  • [9] K.  Heero, “Path planning and learning strategies for mobile robots in dynamic partially unknown environments”, Ph.D. Thesis, Department of Computer Science, Tartu University, 2006.
  • [10] K.Z. Haigh and M.M. Veloso, “Planning, Execution and Learning in a Robotic Agent”, Proceedings of the 4th International Conference on Artificial Intelligence Planning Systems, (AIPS), pp. 120-127, 1998.
  • [11] A.  Stentz, “Optimal and Efficient Path Planning for Partially-Known Environments”, Proceedings of IEEE International Conference on Robotics and Automation, San Diego, CA, USA, pp. 3310-3317, 1994.
  • [12] A.  Stentz, “The Focussed D* Algorithm for Real-Time Replanning”, Proceedings of the International Joint Conference on Artificial Intelligence, Montreal, Quebec, Canada, pp. 1652-1659, 1995.
  • [13] H. Choset, K.M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L.E. Kavraki and S. Thrun, “Principles of Robot Motion”, The MIT Press, Boston, USA, 2005.
  • [14] Ken Hughes, Alade Tokuta, N  Ranganathan, “Trulla : an algorithm for path planning among weighted regions by localized propagations”, Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, Raleigh, NC, pp. 469-475, 1992.
  • [15] M.R.  Jahanbin and F.  Fallside, “Path planning using a wave simulation technique in the configuration space”, Artificial Intelligence in Engineering: Robotics and Processes, (ed) J.S. Gero, Elsevier Science Publishing, pp 121-139.
  • [16] J.P.  Wilson and J.C.  Gallant, “Chapter 1”, In J.P. Wilson and J.C. Gallant (eds.): Terrain Analysis: Principles and Applications, Wiley, New York, pp. 1-27, 2000.
  • [17] Rabus,  B., M.  Eineder, A.  Roth and R.  Bamler, “The Shuttle Radar Topography Mission (SRTM)- a new class of digital elevation models acquired by spaceborne radar”, ISPRS Journal of Photogrammetry and Remote Sensing, Vol. 57, pp. 241-262, 2003.
  • [18]
  • [19] A.  Sharma, V.  Vyas and D.  Deodhare, “An Algorithm for Site Selection in GIS based on Swarm Intelligence”,

    Proceedings of the IEEE Congress on Evolutionary Computation

    , pp. 1020-1027, 2006.
  • [20] R.  Kuc and B.  Barshan, “Navigating Vehicles Through an Unstructured Environment With Sonar”, Proceedings of the IEEE International Conference on Robotics and Automation, Scottsdale, Arizona, pp. 1422-1426, 1989.
  • [21] H.P.  Moravec, “Sensor Fusion in Certainty Grids for Mobile Robots”, AI Magazine, Summer 1988, pp. 61-74.
  • [22] J.  Borenstein and Y. Koren, “The Vector Field Histogram - Fast Obstacle Avoidance for Mobile Robots”, IEEE Journal of Robotics and Automation, Vol. 3, pp. 278-288, 1991.
  • [23] J.  Borenstein and Y.  Koren, “Real-time Obstacle Avoidance for Fast Mobile Robots in Cluttered Environments”, IEEE Transactions on Systems, Man, and Cybernetics, Vol. 19, pp. 1179-1187, 1989.
  • [24] I.  Ulrich and J.  Borenstein, “VFH: local obstacle avoidance with look-ahead verification”, Proceedings of the IEEE International Conference on Robotics and Automation, (ICRA), San Francisco, CA, USA, Vol. 3, pp. 2505-2511, 2000.
  • [25] B.A.  Johnson and R.M.  Beard, “A Lightweight 12-m Cone Penetrometer”, Strength Testing of Marine Sediments: Laboratory and In-situ Measurements, ASTM STP 883, R.C. Chaney and K.R. Demars, Eds., American Society for Testing and Materials, Philadelphia, pp. 125-139, 1985.
  • [26] J.V.  Perumpral, “Cone penetrometer applications: A Review”, Transactions of the American Society of Agricultural Engineers, ASAE, Beltsville, USA, Vol. 30, pp. 939-944, 1987.
  • [27] B.  Gerkey, R.  Vaughan and A.  Howard, “The Player/Stage project: Tools for Multi-robot and Distributed Sensor Systems”, Proceedings of the 11th International Conference on Advanced Robotics, (ICAR), pp. 317-323, 2003.