Robust feedback motion planning via contraction theory

Abstract

We present a framework for online generation of robust motion plans for robotic systems with nonlinear dynamics subject to bounded disturbances, control constraints, and online state constraints such as obstacles. In an offline phase, one computes the structure of a feedback controller that can be efficiently implemented online to track any feasible nominal trajectory. The offline phase leverages contraction theory, specifically, Control Contraction Metrics, and convex optimization to characterize a fixed-size “tube” that the state is guaranteed to remain within while tracking a nominal trajectory (representing the center of the tube). In the online phase, when the robot is faced with obstacles, a motion planner uses such a tube as a robustness margin for collision checking, yielding nominal trajectories that can be safely executed, that is, tracked without collisions under disturbances. In contrast to recent work on robust online planning using funnel libraries, our approach is not restricted to a fixed library of maneuvers computed offline and is thus particularly well-suited to applications such as UAV flight in densely cluttered environments where complex maneuvers may be required to reach a goal. We demonstrate our approach through numerical simulations of planar and 3D quadrotors, and hardware results on a quadrotor platform navigating a complex obstacle environment while subject to aerodynamic disturbances. The results demonstrate the ability of our approach to jointly balance motion safety and efficiency for agile robotic systems.

Keywords

Aerial robotics cognitive control architectures cognitive robotics field and service robotics mechanisms design,and control motion control underactuated robots

1. Introduction

Despite significant progress in the field of motion planning, the problem of safe real-time planning for robots with nonlinear and underactuated dynamics subject to uncertainty has remained an outstanding challenge. A key difficulty is that uncertainty and disturbances in the dynamics force us to reason about the “funnel” (or tube) of possible outcomes (see Figures 1 and 2) that the disturbances may drive the system to, rather than a single planned trajectory. Consequently, the key challenges for planning translate into the ability to guarantee both safety (with respect to constraint satisfaction and collision avoidance) and performance (with respect to some cost function optimality) for the robotic system in cluttered and possibly dynamically changing environments.

1.1. Outline of approach

The goal of this paper is to propose an approach for generating motion plans with certifiable margins of safety. In particular, in an offline stage, one synthesizes the structure of a tracking controller which can be efficiently implemented online to guarantee exponential convergence to any feasible nominal trajectory in the absence of disturbances. Additionally, the offline computation yields a fixed-size invariant “tube” (akin to a funnel) that can be centered around any nominal trajectory as a guaranteed collision-free envelope in the presence of bounded disturbances. In the online phase, when the robot is faced with obstacles, one can use such a tube as a robustness margin during collision checking, thus leading to nominal trajectories that can be safely executed. We stress that this paper does not propose a new motion planning algorithm. Instead, it proposes a framework for using a standard motion planner as a “black box” or “primitive routine,” combined with a feedback control design methodology that together enable the online generation of safe nominal motion plans that can be robustly executed.

The key idea behind our approach is to leverage contraction theory (Lohmiller and Slotine, 1998), a method for analyzing nonlinear systems by studying convergence between pairs of trajectories. This makes it particularly well-suited to the problem we consider here since it does not require us to commit to a particular nominal trajectory in order to analyze the stability properties of a feedback controller designed to track it. In particular, we design tracking controllers by using control contraction metrics (CCMs) (Manchester and Slotine, 2017), a generalization of control Lyapunov functions that can be computed using convex optimization.

2. Problem statement and related work

Consider robotic systems whose dynamics are described by the nonlinear differential equation

\dot{x} (t) = f (x (t)) + B (x (t)) u (t) + B_{w} (x (t)) w (t)

(1)

where

x (t) \in R^{n}

is the state,

u (t) \in R^{m}

is the control input, and

w (t) \in R^{n_{w}}

is the disturbance. The function

f : R^{n} \to R^{n}

captures the drift,

B : R^{n} \to R^{n \times m}

is the input matrix mapping, depicted in column-stacked form as (b₁, …, b_m), and

B_{w} : R^{n} \to R^{n \times n_{w}}

is the disturbance matrix mapping with

\bar{σ} (B_{w} (x)) = 1

for all x, where

\bar{σ} (\cdot)

denotes the maximum singular value. In other words, B_w simply selects the channels where the disturbance is active. A state-input trajectory satisfying (1) is denoted as a pair (x, u). Additionally, we enforce state constraints (e.g., arising from obstacles in the robot’s environment and physical constraints such as joint limits) and input constraints, that is

x (t) \in X

and

u (t) \in U

for all t, where

X

and

U

are defined to be the closures of bounded, open, and connected sets in Euclidean space.

The motion planning problem we wish to address is to find a (possibly non-stationary) policy $π : X \times R \to U$ that (i) drives the state x to a compact region $X_{goal} \subseteq X$ , (ii) satisfies the state and input constraints, and (iii) minimizes a quadratic cost

J (x (t), π) : = \int_{0}^{T_{goal}} 1 + ‖ π (x (t), t) ‖_{R}^{2} d t,

where

{‖ (\cdot) ‖}_{R} : = \sqrt{{(\cdot)}^{T} R (\cdot)}

denotes the weighted norm with respect to R, a strictly positive definite matrix, and T_goal is the first time x(t) enters

X_{goal}

The topic of planning under uncertainty has been approached from two general methodologies within the robotics community. In the first approach, one seeks probabilistic guarantees on safety (e.g., collision probabilities) for a stochastic model of uncertainty. This is elegantly described by the chance-constrained programming framework (Charnes and Cooper, 1959). Typical solution methods generally consider linear systems affected by Gaussian noise (Blackmore et al., 2006, 2011; Ono et al., 2013) or more generally, exploit linear-Gaussian dependencies (Luders et al., 2016). The extension to nonlinear dynamics and/or non-Gaussian noise is inherently difficult; typical methods employ sampling (Monte Carlo) techniques (Janson et al., 2015b; Sun et al., 2015), stochastic Lyapunov theory (Battilotti and De Santis, 2003; Buehler et al., 2016), or Gaussian processes with (probabilistically) robust feedback linearization (Helwa et al., 2019). In the more general case with partial observability and/or noisy sensing, the stochastic formulation is lifted to the belief space planning framework (Kaelbling et al., 1998; Kurniawati et al., 2008; Prentice and Roy, 2009; Platt et al., 2012; Agha-mohammadi et al., 2014). This approach has been typically constrained by limitations such as Gaussian belief state assumptions, linear dynamics, and/or small state and action spaces, for instance, in the Partially Observable Markov Decision Process (POMDP) framework.

In the second approach, in contrast to the stochastic methodology, one considers bounded models of uncertainty, where w(t) is assumed to be piecewise $C^{1}$ and norm-bounded, that is, there exists some strictly positive real number $\bar{w}$ such that ${‖ w (t) ‖}_{2} \leq \bar{w}$ for all t ≥ 0. This is typically described as the robust planning problem, and is the formulation adopted within this work. A fundamental component of solving this problem is based around computing and/or optimizing over reachable sets, or their over-approximations. Figure 3 depicts a rough classification of various methods for computing bounds on the reachable sets, ranked by the conservativeness of the approximation. The figure is by no means exhaustive, and additional references are provided following the figure, but it is useful for understanding the overarching categorization of various methods, ranging from approximative linear analysis, through Lyapunov-based certificates computed using convex optimization, to formal verification using dynamic programming and differential games; see also the recent review (Bansal et al., 2017).

On the exact end of the spectrum, one may leverage logic-based methods (e.g., quantifier elimination) to recursively compute the backward reachable set of a given goal set (Raković et al., 2006a; Kong et al., 2015). In a similar vein, the differential game formulation treats any admissible disturbance as an adversarial agent and one can compute the backward reachable set of unsafe sets (e.g., obstacle locations) as the solution to a Hamilton–Jacobi PDE using level-set methods (Tomlin et al., 2003; Gillula et al., 2010; Chen et al., 2016; Herbert et al., 2017; Fridovich-Keil et al., 2018). While these methods are exact in that they precisely characterize a collision-free “roadmap” to the goal set, and in the differential game formulation, also yield the optimal closed-loop controller, implementation of these methods for dynamically changing or unknown environments and/or high-dimensional systems is computationally prohibitive. A computational relaxation in this spirit is the barrier certificates method (Prajna et al., 2007; Barry et al., 2012) in which one characterizes the unsafe regions in the state-space as the zero-superlevel set of a function whose time derivative on the zero-level set is negative for a given controller and all admissible disturbances. While this yields a more conservative sufficient condition, the method is again incompatible with generalizing to unknown environments discovered in real-time.

As presented earlier, the motion planning problem entails an optimization over the class of state-feedback functions—a computationally intractable task in general with a solution that is incompatible with changing environments (e.g., obstacle locations). In an effort to reduce computational complexity, the prevailing solution approach, and indeed the strategy adopted within this work, is to parameterize general state-feedback policies as a sum of a nominal (open-loop) input u* and a feedback term designed to track the nominal state trajectory x* (induced by u* assuming no disturbances).

π (x (t), t) = u * (t) + k (x * (t), x (t))

(2)

where k(⋅, ⋅) is the feedback tracking controller. Commonly referred to as feedback motion planning, such a solution approach represents a compromise between the general class of state-feedback control laws and a purely open-loop formulation (i.e., no tracking). In order to ensure satisfaction of all constraints in the presence of disturbances, one needs to characterize the reachable set or “funnel” (Burridge et al., 1999), around the trajectory being tracked. Formally, this is defined through the notion of robust control invariant (RCI) tubes.

Suppose (x*, u*) is a state-input trajectory satisfying the nominal dynamics (i.e., (1) with w ≡ 0) and (x, u) is a state-input trajectory satisfying (1) under the action of a parameterized policy (2). Let $T_{goal}^{*}$ be the first time x*(t) enters $X_{goal}$ . An RCI tube is defined as follows.

Definition 2.1

(RCI Tube). Let $Ω : R^{n} \to 2^{R^{n}}$ be a mapping s.t. x ∈ Ω(x) and Ω(x) is a closed and bounded set for every x. Then, Ω(⋅) is an RCI mapping (additionally Ω(x) is an RCI set centered on x) if there exists a tracking controller k(x*, x) s.t. if x(t₀). ∈ Ω(x*(t₀)), then for all allowable realizations of the disturbance w(t), x(t). ∈ Ω(x*(t)) for all $t_{0} \leq t \leq T_{goal}^{*}$ . Given an RCI mapping Ω(⋅), an RCI tube centered on the trajectory $x * (t), t_{0} \leq t \leq T_{goal}^{*}$ , is the swept region $\cup_{t_{0} \leq t \leq T_{goal}^{*}} Ω (x * (t))$ .

