In this article by Richard Grimmett, the author of the book Raspberry Pi Robotic Blueprints, we will see how to do dynamic path planning. Dynamic path planning simply means that you don't have a knowledge of the entire world with all the possible barriers before you encounter them. Your robot will have to decide how to proceed while it is in motion. This can be a complex topic, but there are some basics that you can start to understand and apply as you ask your robot to move around in its environment. Let's first address the problem of where you want to go and need to execute a path without barriers and then add in the barriers.

(For more resources related to this topic, see here.)

Basic path planning

In order to talk about dynamic path planning—planning a path where you don't know what barriers you might encounter—you'll need a framework to understand where your robot is as well as to determine the location of the goal. One common framework is an x-y grid. Here is a drawing of such a grid:

There are three key points to remember, as follows:

1. The lower left point is a fixed reference position. The directions x and y are also fixed and all the other positions will be measured with respect to this position and directions.
2. Another important point is the starting location of your robot. Your robot will then keep track of its location using its x coordinate or position with respect to some fixed reference position in the x direction and its y coordinate or position with respect to some fixed reference position in the y direction to the goal. It will use the compass to keep track of these directions.
3. The third important point is the position of the goal, also given in x and y coordinates with respect to the fixed reference position. If you know the starting location and angle of your robot, then you can plan an optimum (shortest distance) path to this goal. To do this, you can use the goal location and robot location and some fairly simple math to calculate the distance and angle from the robot to the goal.

To calculate the distance, use the following equation:

You can use the preceding equation to tell your robot how far to travel to the goal. The following equation will tell your robot the angle at which it needs to travel:

Thefollowing is a graphical representation of the two pieces of information that we just saw:

Now that you have a goal, angle, and distance, you can program your robot to move. To do this, you will write a program to do the path planning and call the movement functions that you created earlier in this article. You will need, however, to know the distance that your robot travels in a set of time so that you can tell your robot in time units, not distance units, how far to travel.

You'll also need to be able to translate the distance that might be covered by your robot in a turn; however, this distance may be so small as to be of no importance. If you know the angle and distance, then you can move your robot to the goal.

The following are the steps that you will program:

1. Calculate the distance in units that your robot will need to travel to reach the goal. Convert this to the number of steps to achieve this distance.
2. Calculate the angle that your robot will need to travel to reach the goal. You'll use the compass and your robot turn functions to achieve this angle.
3. Now call the step functions for the proper number of times to move your robot for the correct distance.

This is it. Now, we will use some very simple python code that executes this using functions to move the robot forward and turn it. In this case, it makes sense to create a file called robotLib.py with all of the functions that do the actual settings to step the biped robot forward and turn the robot. You'll then import these functions using the from robotLib import * statement and your python program can call these functions. This makes the path planning python program smaller and more manageable. You'll do the same thing with the compass program using the command: from compass import *.

For more information on how to import the functions from one python file to another, see http://www.tutorialspoint.com/python/python_modules.htm.

The following is a listing of the program:

In this program, the user enters the goal location, and the robot decides the shortest direction to the desired angle by reading the angle. To make it simple, the robot is placed in the grid heading in the direction of an angle of 0. If the goal angle is less than 180 degrees, the robot will turn right. If it is greater than 180 degrees, the robot will turn left. The robot turns until the desired angle and its measured angle are within a few degrees. Then the robot takes the number of steps in order to reach the goal.

Avoiding Obstacles

Planning paths without obstacles is, as has been shown, quite easy. However, it becomes a bit more challenging when your robot needs to walk around the obstacles. Let's look at the case where there is an obstacle in the path that you calculated previously. It might look as follows:

You can still use the same path planning algorithm to find the starting angle; however, you will now need to use your sonar sensor to detect the obstacle. When your sonar sensor detects the obstacle, you will need to stop and recalculate a path to avoid the barrier, then recalculate the desired path to the goal. One very simple way to do this is when your robot senses a barrier, turn right at 90 degrees, go a fixed distance, and then recalculate the optimum path. When you turn back to move toward the target, you will move along the optimum path if you sense no barrier.

However, if your robot encounters the obstacle again, it will repeat the process until it reaches the goal. In this case, using these rules, the robot will travel the following path:

To sense the barrier, you will use the library calls to the sensor. You're going to add more accuracy with this robot using the compass to determine your angle. You will do this by importing the compass capability using from compass import *. You will also be using the time library and time.sleep command to add a delay between the different statements in the code. You will need to change your track.py library so that the commands don't have a fixed ending time, as follows:

Here is the first part of this code, two functions that provide the capability to turn to a known angle using the compass, and a function to calculate the distance and angle to turn the tracked vehicle to that angle:

The second part of this code shows the main loop. The user enters the robot's current position and the desired end position in x and y coordinates. The code that calculates the angle and distance starts the robot on its way. If a barrier is sensed, the unit turns at 90 degrees, goes for two distance units, and then recalculates the path to the end goal, as shown in the following screenshot:

Now, this algorithm is quite simple; however, there are others that have much more complex responses to the barriers. You can also see that by adding the sonar sensors to the sides, your robot could actually sense when the barrier has ended. You could also provide more complex decision processes about which way to turn to avoid an object. Again, there are many different path finding algorithms. See http://www.academia.edu/837604/A_Simple_Local_Path_Planning_Algorithm_for_Autonomous_Mobile_Robots for an example of this. These more complex algorithms can be explored using the basic functionality that you have built in this article.

Summary

We have seen how to add path planning to your tracked robot's capability. Your tracked robot can now not only move from point A to point B, but can also avoid the barriers that might be in the way.

Resources for Article:

Further resources on this subject:

• Debugging Applications with PDB and Log Files[article]
• Develop a Digital Clock[article]
• Color and motion finding [article]