The Navigation Algorithm
This section describes the navigation algorithm in detail.
A global planner problem in the Isaac framework is decomposed in three classes: the Planner Model, the Visibility Graph Algorithm, and the Optimizer.
A planner model (
sdk/packages/path_planner/gems/planner_model.hpp) must provide the following:
- A set of functions that provide the information regarding whether or not a given state is collision free.
- Information on whether or not a direct path (an easy path such as straight line) exists between two states and is collision free.
- The distance, or length of the path.
- A way to randomly sample in the space of states.
In the case of the carter platform, a differential base is approximated as circular, allowing fast collision detection using a distance map. A direct path is defined as a short path (< 2m) in straight line (as we can always rotate in the direction). Therefore the planning problem is a 2D problem.
Visibility Graph Algorithm
Inspired from the paper of T. SIMÉON, J.-P. LAUMOND and C. NISSOUX, “Visibility-based probabilistic roadmaps for motion planning”, the visibility graph algorithm provides a very generic algorithm to find a path in a high-dimension space. The goal is to produce a small graph with high visibility coverage.
The graph is built by keeping a set of points (called guard in the paper) that cannot directly be connected to each other. Connections are added whenever there is an intermediate state that directly connects two guards that were not connected by any path.
The Isaac implementation adds a connection between guards not connected by a path of size 2 using only one middle state. This produces a bigger but higher-quality graph.
Once the graph is built, the shortest path can be computed by first finding a connection between these states and a guard and then running a Djikstra algorithm on the graph. The same graph can be pre-computed, manually assisted in case of difficult problems and reused for other shortest path requests as long as the environment is static.
For better performance, build a dense graph by increasing the number of random samples.
The final state is path optimization. The visibility graph during quick path production produces a very chaotic path. A better quality path is then computed using a short-cut: two waypoints are selected randomly and if a direct path between them exists, then all waypoints in between are bypassed. In addition, waypoints to close to an obstacle are moved away from the closest obstacle.
The local planner of Isaac is Linear Quadratic Regulator (LQR) based. Isaac SDK provides a customizable LQR solver. The dynamics of the system as well as the cost function need to be provided to the LQR solver which perform an iterative gradient descent using a linear search to find the best path.
In the case of carter platform the dynamics of the system are those of a differential base:
- \(x(t)\): X position of the base
- \(y(t)\): Y position of the base
- \(\theta (t)\): Orientation of the base
- \(v(t)\): Linear velocity
- \(\theta '(t)\): Angular velocity
- \(al(t)\): Left wheel angular acceleration
- \(ar(t)\): Right wheel angular acceleration
The dynamics are then given by the formula (L is the base length and R the wheel radius):
- \(x'(t) = v(t) \cos( \theta(t) )\)
- \(y'(t) = v(t) \sin( \theta(t) )\)
- \(\theta'(t) = \theta'(t)\)
- \(v'(t) = a(t) = (ar(t) + al(t)) \cdot R / 2\)
- \(\theta''(t) = (ar(t) - al(t)) \cdot R / L\)
Here is an example of a path produced by carter in the local view:
The local map is computed in real time as well as the distance map. In red is the path provided by the global planner, and in blue is the plan produced by the LQR, optimizing speed, distance to obstacle, acceleration, and other factors.