Project Description :

** Problem Statement**:

This project explores the problem
of motion planning in a flow field. A *point mass submarine*
is placed in a makeshift underwater world with obstacles. The information
about the world is already known, namely the position and dimensions of
the obstacles, the boundaries of the world, as well as the flow field that
has been precomputed using CFD Analysis.

The configuration space for the problem is R3. However, the path planning implementation is in the state space which is R6. The state space variables are x,y,and z-positions and the x,y and z velocities.

The robot running the submarine, needs to compute a path from a chosen start point to within a destination zone. There are six thrusters on the submarine, each of which moves it in a different direction(+ve x, y and z, and -ve x, y and z). The force provided by the thrusters is fixed (constant). The thrusters can also be used in combinations of two or three. A default option exists, not to fire any of the thrusters, if the action of the flow field is, by itself, enough to carry the submarine in the direction where it wants to go.

The robot must calculate its path, taking into consideration the obstacles(collision detection), and the constraints, of being able to fire only one, two, or three thrusters at a time. Also the action of the flow field should also be taken into account, as the flow could either retard or accelerate the motion of the sub, or, in regions of zero velocity, have no effect.

The interaction with this application
as well as the visualization of the the motion of the submarine along the
calculated path will be in the C2 virtual environment.

__Methodology__

The basic path planning strategy used, is the Rapidly Exploring Random Tree (LaValle) or RRT (1).The collision detection algorithm utilised here is the simple bounding box method. Since the obstacles provided here are cuboidal, this method suffices. In more complex cases, the bitmap method is recommended.

The flow field is generated using the FLUENT CFD software and is converted to PLOT3D data format. The data is read in as a structured grid of points (71 x 17 x 31). The Visualization Toolkit software is used to read in the grid.

The algorithm is given a start state
(x_{init}, y_{init},z_{init},vx_{init},vy_{init},vz_{init},),
and a goal state (x_{goal}, y_{goal},z_{goal},vx_{goal},
vy_{goal},vz_{goal}). The Random Tree starts off
at the initial state, proceeds to choose the random point and expand
the tree, (for details see the link on RRT). As mentioned earlier the submarine
has six thrusters, that can be fired in combinations of one, two, three
or none at all. The decision on which thruster to fire is done by computing
the state of the submarine as a result of firing the thruster , and
then calculating the distance from the random point chosen by the RRT.
The distance metric used here is the Euclidean metric.

This can be applied in two ways
a) across the **3D distance** and checks which combination brings the
sub closest to the random ** point** or b)across the

__Dynamics:__

When a goal point is picked, the RRT scans all the nodes to find the closest point. Then the dynamics of the system have to be applied in the case of each thruster, to calculate the final position of the point mass(sub).

Let the closest point be denoted
by **x**_{curr}, where x is a 6D vector containing position
and velocity.

Let** C** be a 3D vector containg
the force components of the thrusters, **VF **contain the fluid velocity
components at the point, and **a**_{fin} be the final acceleration
vector.

We then have **a**_{fin}
= **C**/m + Cd***VF**/m -- (Eq 1.), where m is the mass
(1.0) and Cd is the local drag coefficient. This is calculated as the 1/sqrt(Re),
where Re is the Reynolds number. For further details see (2).

Eq. 1 is integrated to give the
velocity vector **v**_{fin}, which is integrated again to give
the final position **p**_{fin} . Integration is done using the
classic fourth-order Runge-Kutta integration. The components of **v**_{fin}
and **p**_{fin} go into the state space vector for the new node.

The default action used is to set
all thrusters to the off state and then compute the poistion based purely
on the flow, If the flow takes the poistion closer to the random point
than the *current distance *of the random point from the closest point,
then no other thruster is fired and the sub is carried with the flow.

Once the tree is built, the closest
point to the destination point is found, and the path is traced back along
the tree to the initial starting position.

Effectiveness:

Since the RRT method is only probabilistically complete (especially after the dynamics and collision checks are implemented), we will see that there are situations where the RRT will fail to converge to within the specified distance to the goal state even after a large number of nodes have been explored. Since the RRT has to go over all the nodes every time a new random point is picked, the computation time will increase with every node that is added.

Situations where the RRT will fail to converge are

