The goal of the project is to compute Optimal Feedback Motion Strategies for a holonomic or a nonholonomic robot in a static workspace. An Optimal Navigation function is computed for a car-like robot in a two-dimensional configuration space. The input is a polygonal description of an environment, a goal region and position and orientation of a car-like robot in it. It is assumed that the car-like robot has a maximum turning angle of forty five degrees. The output is a continuous collision-free path for the car-like robot to its goal. It is also assumed that the workspace is static, that is the obstacles are stationary. Also, the robot is assumed to know the correct description of the configuration space without any sensory defects. There are no velocity constraints on the robot and the robot can start and stop instantaneously.
This concept of computing optimal navigation functions is introduced in the paper "Numerical Computation of Optimal Navigation Functions on a Simplical Complex" by Steven M. La Valle. Also, a complete solution for a point-robot in a two dimensional configuration space was presented in the paper.
Implementation
The solution involves computing a navigation function for the car-like robot. The computation is very much similar to that followed by Dijkstra's algorithm for graphs, except that this approach applies to continuous spaces with geometric and nonholonomic constraints. A cost-to-go function is computed for the robot. The robot is allowed to move a unit distance at each step. This function gives the cost-to-go (or the loss) for a robot, depending on the number of steps it needs to reach the goal.
First, a configuration space is computed for the given environment and a robot. The workspace is discretized along each axis. A resolution of 100x100x30(x,y,orientation) is used. All the configurations of the robot for which it is in goal region or in free space or in collision with an obstacle are determined and are masked suitably. Then the space is initialized as follows: a loss value of zero is assigned to each point in the goal region(with a goal mask), infinite(some large number) to all other points.
After the initialization, the cost-to-go(loss) for each configuration is computed. All the neighbors of a configuration having least loss value are selected and a unit step is applied to each of them(in all the possible ways the robot can move in). Then, a loss value of the point that it lands in, incremented by the loss to make this step, is assigned to the point. The loss value at the point that the robot lands in is computed by interpolating with the loss values of all the neighboring configurations of that point. The 3D space(x,y,orientation) is divided in to tetrahedrons and so the interpolation needs to be done only with four neighboring configurations instead of eight. This reduces the complexity of the computation from exponential to polynomial. This phase gives the loss functional for all the configurations. The following figure is the plot of the loss functional for a point-robot at all the points in a simple configuration space, with no obstacles,at an orientation of zero degrees(ie., the car is horizontal).
In this plot, the black region at the centre is the goal
and each curve corresponds to a particular loss value. It can be seen that
the loss value at points away from the goal is higher as it takes more
steps to reach the goal. The following figure shows the case where there
are points from which the goal can not be reached at all.
Figure 2 shows that the solution computes a motion strategy and not a path, in the sense that the robot realizes that it is not possible for it to go the goal and does not move at all(the loss is infinite). In the above figure, a non-point-robot is considered. It can be seen that for all the points outside the obstacles surrounding the goal, the loss is infinite.
After computing the loss functional, given a robot at
any point and at any orientation in the space, its path to the goal can
be computed. At each stage, the robot, tries to go in all possible ways
and goes to a point with minimum loss. It continues to do so untill it
reaches its goal. Thus in this motion strategy, the main work is done in
initializing the space and computing the loss functional. If the goal and
the configuration space remains unchanged, the path for the robot at any
point and configuration can be computed in no time.
Performance
This strategy is resolution sensitive and the computed path depends very much on the resolution sensitive. The finer the resolution, the better is the path but more is the computation time. Also, the implementation is done for a robot with fixed dimensions. The solution can be easily computed for any other robot too by changing the code suitably. Also, the obstacles, the robot and the goal is assumed to be convex polygons. The solution can be computed with non-convex polygons too by first converting each non-convex polygon to equivalent convex polygons.
The strategy is very efficient for a reasonably good resolution. Also, at higher dimensions the efficeincy is even better due to the fact that the computational complexity is polynomial as opposed to exponential if a grid is used. The division of an n-dimensional space into (n+1)-dimensional units helps to keep the complexity low.
It must be noted that at poor resolutions, the stategy may fail to compute the path even though a path exists. In such cases, choosing a better resolution solves the problem.
References
In all the examples below, the black region is the goal region, grey regions are the obstacles and the red polygon is the car-like robot.
Example 1: Example where a simple path exists.
Example2: A cluttered environment.
Implementation Files
Source Code
Makefile
main.C
init.C
contdp.C
view.C
defs.C
defs.h
contdp.h
Input Data Files