Abstract
In this paper, we propose a cost-aware Rapidly-exploring Random Tree (RRT) path planning algorithm for mobile robots. A mobile robot is presented with a cost map of the field of interest and assigned to move from one location to another. As a robot moves, the robot is penalized by the cost at its current location according to the cost map. The overall cost of the robot is determined by the trajectory of the robot. The goal of the proposed cost-aware RRT algorithm is to find a trajectory with the minimal cost. The cost map of the field can represent environmental parameters, such as temperature, humidity, chemical concentration, wireless signal strength, and stealthiness. For example, if the cost map represents packet drop rates at different locations, the minimum cost path between two locations is the path with the best possible communication, which is desirable when a robot operates under the environment with weak wireless signals. The proposed cost-aware RRT algorithm extends the basic RRT algorithm by considering the cost map when extending a motion segment. We show that the proposed algorithm gives an outstanding performance compared to the basic RRT method. We also demonstrate that the use of rejection sampling can give better results through extensive simulation.
Introduction: Beyond Geometric Feasibility
Standard path planning algorithms like the original Rapidly-exploring Random Tree (RRT) are highly effective at finding feasible paths through cluttered spaces. However, they are often "cost-blind," focusing primarily on avoiding hard obstacles rather than accounting for the quality of the space being traversed.
In real-world scenarios, a robot may need to navigate while maintaining a strong Wi-Fi connection, avoiding high-temperature zones, or staying stealthy. These factors are best represented as continuous "cost maps." The challenge is to find a path that minimizes the integrated cost along the entire trajectory without losing the computational efficiency and probabilistic completeness that make RRT popular.
Methodology: Continuous Mapping and Elite Selection
The proposed Cost-Aware RRT (CA-RRT) algorithm introduces two primary innovations to make path planning sensitive to environmental costs:
1. Continuous Cost Mapping via Gaussian Processes
Instead of using a discrete grid, which often suffers from resolution issues, the authors utilize Gaussian Process (GP) regression to model the environment. This allows the robot to estimate costs at any arbitrary coordinate based on sparse sensor data, providing a smooth, continuous cost field.
2. The CA-RRT Algorithm
The algorithm refines the standard RRT expansion process through two key mechanisms:
- Rejection Sampling: When selecting a random target point ($x_{rand}$), the algorithm uses rejection sampling biased by the cost map. It preferentially selects points in "cheaper" (safer or higher-signal) areas, effectively steering the tree's growth toward better regions from the start.
- Elite Candidate Selection: During the tree expansion phase, the algorithm generates $N_t$ potential trajectories using different control inputs. It performs a two-stage filter: first selecting "elite" candidates that get closest to the target, and then picking the single candidate from that group with the lowest integrated path cost.
Key Results and Performance
The authors compared CA-RRT against a Basic RRT (B-RRT) across six distinct simulated 2D scenarios. These maps were created using Gaussian Processes to simulate realistic, non-uniform environmental parameters like chemical concentrations or signal gradients.
Key findings from the study include:
- Lower Path Costs: CA-RRT consistently found paths with significantly lower total costs. In Scenario 1, CA-RRT achieved an average cost of 0.0579 compared to B-RRT’s 0.0865.
- Sampling Efficiency: The inclusion of rejection sampling provided a measurable boost, particularly in environments with narrow "safe" corridors, by reducing time spent exploring high-cost "dead ends."
- Convergence: As computation time increased, CA-RRT showed a steady improvement in path quality, demonstrating its ability to refine the solution toward a global optimum.
Limitations and Future Work
While CA-RRT offers superior path quality, it introduces higher computational complexity due to GP inference and candidate path generation. Current tests were limited to static 2D environments. Future work will explore the algorithm's performance in dynamic environments—where cost maps change over time—and its scalability to high-dimensional state spaces common in complex robotic systems.
Summary updated