PA-FaSTrack: Planner-aware real-time guaranteed safe planning

Guaranteed safe online trajectory planning is becoming an increasingly important topic of robotic research, due to the need to react quickly in unknown environments. However, as a result of modelling mismatch, some error during trajectory tracking is inevitable. We present Planner-Aware FaSTrack, or PA-FaSTrack, which provides guaranteed Tracking Error Bounds (TEBs) by solving a Hamilton-Jacobi (HJ) variational inequality in the tracking error space. PA-FaSTrack improves upon the state-of-the-art method, FaSTrack [1], by accounting for motion primitives implied by the planning algorithm in the problem formulation. Our method provides a sequence of TEBs, with each TEB corresponding to a segment of the planned path. We also propose necessary modifications to real time tree based planning algorithms in order to make them compatible with the provided TEB sequence. By integrating planning and tracking more closely together, we greatly decrease the degree of conservatism compared to the original FaSTrack, allowing the autonomous system to navigate safely through much narrower spaces. We demonstrate our method using two representative dynamical systems.