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 a path planning method 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. Write the necessary code to inflate the walls to account for the robot size:
   • The function inflateWalls takes an input map and its resolution, creating a clone of the map to store the inflated version, thus preserving the original. It defines the robot's radius as 0.2 units and constructs an elliptical structuring element that matches the robot's size. Using this structuring element, the code performs a dilation operation on the map, effectively enlarging the walls (obstacles) to create a safety buffer around them. This ensures that the robot's path planning avoids collisions by accounting for the robot's size. The function then returns the inflated map with these enlarged obstacles.
2. Write the necessary code to 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. This ensures that the nodes in the PRM graph are not overly dense, thereby maintaining the effectiveness of the path planning.
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.thereby maintaining the effectiveness of the path planning.
4. Write the necessary code in function *inflateWalls* to inflate the walls to account for the robot size:
   • It starts by calling the inflateWalls function to inflate the walls in the map, accounting for the robot's size to ensure paths avoid obstacles. Then, in a loop, it generates random vertices and checks if these vertices are within the inflated walls. If a vertex is inside an obstacle, it is skipped; otherwise, the vertex is added to the PRM graph, and edges are created to connect it to other vertices.
5. Write the necessary code in function *checkIfVertexIsValid* to check if the new_vertex is a certain distance away from the existing vertices:
   • This segment of the PRM algorithm checks the validity of a new vertex by ensuring it is a certain distance away from existing vertices using the checkIfVertexIsValid function. If the vertex is too close to existing vertices, it is skipped. If it is valid, the vertex is added to the graph G, and edges are then created between this new vertex and the existing vertices, further building the roadmap for path planning.
6. Write the necessary code in function *checkIfEdgeIsValid* to check if a edge between two vertices is valid:
   • This segment of the PRM algorithm iterates through existing vertices to check if an edge between each vertex and a new vertex is valid using the checkIfEdgeIsValid function. This function ensures that the edge does not cross any obstacles and adheres to specified distance constraints. If the edge is valid, it is added to the graph G. The number of vertices added is updated and printed, and upon completion, the total number of edges is displayed.

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.

Simulation Results

As shown in [1],