Wavefront expansion algorithm explained

The wavefront expansion algorithm is a specialized potential field path planner with breadth-first search to avoid local minima.^{[1] [2]} It uses a growing circle around the robot. The nearest neighbors are analyzed first and then the radius of the circle is extended to distant regions.^[3]

Motivation

Before a robot is able to navigate a map it needs a plan.^[4] The plan is a trajectory from start to goal and describes, for each moment in time and each position in the map, the robot's next action. Path planning is solved by many different algorithms, which can be categorised as sampling-based and heuristics-based approaches.

Before path planning, the map is discretized into a grid. The vector information is converted into a 2D array and stored in memory. The potential field path planning algorithm determines the direction of the robot for each cell. This direction field is shown overlaid on the robotic map containing the robot and the obstacles. The question for the potential field algorithm is: which cell is labeled with which direction? This can be answered with a sampling-based algorithm.

Wavefront expansion

A sampling-based planner works by searching the graph. In the case of path planning, the graph contains the spatial nodes which can be observed by the robot. The wavefront expansion increases the performance of the search by analyzing only nodes near the robot. The decision is made on a geometrical level which is equal to breadth-first search.^[5] That means, it uses metrics like distances from obstacles and gradient search for the path planning algorithm.

The algorithm includes a cost function as an additional heuristic for path planning.^[6]

Implementation

Practical open-source implementations of the algorithm are available. The map of the world is provided as an array. Obstacles and the start position of the robot are given by special values in the array. The solver determines the goal direction in the imagined wave.

Existing implementations use a queue to store a wave data structure created around the robot. A typical implementation in Python can be realized in around 200 lines of code.

Notes and References

1. A Feedback Motion Plan for Vehicles with Bounded Curvature Constraints . Miraglia, Giovanni and Hook, IV . 1910.06460 . 2019. cs.RO .
2. Fast trajectory planning for multiple site surveillance through moving obstacles and wind . Soulignac, Michael and Taillibert, Patrick . Proceedings of the Workshop of the UK Planning and Scheduling Special Interest Group . 25–33 . 2006.
3. Collective construction of numerical potential fields for the foraging problem . Simonin, Olivier and Charpillet, Francois and Thierry, Eric . 2007 . Research Report RR-6171, Inria-00143302v1.
4. 10.1163/156856306777924671 . 2006 . Brill . 3 . 3 . 285–306 . Chytra Pawashe and Metin Sitti . Two-dimensional vision-based autonomous microparticle manipulation using a nanoprobe . Journal of Micromechatronics. 18241136 . free .
5. 10.2514/6.2009-6113 . 2009 . American Institute of Aeronautics and Astronautics . Anjan Chakrabarty and Jack Langelaan . Energy Maps for Long-Range Path Planning for Small- and Micro- UAVs . AIAA Guidance, Navigation, and Control Conference .
6. 10.1109/tro.2010.2085790 . 2011 . Institute of Electrical and Electronics Engineers (IEEE) . 27 . 1 . 89–98 . Michaël Soulignac . Feasible and Optimal Path Planning in Strong Current Fields . IEEE Transactions on Robotics . 17622757 .