1 - Introduction
The theories behind robot maze navigation is immense. It would take several books just to cover the basics. But this labwork only concentrate on the wavefront planner algorithm which is still powerful methods of intelligent robot navigation. The basic concepts and details of the algorithm are going to be explained in the next chapter. After that, we are going to see the results.
2 - The Algorithm
The wavefront algorithm finds a path from point S (start) to point G (goal) through a discretized workspace such as this (0 designates a cell of free space, 1 designates a cell fully occupied by an obstacle):
\[
\begin{bmatrix}
1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 \\
1 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0 & 0 & 0 & 1 \\
1 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0 & 2 & 0 & 1 \\
1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0 & 0 & 0 & 1 \\
1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0 & 0 & 0 & 1 \\
1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0 & 0 & 0 & 1 \\
1 & 0 & 0 & 0 & 1 & 1 & 1 & 1 & 1 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0 & 0 & 0 & 1 \\
1 & 0 & 0 & 0 & 1 & 1 & 1 & 1 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\
1 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\
1 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\
1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 1 \\
1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 0 & 0 & 0 & 0 & 1 \\
1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 1 & 1 & 0 & 0 & 0 & 0 & 1 \\
1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 \\
\end{bmatrix}
\]
The algorithm consists of a breadth-first search (BFS) on the graph induced by the neighborhood connectivity of cells, starting at G, which is assigned the value 2 at the beginning. As BFS traverses the space, each cell is assigned a value which corresponds to the number of moves required for the shortest path from that cell to the goal.
There are four main steps to running this algorithm.
2.1 - Create a Discretized Map
First, we need an X-Y grid matrix to mark empty space, robot/goal locations, and obstacles. This is the easiest part because it is already given to us. Initial matrix is something like below.
But of course this is not what it really looks like in the robot memory. Instead of the image, it looks much more like the matrix above. The final position is already given in the map. We can find it easily. In order to find it, we should look at the values of each block. If the value of the block is 2, that means the current block is the goal destination. After all of these information, now it is the time to fill the empty blocks.
2.2 - Fill in Wavefront
It is explained above how to find the x and y coordinate of the final location. After finding it, we should look at all of its neighbours. We are going to use the 8 neighbourhood connectivity. If the value of the neighbour block is 0, then we should change its value to 1 more than the current block's value. Let's do it an example, we have a part of the map matrix below and after one step you are going to see the modification of the neighbours of the current block whose value is 2. Red colored block is the current block, blue colored blocks are its neighbours and green colored block is processed block.
\begin{bmatrix}
1 & 1 & 1 & 1 & 1 \\
1 & \color{blue}0 & \color{blue}0 & \color{blue}0 & 1 \\
1 & \color{blue}0 & \color{red}2 & \color{blue}0 & 1 \\
1 & \color{blue}0 & \color{blue}0 & \color{blue}0 & 1 \\
1 & 0 & 0 & 0 & 1 \\
\end{bmatrix}
\begin{bmatrix}
1 & 1 & 1 & 1 & 1 \\
1 & 3 & 3 & 3 & 1 \\
1 & 3 & \color{green}2 & 3 & 1 \\
1 & 3 & 3 & 3 & 1 \\
1 & 0 & 0 & 0 & 1 \\
\end{bmatrix}
As you can see that the values of the neighbours of the current block are 0, so that means the places are empty. We can assign new values to these blocks. We should assign 1 more than the current block's value. The current block's value is 2. If we add one more, we obtain 3. So we need to assign the values of the neighbours of the current block to 3. And we should put all the processed values to the queue. After finishing our job with the current block. Now, we can move next block which is the first block in the queue. We are going to apply the same rule as we did before until there are not any blocks in the queue. End of this loop, you are going to see that we have a weighted map like in the following:
\[
\begin{bmatrix}
1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 \\
1 & 21 & 20 & 19 & 18 & 18 & 1 & 1 & 14 & 14 & 14 & 14 & 14 & 14 & 1 & 1 & 3 & 3 & 3 & 1 \\
1 & 21 & 20 & 19 & 18 & 17 & 1 & 1 & 14 & 13 & 13 & 13 & 13 & 13 & 1 & 1 & 3 & 2 & 3 & 1 \\
1 & 21 & 20 & 19 & 18 & 17 & 16 & 15 & 14 & 13 & 12 & 12 & 12 & 12 & 1 & 1 & 3 & 3 & 3 & 1 \\
1 & 21 & 20 & 19 & 18 & 17 & 16 & 15 & 14 & 13 & 12 & 11 & 11 & 11 & 1 & 1 & 4 & 4 & 4 & 1 \\
1 & 21 & 20 & 19 & 18 & 17 & 16 & 15 & 14 & 13 & 12 & 11 & 10 & 10 & 1 & 1 & 5 & 5 & 5 & 1 \\
1 & 21 & 20 & 19 & 1 & 1 & 1 & 1 & 1 & 13 & 12 & 11 & 10 & 9 & 1 & 1 & 6 & 6 & 6 & 1 \\
1 & 21 & 20 & 19 & 1 & 1 & 1 & 1 & 1 & 13 & 12 & 11 & 10 & 9 & 8 & 7 & 7 & 7 & 7 & 1 \\
1 & 21 & 20 & 19 & 18 & 17 & 17 & 1 & 1 & 13 & 12 & 11 & 10 & 9 & 8 & 8 & 8 & 8 & 8 & 1 \\
1 & 21 & 20 & 19 & 18 & 17 & 16 & 1 & 1 & 13 & 12 & 11 & 10 & 9 & 9 & 9 & 9 & 9 & 9 & 1 \\
1 & 21 & 20 & 19 & 18 & 17 & 16 & 15 & 14 & 13 & 12 & 11 & 10 & 10 & 1 & 10 & 10 & 10 & 10 & 1 \\
1 & 21 & 20 & 19 & 18 & 17 & 16 & 15 & 14 & 13 & 12 & 11 & 11 & 1 & 1 & 11 & 11 & 11 & 11 & 1 \\
1 & 21 & 20 & 19 & 18 & 17 & 16 & 15 & 14 & 13 & 12 & 12 & 1 & 1 & 1 & 12 & 12 & 12 & 12 & 1 \\
1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 & 1 \\
\end{bmatrix}
\]
2.3 - Direct Robot to Count Down
As we know the weighted map, the initial and final points, we can easily find the trajectory. We can either start from the initial or final point. In that algorithm, we are going to start from the initial point. First thing that we need to do is add the current block to the trajectory and look at all of its neighbours and choose the block that has the smallest weight. Then we should look at all of the selected block's neighbours and choose the smallest one again and again until we reach the final position.
There is an important issue that we should consider about here. What if exists more than one neighbour that have the same small value. Which one should we choose? In that algorithm, horizontally and vertically connected pixels have higher priority than diagonally. It depends on the algorithm and you can manipulate this situation by changing the checking order of the pixels. You can see the example below. Red colored block is the current block, blue colored blocks are its neighbours and green colored block is processed block.
\begin{bmatrix}
1 & 21 & 20 & 19 & 18 & 17 \\
1 & 21 & 20 & 19 & 18 & 17 \\
1 & 21 & 20 & 19 & 18 & 17 \\
\color{blue}1 & \color{blue}{21} & \color{blue}{20} & 19 & 18 & 17 \\
\color{blue}1 & \color{red}{21} & \color{blue}{20} & 19 & 18 & 17 \\
\color{blue}1 & \color{blue}1 & \color{blue}1 & 1 & 1 & 1 \\
\end{bmatrix}
\begin{bmatrix}
1 & 21 & 20 & 19 & 18 & 17 \\
1 & 21 & 20 & 19 & 18 & 17 \\
1 & 21 & 20 & 19 & 18 & 17 \\
1 & \color{blue}{21} & \color{blue}{20} & \color{blue}{19} & 18 & 17 \\
1 & \color{green}{21} & \color{red}{20} & \color{blue}{19} & 18 & 17 \\
1 & \color{blue}1 & \color{blue}1 & \color{blue}1 & 1 & 1 \\
\end{bmatrix}
As you can see above, we have two neighbours whose values are 20. But we should choose one of them. According to the algorithm, we choose horizontally and vertically connected pixels instead of diagonally.
trajectory =\begin{bmatrix}
13 & 2 \\
13 & 3 \\
\vdots & \vdots \\
4 & 17 \\
3 & 18 \\
\end{bmatrix}
After running the algorithm, we are going to obtain the weighted map and also trajectory which starts from the initial point to goal point.
3 - The results & conclusion
Basically, the idea is behind the algorithm that you have a map, a starting point and a destination point. From the starting point you expand a "wave". The wave's "height" increases with the distance from the origin. You expand this wave until you no longer have any empty space or you reach the destination. Once the wave has reached the destination you can generate a path (a series of moves) by going "downhill" on the wave, from the destination back to the starting point.
As you can see from the figures, the algorithm works both with the small matrix and big maze. It gives an output which contains the weighted map and the trajectory from the initial position to the final position.
No comments:
Post a Comment