Safe and Robust Planning for Uncertain Robots: A Closed-Loop State Sensitivity Approach

In this work, we detail a comprehensive framework for safe and robust planning for robots in presence of model uncertainties. Our framework is based on the recent notion of closed-loop state sensitivity, which is extended in this work to also include uncertainties in the initial state. The proposed framework, which considers the sensitivity of the nominal closed-loop system w.r.t.~both model parameters and initial state mismatches, is exploited to compute tubes that accurately capture the worst-case effects of the considered uncertainties. In comparison to the current state-of-the-art for safe and robust planning, the proposed closed-loop state sensitivity framework has the important advantage of computational simplicity and minimal assumptions (and simplifications) regarding the underlying robot closed-loop dynamics. The approach is validated via both extensive simulations and real-world experiments. In the experiments we consider as case study a nonlinear trajectory optimization problem aimed at generating an intrinsically robust and safe trajectory for an aerial robot for safely performing an obstacle avoidance maneuver despite the uncertainties. Simulation and experimental results further confirm the viability and interest of the proposed approach.