Optimal Path Planning with Localization constraints

Chinmay Mundane; Manav Mishra; PB Sujit

cs.ROOct 29, 2026

Optimal Path Planning with Localization constraints

Authors: Chinmay Mundane, Manav Mishra, PB Sujit

Organizations: IISER Bhopal, India

Abstract

This paper studies UAV path planning in areas where GPS signals are not present. Because these drones can easily get lost when they don't have GPS, they must rely on dead reckoning and occasionally pass near cellular towers, which act as landmarks to reset and fix their position. The goal is to find the shortest path from a start to a goal location while keeping the estimation uncertainty within a given bound. To solve this problem, we propose a localization-constrained A* planner that treats uncertainty as a hard constraint during search. The robot state and covariance are propagated using an Extended Kalman Filter (EKF), and only paths that satisfy the covariance limit are considered. We also show that the proposed method does not affect heuristic used in A* and remains admissible and consistent, ensuring optimality among all feasible paths. Extensive Monte Carlo simulations and PyBullet quadrotor test show that the proposed method finds shorter and more reliable paths compared to RRT* and PRM, while always satisfying the uncertainty constraint.

Figures & tables

Figure 1: Illustration of the planning problem considered in this work. The robot must navigate from a fixed start to a goal location while avoiding obstacles and periodically using landmarks to bound state uncertainty.

Figure 2: Trajectory generated by the uncertainty-constrained A*, RRT* and PRM planner for an environment with

1\%

obstacle density,

1\%

landmark density, and covariance threshold of

\Sigma_{\max}=15

Figure 3: Tracking error and uncertainty evolution for the A* planner. Position errors remain within their $\pm 3\sigma$ bounds, and localization uncertainty stays below $\Sigma_{\max}$ =15 at all times.

Figure 4: Tracking error and uncertainty evolution for the PRM planner. Position errors remain within their $\pm 3\sigma$ bounds, and localization uncertainty stays below $\Sigma_{\max}$ =15 at all times.

Figure 5: Tracking error and uncertainty evolution for the RRT* planner. Despite longer trajectories and requiring frequent updates, position errors remain within their $\pm 3\sigma$ bounds, and localization uncertainty stays below $\Sigma_{\max}$ =15 at all times.

Figure 6: PyBullet quadrotor simulation showing the planned trajectory (blue), obstacles (red), and landmarks(green). The uav follows the uncertainty-constrained path while navigating through landmark regions.

Figure 7: Tracking and estimation performance in PyBullet simulation. The executed trajectory closely follows the planned path, demonstrating bounded localization uncertainty during flight.

Figure 8: Effect of landmark density on success rate with obstacle percentage at 5% and covariance threshold $\Sigma_{\max}=6$ .

Figure 9: Effect of landmark density on mean path length with obstacle percentage at 5% and covariance threshold $\Sigma_{\max}=6$ .

Figure 10: Effect of landmark density on computation time with obstacle percentage at 5% and covariance threshold $\Sigma_{\max}=6$ .

Figure 11: Effect of obstacle density on success rate with landmark percentage at 10% and covariance threshold $\Sigma_{\max}=6$ .

Figure 12: Effect of obstacle density on mean path length with landmark percentage at 10% and covariance threshold $\Sigma_{\max}=6$ .

Figure 13: Effect of obstacle density on computation time with landmark percentage at 10% and covariance threshold $\Sigma_{\max}=6$ .

Figure 14: Effect of covariance threshold on success rate with landmark percentage at 10% and obstacle percentage at 5%.

Figure 15: Effect of covariance threshold on mean path length with landmark percentage at 10% and obstacle percentage at 5%.

Figure 16: Effect of covariance threshold on computation time with landmark percentage at 10% and obstacle percentage at 5%.

UAV Navigation

Unmanned aerial vehicle (UAV) navigation develops methods for locating and guiding drones safely through known, unknown, and changing environments.

See all 273 papers

UDAV: Uncertainty-Driven Adaptive VLM Waypoint Planner
Sep 14, 2026Ghazal Farhani, Shabnam ShabaniUncertainty Estimation for VLA Models Trajectory Generation
The authors are with the Automotive and Surface Transportation Research Centre, National Research Council Canada, London, Ontario, Canada.
Integrated Exploration-Aware UAV Route Optimization and Path Planning
May 27, 2026Jimin Choi, Grant Stagg, Cameron K. Peterson +1Belief-Space Planning Trajectory Optimization
Department of Aerospace Engineering, University of Michigan, Ann Arbor, MI 48109, USA · Department of Electrical Engineering, Brigham Young University, Provo, UT 84602, USA · Department of Aerospace Engineering, Department of Civil and Environmental Engineering, and Department of Industrial and Operations Engineering, University of Michigan, Ann Arbor, MI 48109, USA
Model Predictive Planner for UAV Navigation in Non-Convex Air Corridors
Jul 27, 2026Henrique Silva, Marcelo A. Santos, Guilherme V. RaffoConstrained Motion Planning Trajectory Generation
Graduate Program in Electrical Engineering, Universidade Federal de Minas Gerais, Belo Horizonte, MG, Brazil · Department of Management, Information and Production Engineering, University of Bergamo, Dalmine, BG, Italy · Department of Electronic Engineering, Universidade Federal de Minas Gerais, Belo Horizonte, MG, Brazil

Sep 14, 2026J/K move · Enter open · S save

UDAV: Uncertainty-Driven Adaptive VLM Waypoint Planner

Ghazal Farhani, Shabnam Shabani

The authors are with the Automotive and Surface Transportation Research Centre, National Research Council Canada, London, Ontario, Canada.

Uncertainty Estimation for VLA Models Trajectory Generation Robot Navigation UAV Navigation Vision-Language Navigation Aerial VLN+1

Open paper AskPDF arXiv

Vision-language models (VLMs) can generate routes directly from aerial imagery for off-road navigation, but their predictions provide no indication of reliability. We present UDAV, an Uncertainty-Driven Adaptive VLM Waypoint Planner for UAV-guided UGV navigation. UDAV draws multiple stochastic trajectory predictions, selects their medoid as a self-consistent nominal route, and estimates predictive uncertainty from their spatial dispersion. When the maximum uncertainty across interior waypoints exceeds a threshold, UDAV invokes a reconsideration stage; otherwise, it returns the medoid directly. We evaluate UDAV on 400 held-out trajectory queries from two UAV flights. Stochastic medoid selection reduces the mean average displacement error (ADE) from 147.4 pixels for a deterministic prediction to 115.9 pixels. The complete planner achieves a mean ADE of 110.4 pixels, a 25.1% reduction relative to deterministic planning, while producing valid trajectories for all queries. UDAV also yields the lowest 90th- and 95th-percentile errors among all evaluated configurations, including a higher-budget K=10 consensus baseline. Relative to the K=5 medoid, UDAV reduces these errors from 225.3 and 326.0 pixels to 199.0 and 290.8 pixels, respectively. These results demonstrate that stochastic VLM predictions provide both a stronger nominal route and an actionable uncertainty signal for selectively mitigating large planning errors.

Abstract

UAV Navigation

Explore similar work

UDAV: Uncertainty-Driven Adaptive VLM Waypoint Planner

Integrated Exploration-Aware UAV Route Optimization and Path Planning

Model Predictive Planner for UAV Navigation in Non-Convex Air Corridors