Project 2 - Path Planning, Control and Exploration

• Keywords: Bezier Curve, Path Planning (A*), Control (PID), Frontier Autonomous Exploration
• Coding & Enviroment: C++, ROS, RViz, Gazebo
• Detailed code implementation can be found in the Github code repo

This project needs to implement the A* algorithm to find the shortest path with inflation radius and Bezier curve optimization, PID control algorithm to follow the path generated by the A* algorithm, and autonomous exploration algorithm to explore the unknown environment.

A* Path Planning

Key steps involved in the A* path planning:

• Use the Bezier curve to effectively create smooth paths.
• Inflate the obstacles with a certain radius to ensure the robot can avoid them.
• Use the A* algorithm to find the shortest path in the inflated map.

Bezier Curve

Defined by a set of control points, the Bezier curve smoothly interpolates between these points. Their use in trajectory planning offers the following benefits:

• Smoothness of Path
• Guarantee of End-Point Convergence
• Applicability to Various Types of Path Planning

For a cubic Bezier curve, which is the most commonly used in trajectory planning, the curve is defined by four control points: $P_0$, $P_1$, $P_2$, $P_3$.

The cubic Bezier curve formula is:

\[B(t) = (1-t)^3P_0 + 3(1-t)^2tP_1 + 3(1-t)t^2P_2 + t^3P_3, \quad t \in [0, 1]\]

where: $P_0$ is the starting point, $P_3$ is the ending point, and $P_1$ and $P_2$ are the intermediate control points that determine the shape of the curve. The general case of a Bezier curve is shown as the following figure:

Inflation Radius

By expanding the radius of the obstacles, we can create a “safety buffer zone” for robots. Here, we first convert the robot radius into grid cells by $inflated\ cells = \frac{robot\ radius}{grid\ resolution}$. Then, iterate through each cell in the occupancy grid and find obstacle cells. We then expand each obstacle cell by the inflation radius in all directions and make sure to check the boundary of the grid.

A* Algorithm

A* is a popular pathfinding algorithm that efficiently finds the shortest path in a weighted graph. It combines the advantages of Dijkstra’s algorithm (guarantees the shortest path) and greedy best-first search (efficient pathfinding).

Key Components of the A* Algorithm:

1. Cost Function

   The total cost $ f(n) $ of a node $ n $ is calculated as:

   \[f(n) = g(n) + h(n)\]

   where:

   • $ g(n) $: The actual cost from the start node to the current node $ n $.
   • $ h(n) $: The heuristic estimate of the cost from the current node $ n $ to the goal node.

2. Heuristic Function ($h(n)$)

   $h(n)$ should be admissible, meaning it should never overestimate the true cost to reach the goal. Common heuristics include:

   • Manhattan Distance:
     \(h(n) = |x_2 - x_1| + |y_2 - y_1|\) (for grid-based movement with 4 directions).
   • Euclidean Distance:
     \(h(n) = \sqrt{(x_2 - x_1)^2 + (y_2 - y_1)^2}\) (for continuous or diagonal movement).

3. Priority & Visited Queue

   • Priority Queue (Open Set): A priority queue containing nodes that are candidates for expansion, sorted by their $f(n)$ values.
   • Visited Set (Closed Set): A set of nodes that have already been expanded to avoid revisiting them.

A* Algorithm Steps:

• Maintain a priority queue to store all the nodes to be visited
• The heuristic function $h(n)$ for all nodes are pre-defined
• The priority queue is initialized with the start state $X_S$
• Assign $g(X_S)=0$, and $g(n)=\infty$ for all other nodes in the graph
• Loop
  • If the queue is empty, return FALSE; break;
  • Remove the node $n$ with the lowest $f(n)=g(n)+h(n)$ from the priority queue
  • Mark node $n$ as visited
  • If the node $n$ is the goal state, return TRUE; break;
  • For all unvisited neighbors $m$ of $n$:
    • If $g(m)=\infty$:
      • Push node $m$ into the queue
    • If $g(m)>g(n)+c(n,m)$:
      • $g(m)=g(n)+c(n,m)$
  • End
• End loop

Implementation Notes

• Angle difference calculation → I choose to approximate the angles for p0 and p3 using their adjacent points, which is angle_diff = abs(calculateAngle(p0, p1) - calculateAngle(p2, p3)).
• Interpolation number → The number should not be too big or too small. If it is too big, the Bezier curve will be too smooth. Although the module for detecting inflated obstacles is continuously working, very smooth curves will impact the computational efficiency. If the number is too small, then the curve will be zigzag, which will bring challenges to the controller. In the code, this number will be the bigger one between the least number 5 and the numerical result of the scaled angle difference value. The robot’s actual speed limit and the sampling number of the raw curve determine that the interpolation number will not be too big.