Intuitively, a tracking controller with an associated RCI tube Ω(⋅) guarantees that the state of the system is always “close” to its nominal value x*(t) (precisely, within set Ω(x*(t)). Thus, by planning a nominal state-input trajectory satisfying the tightened constraints

\begin{array}{l} x * (\cdot) \in \bar{X} : = X ⊖ Ω, \end{array}

(3a)

\begin{array}{c} u * (\cdot) \in \bar{U} : = {\bar{u} \in U : \forall x * (t) \in \bar{X}, \forall x (t) \in X s . t . x (t) \in Ω (x * (t)), \bar{u} + k (x * (t), x (t)) \in U} \end{array},

(3b)

where ⊖ denotes the Minkowski set difference, one can ensure that the robotic system will safely reach the goal region

X_{goal}

(modulo the size of the RCI set) in the presence of disturbances. Note that one can ensure that the system reaches

X_{goal}

(without the extra buffer from the RCI set) by constraining the RCI set to be contained within

X_{goal}

T_{goal}^{*}

. Constraint (3a) ensures that the RCI tube around the trajectory does not intersect any obstacles, while (3b) is defined for a given tracking controller and ensures that the net applied control satisfies the input limit.

For fully-actuated (i.e., feedback linearizable) systems, RCI tubes or funnels may be computed and optimized using sliding control (Slotine, 2007; Lopez et al., 2018). More conservative approximations of these sets may be found using linear reachability analysis, where one computes continuous linearizations of the dynamics about reference trajectories and treats nonlinearities as bounded disturbances (Althoff and Dolan, 2014; Althoff et al., 2015). Alternatively, by bounding the dynamics’ Lipschitz constant or Jacobian, one obtains an exponentially growing outer approximation of the reachable set. For instance, in Pin et al. (2009), a bound on the global Lipschitz constant is used to compute sequentially tightened constraint sets for a reference trajectory to ensure robust constraint satisfaction, while Bravo et al. (2006) present a general framework for computing outer approximations of reachable sets using zonotopes, which, while possessing favorable computational properties, again yield overly conservative approximations. The differential inequality approach in Scott and Barton (2013) and Villanueva et al. (2017) attempts to alleviate the conservativeness resulting from such linear methods through online co-optimization of the reference trajectory and its associated reachable set. In similar spirit, Manchester and Kuindersma (2017, 2019) leverage a measure of the size of the approximate invariant funnels computed using linear analysis (i.e., propagation of ellipsoids under linearized dynamics) within the formulation of the cost function for the nominal trajectory itself. However, these problems are posed as one-off offline computations and not suitable for real-time re-planning on fast robotic systems. In general, treating nonlinearities as bounded disturbances naturally leads to overly conservative approximations.

Convex programming-based verification methods such as sum-of-squares (SOS) programming have gained increasing popularity in feedback motion planning. For instance, the LQR-Trees algorithm (Tedrake et al., 2010) constructs a tree of local LQR feedback controllers; however, it cannot handle scenarios in which the task and environment are unknown until runtime. Recently, the funnel library approach (Majumdar and Tedrake, 2012, 2017) has been proposed to handle online geometric constraints (e.g., obstacles) that force the robot to re-plan in real-time. The approach leverages SOS programming to compute, offline, a library of funnels around a set of nominal trajectories in which the state is guaranteed to remain despite bounded disturbances. These funnels are then sequentially composed online to avoid obstacles. However, this approach is restricted to employing a fixed set of trajectories computed offline. While the richness of the funnel library may be increased by exploiting invariances in the dynamics (Majumdar and Tedrake, 2017) or pre-computing a family of funnels parameterized by shifts to a nominal trajectory (Majumdar et al., 2012), one would ideally like to generate a funnel around any nominal trajectory generated online.

The concept of feedback motion planning is also fundamental within Tube Model Predictive Control (TMPC), whereby one computes a tracking feedback (also termed ancillary) controller that keeps the state within an invariant “tube” around the nominal MPC trajectory despite disturbances. TMPC has been studied extensively for linear systems with bounded disturbances or model uncertainties (Langson et al., 2004; Mayne et al., 2005; Limon et al., 2010; Farina and Scattolini, 2012; Rakovic et al., 2012), and for linear systems with stochastic disturbances (Fleming et al., 2015) (see also the recent review Mayne, 2014). The application of TMPC to nonlinear systems is certainly not new; see, for instance, Raković (2009) where the properties of TMPC for nonlinear systems are explored via lifting the analysis to set dynamics and employing the Banach fixed-point theorem. However, the construction of invariant tubes and the design of the associated ancillary controller in the nonlinear setup is significantly more complicated than in the linear case. In Raković (2009), for instance, the existence of a stabilizing (nonlinear) ancillary controller (that results in contracting set iterates) is simply assumed, while in Kögel and Findeisen (2015), a static linear state-feedback ancillary controller is used to stabilize the “linear” component of the nonlinear dynamics and Lipschitz continuity is used to bound the effect of disturbance propagation. Some techniques to construct the ancillary controller and accompanying invariant tube proposed in existing literature include systems with matched nonlinearities and linear ancillary feedback (Raković et al., 2006b); integral sliding mode ancillary feedback with Lipschitz-based reachability analysis (Rubagotti et al., 2011); ellipsoidal invariant tubes constructed using linear-matrix-inequalities (LMI) and bounds on the Lipschitz constant (Yu et al., 2013) or assuming a polytopic linear differential inclusion model for the dynamics (Yu et al., 2010); ancillary-MPC with Lipschitz-based reachability analysis (Mayne et al., 2011); systems with incrementally conic uncertainties stabilized using linear ancillary feedback (Carsons et al., 2013); and linearization with static or time-varying linear ancillary feedback with nonlinearities treated as bounded disturbances (Cannon et al., 2011). As with the robotic planning literature, methods leveraging linearization or Lipschitz constants are inherently overly conservative. The notion of incremental input-to-state stability (δ-ISS) for discrete-time systems was used in Bayer et al. (2013) to derive the invariant tube as a sublevel set of the associated δ-ISS Lyapunov function, which was assumed to be given. More recently, Köhler et al. (2019) leverage the assumed existence of such a function and implicitly incorporate constraint tightening via constraining the growth of this function (using a nonlinear dynamical representation of the scalar defining the Lyapunov sublevel sets) as part of the MPC optimization problem. Again, these bounding functions are either assumed given, or constructed assuming local linear feedback. In contrast, the work presented herein focuses on the design and optimization of the functions themselves (and the associated feedback controllers), subsequently permitting the use of algorithms such as the one presented in Köhler et al. (2019).

2.1. Statement of contributions

In this paper, we leverage recent advances in contraction theory for control design through the use of control contraction metrics (CCM) (Manchester and Slotine, 2017). Contraction theory (Lohmiller and Slotine, 1998) is a method of analyzing nonlinear systems in a differential framework, that is, via the associated variational system (Crouch and Van der Schaft, 1987, Chp 3), and is focused on the study of convergence between pairs of state trajectories toward each other. Thus, at its core, contraction explores a stronger notion of stability—that of incremental stability between solution trajectories, instead of the stability of an equilibrium point or invariant set. While the analysis in Lohmiller and Slotine (1998) and most other works on contraction theory focus on analyzing the stability of closed-loop vector fields—(Jouffroy, 2003; Sontag, 2010; Sontag et al., 2014; Simpson-Porco and Bullo, 2014; Forni and Sepulchre, 2014); see also the recent review in Aminzarey and Sontag (2014) and references therein, recent results demonstrate the applicability of contraction theory for constructive control design, for example, control via backstepping (Sharma and Kar, 2009; Zamani et al., 2013), control for singularly perturbed systems using multiple time-scales (Rayguru and Kar, 2015), and control via CCMs (Manchester et al., 2015; Manchester and Slotine, 2017; Singh et al., 2017). The concept of δ-ISS has also been studied within a contraction theory framework, for example, in Zamani et al. (2013), where contraction metrics are derived for a class of nonlinear systems stabilized using backstepping. Compared to works on establishing incremental stability through suitable δ-ISS Lyapunov functions (Angeli, 2002, 2009), contraction metrics are an intrinsic characterization of incremental stability (i.e., coordinate invariant) and the search for a suitable metric and associated stabilizing controller via convex optimization boasts obvious practical benefits.

The contributions of this paper are the following. First, on the theoretical side, while the CCM approach is directly inspired by Manchester and Slotine (2017), we present an alternative proof of incremental exponential stability between trajectories using a suitable CCM-derived controller (Section 3.3). This proof employs techniques from calculus of variations and differential geometry in order to derive a tighter characterization of the controller’s disturbance rejection properties and the size of the corresponding invariant tube (Section 3.4), under significantly weaker conditions for the CCM as compared to Manchester and Slotine (2017) and Zamani et al. (2013). Second, we derive a bound on the magnitude of the tracking controller by leveraging properties of CCMs and exploiting analogies with differential control Lyapunov functions (Section 4.5). Third, on the algorithmic side, we present an offline/online framework for robust motion planning where in the offline phase, we formulate a quasiconvex optimization problem using SOS programming that searches for an optimal CCM in order to minimize the cross-section of the invariant tube (Section 4). The optimized tube is used online to obtain nominal motion plans that can be robustly tracked (Section 5). Fourth, on the experimental front, we illustrate our method and several variations on simulated 6-state planar-quadrotor and 10-state 3D quadrotor systems (Section 6). Finally, we present experimental results on a quadrotor hardware testbed by generating robust motion plans using sampling-based planning and polynomial smoothing for a cluttered lab environment and evaluate the performance of the quadrotor in the presence of aerodynamic disturbances induced by unmodeled drag forces (Section 7).

A preliminary version of this work was presented at ICRA 2017 (Singh et al., 2017). In this revised and extended version, we additionally present (i) full proofs for all technical results, (ii) a tutorial on CCM computation via illustrative examples, (iii) extensions to the online planning algorithm to leverage less conservative time-varying tubes, (iv) extensive additional numerical experiments, including failure analysis, and (v) validation on a hardware testbed.

Our approach has several advantages over prior work on robust motion planning and MPC. First, by explicitly enforcing safety in the online planning process, the method is particularly suited to planning in previously unseen and tightly constrained environments, where it might be difficult to find feasible solutions by sequencing a pre-computed set of maneuvers (Figure 1). Second, in contrast to the class of techniques that employ linear reachability analysis to conservatively approximate funnels/tubes for nonlinear systems by treating nonlinearities as bounded disturbances, our analysis directly reasons about intrinsic nonlinearities in the dynamics and thus has the potential to be less conservative for highly nonlinear systems. Notably, our approach allows for both the design and optimization of invariant tubes and tracking feedback controllers. Third, the method is computationally rooted in convex optimization, which in turn carries a smaller computational burden than differential game formulations for computing reachable tubes, which require numerical solutions to PDEs.

2.2. Organization

The paper is organized into four key sections:

• Trajectory Tracking with Contraction Theory

1. Introduction and Definitions; Sections 3.1–3.2: Introduction to Contraction Theory and CCMs as differential Control Lyapunov functions.

2. Exponentially Stabilizing Controllers; Section 3.3: Define the CCM-induced Riemannian distance and prove exponential stabilizability via explicit- and implicitly defined CCM-based feedback controllers.

3. Disturbance Rejection; Section 3.4: Prove Riemannian distance boundedness under the assumption of bounded additive disturbances, thereby defining the RCI set.

• Offline Synthesis of CCMs and Computation of Bounds

1. Characterizing the RCI Tube; Section 4.1: Define a fixed-size ellipsoidal outer approximation to the RCI set to enable fast planning.

2. Synthesizing Optimized CCMs; Sections 4.2–4.4: Formulate minimization of the RCI set’s size as a quasiconvex functional optimization, and illustrate a tractable finite-dimensional reduction via SOS programming.

3. Bounding the CCM Tracking Controller; Section 4.5: Derive bounds on the CCM feedback controller for pairs of states contained within an RCI set.

• Robust Planning; Section 5: Formulate planning with tightened constraints and present a receding-horizon variation to allow online RCI set-aware re-planning.

• Experiments

1. Numerical Experiments—Planar and 3D Quadrotors; Section 6: Illustrate offline CCM synthesis and RCI set computation, online execution via planning with sampling-based planners and polynomial splines, validation of theoretical bounds under varying disturbances.

2. Hardware Experiments—Quadrotor Racecourse; Section 7: Deployment of the CCM feedback controller on an embedded device, calibrating a drag model as the disturbance, planning and execution through a fixed race-course, validation of theoretical bounds and statistical analysis of tracking performance.

Notation: Let $S_{j}$ be the set of symmetric matrices in $R^{j \times j}$ and denote $S_{j}^{\geq 0}$ , respectively $S_{j}^{> 0}$ , to be the set of symmetric positive semi-definite, respectively, positive definite matrices in $R^{j \times j}$ . Given a matrix X, let $\hat{X} : = X + X^{T}$ . The set of $C^{2}$ functions from $D$ to $R$ is denoted by $C^{2} (D, R)$ . We denote the components of a vector $y \in R^{n}$ as y[j], j = 1, …, n, and its Euclidean norm as ‖y‖. Let ${‖ y ‖}_{A} = \sqrt{y^{T} A y}$ denote a weighted norm for $A \in S_{n}^{> 0}$ . Let $\bar{σ} (A)$ denote the maximum, and $\underline{σ} (A)$ the minimum, singular values of a matrix A. Let $\bar{λ} (A)$ denote the maximum, and $\underline{λ} (A)$ the minimum, real eigenvalues of a symmetric matrix A. Let ∂_yF(x) denote the Lie derivative of the matrix-valued function F at x along the vector y. Finally, given sets $A$ and $B$ , the set $A ⊖ B$ is the Minkowski difference.

3. Trajectory tracking with contraction theory

In this section, we demonstrate how to derive trajectory tracking controllers with (i) exponential convergence properties in the absence of disturbances, and (ii) strong boundedness properties in the presence of bounded disturbances, by leveraging contraction theory. In particular, this section of the paper is concerned with the task of robust nonlinear feedback control design. The integration of the results of this section into a feedback planning algorithm will be the focus of Section 5. The core principle behind contraction theory (Lohmiller and Slotine, 1998) is to study the evolution of distance between any two infinitesimally close neighboring trajectories and draw conclusions on the distance between any finitely apart pair of trajectories. We begin by first introducing the fundamentals of contraction theory, in the absence of control.

3.1. Introduction to contraction

Given an autonomous system of the form: $\dot{x} (t) = f (x (t))$ , consider two neighboring trajectories separated by an infinitesimal (virtual) displacement δ_x; formally, δ_x is a vector in the tangent space $T_{x} X$ at x. The dynamics of this virtual displacement are given by

{\dot{δ}}_{x} = \frac{\partial f}{\partial x} δ_{x},

where ∂f/∂x is the Jacobian of f. The dynamics of the infinitesimal squared distance

δ_{x}^{T} δ_{x}

between these two trajectories is then given by

\frac{d}{d t} (δ_{x}^{T} δ_{x}) = 2 δ_{x}^{T} \frac{\partial f}{\partial x} δ_{x},

Then, if the (symmetric part) of the Jacobian matrix ∂f/∂x is uniformly negative definite, that is

\sup_{x} \bar{λ} (\frac{1}{2} \hat{\frac{\partial f (x)}{\partial x}}) \leq - λ < 0,

for some

λ \in R_{> 0}

, one has that the squared infinitesimal length

δ_{x}^{T} δ_{x}

is exponentially convergent to zero at rate 2λ. By path integration of δ_x between any pair of trajectories, one has that the distance between any two trajectories shrinks exponentially to zero. The vector field f is thereby referred to be contracting at rate λ, and λ is referred to as the contraction rate.

Contraction metrics generalize this observation by considering as infinitesimal squared length distance, a symmetric positive definite function $V (x, δ_{x}) = δ_{x}^{T} M (x) δ_{x}$ , where $M : X \to S_{n}^{> 0}$ is a mapping from $X$ to the set of uniformly positive definite n × n symmetric matrices. Formally, M(x) may be interpreted as a Riemannian metric tensor, endowing the space $X$ with the Riemannian squared length element V(x, δ_x). A fundamental result in contraction theory (Lohmiller and Slotine, 1998) is that any contracting system admits a contraction metric M(x) such that the associated function V(x, δ_x) satisfies

\dot{V} (x, δ_{x}) \leq - 2 λ V (x, δ_{x}), \forall (x, δ_{x}) \in T X

for some positive contraction rate λ. Thus, the function V(x, δ_x) may be interpreted as a differential Lyapunov function; see also Forni and Sepulchre (2014) for such an analogy, albeit using Finsler metrics.

3.2. Control Contraction Metrics

Control contraction metrics (CCMs) generalize contraction analysis to the controlled dynamical setting, in the sense that the analysis searches jointly for a controller design and the metric that describes the contraction properties of the resulting closed-loop system. To define CCMs, we first introduce the concept of variational dynamics.

Denote the tangent space of $X$ at $x \in X$ by $T_{x} X$ ¹ and the tangent bundle of $X$ by $T X = \cup_{x \in X} {x} \times T_{x} X$ . The variational dynamics (i.e., dynamics of the virtual displacement δ_x along any nominal state-input trajectory (x(t), u(t))) for the unperturbed controlled system are given by Crouch and Van der Schaft (1987, Chp 3)

{\dot{δ}}_{x} = \overset{≔ A (x, u)}{\overset{}{(\frac{\partial f (x)}{\partial x} + \sum_{j = 1}^{m} u [j] \frac{\partial b_{j} (x)}{\partial x})}} δ_{x} + B (x) δ_{u}

(4)

where

δ_{x} \in T_{x} X

is a tangent vector to a smooth path of states at

x \in X

, and

δ_{u} \in T_{u} U

is a tangent vector to a smooth path of controls at

u \in U

. Consider now, the following definition:

Definition 3.1

(CCM). Let $M : X \to S_{n}^{> 0}$ be a smooth matrix-valued function that is uniformly bounded (i.e., there exist constants $0 < \underline{α} < \bar{α}$ such that $\underline{α} I_{n} \underline{≺} M (x) \underline{≺} \bar{α} I_{n}$ ). Then, M(x) is a CCM for the system $\dot{x} = f (x) + B (x) u$ , if there exists a differential controller, that is, a map $δ_{u} : T X \to T_{u} U$ , such that $\dot{V} (x, δ_{x}) < 0, \forall (x, δ_{x}) \in T X$ .

This definition of a CCM allows us to interpret V(x, δ_x) as a differential control Lyapunov function (CLF) on the tangent bundle $T X$ . In the next section, we study the necessary and sufficient conditions for the existence of CCMs, and provide a precise algebraic definition.

3.2.1. Exponentially stabilizing CCMs

Under the special case of exponential stability, we can provide a more precise algebraic definition of CCMs. Suppose that the following equality holds for all $x \in X$

\partial_{b_{j}} M (x) + \hat{M (x) \frac{\partial b_{j} (x)}{\partial x}} = 0, j = 1, \dots, m .

(5)

Condition (5) implies that the vectors b_j form a Killing vector field for the metric tensor M(x). Under this condition, $\dot{V}$ reduces to

\dot{V} (x, δ_{x}, δ_{u}) = δ_{x}^{T} (\partial_{f} M (x) + \hat{M (x) \frac{\partial f (x)}{\partial x}}) δ_{x} + 2 δ_{x}^{T} M (x) B (x) δ_{u} .

(6)

Then, if the following property holds for some constant λ > 0 and all $(x, δ_{x}) \in T X$

\begin{array}{l} δ_{x}^{T} (\partial_{f} M (x) + \hat{M (x) \frac{\partial f (x)}{\partial x}}) δ_{x} \leq - 2 λ δ_{x}^{T} M (x) δ_{x} \\ for all δ_{x} such that δ_{x}^{T} M (x) B (x) = 0, \end{array}

(7)

then, Manchester and Slotine (2017) show that there always exists an integrable differential feedback controller of the form δ_u(x, δ_x) = K(x)δ_x such that the following inequality holds for all²

(x, δ_{x}) \in X \times R^{n}

\begin{array}{c} \dot{V} (x, δ_{x}) & = δ_{x}^{T} (\partial_{f} M (x) + \hat{M (x) \frac{\partial f (x)}{\partial x}} + \hat{M (x) B (x) K (x)}) δ_{x} \\ \leq - 2 λ δ_{x}^{T} M (x) δ_{x} = - 2 λ V (x, δ_{x}) \end{array}

(8)

Notice that condition (7) simply indicates that for all directions where the variational system lacks controllability (given by the null-space of B^T(x)M(x)), the system is naturally contracting with rate λ. In the scenario where (5) fails to be true, one may leverage the following weaker alternative to conditions (5) and (7)

\begin{array}{l} δ_{x}^{T} (\partial_{f + B u} M (x) + \hat{M (x) A (x, u)}) δ_{x} \leq - 2 λ δ_{x}^{T} M (x) δ_{x} \\ for all δ_{x} such that δ_{x}^{T} M (x) B (x) = 0 . \end{array}

(9)

In Manchester and Slotine (2017), it is proven, by construction, that the weaker condition above is still sufficient to guarantee the existence of a differential feedback controller. However, in this case, the differential controller becomes a function of u as well, that is, δ_u = δ_u(x, δ_x, u). We can now state a more operational definition for CCMs as follows:

Definition 3.2

(CCM). A CCM for the system $\dot{x} = f (x) + B (x) u$ is a smooth, uniformly bounded matrix-valued function M(x) that either jointly satisfies conditions (5) and (7), or condition (9).

3.3. Incrementally stabilizing controllers

We now illustrate how to leverage a differential controller which guarantees stability on an infinitesimal scale by the property $\dot{V} (x, δ_{x}) \leq - 2 λ V (x, δ_{x})$ along any nominal trajectory, to derive a stabilizing tracking controller for any pair of finitely apart trajectories. To do so, we require a definition of trajectory-based exponential stability:

Definition 3.3

(Incremental Exponential Stability). Consider a nominal state-input trajectory pair $(x * (t), u * (t))$ for the unperturbed controlled dynamics (i.e., (1) with w ≡ 0). Suppose there exist λ, C > 0 and a feedback controller k(x*(t), x(t)) such that the resulting state trajectory x(t) for the unperturbed dynamics with control u(x(t)) = u*(t) + k(x*(t), x(t)) satisfies

‖ x * (t) - x (t) ‖ \leq C e^{- λ t} ‖ x * (0) - x (0) ‖ .

(10)

Then, the trajectory x*(t) is said to be incrementally exponentially stabilizable (IES) with rate λ and overshoot constant C.

To derive our IES-guaranteeing stabilizing controller, we require the following definitions regarding curves and distances on $X$ .

Definition 3.4

(Curves and Distances on Riemannian Manifolds). Treating $V (x, δ_{x}) = δ_{x}^{T} M (x) δ_{x}$ as a Riemannian squared differential length element, for a given smooth curve $c : [0,1] \to X$ , we define its length l(c) and energy $E (c)$ as

\begin{array}{c} l (c) : = \int_{0}^{1} \sqrt{V (c (s), δ_{c} (s))} d s \\ E (c) : = \int_{0}^{1} V (c (s), δ_{c} (s)) d s \end{array}

where δ_c(s)≔∂c(s)/∂s. Let Γ(p, q) be the set of smooth curves on

X

that connect points p and q. The Riemann distance between points p and q is defined as the quantity d(p, q)≔ inf_c∈Γ(p,q)l(c), and let the curve γ ∈ Γ(p, q) be a (possibly non-unique) minimizing geodesic which achieves this infimum. Denote

E (p, q) : = d^{2} (p, q)

to be the Riemann energy, and note the identity

E (γ) = d^{2} (p, q)

Given a desired nominal state-input trajectory pair (x*(t), u*(t)), let $γ (\cdot, t) : [0,1] \to X$ denote a minimizing geodesic connecting x*(t) and x(t). Consider the following control law

\begin{array}{c} π (x (t), t) & = u * (t) + \int_{γ (\cdot, t)} δ_{u} (γ (s, t), δ_{γ} (s, t)) d s \\ = u * (t) + \underset{= k (x * (t), x (t))}{\underset{}{\int_{0}^{1} K (γ (s, t)) δ_{γ} (s, t) d s}} . \end{array}

(11)

where δ_γ(s, t)≔∂γ(s, t)/∂s. The geometric interpretation of (11) and inequality (8) is illustrated in Figure 4.

In case the weaker condition (9) is used, δ_u = δ_u(x, δ_x, u), and thus, π(x(t), t) will be given by the solution to the differential equation

π (x (t), t) = u * (t) + \int_{γ (\cdot, t)} δ_{u} (γ (s, t), δ_{γ} (s, t), π (γ (s, t), t)) d s,

(12)

where δ_u(⋅) is designed such that

\dot{V} (x, δ_{x}, u) \leq - 2 λ V (x, δ_{x})

for all (x, δ_x) along the minimizing geodesic. Theorem 3.5, which is central to the approach, proves (under a mild technical assumption) that control law (11) ensures the trajectory x(t) is IES with respect to x*(t) in the sense of Definition 3.3.

Theorem 3.5

(Incrementally stabilizing controller). Let (x*, u*) and (x, u) be state-input trajectory pairs for the nominal dynamics where π(x(t), t) is given by (11) using a CCM which satisfies equations³ (5) and (7). Suppose that the set of time instances where x(t) is on the cut-locus for x*(t) (i.e., the minimizing geodesic between x*(t) and x(t) is not unique), is a strong measure zero subset of $R_{\geq 0}$ . Then, x(t) is IES with respect to x*(t) in the sense of Definition 3.3.

Proof. Outside of the set of times where x(t) falls on the cut-locus of x*(t) (a measure zero set itself), the controller (11) is smooth. Thus, the state-input trajectory pair (x, u) is well-defined in the sense that $x (\cdot) : R_{\geq 0} \to X$ is absolutely continuous and satisfies the nominal dynamics for almost all t. Notice that at each time instant t, (11) implicitly defines a smooth virtual parameterized surface of solutions $c_{t} : (s, t) \in [0,1] \times (- ε, ε) \mapsto X$ (where ɛ is an arbitrarily small positive value), to the nominal dynamics as follows

\frac{\partial c_{t}}{\partial t} (s, t) = f (c_{t} (s, t)) + B (c_{t} (s, t)) π_{t} (c_{t} (s, t), t), c_{t} (\cdot, 0) = γ (\cdot, t),

(13)

where

π_{t} (c_{t} (s, t), t) = u * (t + t) + \int_{0}^{s} δ_{u} (c_{t} (s, t), δ_{c_{t}} (s, t)) d s,

(14)

where

δ_{c_{t}} (s, t) = \partial c_{t} (s, t) / \partial s

. Consider the time derivative of the Riemann energy

E (x * (t), x (t))

between x*(t) and x(t). If x(t) ∉ Cut(x*(t)) (where Cut(x) denotes the cut-locus at x), then the derivative of

E (x * (t), x (t))

is well-defined at t (i.e., the exponential map is a local diffeomorphism) and given by Spivak (1999, Chp 9)

\dot{E} (x * (t), x (t)) = {[{(\frac{\partial V}{\partial δ_{x}} (γ (s, t), δ_{γ} (s, t)))}^{T} \frac{\partial γ (s, t)}{\partial t}] |}_{s = 0}^{s = 1}

(15)

This result is true for all t ≥ 0 such that x(t) ∉ Cut(x*(t)). For completeness, suppose now that x(t) ∈ Cut(x*(t)), a Lebesgue measure zero set. In this scenario, the minimizing geodesic γ(⋅, t) may not be unique and thus the Riemannian energy $E (x * (t), x (t))$ is not smooth. However, it can be shown that the upper Dini derivative of $E (x * (t), x (t))$ , defined as

D^{+} E (x * (t), x (t)) : = \underset{t ↘ 0}{\lim \sup} \frac{E (x * (t + t), x (t + t)) - E (x * (t), x (t))}{t}

exists, and satisfies the inequality⁴ (Adelstein and Epstein, 2017)

D^{+} E (x * (t), x (t)) \leq {[{(\frac{\partial V}{\partial δ_{x}} (γ (s, t), δ_{γ} (s, t)))}^{T} \frac{\partial γ (s, t)}{\partial t}] |}_{s = 0}^{s = 1}

(16)

Notice that the parameterized set of solutions in (13) satisfies (i) c_t(⋅, 0) = γ(⋅, t), (ii)

δ_{c_{t}} (0,0) = δ_{γ} (0, t)

and

δ_{c_{t}} (1,0) = δ_{γ} (1, t)

, and (iii)

\partial γ (0, t) / \partial t = \partial c_{t} (0,0) / \partial t = {\dot{x}}^{*} (t)

and

\partial γ (1, t) / \partial t = \partial c_{t} (1,0) / \partial t = \dot{x} (t)

. Thus, we have the following equality

\begin{array}{c} {[{(\frac{\partial V}{\partial δ_{x}} (γ (s, t), δ_{γ} (s, t)))}^{T} \frac{\partial γ (s, t)}{\partial t}] |}_{s = 0}^{s = 1} & = {[{(\frac{\partial V}{\partial δ_{x}} (c_{t} (s, 0), δ_{c_{t}} (s, 0)))}^{T} \frac{\partial c_{t} (s, 0)}{\partial t}] |}_{s = 0}^{s = 1} \\ = \int_{0}^{1} \frac{\partial}{\partial s} ({(\frac{\partial V}{\partial δ_{x}} (c_{t} (s, 0), δ_{c_{t}} (s, 0)))}^{T} \frac{\partial c_{t} (s, 0)}{\partial t}) d s . \end{array}

(17)

Now since γ(⋅, t) (and thus c_t(⋅, 0)) is a geodesic, it satisfies the Euler–Lagrange equation, which in coordinates reads as

\frac{\partial}{\partial s} (\frac{\partial V}{\partial δ_{x}} (c_{t} (s, 0), δ_{c_{t}} (s, 0))) [i] = 2 \frac{\partial}{\partial s} (\sum_{j = 1}^{n} M_{i j} (c_{t} (s, 0)) δ_{c_{t}} (s, 0) [j]) = δ_{c_{t}} {(s, 0)}^{T} \frac{\partial M}{\partial x [i]} (c_{t} (s, 0)) δ_{c_{t}} (s, 0) .

for all i ∈ {1, …, n} and s ∈ (0, 1). From (13)

\frac{\partial}{\partial s} (\frac{\partial c_{t} (s, t)}{\partial t}) = A (c_{t} (s, t), π_{t} (c_{t} (s, t), t)) δ_{c_{t}} (s, t) + B (c_{t} (s, t)) δ_{u} (s, t) .

that is, the variational dynamics. Leveraging this equality with the Euler–Lagrange equation, one obtains

\begin{array}{c} \frac{\partial}{\partial s} & ({(\frac{\partial V}{\partial δ_{x}} (c_{t} (s, 0), δ_{c_{t}} (s, 0)))}^{T} \frac{\partial c_{t} (s, 0)}{\partial t}) \\ = δ_{c_{t}} {(s, 0)}^{T} (\partial_{\partial c_{t} (s, 0) / \partial t} M (s, 0) + \hat{M (s, 0) A (s, 0)}) δ_{c_{t}} (s, 0) + 2 δ_{c_{t}} {(s, 0)}^{T} M (s, 0) B (s, 0) δ_{u} (s, 0) \\ = \frac{\partial V}{\partial t} (c_{t} (s, 0), δ_{c_{t}} (s, 0), δ_{u} (s, 0)), \end{array}

(18)

where the last line follows by definition, and we use the abbreviation (s, 0) for sake of clarity. Now by deliberate design of δ_u, we have

\frac{\partial V}{\partial t} (c_{t} (s, 0), δ_{c_{t}} (s, 0), δ_{u} (s, 0)) \leq - 2 λ V (c_{t} (s, 0), δ_{c_{t}} (s, 0)),

(19)

for all s ∈ [0, 1]. Combining the inequality above and equations (16) and (17), one obtains the following chain of inequalities:

\begin{array}{c} D^{+} E (x * (t), x (t)) & \leq \int_{0}^{1} \frac{\partial V}{\partial t} (c_{t} (s, 0), δ_{c_{t}} (s, 0), δ_{u} (s, 0)) d s \\ \leq - 2 λ \int_{0}^{1} V (c_{t} (s, 0), δ_{c_{t}} (s, 0)) d s \\ = - 2 λ \int_{0}^{1} V (γ (s, t), δ_{γ} (s, t)) d s = - 2 λ E (x * (t), x (t)) . \end{array}

(20)

It follows that $E (x * (t), x (t)) \leq E (x * (0), x (0)) e^{- 2 λ t}$ . Since $d^{2} (x * (t), x (t)) = E (x * (t), x (t))$ and the metric tensor M(x) is uniformly bounded, one concludes

‖ x (t) - x * (t) ‖ \leq \sqrt{\frac{\bar{α}}{\underline{α}}} ‖ x (0) - x * (0) ‖ e^{- λ t}

Thus, x*(t) is IES with rate λ and overshoot

\sqrt{\bar{α} / \underline{α}}

The construction of the virtual parameterized set of solutions induced by (13) is needed as one cannot directly reason about the evolution of the minimizing geodesic. Instead, we leverage the Euler–Lagrange equation to equate an upper bound on the time derivative of the Riemann energy between trajectories x*(⋅) and x(⋅), that is, eq. (16), and an integral of the Lie derivative of the metric tensor M(x) with respect to the nominal dynamics, that is, equations (17) and (18).

Remark 3.6

It can be shown that conditions (5) and (7) are invariant under state diffeomorphism and metric pushforward (Manchester and Slotine, 2017). This invariance allows one to relax the topological assumptions on the state-space $X$ . In particular, one may take $X$ to be any embedded smooth n-manifold in $R^{k}$ where k ≥ n, as long as the contraction conditions hold on one (and thus, any) choice of local coordinates in $R^{n}$ . This highlights the coordinate-free (i.e., intrinsic) nature of CCM-based stabilization.

3.3.1. Min-norm stabilizing controller

Notice that the proof of Theorem 3.5 relies on establishing the inequality: $D^{+} E (x * (t), x (t)) \leq - 2 λ E (x * (t), x (t))$ , that is, inequality (20), by first demonstrating that the expression on the r.h.s. of inequality (16) is equivalent to the integral of the expression in (18) along the curve c_t(⋅, 0) = γ(⋅, t), and then leveraging the stabilizing property of the differential controller δ_u at each point along γ(⋅, t) in (19). This implies tha the feedback controller k(⋅, ⋅) as constructed in (11) is an exponentially stabilizing controller in the sense of inequality (20). However, this controller is not unique. Indeed, by retaining the policy decomposition as π(x(t), t) = u*(t) + k(x*(t), x(t)), any sufficiently smooth function k(⋅, ⋅) that ensures that the r.h.s. of inequality (16) is upper-bounded by $- 2 λ E$ is sufficient for exponential stability. Theorem 3.5 merely establishes the existence of such a controller, by the explicit construction in (11). This observation allows us to treat the Riemannian energy between the two trajectories $E (x * (t), x (t))$ as an incremental CLF, formalized by the following implicit construction of an exponentially stabilizing CCM controller:

Optimization Problem $O P T_{online}$ —At time t ≥ 0, given a desired/current state pair (x*(t), x(t)) and a minimizing geodesic γ(⋅, t) connecting these two states (i.e., γ(0, t) = x*(t) and γ(1, t) = x(t)), solve

\begin{array}{l} k^{*} (x^{*} (t), x (t)) = \underset{k \in R^{m}}{argmin} & {‖ k ‖}^{2} \\ subject to & 2 δ_{γ}^{T} (1, t) M (x (t)) \hat{\dot{x}} (t) - 2 δ_{γ}^{T} (0, t) M (x^{*} (t)) {\dot{x}}^{*} (t) \leq - 2 λ E (x^{*} (t), x (t)), \end{array}

(21)

where

\hat{\dot{x}} (t) = f (x (t)) + B (x (t)) (u * (t) + k)

represents the nominal dynamics evaluated at x(t) and

{\dot{x}}^{*} (t) = f (x * (t)) + B (x * (t)) u * (t)

A few comments are in order. First, from the earlier discussion, problem $O P T_{online}$ is always feasible given the existence implications from Theorem 3.5. Second, the linear inequality (21) is essentially an online relaxation of (8), in that it only enforces contraction tangent to the given geodesic as computed at time t. In contrast, the differential controller proposed in Manchester and Slotine (2017) obtained by solving an offline feasibility problem, must ensure that the system contracts in all directions with at least rate λ. The online relaxation still guarantees IES as only the flow along the geodesic affects the convergence of x(t) to x*(t). On the other hand, one can often dramatically decrease control effort as compared with computing the controller using (11). Third, problem $O P T_{online}$ is a QP subject to a single linear inequality and thus may be solved analytically (given the geodesic γ(⋅, t)). Indeed, the QP above strongly resembles the min-norm formulation of Sontag’s generalized formula for CLF-based stabilization (Primbs et al., 1999), thereby underscoring the interpretation of the Riemann energy of the minimizing geodesic as an incremental CLF—an observation also made in Manchester and Slotine (2017). We address the online computation of the minimizing geodesic in Appendix A.

Remark 3.7

While the interpretation of the energy as an incremental CLF is an observation also made in Manchester and Slotine (2017), by showing equivalence between the right hand sides of (16), (17), and (20) and using this to establish IES for x*(t), we are able to derive the robustness guarantees for controller (11) under significantly weaker conditions than those necessary in Manchester and Slotine (2017). We explore this distinction next in further detail.

3.4. Disturbance rejection

We now deduce the disturbance rejection properties of an exponentially stabilizing CCM controller and derive the resulting RCI mapping for the closed-loop system stabilized with this controller. Henceforth, (x*(t), u*(t)) is assumed to satisfy the unperturbed dynamics, while x(t) denotes the actual state trajectory (i.e., with disturbances) using the control law u*(t) + k(x*, x), where k(⋅, ⋅) is an exponentially stabilizing CCM controller.

Theorem 3.8

(Disturbance Rejection). Assume there exists a CCM M(x) satisfying conditions (5) and (7) that is uniformly bounded, that is, $\underline{α} I_{n} \underline{≺} M (x) \underline{≺} \bar{α} I_{n}$ , for all $x \in X$ where $\underline{α} > 0$ . Factorize M(x) as Θ(x)^TΘ(x) and define

{\bar{α}}_{w} : = \sup_{x \in X} \bar{σ} (Θ (x) B_{w} (x)) .

Then, the geodesic energy between trajectories x(t) and x*(t), that is,

E (x * (t), x (t))

, satisfies the differential inequality

D^{+} E (x * (t), x (t)) \leq - 2 λ E (x * (t), x (t)) + 2 d (x * (t), x (t)) {\bar{α}}_{w} ‖ w (t) ‖ .

(22)

Suppose, additionally that $‖ w (t) ‖ \leq \bar{w} < \infty$ for all t, and $d (x * (0), x (0)) \leq {\bar{α}}_{w} \bar{w} / λ = : \bar{d}$ . Then, the geodesic distance is upper-bounded by $\bar{d}$ , for all time t ≥ 0. Thus, the RCI mapping may be expressed as

Ω (x *) = {x \in X : d (x *, x) \leq \bar{d}}

(23)

Proof

Inequality (16) implies

\begin{array}{l} D^{+} E (x * (t), x (t)) & \leq 2 δ_{γ}^{T} (1, t) M (γ (1, t)) [f (x (t)) + B (x (t)) u (x (t))] \\ - 2 δ_{γ}^{T} (0, t) M (γ (0, t)) [f (x * (t)) + B (x * (t)) u * (t)] \\ + 2 δ_{γ}^{T} (1, t) M (γ (1, t)) B_{w} (x (t)) w (t) \end{array} .

(24)

where u(x(t)) is given by (11). By the IES property of the nominal system, that is, inequality (20), the expression above can be bounded as

D^{+} E (x * (t), x (t)) \leq - 2 λ E (x * (t), x (t)) + 2 δ_{γ}^{T} (1, t) M (γ (1, t)) B_{w} (x (t)) w (t) .

Defining δ_z(s, t)≔Θ(γ(s, t))δ_γ(s, t), we obtain

D^{+} E (x * (t), x (t)) \leq - 2 λ E (x * (t), x (t)) + 2 δ_{z}^{T} (1, t) Θ (γ (1, t)) B_{w} (x (t)) w (t) .

Recall that the velocity field of a geodesic is parallel along the geodesic (Spivak, 1999), and thus, $V (γ (s, t), δ_{γ} (s, t)) = E (x * (t), x (t))$ for all s ∈ [0, 1]. This implies that $\sqrt{V (γ (s, t), δ_{γ} (s, t))} = ‖ δ_{z} (s, t) ‖ = d (x * (t), x (t))$ . Using the Cauchy–Schwarz inequality, one obtains the stated differential inequality.

Finally, notice that the equality in (22) yields the well-known Bernoulli differential equation. Then, since $‖ w (t) ‖ \leq \bar{w}$ for all t and $d (x * (0), x (0)) \leq \bar{d}$ , by the Comparison Lemma (Khalil, 2002), the bound holds for all t ≥ 0.

Remark 3.9

Notice that the differential inequality derived in (22) may be integrated in time to yield a δ-ISS statement similar to Definition 2.3 in Zamani et al. (2013). Specifically, in Zamani et al. (2013), the authors prove δ-ISS by leveraging a decrescent condition similar to inequality (8) with respect to tangent vectors δ_w defined on the tangent space of the disturbance manifold $W$ , yielding a differential ISS condition. The key difference here then is by leveraging the separability of the disturbance term (i.e., inequality (24)), and by using a tracking controller that enforces contraction along the minimizing geodesic at each time (see Figure 4), we do not need to impose similar differential ISS like conditions on V(x, δ_x).

Remark 3.10

We further note that a similar bound is also proved in Manchester and Slotine (2017) . However, as their proof also leverages a differential treatment of disturbance, the result requires stronger conditions (akin to the Killing field condition in (5)) between M(x) and the disturbance input matrix B _w . Our result circumvents the need for a differential treatment of disturbance by directly leveraging the stability property of inequality (24), as proven in Theorem 3.5.

So far we have shown how the existence of a feasible CCM M(x) allows us to construct exponentially stabilizing controllers with bounded-input-bounded-output disturbance rejection guarantees. In the next section, we demonstrate how one can leverage convex optimization techniques, specifically SOS programming, to compute offline, an optimized CCM that minimizes the size of the RCI tube.

4. Offline synthesis of CCMs and computation of bounds

In this section, we show how to compute CCMs offline that minimize a certain measure of the size of the RCI set which in turn minimizes the deviation of the perturbed trajectory from the nominal, thereby reducing the amount by which we must tighten the set $X$ . Specifically, we demonstrate how to transform the CCM conditions (5), (7), and (9) into convex constraints, and formulate the synthesis problem as a quasiconvex optimization problem with appropriate objective functions. We illustrate the resulting methodology with a representative example. Finally, we derive bounds on the magnitude of the CCM feedback min-norm controller, allowing us to define the tightened control constraints for planning. We begin by characterizing an outer approximation to the RCI set defined in Section 3.4 that is of fixed size and shape, and will be instrumental in formulating the CCM synthesis optimization problem.

4.1. Ellipsoidal RCI tube

Notice that the mapping in (23) is given using the Riemann distance which may depend upon a spatially varying metric. In order to efficiently plan a nominal trajectory whose associated RCI tube does not collide with obstacles, we would prefer a mapping that is independent of x*. To this end, consider the following technical lemma (the proof is provided in Appendix B).

Lemma 4.1

(Geodesic Boundedness) Consider points $x * \in \bar{X}, x \in X$ s.t. x ∈ Ω(x*) where Ω(x*) is given in (23). Suppose the CCM M(x) satisfies $M (x) ≽ \underline{M}$ for all $x \in X$ , where $\underline{M} ≽ \underline{α} I_{n}$ . Define the ellipsoid

\tilde{Ω} (x *) : = {x \in X : ‖ x - x * ‖_{\underline{M}}^{2} \leq {\bar{d}}^{2}}

(25)

and suppose

\tilde{Ω} (x *) \subset X

. Then, the minimizing geodesic γ is contained entirely within

\tilde{Ω} (x *)

, that is,

γ (s) \in \tilde{Ω} (x *)

for all s ∈ [0, 1], and thus

Ω (x *) \subseteq \tilde{Ω} (x *)

Equation (25) gives an ellipsoidal outer approximation of the RCI tube as defined in (23), and is thus independent of x* (see Figure 5 for an illustration). This is essential for two reasons: (i) it drastically simplifies collision checking with respect to obstacles by avoiding geodesic computations, and (ii) the tightened state constraint set $\bar{X}$ in problem MPC must be taken to be $X ⊖ \tilde{Ω}$ to ensure that the minimizing geodesic lies within $X$ , that is, where the CCM conditions hold.

4.2. Sum-of-squares programming

Our computational approach is rooted in SOS programming, for which we provide a brief review here. For a more detailed review of SOS programming and its applications, please refer to Parrilo (2000); Ahmadi and Majumdar (2016); Majumdar and Tedrake (2017). We begin by discussing semi-definite programs (SDPs), a class of convex optimizations problems formulated over the space of symmetric positive semi-definite matrices. A symmetric matrix $X \in S_{n}^{\geq 0}$ is positive semi-definite (psd) if x^TXx ≥ 0 for all $x \in R^{n}$ , x ≠ 0, and is denoted as X ≽ 0. An SDP in standard form is written as

\begin{array}{c} \underset{X \in S_{n}^{\geq 0}}{minimize} & Tr (C^{⊤} X) \\ subject to & A_{i}^{⊤} X = b_{i}, i = 1,2, \dots, m, \\ X ≽ 0 \end{array}

(26)

where C and

{A_{i}}_{i}

are elements of

S_{n}

, and “Tr” denotes the trace operator. SOS programs provide a means of certifying nonnegativity of polynomials either globally or over basic semialgebraic sets. A basic semialgebraic set

S

is a subset of the Euclidean space characterized by a finite set of polynomial inequalities and equalities, that is

S : = {x \in R^{n} : ϕ_{i} (x) \geq 0, ψ_{j} (x) = 0},

(27)

where {ϕ_i}, {ψ_j} are multivariate polynomials in x. The simplest task within this class of problems is verifying the nonnegativity of a given polynomial p over

S

, already NP-hard (Parrilo, 2000). SOS programming provides a convex relaxation approach to accomplish this task. Specifically, a polynomial p is termed SOS if it can be written in the form

\sum_{k} z_{k}^{2}

for some other polynomials z_k. While such a decomposition is not necessary, it is sufficient to guarantee (global) nonnegativity of p. Moreover, if one can find a set of SOS polynomials L_i and ordinary polynomials q_j such that

p - \sum_{i} L_{i} ϕ_{i} + \sum_{j} q_{j} ψ_{j} is SOS

(28)

then one obtains a certificate of nonnegativity of p(x) over

S

. Indeed, in the above equation when ϕ_i(x) ≥ 0 and ψ_j(x) = 0, that is,

x \in S

, one has that p(x) ≥∑_iL_i(x)ϕ_i(x) ≥ 0, as required. Such a certificate is the extension of the generalized S-procedure (Iwasaki and Hara, 2005) to the setting of real-valued polynomials (Parrilo, 2000), and additionally constitutes a necessary condition for a subclass of semialgebraic sets (Putinar, 1993). More complex necessary and sufficient conditions for verifying nonnegativity over any semialgebraic set using SOS decompositions also exist, and leverage the Stengle Positivstellensatz involving products of ϕ_i (Parrilo, 2000). For our purposes, however, the certificate of nonnegativity as per equation (28) is sufficient.

The computational advantage of SOS programming stems from its intrinsic link to SDPs. Specifically, a polynomial p of degree 2d is SOS if and only if p(x) = z(x)^TQz(x), where Q≽0 and z is a vector of monomials up to order d. Thus, certifying that a polynomial is SOS reduces to the task of finding a psd matrix Q subject to a finite set of linear equalities, thus taking the form in (26). Certificates of the form in (28) will form the building block for the computation of CCMs, as discussed next.

4.3. Synthesizing optimized CCMs

As shown in Manchester and Slotine (2017), condition (7) can be written as a pointwise Linear-Matrix-Inequality (LMI) by introducing the dual metric W(x)≔M(x)⁻¹ and the change of variables η_x≔M(x)δ_x. Specifically, define a matrix B_⊥(x) whose columns form a basis for the null-space of B(x)^T (i.e., B(x)^TB_⊥(x) = 0). Then, conditions (5) and (7) are equivalent to

\partial_{b_{j}} W (x) - \hat{\frac{\partial b_{j} (x)}{\partial x} W (x)} = 0, j = 1, \dots, m

(29)

B_{⊥}^{T} \underset{≔ G (x)}{\underset{}{(- \partial_{f} W (x) + \hat{\frac{\partial f (x)}{\partial x} W (x)} + 2 λ W (x))}} B_{⊥} ≼ 0

(30)

The equivalent re-formulation of the weaker condition (9) in terms of the dual metric is given by LMI (30) and

B_{⊥}^{T} (\partial_{b_{j}} W (x) - \hat{\frac{\partial b_{j} (x)}{\partial x} W (x)}) B_{⊥} = 0, j = 1, \dots, m

(31)

In this section, we replace the CCM feasibility problem, that is, conditions (29) and (30), with a quasiconvex optimization problem to minimize the size of the outer approximation for the RCI mapping. A trade-off between the overshoot constant

\sqrt{\bar{α} / \underline{α}}

and contraction rate λ was briefly discussed in Manchester and Slotine (2014). Here, we further explore this aspect by formulating a global optimization program to characterize such a trade-off and minimize conservatism. Ideally, one would directly like to minimize the bound in (25) subject to conditions (29) and (30). However, this problem is non-convex and infinite-dimensional.

First, to address the infinite-dimensionality of the problem, we consider a finite-dimensional approximation whereby the dual metric W(x) is parameterized as a polynomial matrix and the LMIs are written as SOS constraints, enforced over the semialgebraic set $X$ using the relaxations introduced in the previous section. Second, we propose an objective function that can be solved using line search and quasiconvex optimization. Formally, we define the offline synthesis problem $O P T_{\hat{C C M}}$ :

Optimization Problem $O P T_{\hat{C C M}}$ —solve

\begin{array}{c} \min_{λ \in R_{> 0}} J_{C C M} (λ) : = \min_{\begin{array}{l} W \in C^{\infty} (X, S_{n}^{> 0}) \\ \bar{W} \in S_{n}^{> 0}, \underline{β}, \bar{β} \in R_{> 0} \end{array}} \frac{1}{λ^{2}} (\frac{\bar{β}}{\underline{β}}) \\ subject to eq . (29), eq . (30) \end{array}

(32)

\begin{array}{c} \underline{β} I_{n} ≼ W (x) ≼ \bar{W} ≼ \bar{β} I_{n} \end{array}

(33)

where the conditions hold uniformly for all

x \in X

The cost function above is an upper bound on the worst-case (normalized) Euclidean distance within the ellipsoid defined in (25) since from (33), we have $\bar{α} = 1 / \underline{β}$ , $\underline{α} = 1 / \bar{β}$ , and $\underline{M} = {\bar{W}}^{- 1}$ . Thus

\sup_{x \in \tilde{Ω} (x *)} \frac{{‖ x - x * ‖}^{2}}{{\bar{w}}^{2}} = \frac{{\bar{α}}_{w}^{2}}{λ^{2} \underline{α}} \leq \frac{1}{λ^{2}} (\frac{\bar{α}}{\underline{α}}) = \frac{1}{λ^{2}} (\frac{\bar{β}}{\underline{β}})

Recognizing that for a fixed contraction rate λ, the CCM conditions define a convex feasibility region for W(x), and that minimization of the condition number of a positive definite matrix over a closed convex set (i.e., the inner minimization in problem $O P T_{\hat{C C M}}$ ) is quasiconvex (Lu and Pong, 2011), problem $O P T_{\hat{C C M}}$ can then be solved using line search on λ and bisection search over the condition number of W(x) (i.e., a sequence of feasibility problems). In the implementation of the bisection search, we further minimized the expression $Tr (W_{s} \bar{W})$ where $W_{s} \in S_{n}^{> 0}$ is a diagonal scaling matrix. This is motivated by the following observation:

Remark 4.2

One may alternatively choose the following objective for problem $O P T_{\hat{C C M}}$

\frac{1}{λ^{2}} (\frac{Tr (W_{s} \bar{W})}{\underline{β}})

where

W_{s} \in S_{n}^{> 0}

is a diagonal scaling matrix. This cost function is motivated by the observation

{(\frac{\bar{d}}{\bar{w}})}^{2} = \frac{{\bar{α}}_{w}^{2}}{λ^{2}} \leq \frac{\bar{α}}{λ^{2}} = \frac{1}{\underline{β} λ^{2}},

and that the projection of the ellipsoid (25) along the i^th state is the interval:

[- \bar{d} \sqrt{{\bar{W}}_{i i}}, \bar{d} \sqrt{{\bar{W}}_{i i}}]

. Thus, this cost function attempts to minimize the size of this interval along each state dimension (relatively weighted by the matrix W_s). Since Tr(⋅) is convex and

\underline{β} = λ_{\min} (W)

is concave, for a fixed λ, we obtain another quasiconvex objective for the inner minimization in problem

O P T_{\hat{C C M}}

, which can be solved using bisection search.

4.4. Illustrative example

Consider the 6-state planar-quadrotor system depicted in Figure 6. Notably, this system is underactuated and has unstable zero dynamics, and thus represents a challenging system to benchmark the approach.

The state vector is defined as ${(p_{x}, p_{z}, ϕ, {\dot{p}}_{x}, {\dot{p}}_{z}, \dot{ϕ})}^{T}$ . For synthesizing the dual CCM however, it will be helpful to consider the alternative state-space representation: ${(p_{x}, p_{z}, ϕ, v_{x}, v_{z}, \dot{ϕ})}^{T}$ where (v_x, v_z) describe the velocity in the body frame of the vehicle with v_x representing the slip velocity (lateral) and v_z representing the velocity along the thrust axis. The dynamics in control affine form for this state representation are given as

\dot{x} = [\begin{array}{c} {\dot{p}}_{x} \\ {\dot{p}}_{z} \\ \dot{ϕ} \\ {\dot{v}}_{x} \\ {\dot{v}}_{z} \\ \ddot{ϕ} \end{array}] = [\begin{array}{c} v_{x} \cos (ϕ) - v_{z} \sin (ϕ) \\ v_{x} \sin (ϕ) + v_{z} \cos (ϕ) \\ \dot{ϕ} \\ v_{z} \dot{ϕ} - g \sin (ϕ) \\ - v_{x} \dot{ϕ} - g \cos (ϕ) \\ 0 \end{array}] + [\begin{array}{c} 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 1 / m & 1 / m \\ l / J & - l / J \end{array}] [\begin{array}{c} u [1] \\ u [2] \end{array}],

(34)

where m and J denote the mass and moment of inertia about the out-of-plane axis. The inertial properties were taken from Steinhardt and Tedrake (2012), and were m = 0.486 kg, J = 0.00383 kg m², and l = 0.25 m. We impose the state-space bounds: (v_x, v_z) ∈ [−2, 2] × [−1, 1] m/s and

(ϕ, \dot{ϕ}) \in [- 45^{o}, 45^{o}] \times {[- 60,60]}^{o}

/s, which can be consolidated into a vector h(x) ≥ 0.

By translation invariance of the dynamics, we expect that W will not be a function of (p_x, p_z). Furthermore, by leveraging this state representation, condition (29) requires that W is not a function of v_z or $\dot{ϕ}$ . Thus, for this 6-state system, the CCM must necessarily only be a function of (v_x, ϕ). The remaining constraints in problem $O P T_{\hat{C C M}}$ may be written as

\begin{array}{c} \underline{β} \geq 1 \end{array}

(35)

\begin{array}{c} \bar{β} I_{6} - \bar{W} ≽ 0 \end{array}

(36)

\begin{array}{c} h (x) \geq 0 \Rightarrow \bar{W} - W (x) & ≽ 0 \end{array}

(37)

\begin{array}{c} h (x) \geq 0 \Rightarrow W (x) - \underline{β} I_{6} & ≽ 0 \end{array}

(38)

\begin{array}{c} h (x) \geq 0 \Rightarrow - B_{⊥}^{T} G (x) B_{⊥} & ≽ ϵ I_{4} . \end{array}

(39)

where ϵ is a small positive number and the constraint

\underline{β} \geq 1

is imposed to ensure uniform definiteness. A simple choice for B_⊥ for this problem is the matrix

[\begin{array}{c} I_{4} \\ 0_{2 \times 2} \end{array}] .

To enforce the semi-definite constraints of the type h(x) ≥ 0 ⇒ F(x)≽0 where $F : X \to R^{n_{F} \times n_{F}}$ is some matrix-valued function, we leverage the quadratic form of the matrices by introducing auxiliary indeterminates y of dimension n_F so that the constraint may be equivalently written as h(x) ≥ 0 ⇒ y^TF(x)y ≥ 0, for all y. Leveraging certificates of the form in (28), we pose the constraints

\begin{array}{c} y^{T} F y - \sum_{i} L_{i} h_{i} is SOS \\ {L_{i}} is SOS, \end{array}

where {L_i} is a collection of SOS functions in (x, y). To keep the scale of the problem under control, one may wish to enforce simplifying structural properties for the multipliers L(x, y) such as only retaining monomials quadratic in y (to reflect that y^TF(x)y only contains quadratic terms in the auxiliary indeterminate y).

We parameterized W(x) as a polynomial matrix in (v_x, ϕ) with up to degree 4 monomials (higher degree monomials yielded no further improvement in the bounds). The trigonometric terms from the dynamics function were approximated using Chebyshev polynomial expansions up to third order. To account for this approximation, satisfaction of the LMIs was numerically checked post optimization using the true dynamics by gridding over (v_x, v_z, ϕ).

Having cast the optimization as a SOS program, we may now proceed with solving problem $O P T_{\hat{C C M}}$ . We swept through a range for the contraction rate λ, solving a sequence of SOS feasibility programs for each λ, using the Spotless Polynomial Toolbox (Tobenkin et al., 2013) and Mosek solver (ApS, 2017). Figure 7 plots the optimal curve for J_CCM as a function of λ. The optimal contraction rate was determined to be λ = 0.83 and the corresponding dual metric W(x) contained 15 unique monomials in (ϕ, v_x). Assuming a cross-wind acting along either direction of the inertial p_x axis with effective acceleration up to 0.1 m/s², we used gridding to determine $\bar{d} = 0.038$ . The resulting projections of the ellipsoid (25) are shown in Figure 8.

Notice that the projection of the outer ellipsoidal approximation of the RCI set onto the p_x-p_z plane has a major axis equal to 74 cm, and a minor axis equal to 41 cm, which compare quite favorably with the quadrotor wingspan, equal to 50 cm. That is, the size of the RCI tube is rather small and thus the nominal motion planner for the quadrotor is not overly constrained by the tightening of the state constraints.

4.5. Bounding the CCM tracking controller

Having synthesized a CCM, the corresponding RCI set as well as its ellipsoidal outer approximation used for planning are characterized by (23) and (25), respectively, allowing us to define the tightened state constraints $\bar{X}$ . We now turn our attention to bounding the feedback control effort so that we may compute the tightened control constraint set $\bar{U}$ .

There are two ways to compute bounds on the CCM feedback controller. For systems that satisfy the stronger Killing field condition given by (29), Theorem 4.4 provides a bound on the magnitude of the optimized tracking controller computed using problem $O P T_{online}$ . We require the technical lemma at the bottom of this page.

Lemma 4.3

(Norm Bound for Tracking Controller) . Let S be a symmetric matrix in $R^{n \times n}$ and Y a full row-rank matrix in $R^{m \times n}$ . Construct matrices $\bar{Y} \in R^{n \times m}$ and ${\bar{Y}}_{⊥} \in R^{n \times (n - m)}$ such that the columns of $\bar{Y}$ form an orthonormal basis for the column space of Y ^T , that is, Col(Y ^T ), and the columns of ${\bar{Y}}_{⊥}$ form an orthonormal basis for the null-space of Y, that is, $N (Y)$ . Suppose, then, that the following conditions hold

\begin{array}{c} η_{z}^{T} S η_{z} \leq 0 & \forall η_{z} \in N (Y) \subset R^{n} \end{array}

(40)

\begin{array}{c} κ \leq \frac{2 {\bar{δ}}_{u}}{θ}, w h e r e κ & = {\begin{cases} 2 (\frac{\bar{σ} ({\bar{Y}}_{⊥}^{T} S \bar{Y})}{{\underline{σ}}_{> 0} (Y \bar{Y})}) & i f \bar{λ} (S_{Y}) \leq 0 \\ \sqrt{{(\frac{\bar{λ} (S_{Y})}{{\underline{σ}}_{> 0} (Y \bar{Y})})}^{2} + 4 {(\frac{\bar{σ} ({\bar{Y}}_{⊥}^{T} S \bar{Y})}{{\underline{σ}}_{> 0} (Y \bar{Y})})}^{2}} & e l s e \end{cases} \end{array}

(41)

for some constants ${\bar{δ}}_{u} \in R_{\geq 0}$ and $θ \in R_{> 0}$ , where ${\underline{σ}}_{> 0} (\cdot)$ denotes the smallest non-zero singular value, and $S_{Y} : = {\bar{Y}}^{T} S \bar{Y}$ . Then

θ η_{z}^{T} S η_{z} \leq 2 {\bar{δ}}_{u} ‖ Y η_{z} ‖ \forall η_{z} s . t . ‖ η_{z} ‖ \leq 1 .

(42)

Proof

See Appendix B .

We now leverage Lemma 4.3 to derive a bound on the magnitude of the min-norm CCM controller defined in Problem $O P T_{online}$ . The proof of the following theorem is provided in Appendix B.

Theorem 4.4

(Tracking Control Effort) . Define

F (x) : = - \partial_{f} W (x) + \hat{\frac{\partial f (x)}{\partial x} W (x)} + 2 λ W (x)

Assume the dual CCM W(x) satisfies conditions (29) and (30). Factorize W(x) as L(x)^TL(x) and define S(x) = L^−TFL⁻¹ and Y(x) = B^TL⁻¹. Let ${\bar{δ}}_{u}$ be a positive real number such that the matrices S(x) and Y(x) satisfy property (41) for all $x \in X$ with $θ = \bar{d} = {\bar{α}}_{w} \bar{w} / λ$ , where $\bar{Y}$ and ${\bar{Y}}_{⊥}$ are defined as stated in Lemma 4.3. Then, the min-norm CCM controller k*(x*, x) satisfies the bound

‖ k^{*} (x *, x) ‖ \leq {\bar{δ}}_{u}

(43)

for all

x *, x \in X

such that x ∈ Ω(x*).

Corollary 4.5

A tight approximation for ${\bar{δ}}_{u}$ in Theorem 4.4 may be computed as

{\bar{δ}}_{u} = \frac{\bar{d}}{2} \sup_{x \in X} κ (x),

(44)

where κ is as given in (41) and

S, Y, \bar{Y}, {\bar{Y}}_{⊥}

are as defined in Theorem 4.4. A simpler, yet loose approximation is given by

{\bar{δ}}_{u} = \frac{\bar{d}}{2} \sup_{x \in X} (\frac{\bar{λ} (L^{- T} F L^{- 1})}{{\underline{σ}}_{> 0} (B^{T} L^{- 1})}),

(45)

where we omit the explicit dependence on x for notational clarity.

For systems that do not satisfy the strong Killing field condition in (29), and instead satisfy the alternative pair of conditions, that is, (30) and (31), one may bound the feedback control by augmenting the state with u, and treating $\dot{u}$ as the actual input. The RCI tube obtained via the analysis in Section 3.4 then captures a bound on both x and u. We will shortly illustrate both methods for bounding the control effort.

5. Robust planning

We are finally ready to formalize the robust planning algorithm. The core part of the algorithm relies on computing nominal motion plans (x*, u*) for the unperturbed dynamics using tightened constraints. Given the ellipsoidal tube derived in Section 4.1 and the control effort bounds computed in the previous section, these tightened constraints are given as

\begin{array}{c} x * (\cdot) \in \bar{X} : = X ⊖ \tilde{Ω}, \end{array}

(46a)

\begin{array}{l} u * (\cdot) \in \bar{U} : = {\bar{u} \in U : \forall x * (t) \in \bar{X}, \forall x (t) \in X s . t . x (t) \in Ω (x * (t)), \bar{u} + k^{*} (x * (t), x (t)) \in U} . \end{array}

(46b)

Notice that the state constraint is shrunk by the fixed-size ellipsoid $\tilde{Ω}$ to ease computation (e.g., collision checking). On the other hand, since x(t) is guaranteed to lie in the true RCI tube Ω(⋅) defined using the geodesic distance, the control constraint is shrunk based on the bounds computed in the previous section for the min-norm CCM controller. Having computed such a plan, one may adopt two different frameworks for its robust execution. In the first approach, one could simply execute the controller derived from problem $O P T_{online}$ until the robot enters $X_{goal}$ . In the second approach, provided there exist sufficient online computational resources, one can use a receding-horizon algorithm in which the nominal trajectory is periodically and locally re-updated over a short time-horizon T < T_goal. This allows one to reduce the tracking cost (as information about the realized disturbances is taken into account). We outline such a receding-horizon strategy next.

5.1. Receding horizon implementation

Given a robust motion plan (i.e., a nominal state-input trajectory (x*, u*) such that the RCI tube centered on x* does not intersect any obstacles), one can make local updates to it using the following MPC problem solved at the discrete-time instants $t_{i}, i \in N_{\geq 0}$ :

Optimization Problem MPC—given current state x(t_i) and a robust motion plan (x*, u*) with associated RCI mapping Ω(⋅), solve

\begin{array}{l} \min_{\begin{array}{l} \bar{u} (\cdot) \in C^{2} ([t_{i}, t_{i} + T], \bar{U}) \\ \bar{x} (t_{i}) \in \bar{X} \\ T_{i} \in R_{> 0} \end{array}} \int_{t_{i}}^{t_{i} + T} ‖ \bar{u} (τ) ‖_{R}^{2} d τ - μ T_{i} \\ subject to x (t_{i}) \in Ω (\bar{x} (t_{i})), \end{array}

(47)

\dot{\bar{x}} (τ) = f (\bar{x} (τ)) + B (\bar{x} (τ)) \bar{u} (τ),

(48)

\bar{x} (τ) \in \bar{X}, \bar{u} (τ) \in \bar{U} \forall τ \in [t_{i}, t_{i} + T]

(49)

\bar{x} (t_{i} + T) = x * (T_{i}), t_{i} + δ \leq T_{i} \leq T_{goal}^{*},

(50)

where μ > 0 is a weighting factor. The time T_i (hereby termed as the “re-join” time), also an optimization variable within problem MPC, marks the point where the MPC state trajectory re-joins the initial motion plan and will be used to ensure persistent feasibility (see Lemma 5.1).

Problem MPC should be understood as a local re-optimization step—thus, it should be solved using local methods such as trajectory optimization techniques (Betts, 2010) or elastic bands (Quinlan and Khatib, 1993) (as opposed to fully-fledged global planners). Notice that the initial value of the updated nominal state trajectory, that is, $\bar{x} (t_{i})$ , is also an optimization variable above subject to the RCI constraint (47). This permits more drastic updates to the nominal trajectory, for example, to counteract persistent disturbances. The terminal constraint given by (50) is used to ensure recursive feasibility for the MPC problem, as addressed by the next lemma.

Let $(x_{T}^{*} (t; x (t_{i})), u_{T}^{*} (t; x (t_{i}))) : [t_{i}, t_{i} + T] \to \bar{X} \times \bar{U}$ and $T_{i}^{*}$ denote the solution to problem MPC at time-step t_i. The beginning segment, t ∈ [t_i, t_i + Δ), is implemented using (2), before re-solving. This defines the sampled MPC strategy commonly employed for continuous-time systems. The following lemma establishes recursive feasibility for problem MPC. The proof is provided in Appendix B.

Lemma 5.1

(Recursive Feasibility for MPC) . Suppose problem MPC is feasible at the initial solve step t₀ = 0. Then, the problem is feasible for all $t_{i}, i \in N_{\geq 0}$ .

Remark 5.2

For $E (x * (0), x (0)) > 0$ , the Comparison Lemma gives the following analytical, time-varying bound corresponding to the differential inequality (22)

E (x * (t), x (t)) \leq {[\sqrt{E (x * (0), x (0))} e^{- λ t} + \bar{d} (1 - e^{- λ t})]}^{2}

Thus, for the MPC problem solved at time t_i, one can modify constraint (47) to

E (\bar{x} (t_{i}), x (t_{i})) \leq E (x_{T}^{*} (t_{i}; x (t_{i - 1})), x (t_{i}))

(51)

This constraint states that the Riemann energy between the start of the new nominal MPC trajectory $\bar{x} (t_{i})$ and current state x(t_i) is at most the Riemann energy between the current MPC reference (as computed at the previous solve time t_i−1) and current state x(t_i). This constraint may then be leveraged along with the shifted time-varying bound

E (x * (t), x (t)) \leq {[\sqrt{E (\bar{x} (t_{i}), x (t_{i}))} e^{- λ (t - t_{i})} + \bar{d} (1 - e^{- λ (t - t_{i})})]}^{2}, t \in [t_{i}, t_{i} + T]

within the ellipsoid (25) to obtain a time-varying tube that is smaller than the static ellipsoid and therefore, will be less conservative when incorporated within the tightened constraint set given in (46a). We demonstrate such an implementation in Section 6.

Algorithm 1 provides pseudocode for the overall approach.

Algorithm 1

1: OFFLINE:

2: Inputs: dynamics model, $U$ (control input constraints)

3: Compute: Optimized metric & contraction rate: (W(x), λ), Ellipsoidal RCI set: $\tilde{Ω}$ , controller-bound: ${\bar{δ}}_{u}$ .

4: ONLINE:

5: Inputs: x(0) (initial state), $X$ (state constraints), $X_{goal}$

6: Compute nominal (x*, u*), such that $x^{*} (\cdot) \in \bar{X}, u^{*} (\cdot) \in \bar{U}$

7: Initialize: t_plan ← 0

8: At each time t

9: if New obstacles reported or goal region is changed then

10: Re-plan nominal (x*, u*)

11: else

12: if t − t_plan = Δ then

13: $(x_{T}^{*} (\cdot; x (t)), u_{T}^{*} (\cdot; x (t)), T^{*}) \leftarrow MPC (x (t), x^{*}, u^{*}, \tilde{Ω})$

14: Update t_plan ← t

15: end if

16: end if

17: Apply control $u_{T}^{*} (t; x (t_{plan})) + k^{*} (x_{T}^{*} (t; x (t_{plan})), x (t))$

6. Numerical experiments

We now verify our approach in simulation by continuing with the 6-state planar-quadrotor system in Section 6.1, and then apply the framework to the more challenging 10-state 3D quadrotor model in Section 6.2. All simulation code (MATLAB) is available at https://github.com/StanfordASL/RobustMP.

6.1. Planar quadrotor

We continue with the planar-quadrotor system introduced in Section 4.4. With the CCM computed, we now address the planning component of this task. First, we impose the control bounds on the thrust for each propeller to the range [0.1, 2]mg. With the computed CCM from Section 4.4, the feedback control effort bound ${\bar{δ}}_{u}$ was determined using (45) to be 0.6 N (0.13 mg), a fraction of the overall control range.

6.1.1. Planning with static tubes

Having computed (offline) the RCI mapping, we tested Algorithm 1 on the previously unseen densely cluttered environment in Figure 1. The disturbance direction fluctuated (continuously) between left and right to try to push the vehicle into an obstacle. Problem MPC was re-solved every Δ = 1s with horizon T = 2s using the pseudospectral collocation method and the SNOPT solver, and leveraged the static bound given in (25). The tracking controller was implemented using zero-order-hold at 200 Hz. Leveraging a suboptimal implementation in MATLAB, the time required to compute the tracking controller (on a 3.5 GHz Intel equipped with 16 GB of RAM) is on the order of 3.5 ms. On average, each MPC problem took 0.35 s to solve. This compares favorably with the re-solve time of 1 s. Furthermore, we expect that this computational performance can be significantly improved with a more efficient implementation and by using trajectory optimization methods that fully exploit the local nature of the problem. We do not report the computation of the nominal trajectory (line 6 in Algorithm 1) since it highly depends on the motion planner used and is not a focus of this paper.

This example provides evidence that Algorithm 1 can be used for the online generation of safe motion plans that can be reliably executed (provided that the nominal motion plan can also be computed in real-time). This example also illustrates the benefits of our method as compared to the funnel library approach (Majumdar and Tedrake, 2017). A pre-computed set of trajectories (as required by Majumdar and Tedrake, 2017)) would be unlikely to contain a sequence leading from the start to goal while maneuvering through the very tight spaces between obstacles.

6.1.2. Planning with dynamic tubes

We also illustrate the use of the RCI tubes using the time-varying bounds referenced in Remark 5.2. Due to the increased complexity of the MPC problem, the lookahead and re-solve times were increased to T = 4 s and Δ = 1.5 s, respectively. Figure 9 illustrates the benefits of dynamically updating tubes. We draw attention to the MPC solver making effective use of the time-varying tube to generate tighter nominal trajectories that would have been deemed infeasible using the fixed-size tube.

6.2. 3D Quadrotor

6.2.1. Dynamics and constraints

We adopt the state-space representation $x = {(p_{x}, p_{y}, p_{z}, {\dot{p}}_{x}, {\dot{p}}_{y}, {\dot{p}}_{z}, f, ϕ, θ, ψ)}^{T}$ where position $p = {(p_{x}, p_{y}, p_{z})}^{T} \in R^{3}$ and corresponding velocities are expressed in the global inertial (vertical axis pointing down) frame. Adopting the North-East-Down frame convention for the quadrotor body and the XYZ Euler-angle rotation sequence, the attitude (roll, pitch, yaw) is parameterized as (ϕ, θ, ψ) and $f \in R_{> 0}$ is the net (normalized by mass) thrust generated by the four rotors. For the purposes of controller design, we consider as control inputs $u : = {(\dot{f}, \dot{ϕ}, \dot{θ}, \dot{ψ})}^{T}$ . Actual implementation embeds the $\dot{f}$ term within an integrator and the resulting thrust and angular velocity reference are passed to a lower-level controller on the quadrotor that is assumed to operate at a much faster time-scale. Given this parameterization, the dynamics of the quadrotor may be written as

[\begin{array}{c} {\ddot{p}}_{x} \\ {\ddot{p}}_{y} \\ {\ddot{p}}_{z} \end{array}] = g e_{3} - f {\hat{b}}_{z} = [\begin{array}{c} - f \sin (θ) \\ f \cos (θ) \sin (ϕ) \\ g - f \cos (θ) \cos (ϕ) \end{array}]

(52)

where g is the local gravitational acceleration, e₃ = (0,0,1)^T, and

{\hat{b}}_{z}

is the body-frame z-axis. The dynamics of (τ, ϕ, θ, ψ) reduce trivially to first-order integrators. We impose the bounds: (ϕ, θ) ∈ [−60°,60°]² and f ∈ (0.5, 2)g, sufficient for executing fairly aggressive maneuvers.

6.2.2. CCM computation

Notice that yaw is completely decoupled in these equations of motion. This property, manifested here trivially by choice of the rotation order, reinforces the oft-cited independence of yaw-motion from roll and pitch for quadrotors. One may however design trajectories using any other rotation parametrization and simply transform into this coordinate frame, without loss of generality. A consequence of this parametrization however is that we can decouple the “yaw” part of the dual metric, using the following block-partition model

W (x) = [\begin{array}{c} W_{⊥} & W_{u} & 0_{6 \times 1} \\ * & W_{‖} & 0_{3 \times 1} \\ * & * & W_{ψ} \end{array}]

where

W_{⊥} \in S_{6}^{> 0}

W_{‖} \in S_{3}^{> 0}

W_{ψ} \in R_{> 0}

, and

W_{u} \in R^{3 \times 3}

. Note that the zeros in the last column and row reflect the independence of yaw-motion from the rest of the dynamics. Given

B = {[0_{4 \times 6}, I_{4}]}^{T}

, take

B_{⊥} = {[I_{6}, 0_{4 \times 6}]}^{T}

. The weak Killing field condition in (31) requires that W_⊥ is not a function of (f, ϕ, θ, ψ) (this further justifies the sub-partitioning of the 9 × 9 top left block of W(x) as shown above). Further, by noting the translational and yaw invariance of the dynamics, we only parametrize W_u and W_‖ as functions of (f, ϕ, θ) and pick W_⊥ to be a constant positive definite matrix and W_ψ a constant positive scalar. Such a parameterization also reflects the intuition that differential stabilizability of the quadrotor, as captured by the CCM, should be independent of the quadrotor’s position, velocity, and yaw orientation.

Given the block diagonal structure of W, we solve for the top left 9 × 9 block of the metric using the SOS formulation discussed in Section 4 and independently design W_ψ. Once again, the trigonometric terms in the dynamics were approximated using Chebyshev polynomial expansions up to third order (the resulting W(x) was numerically verified using the true dynamics via gridding over (f, ϕ, θ)). Figure 10 plots the optimal curve for J_CCM as a function of λ (for the top left 9 ×9 block of W(x)), using the alternative objective given in Remark 4.2. The scaling matrix W_s was chosen in order to prioritize the Euclidean error, and set as diag(15, 15, 15, 1, 1, 1, 1, 1, 1).

The optimal contraction rate can be seen to be λ = 1.29 and the corresponding dual metric W(x) contained 35 unique monomials in (f, ϕ, θ). Assuming a cross-wind acting in any direction with effective acceleration up to 0.1 m/s², we used gridding to compute a value of 0.0432 for $\bar{d}$ . The resulting projections of ellipsoid (25) onto the position coordinates corresponded roughly to a sphere of radius $\approx 8.8$ cm. The projection onto the thrust axis yielded the interval [−0.05, 0.05]g and the projections in the (ϕ, θ) axes corresponded to the intervals [−11.1°, 11.1°] and [−7.45°, 7.455°], respectively. Since the projection of the bound along the yaw axis is given by $\bar{d} \sqrt{W_{ψ}}$ (see Remark 4.2), we set W_ψ = 17.3, yielding the interval [−10°, 10°]. The resulting tightened constraints are still permissive of aggressive maneuvers within cluttered obstacle environments, as demonstrated next.

Remark 6.1

Note that although the SOS optimization gives us the lower and upper bounds on the eigenvalues of W(x) (and hence M(x)) due to explicit encoding of the uniform definiteness constraint, which can subsequently be used with λ to compute an upper bound on $\bar{d}$ , this bound is likely to be loose. This is inevitable due to the SOS S-procedure relaxation. In contrast, we used gridding to compute a tighter value for $\bar{d}$ . While this remains a viable technique for smaller state-spaces or cases where the metric only depends on a few components of the state (e.g., notice that for the 10-dimensional quadrotor, the metric only depends on 3 states), there are obvious scalability implications. We refer the reader to the conclusions section of this article for a more involved discussion on scalability challenges.

6.2.3. Simulation under nominal disturbances

To verify the controller performance, we randomly initialized obstacle environments for the quadrotor, an example is depicted in Figure 11. Trajectory planning was performed by first computing a waypoint path using geometric FMT* (Janson et al., 2015a), and then smoothing this path using polynomial splines with the min-snap algorithm in Richter et al. (2016). Finally, differential flatness was leveraged to recover the open-loop state and control trajectories. Collision checking was performed by leveraging the configuration space representation of the obstacles, that is, polytopes, inflated by the size of the quadrotor (approximated as a 20 cm radius ball) and the projection of the tube bound onto position coordinates (a further 8.8 cm radius ball).

Figure 12 shows an example of such a computed trajectory, along with the surrounding tube margin. The maximum speed along this trajectory is 3.00 m/s and the maximum pitch angle is approximately 21.5°. The yaw trajectory was designed to follow the horizontal plane velocity. Note that both these aspects of planning (waypoint generation using sampling-based planning and polynomial smoothing) are real-time algorithms and therefore can be efficiently executed in receding-horizon fashion. Robustness is easily accounted for via inflating the obstacles by the tube margin.

Figure 13 illustrates the simulation results for the computed trajectory in Figure 12 for a variety of disturbance time-series (sinusoidal varying and fixed direction signals with constant magnitude set to $\bar{w}$ ). The feedback controller is implemented using zero-order-hold at 250 Hz. All errors are observed to respect the theoretically computed bounds.

6.2.4. Assessing conservatism

Within this section, we empirically evaluate the conservatism in the computed tracking bounds by planning trajectories assuming the disturbance bound of 0.1 m/s² and increasing the actual disturbance level during simulation. We randomly generated 100 new trajectories similar to Figure 11 with varying obstacle placements. For each of the 100 nominal trajectories, we simulated the 24 disturbance time-series from Figure 13 with $\bar{w} \in {0.1,0.15,0.2,0.25,0.3,0.35,0.4,0.45,0.5}$ m/s². Figure 14 plots the empirical cdf of the tracking errors for different simulation disturbance thresholds. For reference, we also delineate the theoretical tracking bounds computed with the simulation $\bar{w}$ values. The conservatism evaluations, therefore, are: (i) gap between actual tracking errors and the theoretical upper bound, and (ii) extent of violation of a bound computed with a lower $\bar{w}$ .

It can be noted in Figure 14a that the gap between the largest geodesic energy tracking error observed for a fixed disturbance threshold and the theoretical upper bound is impressively tight. Indeed, this gap ranged between 1.56 and 30% of the theoretical bounds. Additionally, in all cases, the tracking errors crossed the theoretical bound computed using a value of $\bar{w}$ that was 0.05 m/s² smaller than the simulated level. Both these indicators validate that the analysis in Theorem 3.8 is in fact practically useful and not overly conservative. The larger gaps between the maximum translational error and the theoretical bound, as observed in Figure 14b, are to be expected since the geodesic energy is a full state error measure.

7. Hardware experiments

Finally, we illustrate the approach on an open-source quadrotor platform, shown in Figure 15. The quadrotor consists of (i) a standard DJI F330 frame, (ii) Pixhawk autopilot running the estimator and lower-level thrust and angular rate controllers, and (iii) a companion on-board ODROID-XU4 computer running a ROS node for computing the CCM controller (which generates the thrust and angular rate setpoints for Pixhawk). The code for the ROS node is available for download at https://github.com/ssingh19/asl_flight. There is also an Optitrack motion capture system providing inertial position and yaw estimates at 120 Hz, which is fused with the onboard EKF on the Pixhawk.

7.1. Controller implementation

7.1.1. CCM controller

During simulation, it was often observed that the computed geodesics at each sampling instant were nearly a straight line between x*(t) and x(t). Therefore, to further improve the runtime efficiency of the controller (i.e., avoid having to solve problem $O P T_{γ}$ on the embedded hardware), we investigated the performance of the controller using the straight-line approximation of the geodesic.

While it is theoretically possible to bound the numerical error resulting from this approximation and the induced error within inequality (21) via bounding the Christoffel symbols associated with M(x), such an error analysis is likely to be conservative. Instead, we extracted tuples of (x*(t), x(t)) from the simulations in Section 6.2.4, and performed the following evaluations—see Figure 16:

• Compare $E (γ)$ and $E (c_{sl})$ , where c_sl(⋅, t) is the straight-line connecting x*(t) and x(t).

• Magnitude of the induced error (i.e., violation) in inequality (21) with u_x set to the feedback computed using c_sl. This is the main evaluation criterion, from a stability performance perspective.

From Figure 16a, we observe that $E (c_{sl})$ almost perfectly matches $E (γ)$ , while Figure 16b illustrates that the resulting violation in the stability inequality (21) is practically negligible. To account for potential time-compounding effects of using the straight-line approximation (the evaluations in Figure 16 are pointwise in time), we repeated all simulations from Section 6.2.4 using c_sl in place of the geodesic to compute the tracking controller. Figure 17 presents the resulting tracking error cdfs. From Figure 17, one notices that the theoretical tracking bounds are indeed violated due to the compounding effect of the error induced within the stability inequality (21).Ĺ However, the cdfs evaluated at the theoretical tracking bounds ranged from 0.999940 to 1.0. Equivalently, the cumulative time spent in violation of the geodesic energy theoretical bound, over all the simulations represented within Figure 17, was 7.044 s. Compared to the 40.71 h of simulation time represented in this figure, this is equivalent to a proportional violation of 0.004%. The translational tracking error however, remained always below the theoretical upper bound. Therefore, the straight-line approximation of the geodesic is a relatively mild simplification for this quadrotor example.

7.1.2. Lower-level Pixhawk controller

The lower-level controller on the Pixhawk autopilot is tasked with tracking the thrust and angular rate control setpoints generated by the CCM controller. The Euler rate commands from the CCM controller are converted into desired body-rates onboard Pixhawk and fed into the existing PID control loop. For thrust control, the commanded normalized thrust f_c from the CCM controller and the estimated inertial acceleration $\hat{\ddot{p}}$ (obtained via moving-average finite-differencing) are used to first compute an error

e_{f} = (f_{c} - g e_{3} \cdot {\hat{b}}_{z}) - (- \hat{\ddot{p}} \cdot {\hat{b}}_{z})

which is simply the difference in the desired and actual inertial accelerations, projected along the instantaneous

{\hat{b}}_{z}

axis. This error is then converted into a normalized throttle command τ ∈ [0, 1] using the following discrete-time recursion

τ (t_{k}) = τ (t_{k - 1}) + k_{p} e_{f} (t_{k}),

where the time indices t_k, k = 0, 1, 2, … delineate the Pixhawk sampling times and k_p is a constant positive gain. The CCM controller on board the ODROID is run at 250 Hz, while the Pixhawk control loop runs between 350 and 400 Hz. The controller above is similar to the thrust controller proposed in Tal and Karaman (2018), and corresponds to an iterative scheme where the proportional error term is used to simply correct the previous throttle command, precluding the need to estimate the complex mapping between desired thrust and throttle, or rely on the popular yet oversimplifying quadratic model of this relationship.

7.2. Calibrating disturbance bound

The role of aerodynamic disturbance for experiments was played by the (neglected in eq. (52)) drag force. To calibrate an upper bound for planning, we flew two calibration trajectories: a figure-eight (presented in this section) and a “race-course” (similar to the one in the next section). We describe the figure-eight trajectory here. The nominal trajectory was set as: p_x(t) = r_x − r_x cos(ωt), p_y(t) = r_y sin(2ωt), p_z(t) = h, where we set r_x = 1 m, r_y = 0.7 m, ω = 2π/10, and a constant altitude h = 1.5 m. The drag force was estimated based on the following augmented translational dynamics

\ddot{p} = g e_{3} - f {\hat{b}}_{z} + R \underset{: = f_{d}}{\underset{}{(d_{0} - D R^{T} \dot{p})}}

(53)

where R is the rotation matrix, D = diag(μ_x, μ_y, 0), and

d_{0} = {(d_{0_{x}}, d_{0_{y}}, 0)}^{T}

. Here, d₀ plays the role of a fixed perturbing force stemming from propeller misalignment, and D models the linear velocity drag coefficients, commonly accepted to be the dominant component of drag within the expected flight regime; for instance, see Kai et al. (2017) and Faessler et al. (2018). As in Faessler et al. (2018), we neglect the body z component of drag.

To estimate the net body-frame drag f_d, we leveraged a smoothed finite-differenced estimate of $\ddot{p}$ , the estimated rotation matrix R, and the commanded normalized thrust f_c in place of f. This last substitution is justified courtesy of the lower-level thrust controller presented in the preceding section. Figure 18 shows a series of plots from the figure-eight experiment, including (a) XY desired and actual traces, (b) translational tracking errors, (c) body-frame velocity, (d) estimated body-frame drag, (e) Euclidean norm of net drag, and (f) geodesic energy.

Concatenating the data from the figure-eight and race-course trajectories, we used least-squares to compute the following estimates: $μ_{x} = 0.302, μ_{y} = 0.288, d_{0_{x}} = - 0.008, d_{0_{y}} = - 0.026$ . Figure 19 plots an overlap of the estimated and predicted drag for the figure-eight trajectory, and illustrates good agreement. In the next section, we present additional plots of estimated and predicted drag from the planning experiments to further validate the use of this drag model as a viable disturbance model.

Remark 7.1

In these experiments, the unknown but bounded perturbing force was taken to be the linear drag model. However, given the recent advancements in leveraging drag models within planning, for instance, as in (Faessler et al., 2018), one may also incorporate the learned drag model within the nominal dynamics and compute a drag-compensated CCM. While this would necessitate including coupled velocity and yaw dependence within the CCM, it would allow the separation of drag from the unmodeled aerodynamic disturbances, thereby reserving the disturbance bound margin for purely exogenous effects. For the purposes of illustrating the methodology in this paper, we reserve this extension for future work.

7.3. Robust planning

Equipped with a calibrated disturbance model, we ran the robust trajectory planner (geometric FMT* plus polynomial spline smoothing) introduced in the simulation section on the “race-course” test environment shown below in Figure 20. The computed trajectory was setup to create a challenging loop through the obstacle course through intermediate waypoints placed, for example, at the center of gates and in between the poles. To ensure robustness with respect to the drag model, a single constant was used to scale time (effectively, speed) along the trajectory such that the drag-adjusted invariant tube was collision-free. Importantly, the drag model was not re-estimated for these test trajectories. The computed trajectory along with the invariant tube is shown in Figure 21. Figure 22 plots the nominal body-frame velocity and expected drag.

The maximum expected drag along the nominal trajectory is 0.59 m/s², corresponding to a top speed of 1.9 m/s. This results in an adjusted translation error bound of 52 cm and a geodesic energy bound of ${\bar{d}}^{2} = 0.0648$ . The trajectory in Figure 21 was computed with a combined collision margin of 52 cm plus an additional 27 cm to account for the size of the quadrotor (see Figure 15).

Following the computation of the nominal trajectory, the quadrotor was flown through the obstacle loop three times, with a 25% increase in speed (respectively, decrease in lap time) with each successive lap. A time-lapse⁵ during the fastest lap is shown in Figure 23.

Note that the trajectory was not re-computed during the experiment since the purpose here was to (i) illustrate that the theoretically computed tracking error bounds are indeed physically meaningful and achievable on an actual hardware testbed, and (ii) evaluate the conservatism in the tracking bounds via greater experienced disturbance than planned for. In particular, only the first speed setting is theoretically robustly collision-free, in that the invariant tube computed using the max expected drag is collision-free. The subsequent laps executed at higher speeds have significantly higher expected drag and the resulting scaled tubes are not collision-free. Given the simulation results presented in Figure 14b however, we anticipated (and observed) collision-free execution at the higher speeds as well.

The experiment results for the three laps are presented in Figures 24 to 26, and comprise (i) XY desired and actual traces, (ii) translational and geodesic energy tracking errors, and (iii) estimated and predicted body frame and net drag. The statistics for the three laps are presented in Table 1.

Table 1.

Nominal trajectory extremes (max drag corresponds to the prediction from the linear drag model), drag-adjusted bounds, and actual flight error statistics. All three laps respect the theoretical upper bounds computed prior to the flights.

	Flight extremes			Bounds		Errors rms (max)
	Speed	Bank	Drag	$E$	‖p − p*‖	$E$	‖p − p*‖
	[m/s]	[deg]	[m/s²]	—	[cm]	—	[cm]
Lap 1	1.91	11.46	0.59	0.0648	52	0.017 (0.044)	13.18 (25.4)
Lap 2	2.54	19.8	0.78	0.112	68	0.025 (0.074)	14.51 (30.17)
Lap 3	3.81	39.04	1.15	0.247	101	0.036 (0.124)	16.24 (38.91)

The key takeaway from the preceding numbers and plots is that all theoretical (a priori) geodesic energy and translational error tracking bounds computed using the calibrated drag model are validated within all three flights. Additionally, we note the following interesting observations. First, the peaks in the geodesic energy curves line up quite well with the local peaks in aerodynamic disturbance due to either drag model or ground/interaction effects. Indeed, as the video illustrates, there are notable perturbations in the vicinity of the box (see also Figure 27), ground, and poles. Second, on each successive lap, as the trajectory becomes more aggressive, the geodesic energy tracking error becomes more concentrated at smaller fractions of the theoretical upper bounds. This is potentially due to the fact that the min-norm formulation of the feedback controller in Section 3.3.1 possesses a “minimally invasive” property akin to trigger or event-based control, in that, the optimal feedback can be zero. However, for the faster trajectories, the feedback is non-zero for a greater proportion of the time, thereby resulting in proportionally tighter tracking. Further, we observe that the geodesic energy at higher velocities would violate the bounds generated for lower velocities. This suggests that the bounds computed during CCM synthesis are not overly conservative, which confirms the numerical results presented in Section 6.2.4.

Finally, we stress that the purpose of this experiment was to illustrate the feasibility of converting the rich theoretical analysis into a practical tool capable of running on modern hardware, and obtain true validation of the complete planning methodology. Further performance improvements may be obtained through incorporating richer dynamic models and leveraging higher rate dedicated controllers.

8. Conclusions and future work

We presented a framework for robust motion planning for robots with nonlinear dynamics subject to bounded disturbances, input constraints, and online state constraints. Our approach allows one to generate certifiably safe trajectories online when faced with a priori unknown environment. We leveraged recent advances in contraction theory in the form of CCMs to synthesize a tracking feedback controller and an optimized invariant tube, valid for any dynamically feasible trajectory. Such an invariant tube is consequently used as a robustness margin during online trajectory generation. Importantly, our approach is modular in the sense that we do not put any constraints on the planner itself; indeed we demonstrated our approach using a variety of planning techniques such as pseudospectral discretization with nonlinear programming and sampling-based planning with polynomial spline smoothing. The two relevant components of the approach are the tracking feedback controller and the invariant tube—both of which can be interfaced with the vast majority of planning techniques. We validated our approach in simulation and on a quadrotor hardware testbed, and extensively tested the limits of performance of the synthesized controller to study potential failure modes. This work introduces several promising future directions, discussed next.

Computation: The primary computational challenge stems from the offline synthesis of the CCM. Computing CCMs requires solving a semi-infinite optimization problem (Hettich and Kortanek, 1993)—a daunting task. SOS programming allows us to obtain sufficient certificates for the infinite-dimensional constraints but at a cost of scalability. The size of the equivalent SDP re-formulation of a SOS constraint scales as O(n^d) where n is the state-space dimension and 2d is the degree of the polynomial. While there are some alternative relaxations available based on LP and SOCP (Ahmadi and Majumdar, 2014), these are more conservative than the SDP re-formulation. Looking forward, two interesting avenues of work involve (i) leveraging non-polynomial SOS decompositions, and (ii) leveraging sampling-based algorithms to obtain high probability asymptotic guarantees, for example, as in Zhang et al. (2010).

Conservatism: Our analysis is conservative due to the fact that we derive a globally valid invariant tube, as opposed to the funnel library approach in which one computes bounds valid locally around a trajectory. This issue was slightly tempered by the introduction of less conservative time-varying tubes, used within a local receding-horizon re-planner as for the planar-quadrotor example. A more promising method for reducing the conservatism is to partition the state-space into regions in which CCMs are computed locally, while ensuring continuity/smoothness at the region boundaries. Such a computational approach may also allow the use of lower-order polynomial expansions, thereby reducing the size of the SOS programs.

A second limitation of the approach is that the invariant tubes are computed based on a worst-case disturbance bound assumption. A less conservative solution may be achieved by decomposing the disturbance term into an unknown constant (or slowly-varying) mean, estimated online, plus zero-mean stochastic noise. In this way, one may leverage modern adaptive and disturbance estimation techniques to counteract the constant disturbance term, and a stochastic modification of CCM theory to obtain probabilistic invariance guarantees, for example, based on super-martingale analysis (Steinhardt and Tedrake, 2012).

Perception Uncertainty: In this work, we assumed full state knowledge and the ability to perfectly sense obstacles in the neighborhood of the robot. In reality, noisy sensors inject uncertainty into both these terms. In this case, providing almost sure guarantees is practically infeasible and one must additionally incorporate perception/sensing uncertainty, for instance, in a Bayesian sense, to obtain (at most) probabilistic guarantees. This is, however, a well-known open problem.

We believe that the modular approach presented herein for automatically synthesizing feedback controllers that are optimized for robust performance, and accompanied by guarantees readily integrable into existing planning algorithms, serves as a valuable tool for a practitioner to systematically balance performance and safety.

Footnotes

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the Office of Naval Research (Grant no. N00014-15-1-2673).

ORCID iDs

Sumeet Singh

Anirudha Majumdar

Notes

Appendix

References

Adelstein

Epstein

(2017) Morse theory for the uniform energy. Journal of Geometry 108(3): 1193–1205.

Agha-mohammadi

Chakravorty

Amato

(2014) FIRM: Sampling-based feedback motion planning under motion uncertainty and imperfect measurements. Int. Journal of Robotics Research 33(2): 268–304.

Ahmadi

Majumdar

(2014) DSOS and SDSOS optimization: LP and SOCP-based alternatives to sum of squares optimization. In: IEEE Annual Conf. on Information Sciences and Systems.

Ahmadi

Majumdar

(2016) Some applications of polynomial optimization in operations research and real-time decision making. Optimization Letters 20(4): 709–729.

Althoff

Scherer

(2015) Online safety verification of trajectories for unmanned flight with offline computed robust invariant sets. In: IEEE/RSJ Int. Conf. on Intelligent Robots and Systems.

Althoff

Dolan

(2014) Online verification of automated road vehicles using reachability analysis. IEEE Transactions on Robotics 30(4): 903–918.

Aminzarey

Sontag

(2014) Contraction methods for nonlinear systems: A brief introduction and some open problems. In: Proc. IEEE Conf. on Decision and Control.

Angeli

(2002) A Lyapunov approach to incremental stability properties. IEEE Transactions on Automatic Control 47(3): 410–422.

Angeli

(2009) Further results on incremental input-to-state stability. IEEE Transactions on Automatic Control 54(6): 1386–1391.

10.

ApS

(2017) MOSEK Optimization Software. Available at: https://mosek.com/

11.

Bansal

Chen

Herbert

, et al. (2017) Hamilton-Jacobi reachability: A brief overview and recent advances. In: Proc. IEEE Conf. on Decision and Control.

12.

Barry

Majumdar

Tedrake

(2012) Safety verification of reactive controllers for uav flight in cluttered environments using barrier certificates. In: Proc. IEEE Conf. on Robotics and Automation.

13.

Battilotti

De Santis

(2003) Stabilization in probability of nonlinear stochastic systems with guaranteed region of attraction and target set. IEEE Transactions on Automatic Control 48(9): 1585–1599.

14.

Bayer

Bürger

Allgöwer

(2013) Discrete-Time Incremental ISS: A Framework for Robust NMPC. European Control Conference.

15.

Betts

(2010) Practical Methods for Optimal Control and Estimation Using Nonlinear Programming. Second edition. SIAM.

16.

Blackmore

Williams

(2006) A Probabilistic Approach to Optimal Robust Path Planning With Obstacles. In: American Control Conference.

17.

Blackmore

Ono

Williams

(2011) Chance-constrained optimal path planning with obstacles. IEEE Transactions on Robotics 27(6): 1080–1094.

18.

Bravo

Alamo

Camacho

(2006) Robust MPC of constrained discrete-time nonlinear systems based on approximated reachable sets. Automatica 42(10): 1745–1751.

19.

Buehler

Paulson

Mesbah

(2016) Lyapunov-Based Stochastic Nonlinear Model Predictive Control: Shaping the State Probability Distribution Functions. In: American Control Conference.

20.

Burridge

Rizzi

Koditschek

(1999) Sequential composition of dynamically dexterous robot behaviors. Int. Journal of Robotics Research 18(6): 534–555.

21.

Cannon

Buerger

Kouvaritakis

, et al. (2011) Robust tubes in nonlinear model predictive control. IEEE Transactions on Automatic Control 56(8): 1942–1947.

22.

Carson

III Açikmeşe

Murray

, et al. (2013) A robust model predictive control algorithm augmented with a reactive safety mode. Automatica 49(5): 1251–1260.

23.

Charnes

Cooper

(1959) Chance-constrained programming. Management Science 6(1): 73–79.

24.

Chen

Herbert

Vashishtha

, et al. (2016) Decomposition of reachable sets and tubes for a class of nonlinear systems. Available at: https://arxiv.org/abs/1611.00122

25.

Crouch

van der Schaft

(1987) Variational and Hamiltonian Control Systems. Springer.

26.

Faessler

Franchi

Scaramuzza

(2018) Differential flatness of quadrotor dynamics subject to rotor drag for accurate tracking of high-speed trajectories. IEEE Robotics and Automation Letters 3(2): 620–626.

27.

Farina

Scattolini

(2012) Tube-based robust sampled-data MPC for linear continuous-time systems. Automatica 48(7): 1473–1476.

28.

Fleming

Kouvaritakis

Cannon

(2015) Robust tube MPC for linear systems with multiplicative uncertainty. IEEE Transactions on Automatic Control 60(4): 1087–1092.

29.

Forni

Sepulchre

(2014) A differential Lyapunov framework for contraction analysis. IEEE Transactions on Automatic Control 3(59): 614–628.

30.

Fridovich-Keil

Herbert

Fisac

, et al. (2018) Planning, fast and slow: A framework for adaptive real-time safe trajectory planning. In: Proc. IEEE Conf. on Robotics and Automation.

31.

Gillula

Huang

Vitus

, et al. (2010) Design of guaranteed safe maneuvers using reachable sets: Autonomous quadrotor aerobatics in theory and practice. In: Proc. IEEE Conf. on Robotics and Automation.

32.

Helwa

Heins

Schoellig

(2019) Provably robust learning-based approach for high-accuracy tracking control of lagrangian systems. IEEE Robotics and Automation Letters 4(2): 1587–1594.

33.

Herbert

Chen

Han

, et al. (2017) FaSTrack: a modular framework for fast and guaranteed safe motion planning. In: Proc. IEEE Conf. on Decision and Control.

34.

Hettich

Kortanek

(1993) Semi-infinite programming: Theory, methods, and applications. SIAM Review 35(3): 380–429.

35.

Iwasaki

Hara

(2005) Generalized KYP lemma: unified frequency domain inequalities with design applications. IEEE Transactions on Automatic Control 50(1): 41–59.

36.

Janson

Schmerling

Clark

, et al. (2015a) Fast Marching Tree: A fast marching sampling-based method for optimal motion planning in many dimensions. Int. Journal of Robotics Research 34(7): 883–921.

37.

Janson

Schmerling

Pavone

(2015b) Monte Carlo motion planning for robot trajectory optimization under uncertainty. In: Int. Symp. on Robotics Research.

38.

Jouffroy

(2003) A simple extension of contraction theory to study incremental stability properties. In: European Control Conference.

39.

Kaelbling

Littman

Cassandra

(1998) Planning and acting in partially observable stochastic domains. Artificial Intelligence 101(1–2): 99–134.

40.

Kai

Allibert

Hua

, et al. (2017) Nonlinear Feedback Control of Quadrotors Exploiting First-Order Drag Effects. In: IFAC World Congress.

41.

Khalil

(2002) Nonlinear Systems. Third edition. Prentice Hall.

42.

Kögel

Findeisen

(2015) Discrete-time robust model predictive control for continuous-time nonlinear systems. In: American Control Conference.

43.

Köhler

Soloperto

R, A MM

Allgower

, et al. (2019) A Computationally Efficient Robust Model Predictive Control Framework for Uncertain Nonlinear Systems. Submitted to IEEE Transactions on Automatic Control; Available at: https://www.ist.uni-stuttgart.de/de/institut/team/PDFs_MA-Seiten/JK/Robust_Nonlin.pdf

44.

Kong

Gao

Chen

, et al. (2015) dreach: δ-Reachability analysis for hybrid systems. In: Int. Conf. on Tools and Algorithms for the Construction and Analysis of Systems.

45.

Kurniawati

Hsu

Lee

(2008) SARSOP: Efficient Point-Based POMDP Planning by Approximating Optimally Reachable Belief Spaces. In: Robotics: Science and Systems.

46.

Langson

Chryssochoos

Raković

, et al (2004) Robust model predictive control using tubes. Automatica 40(1): 125–133.

47.

Leung

Manchester

(2017) Chebyshev pseudospectral method for nonlinear stabilization using control contraction metrics. In: American Control Conference. To Appear.

48.

Limon

Alvarado

Alamo

, et al. (2010) Robust tube-based MPC for tracking of constrained linear systems with additive disturbances. Journal of Process Control 20(3): 248–260.

49.

Lin

Sontag

(1991) A universal formula for stabilization with bounded controls. System and Control Letters 16(6): 393–397.

50.

Lohmiller

Slotine

JJE

(1998) On contraction analysis for non-linear systems. Automatica 34(6): 683–696.

51.

Lopez

Slotine

JJE

How

(2018) Robust collision avoidance via sliding control. In: Proc. IEEE Conf. on Robotics and Automation.

52.

Pong

(2011) Minimizing condition number via convex programming. SIAM Journal on Matrix Analysis and Applications 32(4): 1193–1211.

53.

Luders

Ellertson

How

, et al. (2016) Wind uncertainty modeling and robust trajectory planning for autonomous parafoils. AIAA Journal of Guidance, Control, and Dynamics 39(7): 1614–1630.

54.

Majumdar

Tedrake

(2012) Robust Online Motion Planning With Regions of Finite Time Invariance. In: Workshop on Algorithmic Foundations of Robotics.

55.

Majumdar

Tedrake

(2017) Funnel libraries for real-time robust feedback motion planning. Int. Journal of Robotics Research 36(8): 947–982.

56.

Majumdar

Tobenkin

Tedrake

(2012) Algebraic verification for parameterized motion planning libraries. In: American Control Conference.

57.

Manchester

Tang

Slotine

JJE

(2015) Unifying Classical and Optimization-Based Methods for Robot Tracking Control With Control Contraction Metrics. In: Int. Symp. on Robotics Research.

58.

Manchester

Slotine

JJE

(2014) Output-Feedback Control of Nonlinear Systems Using Control Contraction Metrics and Convex Optimization. In: Australian Control Conference.

59.

Manchester

Slotine

JJE

(2017) Control Contraction Metrics: Convex and Intrinsic Criteria for Nonlinear Feedback Design. IEEE Transactions on Automatic Control In Press.

60.

Manchester

Kuindersma

(2017) DIRTREL: Robust Trajectory Optimization with Ellipsoidal Disturbances and LQR Feedback. In: Robotics: Science and Systems.

61.

Manchester

Kuindersma

(2019) Robust direct trajectory optimization using approximate invariant funnels. Autonomous Robots 43(2): 375–387.

62.

Mayne

(2014) Model predictive control: Recent developments and future promise. Automatica 50(12): 2967–2986.

63.

Mayne

Kerrigan

van Wyk

, et al. (2011) Tube-based robust nonlinear model predictive control. Int. Journal of Robust and Nonlinear Control 21(11): 1341–1353.

64.

Mayne

Seron

Raković

(2005) Robust model predictive control of constrained linear systems with bounded disturbances. Automatica 41(2): 219–224.

65.

Nguyen

Shew

Xia

(2016) Maximizing the sum of a generalized Rayleigh quotient and another Rayleigh quotient on the unit sphere via semidefinite programming. Journal of Global Optimization 64(2): 399–416.

66.

Ono

Williams

Blackmore

(2013) Probabilistic planning for continuous dynamic systems under bounded risk. Journal of Artificial Intelligence Research 46(1): 511–577.

67.

Parrilo

(2000) Structured Semidefinite Programs and Semialgebraic Geometry Methods In Robustness and Optimization. PhD Thesis, Massachusetts Inst. of Technology.

68.

Raimondo

Magni

, et al. (2009) Robust model predictive control of nonlinear systems with bounded and state-dependent uncertainties. IEEE Transactions on Automatic Control 54(7): 1681–1687.

69.

Platt

Kaelbling

Tomas

, et al (2012) Non-Gaussian belief space planning: Correctness and complexity. In: Proc. IEEE Conf. on Robotics and Automation.

70.

Prajna

Jadbabaie

Pappas

(2007) A framework for worst-case and stochastic safety verification using barrier certificates. IEEE Transactions on Automatic Control 52(8): 1415–1428.

71.

Prentice

Roy

(2009) The belief roadmap: Efficient planning in linear POMDPs by factoring the covariance. Int. Journal of Robotics Research 28(11–12): 1448–1465.

72.

Primbs

Nevistić

Doyle

(1999) Nonlinear optimal control: A control Lyapunov function and receding horizon perspective. Asian Journal of Control 1(1): 14–24.

73.

Putinar

(1993) Positive polynomials on compact semi-algebraic sets. Indiana Univ. Mathematics Journal 42(3): 969–984.

74.

Quinlan

Khatib

(1993) Elastic bands: Connecting path planning and control. In: Proc. IEEE Conf. on Robotics and Automation.

75.

Raković

(2009) Set theoretic methods in model predictive control. In: Nonlinear Model Predictive Control. Springer Berlin Heidelberg.

76.

Raković

Kerrigan

Mayne

, et al. (2006a) Reachability analysis of discrete-time systems with disturbances. IEEE Transactions on Automatic Control 51(4): 546–561.

77.

Rakovic

Kouvaritakis

Cannon

, et al. (2012) Parameterized tube model predictive control. IEEE Transactions on Automatic Control 57(11): 2746–2761.

78.

Raković

Teel

Mayne

, et al. (2006b) Simple robust control invariant tubes for some classes of nonlinear discrete time systems. In: Proc. IEEE Conf. on Decision and Control.

79.

Rayguru

Kar

(2015) Contraction Based Stabilization of Approximate Feedback Linearizable Systems. In: European Control Conference.

80.

Richter

Bry

Roy

(2016) Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments. In: Robotics Research. Springer.

81.

Rubagotti

Raimondo

Ferrara

, et al. (2011) Robust model predictive control with integral sliding mode in continuous-time sampled-data nonlinear systems. IEEE Transactions on Automatic Control 56(3): 556–570.

82.

Scott

Barton

(2013) Bounds on the reachable sets of nonlinear control systems. Automatica 49(1): 93–100.

83.

Sharma

Kar

(2009) Contraction Based Adaptive Control of a Class of Nonlinear Systems. In: American Control Conference.

84.

Simpson-Porco

Bullo

(2014) Contraction theory on Riemannian manifolds. System and Control Letters 65(1): 74–80.

85.

Singh

Majumdar

Slotine

JJE

, et al. (2017) Robust online motion planning via contraction theory and convex optimization. In: Proc. IEEE Conf. on Robotics and Automation. Extended Version, Available at: http://asl.stanford.edu/wp-content/papercite-data/pdf/Singh.Majumdar.Slotine.Pavone.ICRA17.pdf

86.

Slotine

JJE

(2007) Sliding controller design for non-linear systems. Int. Journal of Control 38(2): 421–434.

87.

Sontag

(2010) Contractive systems with inputs. In: Perspectives in Mathematical System Theory, Control, and Signal Processing. Springer Berlin Heidelberg.

88.

Sontag

Margaliot

Tuller

(2014) On three generalizations of contraction. In: Proc. IEEE Conf. on Decision and Control.

89.

Spivak

(1999) A Comprehensive Introduction to Differential Geometry, Vol. I. Third edition. Publish or Perish, Inc.

90.

Steinhardt

Tedrake

(2012) Finite-time regional verification of stochastic non-linear systems. Int. Journal of Robotics Research 31(7): 901–923.

91.

Sun

Patil

Alterovitz

(2015) High-frequency replanning under uncertainty using parallel sampling-based motion planning. IEEE Transactions on Robotics 31(1): 104–116.

92.

Tal

Karaman

(2018) Accurate tracking of aggressive quadrotor trajectories using incremental nonlinear dynamic inversion and differential flatness. In: Proc. IEEE Conf. on Decision and Control.

93.

Tedrake

Manchester

Tobenkin

, et al. (2010) LQR-trees: Feedback motion planning via sums-of-squares verification. Int. Journal of Robotics Research 29(8): 1038–1052.

94.

Tobenkin

Permenter

Megretski

(2013) SPOTLESS polynomial and conic optimization. Software Available from: https://github.com/spottoolbox/spotless

95.

Tomlin

Mitchell

Bayen

, et al. (2003) Computational techniques for the verification of hybrid systems. Proc. of the IEEE 91(7): 986–1001.

96.

Villanueva

Quirynen

Diehl

, et al. (2017) Robust MPC via min–max differential inequalities. Automatica 77(1): 311–321.

97.

Böhm

Chen

, et al. (2010) Robust model predictive control with disturbance invariant sets. In: American Control Conference.

98.

Maier

Chen

, et al. (2013) Tube MPC scheme based on robust control invariant set with application to Lipschitz nonlinear systems. System and Control Letters 62(2): 194–200.

99.

Zamani

van de Wouw

Majumdar

(2013) Backstepping controller synthesis and characterizations of incremental stability. System and Control Letters 62(10): 949–962.

100.

Zhang

López

(2010) A new exchange method for convex semi-infinite programming. SIAM Journal on Optimization 20(6): 2959–2977.