Sparsity-Promoting Reachability Analysis and Optimization of Constrained Zonotopes

TL;DR

Methods to formulate and solve optimization problems for dynamic systems in real time using constrained zonotope reachability analysis and a combined set-valued state estimation and moving horizon estimation algorithm are presented and experimentally demonstrated in the context of robot localization.

Abstract

The constrained zonotope is a polytopic set representation widely used for set-based analysis and control of dynamic systems. This paper develops methods to formulate and solve optimization problems for dynamic systems in real time using constrained zonotope reachability analysis. An alternating direction method of multipliers (ADMM) algorithm is presented that makes efficient use of the constrained zonotope structure. To increase the efficiency of the ADMM iterations, reachability calculations are presented that increase the sparsity of the matrices used to define a constrained zonotope when compared to typical methods. The developed methods are used to formulate and solve predictive control, state estimation, and safety verification problems. Numerical results show that optimization times using the proposed approach are competitive with state-of-the-art QP solvers and conventional problem formulations. A combined set-valued state estimation and moving horizon estimation algorithm is presented and experimentally demonstrated in the context of robot localization.

Figures (10)

• Figure 1: Online reachability analysis with constrained zonotopes is used to construct the feasible space of an optimal control or estimation problem. ADMM is then used to find a trajectory within the reachable sets that minimizes a specified quadratic objective function.
• Figure 2: Reachable sets $\mathcal{X}_k$ for the second-order linear system \ref{['eq:second-order-sys']}. The gray set is the $N=15$-step reachable set $\mathcal{X}_{N}$.
• Figure 3: Sparsity plots for matrices $G_N$ and $A_N$ where $\mathcal{X}_N = \left\langle G_N, \mathbf{c}_N, A_N, \mathbf{b}_N \right\rangle$ using the standard \ref{['eq:reachability-trad']}, graph of function \ref{['eq:reachability-state-update']}, and sparsity-promoting \ref{['eq:reachability-alt-int']} reachability calculations to build the $N$-step reachable set $\mathcal{X}_N$.
• Figure 4: Optimal trajectory for the MPC optimization problem with $N=55$. The blue sets are the time-varying position constraint sets $\mathcal{P}_k$, and the black line corresponds to the reference states $\mathbf{x}^r_k$. The red dots are the optimized trajectory produced by the ADMM solver in ZonoOpt. The green obstacle polytopes are displayed only for illustrative purposes and are not directly considered in the MPC problem formulation.
• Figure 5: MPC solution times versus MPC horizon using an Ubuntu 22.04 desktop with an Intel® Core TM i7-14700 × 28 processor and 32 GB of RAM. CZ denotes a constrained zonotope problem formulation (Algorithm \ref{['alg:mpc-reachability']}) while H-rep denotes a sparse MPC problem formulation using H-rep polytopic constraints (Eq. \ref{['eq:hrep-mpc']}). All solvers are invoked via their respective Python interfaces.

Theorems & Definitions (3)

• Proposition 1
• Theorem 1
• Corollary 1