You have presented a map of the robot. This is the correct first step. Now this map has to be reformulated as a sequence of decision which path to take.
If you do this, you can build a tree. In this tree, each stage (level) of the tree you advance from one note to the other. So, if your start node is 1 this is the first node in your tree.
From the first node, you can have move to 5 paths which lead to adjacent nodes. Let's indicate that these are reachable within one step from the starting node by adding S1 as prefix. Now if we add these 5 choices to the starting node we get:
We can continue expanding these nodes, by listing their adjacent nodes. Usually it is bes t to omit the node you came from into this nodes again and again increasing the depth of the tree. (The tree might not match exactly your map but it is close)
Now if you are in a uniform (and unitary) cost environment the cost of taking a decision, or moving form a parent node to a child node is 1. If you actually calculate distances each edge (line) in this tree will have a different cost which will be the distance metric between the nodes.
Any graph base path planning algorithm will try to build such a tree and the goal is the find the minimum cost path from the start until one of the nodes (leafs) in this tree will be your goal node. The difference is how they get to this path.
Breadth First Search: expands all nodes in the tree going level by level. Starting at the S1 level all S1 nodes will be expanded. If none of them contains the target node, the next level will be extended...and so on
Depth First Search: Similar to BFS, but goes downwards, not sideways. It expands the nodes downwards until there the maximum depth is reached than goes to the next branch.
Dijkstra: It is more efficient then the two above because it uses an internal rule to select the nodes that is more application oriented. For each node expanded is ads cost information about the cost to get to that node. The cost is the edges traversed to get to the node (if the cost it uniform 1) or the sum of costs (e.g. cartesian distance between the nodes). This cost is what selects the next node to be extended. Regardless of the level in the tree, you always select the node for expansion, which has minimal cost.
The algorithm performs better then BFS if the cost is not uniform. It is more ore less the same as BFS in a uniform unitary cost map.
For implementation, usually there are two lists, closed and open. The closed list contains all nodes which have been expanded. The open list contains all nodes which have not yet been expanded.
- First step is to add the first node to the open list.
- Select the node with minimal cost in the open list
- Check if this node is the target node, if yes, recreate the path using the pointers to its parents set below. if not:
- Expand the selected node, this is where we get the adjacent nodes
- Add all newly obtained nodes and add them to the open list
- Calculate the cost to get to the expanded nodes (parent cost + edge cost)
- Connect a pointer from the children nodes to the parent node.
- Add delete the expanded node from the open list and add it to the closed list
Go to 2
Please note that some implementations and explanations consider the presence of the target node in the open list a stop condition. This is wrong, it has to be the in the open list and has to have the minimum cost in the open list for the algorithm to complete. Otherwise it is not guaranteed that the shortest path is found
A:* it is based on Dijkstra, but has one difference. The cost calculated in step 6 has one further component. It is an approximated cost of how much would it take to get from the current node to the target node. If this approximation is less or equal then the actual real cost to get to the target node then the algorithm will function correctly. E.g. in a uniform cost environment this approximation can be 1. On a map, this approximation could be the Cartesian distance between the current node and the target node. Please note, that the approximation plays a role only is the selection of the node, the actual cost component is calculated based on the actual cost only, not the approximated part.