Robot Navigation - A Star Algorithm

Objective

The aim of this lab is to understand the A* algorithm and implement it in Matlab.

1 - Introduction

In computer science, A* is a computer algorithm that is widely used in pathfinding and graph traversal, the process of plotting an efficiently traversable path between points, called nodes. A* achieves better time performance by using heuristics.

2 - The Algorithm

A* uses a best-first search and finds a least-cost path from a given initial node to the goal node. As A* traverses the graph, it follows a path of the lowest expected total cost or distance, keeping a sorted priority queue of alternate path segments along the way.

It uses a knowledge-plus-heuristic cost function of node x to determine the order in which the search visits nodes in the tree. The cost function is a sum of two functions:

1. the past path-cost function, which is the known distance from the starting node to the current node x (denoted g(x))
2. a future path-cost function, which is an admissible "heuristic estimate" of the distance from x to the goal (denoted h(x)).

The h(x) part of the f(x) function must be an admissible heuristic; that is, it must not overestimate the distance to the goal. Thus, for an application like routing, h(x) might represent the straight-line distance to the goal, since that is physically the smallest possible distance between any two points or nodes.

If the heuristic h satisfies the additional condition h(x) = d(x,y) + h(y) for every edge (x, y) of the graph (where d denotes the length of that edge), then h is called consistent. In such a case, A* can be implemented more efficiently. No node needs to be processed more than once and. Now let's look at closely to the each steps.

2.1 - The Open and Closed Lists

In order to run A* algorithm we need 2 lists which are open and closed list.

1. One to write down all the nodes that are being considered to find the shortest path (called the open list)
2. One to write down the nodes that does not have to consider it again (called the closed list)

The robot begins by adding his current position (we’ll call this starting position point "A") to the closed list. Then, it adds all neighbours to its current position to the open list.

In the following figure, you can see an example of what this would look like if the robot was out in the open (red dot represents the robot's current position, green lines represents the open list):

Figure 1: The visualization of the experiment.

Now the robot need to determine which of these options would be on the shortest path, but how can it choose? Well in the A* path algorithm, this is done by giving a score to each node, which is called path scoring. Let's see how it works!

2.2 - Path scoring

We’ll give each node a score G + H where:

1. G is the movement cost from the start point A to the current node. In the beginning it is small, but it will increase as we get farther away from the start point.
2. H is the estimated movement cost from the current node to the destination point. This is often called the heuristic because we don't really know the cost yet - it's just an estimation.

You may be wondering what we mean by "movement cost". Well it is quite simple - just Euclidean distance from the current node to the goal.

\begin{equation}
\mathrm{d}(\mathbf{p},\mathbf{q})=\sqrt{(p_1-q_1)^2 + (p_2-q_2)^2}.
\end{equation}

However, keep in mind you can make this different. Depends on your application, you can calculate movement cost differently. For example, let's think about 3D environment and if you had different terrain types, you might make some cost more to move through a swamp, water etc.

That's the general idea - now let’s dive into more specifics about figuring out G and H.

2.3 - More about G

Recall that G is the movement cost from the start point A to the current node.

In order to calculate G, we need to take the G of its parent (the node where we came from) and to add the cost between current and its parent to it. Therefore, the G of each node will represent the total cost of the generated path from point A until the node.

For example, this diagram shows a path from the start point A to the current node. (Blue line represent the path and its cost represent G, green lines represents the open list):

Figure 2: The visualization of the experiment.

2.4 - More about H

Recall that H is the estimated movement cost from the current square to the destination point.

The closer the estimated movement cost is to the actual cost, the more accurate the final path will be. If the estimate is off, it is possible the path generated will not be the shortest.

To put it simply, we will use the "Euclidean distance method" that just calculate the straight-lien between two nodes without taking into account of any obstacles or differences of land.

2.5 - Step by step

So now that you know how to compute the score of each node (we’ll call this F, which again is equal to G + H), let’s see how the A* algorithm works.

The robot will find the shortest path by repeating the following steps:

1. Get the node on the open list which has the lowest score. Let’s call this node S.
2. Remove S from the open list and add S to the closed list.
3. For each node T in S’s neighbour:

•     If T is in the closed list: Ignore it.
•     If T is not in the open list: Add it and compute its score.
•     If T is already in the open list: Check if the F score is lower when we use the current generated path to get there. If it is, update its score and update its parent as well.

3 - The results & conclusion

From the following figures, you can see that the algorithm is working in both graphs. This laboratory work was very helpful to understand the theoretical background of the A* algorithm. It helped me to explore more information about it and how to apply to the robot path finding.

Figure 3: The visualization of the experiment.

Figure 4: The visualization of the experiment.

MATLAB Codes & Report