Trajectory Planning Under Stochastic and Bounded Sensing Uncertainties Using Reachability Analysis

Abstract

Trajectory planning in the presence of motion and sensing uncertainties is an active field of research for autonomous systems. Previous works predict state and state estimation uncertainties along candidate trajectories in order to check for collision safety. However, these works assume either a stochastic or a bounded sensing uncertainty model, which may not always be valid. For instance GNSS pseudorange measurements are typically modeled to contain stochastic uncertainties along with an additional bias due to signal reflections in urban environments. Thus, given bounds for the additional bias, the trajectory planner needs to account for the presence of both stochastic and bounded sensing uncertainties. In our prior work [1] we presented a reachability analysis to predict state and state estimation uncertainties in the presence of both stochastic and bounded sensing uncertainties. However, we ignored the correlation between the state and the state estimation uncertainties, which led to an imperfect approximation of the state uncertainty. Thus, in this paper we first improve our prior reachability analysis by predicting state uncertainty as a function of independent (and hence uncorrelated) quantities. Next, we design a metric for the predicted state uncertainty in order to compare candidate trajectories within a planning framework. Finally, we demonstrate the applicability of the trajectory planner for GNSSbased navigation of fixed-wing unmanned aerial systems (UAS) in urban environments. We statistically validate the collision safety of the planned trajectories for a single UAS in a static environment and for multiple UAS in a shared airspace.