Path planning with occupancy grid maps
1. Description
The basic value iteration [1] algorithm for searching shortest paths is implemented in the MRPT for occupancy grids, and circular robots, in the class mrpt::nav::PlannerSimple2D.
The method comprises two steps:
- Growth of the obstacles by the robot radius. This assure that just one single free cell is enough for the robot to move without collision.
- The value iteration algorithm, starting at the source position, increase iteratively the area covered by shortest paths until the target cell is reached.
Note that this is a very simple method, not suitable for robots with shapes very different from circular and/or moving in cluttered environmnets. For those cases, see the obstacle avoidance methods.
2. Usage
The basic usage requires declaring the gridmap, the mrpt::nav::PlannerSimple2D object, setting the robot radius, and invoking mrpt::nav::PlannerSimple2D::computePath(). The following code fragment is taken from the example MRPT/samples/pathPlanning.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 |
// Load,create,... the grid map: COccupancyGridMap2D gridmap; // ... // Find path: CPathPlanningCircularRobot pathPlanning; pathPlanning.robotRadius = 0.30f; std::deque thePath; bool notFound; CPoint2D origin( 20, -110 ); CPoint2D target( 90, 40 ); pathPlanning.computePath( &gridmap, &origin, &target, thePath, notFound, 100.0f /* Max. distance */ ); // Process the output path in "thePath" // .... |
3. Result
Click on the thumb to see full-resolution image. This is the output from the example pathPlanning
:
References
- [1] Value iteration algorithm (Wikipedia) – http://en.wikipedia.org/wiki/Markov_decision_process#Value_iteration.