FaSTrack:A Modular Framework for Real-Time Motion Planning and Guaranteed Safe Tracking

Real-time, guaranteed safe trajectory planning is vital for navigation in unknown environments. However, real-time navigation algorithms typically sacrifice robustness for computation speed. Alternatively, provably safe trajectory planning tends to be too computationally intensive for real-time replanning. We propose FaSTrack, Fast and Safe Tracking, a framework that achieves both real-time replanning and guaranteed safety. In this framework, real-time computation is achieved by allowing any trajectory planner to use a simplified planning model of the system. The plan is tracked by the system, represented by a more realistic, higher dimensional tracking model. We precompute the tracking error bound (TEB) due to mismatch between the two models and due to external disturbances. We also obtain the corresponding tracking controller used to stay within the TEB. The precomputation does not require prior knowledge of the environment. We demonstrate FaSTrack using Hamilton–Jacobi reachability for precomputation and three different real-time trajectory planners with three different tracking-planning model pairs.

I. Introduction

Achieving safe navigation in real time is difficult for many common dynamical systems due to the computational complexity of generating and formally verifying the safety of dynamically feasible trajectories. To achieve real-time planning, many algorithms use highly simplified model dynamics or kinematics to create a nominal trajectory that is then tracked by the system using a feedback controller, such as a linear quadratic regulator (LQR). These nominal trajectories may not be dynamically feasible for the true autonomous system, resulting in a tracking error between the planned path and the executed trajectory. This concept is illustrated in Fig. 1. Additionally, external disturbances (e.g., wind) can be difficult to account for using real-time planning algorithms. Common practice techniques augment obstacles by an ad hoc safety margin, which may alleviate the problem but is performed heuristically and, therefore, does not guarantee safety.