In this motion planning problem we are given a robot in a factory, whose job is to deliver parts from source regions to destination regions. There can be any number of parts, sources, and destination regions. When the factory is running, requests for part deliveries come up at random. A request consists of a part, a source, and a destination. For example, the robot may be asked to go to source #1 to pick up part #1 and deliver it to destination #2.

At the time that the motion strategy is computed, the robot doesn't know which requests will be made, or what the order of the requests will be. Since the "goal" is not know at the time the strategy is computed, this is an uncertainty in prediction problem. The way that this problem is solved is to come up with a function that will tell the robot where to go for any request and for any position that it may be in the factory. This function must take into account the fact that the robot may be able to go through passages when it is going to pick up a part that it may not fit through when it is carrying the part to the destination.

In general, this problem could contain any type of robot (articulated, car-like, etc.) and any number of parts, sources, and destinations. However, I made several assumptions to make the solution easier and feasible to solve on the computers I had available. First of all, I assumed that the robot is only able to translate in a 2-D world. I also assumed that there were only two sources, two destinations, and one part. The last main assumption that I made was that the robot can pass through the source and destination regions (they are not like obstacles) and when picking up / dropping off a part, the robot only has to touch the outside of the source / destination region. The transfer of the part takes place immediately.

To solve this problem, we divide the problem into different layers of a state space. Each action that the robot is asked to perform has its own layer of the state space. Examples of actions include: pick up the part at source #1, deliver part to destination #2, etc. The "no request mode" also has a layer of the state space. The robot is in no request mode when it doesn't have any requests to deliver parts. A function is found for each of the layers of the state space that tells the robot what to do if it is in that particular layer. We call this function the loss function. The loss function for each (x,y) position that the robot can have in the world represents the time that it would take for the robot to complete its action for that layer from the particular (x,y) position. We pre-compute the loss function for each position of each layer of the state space using dynamic programming. After we have all the losses, the robot is guided through the state space by choosing the action that will minimize the losses. This method minimizes the time that parts wait to be delivered. For a detailed explanation of this method, see the papers written by Steve LaValle et. al., listed at the end of this page.

This method is resolution complete. It uses a bitmap representation of the world to store the losses and also the positions of the obstacles, sources, and destinations. The finer this bitmap is, the more likely the method is to find a solution if one exists. If the bitmap is too coarse, the method will not find a solution, no matter how easy the problem is. The problem with increasing the size of the bitmap is that if the size is doubled, the number of computations that must be done is squared. This program must loop through the bitmap for each layer multiple times so its speed is very dependent on the dimension of the bitmap. The resolution of this program also depends on the time step that is chosen to calculate the loss function. A large time step results in lower resolution.

As already mentioned, the speed of this algorithm depends on the size of the bitmap. The speed decreases exponentially as the dimension of the configuration space increases for the problem. The speed of the algorithm also decreases linearly as the number of parts, sources, or destinations is increased. The amount of time the algorithm needs to converge depends somewhat on the complexity of the world. Because of the dynamic programming principle, the losses slowly propagate out from the source and destination regions, with only a few more of the bitmap cells being affected each time step. The pre-processing is complete after the losses have "spread" throughout the world, causing the actions to remain the same for the remaining time steps. The spreading of the loss values would take longer for a world with a maze of obstacles than it would for one with many connected open regions, thus taking the algorithm longer to converge.

There are three main tasks to implement this algorithm. The first one is to read in the data for the world, parts, sources, destinations, and obstacles and divide the problem up into several layers of a state space. This data must be organized in a way to make the rest of the implementation easier. The second task is to compute the losses for each point on the state space. The final task is to use the loss functions to guide the robot as it responds to part requests.

In my implementation, I used a C++ class to represent a layer of the state space. Structures were used for the mode (the part, source, and destination) of each layer, and for the data (current loss, next loss, etc.) at each node of the bitmaps. My program puts the correct source, destination, and obstacle region into each layer by use of rigid assignment statements. An improvement would be to use a cleaver loop to fill the layers with the correct information. To find the loss functions, I defined member functions of the layer class that made use of the dynamic programming principle. A member function was also defined to solve for the best next action when the robot is fulfilling part requests.