1) Where the start or destination
position are placed too close to an obstacle. In this case there are two
possibilities of failure

a) The destination
point cannot be reached easily without collision irrespective of where
the randomly chosen point lies.

b) There are
vortices formed near the obstacles, which prevent the point mass from moving
across them to reach the destination point. In other words, the thruster
combinations would not be able to overcome the flow, and will hence be
offset by the flow to different positions away from the destination.

2) The destination point lies in the middle of a vortex, which will cause the same effect as b)

3) The number of nodes are too small,
or the incremental distance is too small, or the integration step size,
is either too large or too small. The interaction mechanisms provided in
the application, will help overcome this problem.

4) The source point is in a space where there are too
many collisions.

**Computed Examples**

__Snapshots of the application__

__Animated examples:__

( Hit the reload button on browser to replay the animation. The animation has been put in slow motion to allow a (hopefully) clearer visualization of the movement of the submarine and the thruster configurations )

Case 1: A simple case (Goal bias
20 %)

Here the start and destination point are in the open,
unobstructed by any obstacles. However, there is still the flow field to
be taken into consideration. This means that the sub, cannot neccesarily
go directly towards the random point picked. This is why the path is wiggly
in places. However, the start and destination points lie downstream of
each other, hence the sub moves with the flow from time to time, not requiring
the thrusters to be fired often. The RRT picks the goal state 20% of the
time.

Case 2: A case where the start
and destination point are obscured by the obstacles, but convergence still
occurs

Here, it was seen that the path did converge,
although the time taken to converge was more. This could have been due
to the large number of collisions that had to be overcome, due to the goal
bias. Also it took a larger number of nodes to converge than for the previous
case. The constraining factors here could be attributed to the vortex pockets
in the vicinity downstream of the obstacle.

Case 3: A tough case. No convergence:

This case shows the start position and the goal position
in totally obscured portions of the space. The region where the goal lies
is in between the face of the obstacle and the exit. It was found that
convergence could not occur even when the velocity constraints and fluid
field were turned off(holonomic case). This is because it was not possible
for the tree to reach the goal easily without colliding. It took a large
number of node iterations(about 7000) to achieve convergence in the holonomic
case. The velocity field in the vicinity of this region tends heavily
towards pushing the submarine in the x direction, and thereby preventing
feasible motion in the y and z direction. Hence, in the non holonomic case,
it was not possible to find a solution.

Interaction

Interaction with the application is through the C2 (CAVE) Surround Screen VR device. This can be operated in simulator mode, to provide desktop interaction. However, the major interface will be inside the actual C2. The hardware used for the interaction is the wand and track glasses (mouse pad and keys in desktop mode). The software used for the interaction is the C2 libraries. OpenGL is used to perform all the rendering. The parameter adjustment, and activation of the path planning algorithms are done through a menu interfacing system. The user interactively picks a start and destination point, and then adjusts parameters like the the no. of nodes, etc, after which, the RRT generating algorithm is activated. The user can then display the tree, or path, and perform the animation of the submarine through the flow field. There are two types of animation, walk through and continous. In the former, the user can step through the animation by pressing down on the wand buttons, and in the latter, can watch the sub make its way through at a stretch.

Conclusions and Future Work

The application was demonstrated and run in the C2. Also,
experimentation was done with different cases by adjusting the different
parameters to find the path that best converged.

There are several avenues for future work and improvement.
Some of which are illustrated below

Performance and interaction could be improved by parallel
processing.

The number of degrees of freedom in the application could
be extended.(To model a real prototype submarine, with 6 degrees of freedom
(x,y,z,yaw,pitch,roll) ).

The obstacles could be made non convex (Improved Collision Detection algorithms).

Implementation Files

Source Code:

Makefile

main.C

NHRRT.C

NHRRT.h

RRT.C

RRT.h

Input Data Files:

Obstacle File: This contains
the number of obstacles and the starting and ending points of the bounding
box for them

f001.dat,f001.grd: These
are data files for the CFD data. Since they are too large, they have been
compressed and stored in the archive.

References:

1. Rapidly Exploring Random Trees, Steven LaValle

2. Fundamentals of Fluid Mechanics Munson, B.F, Okiishi, T.F, Young, D.F, 2nd Ed. John Wiley and sons, Inc., NewYork, N.Y., 1994