Robust Tracking with Model Mismatch for Fast and Safe Planning: an SOS Optimization Approach

TL;DR

This paper introduces an SOS-based optimization framework for safe, real-time motion planning with model mismatch, providing guarantees of dynamic feasibility for high-dimensional systems by offline computation of tracking bounds.

Contribution

It develops a bilinear SOS optimization method to compute feedback tracking controllers and safety bounds, enabling scalable, safe planning for complex systems beyond traditional HJ analysis.

Findings

SOS approach improves scalability of safety guarantees

Trade-off between computational efficiency and conservativeness analyzed

Framework successfully applied to high-dimensional systems

Abstract

In the pursuit of real-time motion planning, a commonly adopted practice is to compute a trajectory by running a planning algorithm on a simplified, low-dimensional dynamical model, and then employ a feedback tracking controller that tracks such a trajectory by accounting for the full, high-dimensional system dynamics. While this strategy of planning with model mismatch generally yields fast computation times, there are no guarantees of dynamic feasibility, which hampers application to safety-critical systems. Building upon recent work that addressed this problem through the lens of Hamilton-Jacobi (HJ) reachability, we devise an algorithmic framework whereby one computes, offline, for a pair of "planner" (i.e., low-dimensional) and "tracking" (i.e., high-dimensional) models, a feedback tracking controller and associated tracking bound. This bound is then used as a safety margin when generating motion plans via the low-dimensional model. Specifically, we harness the computational tool of sum-of-squares (SOS) programming to design a bilinear optimization algorithm for the computation of the feedback tracking controller and associated tracking bound. The algorithm is demonstrated via numerical experiments, with an emphasis on investigating the trade-off between the increased computational scalability afforded by SOS and its intrinsic conservativeness. Collectively, our results enable scaling the appealing strategy of planning with model mismatch to systems that are beyond the reach of HJ analysis, while maintaining safety guarantees.