Mobile Robot Control 2024 Ultron:Solution 3

Exercise 3: Global Navigation

A* Algorithm

The A* algorithm is a widely used heuristic search algorithm for finding the shortest path from a start node to a goal node in a graph. It combines the strengths of Dijkstra's algorithm and Greedy Best-First-Search. The core idea of A* is to use a heuristic function to prioritize paths that seem more promising to reach the goal.

The steps for running the algorithm for A* are as follows.

1. Initialization:
   • Create the open list (priority queue) and closed list (empty).
   • Initialize the start node with g(start) = 0 and f(start) = h(start), then add it to the open list.
2. Main Loop:
   • While the open list is not empty:
     • Select the node current from the open list with the lowest f value.
3. Goal Check:
   • If current is the goal node, reconstruct and return the path from start to goal.
4. Process Neighbors:
   • For each neighbor of current:
     • Ignore the neighbor if it's in the closed list.
     • Calculate the tentative g value (tentative_g = g(current) + cost(current, neighbor)).
     • If the neighbor is not in the open list or the tentative g is lower than g(neighbor):
       • Update g(neighbor), f(neighbor), and set the parent of the neighbor to current.
       • Add the neighbor to the open list if it's not already there.
5. Path Reconstruction or Failure:
   • If the goal is reached, reconstruct the path using the parent references.
   • If the open list is empty and the goal has not been reached, return failure indicating no path exists.

A* implementation assignment

1. Find the index of the node with minimum f and store it in the variable nodeID_minimum_f：
   • First, the code initializes the variable min_f to infinity to ensure that any node's f value will be smaller than this initial value. Then, it iterates through each node index nodeID in the open list open_nodes. For each node, it accesses its f value via _nodelist[nodeID].f. This f value is the sum of the actual cost g from the start node to the current node and the estimated cost h from the current node to the goal node. If the current node's f value is smaller than the current minimum f value, the code updates min_f to the current node's f value and stores the index of the node with the smallest f value in nodeID_minimum_f. By comparing the f values of all nodes in the open list, the code ultimately determines the node with the smallest f value, and its index is stored in nodeID_minimum_f for use in the next step of the A* algorithm.
2. Explore the nodes connected to the current node:
   • For each neighbor of the current node, the code checks if the neighbor is not in the closed list by searching through the closed_nodes list. If the neighbor is not in the closed list, the code calculates the tentative cost-to-come tentative_g, which is the cost from the start node to the current node plus the distance from the current node to the neighbor. If this tentative cost-to-come is lower than the neighbor's current g value, the code updates the neighbor's g value to the tentative g, recalculates the neighbor's f value (which is the sum of the updated g value and the heuristic estimate h value), and sets the neighbor's parent node to the current node.
3. Put the node IDs of nodes in the optimal path:
   • It initializes the current node ID to the goal node's ID _goal_nodeID and then iterates in a loop, tracing back through the parent nodes until reaching the start node. In each iteration, the current node ID is added to the front of the path list path_node_IDs, and the current node ID is updated to its parent node's ID. This process continues step by step along the parent node chain, ultimately building the optimal path from the start node to the goal node. The path node IDs are stored in path_node_IDs in sequence, representing the optimal path from start to goal.

Probabilistic Road Map

PRM implementation assignment

1. Find the index of the node with minimum f and store it in the variable nodeID_minimum_f
2. Explore the nodes connected to the current node:
3. Put the node IDs of nodes in the optimal path:

Answers of the questions

1. How could finding the shortest path through the maze using the A* algorithm be made more efficient by placing the nodes differently? Sketch the small maze with the proposed nodes and the connections between them. Why would this be more efficient?

2. Would implementing PRM in a map like the maze be efficient?

3. What would change if the map suddenly changes (e.g. the map gets updated)?

4. How did you connect the local and global planner?

5. Test the combination of your local and global planner for a longer period of time on the real robot. What do you see that happens in terms of the calculated position of the robot? What is a way to solve this?

6. Run the A* algorithm using the gridmap (with the provided nodelist) and using the PRM. What do you observe? Comment on the advantage of using PRM in an open space.