State lattice based planning
The kinodynamics constraints of the robot are encoded in the state lattice graph, and any path in this graph is feasible. After constructing the graph, any graph search algorithm can be used for planning.
A robot's configuration space is usually discretized to reduce the computational complexity of planning at the expense of completeness. However, it is difficult to search this space while satisfying the robot's differential constraints. State lattices are a special way of discretization of robot state space that ensures (by construction) that any path in the graph complies with the robot's constraints, thereby eliminating the need to consider them explicitly during planning.
The state lattice is specified by regular sampling of nodes in the state space and edges between them. Edges correspond to feasible and local paths between nodes (also called motion primitives or control set). Since the state lattice is a directed graph, any graph search algorithm is appropriate for finding a path in it.
Each lattice node represents a state in the configuration space of the robot. For example, we can consider a 3-dimensional state that includes 2D position and heading for a mobile robot.
Additional dimensions can be added to impose continuity constraints on the path. For example, we can add the robot's curvature as the 4th dimension to ensure that its curvature never changes suddenly at nodes. In this case, the curvature will slowly change along the edges.
The state lattice-based planner is resolution complete only if any path between two nodes in the lattice can be constructed using edges present in the graph. An important property is that we can achieve this using only a finite (and small in practice) set of motion primitives.
Outline of proof: Assume that two paths with identical endpoints which are "sufficiently" close together to be equivalent. This equivalence is motivated by practical applications where there is a certain error in path following, and so close paths are same in practice. Now, any path between two nodes (which may be infinitely far away) can be decomposed into an equivalent path that is composed using a finite set of motion primitives.
See paper for details on generating motion primitives, sampling heading direction and computing more informed heuristic for the chosen primitive set.
- Efficient constrained path planning via search in state lattices - Pivtoraiko and Kelly
- Differentially Constrained Mobile Robot Motion Planning in State Lattices - Pivtoraiko, Knepper and Kelly
- SBPL slides - Likachev