RViz Simulation

PID Control & Path Following

Preliminaries

The three components of PID control are:

• Proportional (P): The proportional term produces an output that is directly proportional to the current error. It aims to reduce the error by applying a correction proportional to the magnitude of the error.
• Integral (I): The integral term accumulates past errors over time, aiming to eliminate residual steady-state error that may be caused by system biases.
• Derivative (D): The derivative term predicts future errors based on the rate of change of the error, helping to dampen oscillations and improve system stability.

The PID control formula is:

\[u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{de(t)}{dt}\]

Ziegler-Nichols Method is applied to tune the PID parameters.

Implementation Notes

• To make the tuning fast and convenient, I put the PID gain parameters into a testing launch file (no need to type rosrun one by one manually) and also disabled Gazebo’s GUI (save time for shutting down and restarting the GUI). In this way, I can restart and initialize the map status in the RViz and use the rosparam set command to finetune the parameters dynamically. The private_nh and the method of initializing the gain values should be adjusted accordingly. For example, the kp_heading should be initialized using private_nh.getParam("kp_heading", kp_heading_), instead of using private_nh.param(xxx). After implementing A* path planning and with the raw skeleton of the frontier explore code, I finished finetuning the parameters roughly to guarantee the controller can basically work.
• With the implemented A* script, I disabled all the commands related to the “Robot Stop” in the PID controller source file, which can greatly reflect the performance of the controller when the robot is getting closer to its steady state. Also, I increased the max linear/orientation speed limits to ensure the performance of the controller was not masked by the robot’s slow speed. Typically, the goal_distance_tolerance is decreased to 0.05 from default 0.2, which is just to test the linear distance controller. (There is no point in finetuning the PID controller if the frontier exploration, which will update the inflated grids, is not implemented. So, I can just try my best to optimize the distance controller.)
• Currently, the robot’s absolute max speed is not that high. Therefore, I only use the PD controller without the I-term to maximize stability. The optimized PD controller performs quite well under a reasonable max speed limit (higher than the default limit). The tuning process was conducted using the Ziegler-Nichols Method.

RViz Simulation

Frontier Exploration

Preliminaries

Frontier points are identified as the boundary points between explored and unexplored regions. These are locations where the robot can make progress by exploring new, previously unknown space.A frontier point is defined as a free space (value 0 in the grid) that is adjacent to an unknown area (denoted by -1), while the robot has direct access to this free space.

This frontier exploration task processes occupancy maps, inflates obstacles, and detects frontiers for autonomous exploration. When the robot reaches a goal, it triggers the next frontier selection.

Implementation Notes

The general steps of the frontier exploration are:

1. Build inflated obstacle: Refer the A* part. When selecting the forward point, it is necessary to avoid choosing a location within the inflated radius area to prevent the robot from colliding with obstacles.
2. Frontier Detection:
   • Iterate over every cell in the occupancy grid.
   • Identify frontier cells: A frontier is a free cell (0) adjacent to an unknown cell (-1).
   • Store valid frontier coordinates in frontiers.
3. Frontier Clustering:
   • Traverse each frontier point.
   • Create a queue, add the current frontier point to the queue, and mark it as clustered.
   • Remove a point from the queue and expand to its neighbors.
   • If a neighbor is an unclustered frontier point and reachable, add it to the queue.
4. Find the largest cluster: To select the largest cluster from a given list of frontier clusters. A cluster is a group of adjacent frontier points (unexplored boundaries of the known map). The function determines which cluster has the most points and returns its index.
   • Initialize tracking variables
   • Iterate through all clusters
   • Compare cluster sizes
   • Return the index of the largest cluster

Something to note while implementing:

• When the frontier points are getting clustered, we should use push_back or emplace_back to push the current cluster into the cluster list clusters, though there is no significant difference between these two commands. They both copy value-by-value, which cluster will not be empty at the end of each loop. If we use std::move to push the cluster, it will move the cluster itself without copying, which will make the cluster empty at the end of each loop. This will prevent adjacent clusters from forming an integrated larger cluster.

RViz Simulation

Real-World Deployment

With all the components implemented, the deployment on the Turtlebot3 is done to test the performance, which is shown as the following video (both the simulation visualization and the real-world deployment are shown simultaneously):