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

The Probabilistic Road Map (PRM) algorithm is designed for high-dimensional spaces. It operates in two phases: the construction phase and the query phase. In the construction phase, the algorithm randomly samples points in the free space (areas without obstacles) to generate nodes. It then attempts to connect these nodes by checking if a direct path between any two nodes is collision-free. If the path is clear, an edge is added between the nodes, creating a sparse graph where edges represent feasible paths. In the query phase, the start and goal points are integrated into the graph by connecting them to the nearest nodes.

PRM implementation assignment

1. About InflateWall:
   • The black-white map need to be converted because the wall is black and the dilate only inflate white part. So after converting, the wall could be inflated successfully. The size of inflation is related to the radius of the robot.
     

     Inflated Map
2. Check if the new_vertex is a certain distance away from the existing vertices:
   • The function first defines a minimum distance threshold min_dist as 0.1. It then iterates over all existing vertices, calculating the distance dist between each existing vertex and the new vertex. If any existing vertex is found to be within the min_dist of the new vertex, the acceptVert flag is set to false and the loop breaks, indicating that the new vertex is too close to an existing vertex and should not be accepted. Finally, the function returns the value of acceptVert, indicating whether the new vertex meets the distance requirement.
3. Write the necessary code to check if a edge between two vertices is valid:
   • The function checkIfEdgeCrossesObstacle takes two vertices, the map, and its resolution as inputs. It converts the vertices' coordinates from world space to map space and creates cv::Point objects representing their positions. Using OpenCV's LineIterator, the function iterates over the pixels forming the line between these points. It checks each pixel to see if it corresponds to an obstacle, using a predefined threshold thresh_occ. If any pixel along the line indicates an obstacle, the EdgeCrossesObstacle flag is set to true, signifying the edge is invalid as it crosses an obstacle. If no obstacles are found, the edge is considered valid. The function returns the EdgeCrossesObstacle flag, ensuring that paths in the PRM do not intersect with obstacles.

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?

Finding the shortest path through a maze using the A* algorithm can be made more efficient by strategically placing nodes to reduce the search space and focusing the algorithm's attention on the most promising paths. It is more efficient to place nodes only at critical points (e.g., intersections, turns, and wide open spaces) and ensure that these nodes are connected by the shortest paths without unnecessary intermediate nodes.

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

Using PRM in a maze is not a good idea. PRM is highly flexible and capable of handling various types of environments. However, the structure like maze presents challenges. The narrow passages and numerous walls in a maze often cause randomly placed samples to fall into walls or dead ends, reducing the efficiency of roadmap construction.

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

If the map is suddenly changed, the robot will still continue traveling according to the result of the original path planning, and may hit the obstacles that suddenly appear. This is because the A* algorithm plans a series of coordinate points leading to the destination based on the obstacles of the original map, and it cannot process them in the face of suddenly appearing obstacles, so collisions will occur. It is worth noting that A* does not change in real time, and the generated paths do not change if the map is not updated.

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

The global planner computes an approximate path from the starting point to the destination, a process that generates a list of (x, y) coordinate points. The local planner then moves along these points towards the destination while avoiding obstacles and preventing collisions.

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.

When using a grid map, the paths generated are often orthogonal, mainly up and down, left and right, and less diagonal. With PRM, the generated paths consist of straight lines in different diagonal directions. Thus, when using PRMs in open spaces, a series of diagonal paths can be generated, which is more efficient compared to orthogonal paths.

Simulation Results

A* without PRM

As shown in A* without PRM, the A* algorithm generates the blue path to the destination and the robot follows the blue path to the destination. In this process A* generates a list of coordinate points and the APF algorithm takes the coordinate points in the list one by one as the destination and thus moves towards the final destination thus realizing global planning. The robot basically follows the blue route during its movement, and at some times it leaves the blue route in order to avoid obstacles.

A* with PRM

As shown in A* with PRM, the PRM algorithm generates the Inflated Map along with the blue path to the destination. The robot follows the blue path completely smoothly and quickly to reach the destination.