Methodology
This section describes how the robot’s motion, the environment, and the covariance constraint are modeled in the planner. It also explains how to implement an EKF for A*’s discretized nodes as a continuous-time dependent system.
Uncertainty Metric
To quantify localization uncertainty, we can use any scalar metric derived from the EKF covariance. We use the trace of the positional covariance.
The trace of the covariance matrix is used because the matrix is symmetric and positive semi-definite. Therefore, all diagonal entries are non-negative, giving a positive scalar quantity.
The uncertainty constraint is:
The trace of the positional covariance must remain below a user-defined threshold.
Any trajectory segment between two nodes that violates this constraint is considered infeasible.
Uncertainty-Aware Edge Evaluation
After a segment is created using Algorithm 1, each candidate path segment is evaluated using the uncertainty-check function, summarized in Algorithm 2. The robot’s motion and EKF propagation are simulated simultaneously, and the uncertainty metric is monitored throughout execution. A node that does not follow the constraint is rejected.
Theoretical Analysis
This section provides a theoretical guarantee regarding the optimality of the proposed Uncertainty-Constrained A* planner. The problem is formulated as a search over a graph where edge validity is strictly determined by the EKF covariance bound.
Graph Formulation
The cost of an edge is the arc length of the trajectory generated by the carrot chase controller.
Experiments and Results
This section presents simulation results evaluating the proposed uncertainty-constrained planner in GPS-denied environments. The experiments demonstrate how enforcing a bound on localization uncertainty affects path feasibility, estimation accuracy, and path quality for different planners.
Impact of Landmark Density
The effect of landmark density is evaluated by changing the percentage of landmarks while keeping obstacle density and the uncertainty threshold fixed. For each case, the success rate, path length, and computation time are reported as averages over successful runs from all planners. The same procedure is used to evaluate the effects of obstacle density and uncertainty threshold.
In terms of path length, A consistently produces the shortest paths across all landmark densities. Its paths remain close to the Euclidean lower bound, reflecting the optimality guaranteed by its admissible heuristic. PRM and RRT generate noticeably longer paths due to randomized exploration and suboptimal roadmap connectivity. Increasing landmark density reduces path-length variability for all planners, but A* remains the most efficient in terms of distance traveled.
A has the lowest computation time overall, followed by PRM and then RRT. As landmark density increases, computation time rises for all planners.
Impact of Obstacle Density
For all obstacle densities, A consistently produces the shortest paths, demonstrating the robustness of its optimality guarantees even in cluttered environments. PRM and RRT generate longer paths due to detours caused by random sampling and limited local connectivity. As obstacle density increases, path length increases slightly for all planners, but the relative ordering remains unchanged.
A has the lowest computation time at all obstacle densities, followed by PRM and then RRT. This reflects the increased number of node expansions required in cluttered environments for sampling-based planners.
Impact of Uncertainty Threshold
The uncertainty threshold is evaluated with landmark density fixed at 10% and obstacle density fixed at 5%.
Conclusions
This paper presents path planning in GPS-denied environments where localization uncertainty grows due to dead reckoning. Uncertainty-constrained A* preserves its optimality guarantees and returns the shortest path among all feasible trajectories that satisfy the uncertainty constraint, while also showing low computation time overall.
Experimental results show that while geometrically optimal paths are often invalid under uncertainty constraints, the method successfully identifies feasible detours through landmark-rich areas. However, the approach assumes a known environment and known landmark locations, which may not always hold in real GPS-denied scenarios. Future work will focus on relaxing these assumptions, handling dynamic obstacles, and exploring inter-agent belief filters for multi-robot navigation under uncertainty.
Frequently Asked Questions
How does the planner measure localization uncertainty? It uses the trace of the positional covariance derived from the EKF.
When is a trajectory segment considered infeasible? A segment is infeasible if it violates the user-defined uncertainty threshold between two nodes.
How is the cost of an edge defined? It is the arc length of the trajectory generated by the carrot chase controller.
What assumptions does the approach make? It assumes that the environment and landmark locations are known.