I have barely touched upon the details of this algorithm. The exact procedure employed to calculate the loss functions is too detailed to cover here. A better understanding can be obtained by examining the example below. There have also been two papers written on this subject that cover it in detail. I based my implementation of this algorithm upon the following two papers.

• LaValle, Steven M. and Sharma, Rajeev. On Motion Planning in Changing, Partially Predictable Environments. International Journal of Robotics Research, vol 16, no 6, pp 775-805.

• Sharma, Rajeev; LaValle, Steven M.; and Hutchinson, Seth A. Optimizing Robot Motion Strategies for Assembly with Stochastic Models of the Assembly Process. The Beckman Institute for Advanced Science and Technology, University of Illinois, Artificial Intelligence Technical Report UIUC-BI-AI-RCV-94-11.

For an example problem I have considered a problem with two sources, one part, and two destination regions. The drawing of the world is shown below with the sources, destinations, robot, part, and obstacles labeled. For this example, I used a 35 X 35 bitmap representation of the world, a velocity of 2.5 units/sec, a time step of 1 sec, and I considered 24 possible actions at each position. The preprocessing of this example had to loop for about 60 time steps, and the computer took approximately ten minutes to complete the computation.

• Figure 1: The factory with source, destination, and obstacle regions
  
  

  Figure 2 below shows the loss function for the layer of the state space that corresponds to the request to pick up a part from Source #1 in Figure 1. The axis that is labeled 0-150 corresponds to the bottom of the world shown in Figure 1, and the axis labeled 0-100 corresponds to the left side of the world. The vertical axis corresponds to the loss value at each point. The areas with very high loss correspond to the obstacles. These regions are the shape of the obstacle regions in configuration space.

  The loss function is shaped exactly as it should be, with the highest loss occurring the farthest from the source region and the lowest losses occurring at the source region. The shape of the source region in configuration space may be seen in the front bottom corner of the loss plot, where the losses are zero. Notice that there is a thin "shelf" in the loss plot on the front side of the large obstacle. This area corresponds to the path that the robot can take to the left of the large obstacle. In order to understand how the robot follows the loss function diagram, imagine placing an object onto the loss diagram and watching it slide down the diagram to the source region. Recall that there is a different loss function for each different request. When a request is made, the correct loss diagram is retrieved for that request and the robot follows the loss function down to the "goal area".

• Figure 2: The values of the loss function over the factory
  
  

  Figure 3 is the same plot as shown above, but it is rotated so we can see different areas of the plot. This plot shows that there is indeed a path between the large middle obstacle region and the smaller one behind it. The interesting thing about this graph is the small ridge that can be seen between the large obstacle and the smaller one on its left. This ridge is a local maxima of the loss function. This maximum tells the robot which side of the large obstacle to go around. Imagine it is on top of the large obstacle (see Figure 1) and it is told to go to source region #1 (in the bottom left corner). If the robot is on this side of the ridge, it is closer for it to come around this side of the obstacle on its way to source #1, if it is on the other side, it should go the other way around.

• Figure 3: The loss function viewed at a different angle
  
  

  Figure 4 is an animated gif showing the robot fulfilling requests. When a request is made, the source region is displayed in blue and the destination region is displayed in black. The robot (the small red triangle) then goes to pick up the part at the source region. When the robot gets to the source, it changes into the part (the orange polygon), signifying that the part has been picked up. The robot then carries the part to the destination region. When it gets to the destination, the request is full-filled and the source, destination, and part disappear until another request is made.

  An interesting case is illustrated by this example. This case is when the source is in the lower left corner of the factory, and the destination is in the upper left corner. In this case, the robot is able to go to the left of the large obstacle on its way to pick up the part but must go the long way around on the way to deliver it. It must go around because the part is too large to fit through the passage to the left of the large obstacle. Also notice how the robot moves down toward the source regions when there are no requests. It does this so that it can minimize the time that the parts must wait to be delivered. Remember that the robot does not know which requests will be made next until they are actually made.

• Figure 4: The robot fulfilling requests