ComS 476 Motion Strategy Project
The Stanek Marks Anticipating
Rapidly Exploring Random Tree
( The SMARRT Algorithm )

by Jason Stanek and Matthew Marks

May 6, 2000


Project Description

Project Focus

The project focuses on an pseudo algorithm for a 2D Pursuer robot that attempts to reach a mobile Goal Position.  The Pursuer moves in a 2D world with translating and rotating objects, where the paths of all obstacles are predictable.  The initial location of the Goal Position is determined by the user of the program, and can be constantly moved.  Since the goal is a location, it can exist anywhere in the C-space, including inside the bounds of obstacles.

Both the Pursuer and Goal Position move at equal, pre-defined distances, per each increment of time.

The desired effect of the application is that the Pursuer is able to reach the Goal Position in any 2D environment that is solvable.  The Goal Position is arrived at by using a combination of RRTs, Anticipation of the Goal Position's movements, and near-sighted, line-of-sight movements.  The Pursuer has complete knowledge of all objects in the world and the location of the Goal Position.

We have named this the SMARRT algorithm (Stanek Marks Anticipating Rapidly Exploring Random Tree).

Critical Events

Since the Goal Position can be stationary or moving, we decided continuously build small RRTs until the Goal Position is reached, as opposed to creating one large RRT that lead from the initial position of the Pursuer to the Goal Position.  The reasoning for this was, by the time the Pursuer traveled a branch of the RRT, all the way to the position where the Goal Position had initially been, it is highly possible that the Goal Position will have moved.  The path towards a moving Goal Position has tended to be more accurate by using a series of smaller RRT's that don't cover a large amount of Cfree, but enough area is covered to move the Pursuer robot in the general location of its goal.  What this calls for is a dynamic RRT.  For this purpose, we built an RRT limited in size and biased toward the Goal Position.  This smaller RRT provides near-sighted knowledge of
how to begin moving toward the Goal Position.  After a few moves, the branches on the RRT that the robot did not travel on are recycled and used to extend the current branch further toward the Goal Position.  This method, alone, tends to work well for worlds where the paths to the Goal Position are straight-forward with obstacles that do not trap the Pursuer.

If the Pursuer is in a world where the Pursuer can get stuck among obstacles, such that it can not get any closer to the Goal Position by recreating the small RRT, then the Pursuer changes tactics.  Instead of making an RRT of limited size, the Pursuer begins to add additional nodes to the RRT with the intention of covering as much of the world as necessary until it finds a path that is closer to the Goal Position. This technique tends to help push the Pursuer out of a position that a limited-in-size RRT can not do, as might happen in a non convex polygon.  The following images display an example of when the creation of a larger RRT would be necessary.  Also, the Computed Example, Sample 1, gives a running example of this tactic.

Build larger RRT --> 

These two tactics of the Pursuer enable it to reach the Goal Position very consistently.  There are additional features of the algorithm which improves its ability to "catch" the Goal Position which will be detailed below, but the above is what is essential.

The Pursuer has a modest calculation for anticipating the Goal Position's movements.  The Pursuer keeps track of the general direction the Goal Position has been traveling in, and plots where the Goal Position will be if it continues in the same direction.
This anticipated position is what the RRT is biased toward, when the Pursuer robot is a significant distance away from the Goal Position.  As the Pursuer gets closer to the Goal Position, the RRT changes to be biased toward the actual point of the Goal Position.

Now there can be a conflict between the anticipation of the location of the Goal Position, and the tactic of the RRT expanding throughout Cfree when the Pursuer can not get closer to the Goal Position.  The reason for this conflict is as follows:  If, as in the two images above, the Pursuer gets caught in an obstacle and can not move closer to the Goal Position, it expands its RRT.  However, if the Goal Position was to move up and down slightly, then the Pursuer would always be able to move closer to the Goal Position by small increments, without ever getting out of the obstacle. And so, the Pursuer would never reach the Goal Position.

To avoid this conflict, a record of the location of the Goal Position is maintained.  Then, before the Pursuer builds its biased RRT, a calculation is done to determine if the past movements of the Goal Position have moved a significant amount, or if all of the Goal Position's movements have kept it in the same local area.    If the Goal Position has been moving by noticeable amounts, everything is done as expected.  Otherwise, the back and forth movements of the Goal Position are ignored, and the goal is considered stationary.  Since the Goal Position appears stationary, if the Pursuer gets caught in an obstacle, the giggling motion of the Goal Position will not prevent the Pursuer from finding a way out of the obstacle and toward the goal.

Algorithm Analysis

The Stanek Marks Anticipating Rapidly Exploring Random Tree ( The SMARRT Algorithm )
The whole of the SMARRT algorithm works by the following pseudo code.

Track_Goal_Position( )

     Initialize_RRT( small_number_of_nodes );

     While ( Pursuer has not reached the Goal_Position )

          If ( Pursuer is very close to Goal_Position and in line-of-sight )
                Move_Directly_Toward_Goal_Position ( );

                 If ( Pursuer can get closer to the predicted area Goal_Position will be in )
                        Move_Pursuer_Closer( );
                        Build_New_RRT ( );

                Else //Can not get any closer to the goal.
                        Add_Nodes_2_RRT( );
                        While (Pursuer is not any closer to the Goal_Position )
                                 Add_Nodes_to_RRT( );

                        While ( not (Number_Nodes_In_RRT   equal   small_number_of_nodes)  )
                                 Move_Pursuer_Closer( );
                                 Remove_Nodes_From_RRT_That_Are_Behind_Pursuer ( );

End Track_Goal_Position.

What may be implemented next?

Project Assumptions

All worlds have a computable solution.  There are no current upper bounds for the size of an RRT that is created in Cfree, it is the system's resources and the complexity of the map of the world that limits what the SMARRT algorithm can do.

What Was Most Difficult?

    Determining an accurate metric for the RRT.  After some calculations and tests, we constructed a metric which is valid for the short RRT, long RRT and the idea of anticipating the goal's movements.  This worked well when working with static obstacles.  Once we moved to the next step, where we were using simulations with predictable moving obstacles, the metric was no longer sufficient.  We devised a way that the final application's metric also incorporated time.

How Well Does The Application Work?

The SMARRT algorithm has performed beyond our expectations.  In cases where there is a solution available for a Pursuing robot to reach a moving goal, the application has succeeded.  Some times this success requires a significant amount of waiting, but the program still succeeds.

Computed Examples
    (My apologies for the large size of the examples.  From the ISU campus, the download time is minimal.  Unfortunately, I can't say the same for any place else.)

Sample 1.  size: 669k
    Nearly stationary Goal Position, where the Pursuer is momentarily caught in an obstacle.

Sample 2.  size: 460k
    Small example of the Pursuer using anticipation to locate where the Goal Position might move toward.

Sample 3.  size: 752k
    Example with moving obstacles.

Sample 4.  size: 564k
    Another example with moving obstacles.

Implementation Files

Source Code:

Input Data Files:

with props to grif