Tube-Certified Trajectory Tracking for Nonlinear Systems With Robust Control Contraction Metrics

This paper presents an approach towards guaranteed trajectory tracking for nonlinear control-affine systems subject to external disturbances based on robust control contraction metrics (CCM) that aims to minimize the $\mathcal L_\infty$ gain from the disturbances to nominal-actual trajectory deviations. The guarantee is in the form of invariant tubes, computed offline and valid for any nominal trajectories, in which the actual states and inputs of the system are guaranteed to stay despite disturbances. Under mild assumptions, we prove that the proposed robust CCM (RCCM) approach yields tighter tubes than an existing approach based on CCM and input-to-state stability analysis. We show how the RCCM-based tracking controller together with tubes can be incorporated into a feedback motion planning framework to plan safe trajectories for robotic systems. Simulation results illustrate the effectiveness of the proposed method and empirically demonstrate reduced conservatism compared to the CCM-based approach.


I. INTRODUCTION
Motion planning for robots with nonlinear and underactuated dynamics -with guaranteed safety in the presence of uncertainties -remains to be a challenging problem.The uncertainties can cause the robot's actual state trajectory to significantly deviate from its nominal behavior, causing collisions, especially when a nominal input trajectory is executed in an open-loop fashion (see Fig. 1 for an illustration).Feedback motion planning (FMP) aims to mitigate the effect of uncertainties through the use of a feedback controller that tracks a nominal (or desired) trajectory.A common practice in FMP to ensure vehicle safety with respect to dynamic constraints and collision avoidance involves design of the tracking controller and computation of a tube or funnel about a nominal trajectory which is guaranteed to contain the actual trajectory in the presence of uncertainties or disturbances.Fig. 1: Planning and control of a planar VTOL vehicle in the presence of wind disturbances.Light-blue and lightgreen shaded areas denote the tube associated with the CCM controller from [1], and the proposed RCCM controller, respectively.Dashed lines denote the planned trajectory without using tubes (left) and with CCM (right) and RCCM (middle) tubes.OL: open loop.case of fully-actuated systems, the tubes may be computed and optimized using sliding mode control [2].Approximation of such sets may also be obtained by linear reachability analysis via linearization around a nominal trajectory and treating nonlinearities as bounded disturbances [3]; however, these results are generally overly conservative.In [4], the authors used linear analysis (i.e., propagation of ellipsoids under linearized dynamics) to compute the size of approximate invariant funnels, and further leveraged it to optimize the nominal trajectory.However, the linearity assumption usually only holds in a small region around the nominal trajectory; furthermore, these methods usually rely on one-off offline computations and are not suitable for real-time motion planning.
Convex programming-based verification methods such as sum of squares (SOS) programming have also gained popularity in FMP.For instance, the LQR tree algorithm in [5] combines local LQR feedback controllers with funnels to compose a nonlinear feedback policy to cover reachable areas.This method requires the task and environment to be predefined due to reliance on offline computations and is not suitable for real-time planning.The funnel library approach in [6] aims to alleviate this issue and enable online re-planning by leveraging 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 composed online for re-planning to avoid obstacles.However, this method is still restricted to a fixed set of trajectories computed offline.
The concept of tube or funnel has also been explored extensively within Tube Model Predictive Control (TMPC), where one computes a tracking feedback (also termed as ancillary) controller that keeps the state within an invariant tube around the nominal MPC trajectory despite disturbances.TMPC has been extensively studied for linear systems with bounded disturbances or model uncertainties [7]- [9].The construction of invariant tubes and ancillary controllers in the nonlinear setup is much more complicated than in the linear case.For instance, [10] simply assumed the existence of a stabilizing (nonlinear) ancillary controller that results in contracting set iterates.Similarly, assuming the existence of a stabilizing feedback controller and a Lyapunov function, [11] constructed a tube based on a Lipschitz constant of the dynamics.This approach, although simple to apply, becomes very conservative for larger prediction horizons.In [12], a quadratic Lyapunov-type function with a linear auxiliary controller is computed offline, which is then used to design a robust MPC scheme for a limited class of nonlinear systems, i.e., linear systems with Lipschitz nonlinearities.For the special case of feedback linearizable systems, [13] used a boundary layer sliding controller as an auxiliary controller, which enables the tube to be parameterized as a polytope and its geometry to be co-optimized in the MPC problem.The authors of [14] used incremental input-to-state stability (δ-ISS) for discretetime systems to derive invariant tubes as a sublevel set of the associated δ-ISS Lyapunov function, which was assumed to be given.Recently in [15], for incrementally (exponentially) stabilizable nonlinear systems subject to nonlinear state and input dependent disturbances/uncertainty, the authors leveraged scalar bounds of an incremental Lyapunov function, computed offline, to online predict the tube size, which is incorporated in the MPC optimization problem for constraint tightening.
Recent work has explored contraction theory within FMP.Contraction theory [16] is a method for analyzing nonlinear systems in a differential framework and is focused on studying the convergence between pairs of state trajectories towards each other, i.e., incremental stability.It has recently been extended for constructive control design, e.g., via control contraction metrics (CCM) for both deterministic [17] and stochastic systems [18], [19].Compared to incremental Lyapunov function approaches for studying incremental stability, contraction metrics is an intrinsic characterization of incremental stability (i.e., invariant under change of coordinates); additionally, the search for a CCM and the stabilizing controller can be formulated as a convex optimization problem.Leveraging CCM, the authors of [1] designed a feedback tracking controller for a nominal nonlinear system and derived tubes in which the actual states are guaranteed to remain despite bounded disturbances using input-to-state stability (ISS) analysis.For systems with matched uncertainties, the authors of [20] designed an L 1 adaptive controller to augment a nominal CCM controller and showed that the resulting tube's size could be systematically reduced by tuning some parameters of the adaptive controller, while the method in [21] based on robust Riemannian energy conditions and disturbance estimation guaranteed exponential convergence to nominal trajectories despite the uncertainties.Finally, robust CCM was leveraged in [22] to synthesize nonlinear controllers that minimize the L 2 gain from disturbances to outputs.This method, however, does not provide tubes to quantify the transient behavior of states and inputs.

B. Contribution
For nonlinear control-affine systems subject to bounded disturbances, this paper presents a tracking controller based on robust CCM (RCCM) to minimize the L ∞ gain from disturbances to state and input trajectory deviations.By solving convex optimization problems offline, the proposed RCCM scheme produces a fully nonlinear tracking controller with explicit disturbance rejection property together with certificate tubes around nominal trajectories, for both states and inputs, in which the actual state/input variables are guaranteed to stay despite disturbances.In comparison, most of existing work in FMP and TMPC usually first designs ancillary controllers without considering the disturbances and then derives invariant tubes/funnels in the presence of disturbances using either ISS analysis (e.g., [1]), Lipschitz properties of the dynamics (e.g., [11]) or SOS verification (e.g., [5], [6]).We further prove, under mild assumptions, that the proposed RCCM approach yields tighter tubes than the CCM approach in [1], which ignores the disturbance in designing the tracking controller and relies on ISS analysis to derive the tubes.As an additional contribution, we illustrate how the RCCM controller and the tubes can be incorporated into an FMP framework to plan guaranteed-safe trajectories, and verify the proposed RCCM scheme on a planar vertical take-off and landing (VTOL) vehicle and a 3D quadrotor.Specifically, compared to the CCM approach, our RCCM approach demonstrates improved tracking performance and reduced tube size for both states and inputs, leading to more aggressive yet safe trajectories (See Fig. 1).
Organization of the paper.Section II states the problem and some preliminary material.Section III presents the RCCM minimizing the L ∞ gain and its application in designing nonlinear trajectory tracking controllers with certificate tubes for transient performance guarantee.In Section V, the proposed RCCM controller is compared with an existing CCM controller.Section IV illustrates how the RCCM controller can be incorporated into a feedback motion planning framework.Verification of the proposed controller on simulated planar VTOL and 3D quadrotor examples is included in Section VI.
Notations.Let R n , R + and R m×n denote the n-dimensional real vector space, the set of non-negative real numbers, and the set of real m by n matrices, respectively.I n and 0 denote an n × n identity matrix, and a zero matrix of compatible dimensions, respectively.• denotes the 2-norm of a vector or a matrix.The space L ∞e is the set of signals on [0, ∞) which, truncated to any finite interval [a, b], have finite amplitude.The L ∞ -and truncated L ∞ -norm of a function x : R + → R n are defined as x L∞ sup t≥0 x(t) and x L [0,T ] ∞ sup 0≤t≤T x(t) , respectively.Let ∂ y F (x) denote the Lie derivative of the matrix-valued function F at x along the vector y.For symmetric matrices P and Q, P > Q (P ≥ Q) means P − Q is positive definite (semidefinite).X is the shorthand notation of X + X .

II. PROBLEM STATEMENT AND PRELIMINARIES
Consider a nonlinear control-affine system where x(t) ∈ R n is the state vector, u(t) ∈ R m is the control input vector, w(t) ∈ R p is the disturbance vector and z(t) ∈ R q denotes the variables related to the performance (with z = x or z = u as a special case), and f (x), B(x) and B w (x) are known vector/matrix functions of compatible dimensions.
We use b i and b w,i to represent the ith column of B(x) and B w (x), respectively.
For the system in (1), assume we have a nominal state and input trajectory, x (•) and u (•), which satisfy the nominal dynamics: where w is the vector of nominal disturbances (including w (t) ≡ 0 as a special case).
For the system (1), this paper is focused on designing a state-feedback controller in the form of to minimize the gain from disturbance deviation, w − w , to output deviation, z − z , of the closed-loop system (obtained by applying the controller (3) to (1)): Formally, such gain is quantified using the concept of universal L ∞ gain defined as follows.Hereafter, we use universal L ∞ gain and L ∞ gain interchangeably.
Definition 1. (Universal L ∞ gain) A control system (4) achieves a universal L ∞ -gain bound of α if for any target trajectory x , w , z satisfying (4), any initial condition x(0), and input w such that w − w ∈ L ∞e , the condition holds for a function β(x 1 , x 2 ) ≥ 0 with β(x, x) = 0 for all x.
Remark 1.The L ∞ -gain bound α in Definition 1 naturally provides certificate tubes to quantify how much the actual trajectory z(•) deviates from the nominal trajectory z (•).For instance, by setting z = x and x(0) = x (0) and using a worst-case estimate of w − w L [0,T ] ∞ , denoted by w (i.e., w − w L [0,T ] ∞ ≤ w), the inequality (5) implies Remark 2. Definition 1 is inspired by the concept of universal L 2 gain in [22].However, unlike the L ∞ gain in Definition 1, the L 2 gain does not produce tubes to quantify the transient behavior of the variable z.

A. Preliminaries
CCM is a tool for controller synthesis to ensure incremental stability of a nonlinear system by studying the variational system, characterized by the differential dynamics.In this paper, we propose RCCM to design the controller (3) to achieve or minimize an L ∞ -gain bound.The differential dynamics associated with (1) are given by where A(x, u, w) where Our solution also involves the differential L ∞ gain.
Definition 2. (Differential L ∞ gain) A system with its differential dynamics represented by (7) has a differential L ∞ -gain bound of α > 0 if for all T > 0, we have for some function β(x, δx) with β(x, 0) = 0 for all x.
Before proceeding to the main results, we first introduce some notations related to Riemannian geometry, most of which are from [22].A Riemannian metric on R n is a symmetric positive-definite matrix function M (x), smooth in x, which defines a "local Euclidean" structure for any two tangent vectors δ 1 and δ 2 through the inner product δ 1 , δ 2 x δ 1 M (x)δ 2 and the norm δ 1 , δ 2 x .A metric is called uniformly bounded if a 1 I ≤ M (x) ≤ a 2 I holds ∀x and for some scalars a 2 ≥ a 1 > 0. Let Γ(a, b) be the set of smooth paths between two points a and b in R n , where each and c s (s) ∂c ∂s .Given a metric M (x), the energy of a path c is defined as E(c)

III. ROBUST CCM FOR TUBE-CERTIFIED TRAJECTORY TRACKING
We first introduce an approach to designing a fully nonlinear controller in the form of (3) to achieve a given L ∞ -gain bound or minimize such a bound, leveraging RCCM.We then present the derivation and optimization of the certificate tubes around nominal state and control input trajectories, in which the actual states and inputs are guaranteed to stay.

A. RCCM for universal L ∞ gain guarantee
Existing work, e.g., [23], provides solutions to controller design for a linear time-invariant (LTI) system for standard L ∞ -gain guarantee/minimization using linear matrix inequality (LMI) techniques.We now extend this result to nonlinear systems for differential L ∞ -gain guarantee/minimization, summarized in the following lemma.
Lemma 1.The closed-loop system (4) has a differential L ∞gain bound of α > 0 if there exists a uniformly-bounded symmetric metric M (x) > 0 and positive constants λ and µ such that for all x, w, we have where ∂M ∂xi ẋi with ẋi given by (4).
Multiplying the preceding inequality by [δ x , δ w ] and its transpose from the left and right, respectively, gives Multiplying ( 10) by [δ x , δ w ] and its transpose from the left and right leads to Plugging the preceding inequality into (12), we obtain that for any t ∈ [0, T ], we have which is equivalent to (9) with the definition of β(x, δ x ) αλV (x, δ x ).The proof is complete.
Remark 3. In case the metric M (x) depends on x i , an element of x, whose derivative is dependent on the input u (or w), Ṁ and thus the condition (10) will depend on u (or w).In this case, a bound on u (or w) needs to be known in order to verify the conditions (10) and (11).
We term the metric V (x, δ x ) = δ x M (x)δ x as a robust CCM (RCCM).Given a closed-loop system, Lemma 1 provides conditions to check whether a constant is a differential L ∞gain bound of the system.We next address the problem of how to design a controller to achieve a desired universal L ∞ -gain bound given an open-loop plant (1).
Control law construction: Similar to [22], we use M (x) as a Riemannian metric to choose the path of minimum energy joining x and x and construct the control law at any time t: where E(c) 1 0 c s M (c(s))c s (s)ds and the matrix function K(•) will be introduced later in (17).Following [17], we make the following assumption to simplify the subsequent analysis.
Assumption 1.For the control system (1), ( 14), the set of times t ∈ [0, ∞) for which x(t) is in the cut locus of x (t) has zero measure.
Without this assumption, the main results (Theorem 1) still hold if the derivative of the Riemannian energy, E(x, x ), used in proof of Theorem 1, is replaced with its upper Dini derivative, as done in [1].The main theoretical results for synthesizing a controller using RCCM to guarantee a universal L ∞ -gain bound can now be presented.
Theorem 1.For the plant (1) with differential dynamics (6), suppose there exists a uniformly-bounded metric W (x) > 0, a matrix function Y (x), and positive constants λ, µ and α such that for all x, u, w, where Ẇ n i=1 ∂W ∂xi ẋi .Then for any target trajectory u , x , w satisfying (2), if Assumption 1 holds, the RCCM controller (14) with achieves a universal L ∞ -gain bound of α for the closed-loop system.
At any time t = t i ∈ [0, ∞], consider the following smoothly parameterized paths of states, controls, disturbances, and outputs for s ∈ [0, 1]: Differentiating these four paths with respect to s at fixed time t = t i with subscript s denoting ∂ ∂s yields: Now suppose that on some time interval [t i , t i + ) and for each s ∈ [0, 1], we fix the control and disturbance inputs to their values at t = t i , and the state c(t, s) evolves according to (1).Here, the interval [t i , t i + ] can be arbitrarily small to guarantee the existence of solutions.By changing the order of differentiation with respect to t and s, we can show that (19) satisfies the closed-loop differential dynamics (7) with Note that where ( 21) is due to (20), and ( 22) can be obtained by multiplying ( 10) by [c s , w s ] and its transpose from the left and right, respectively.Integrating (22) over s ∈ [0, 1] and leveraging w s (t, s) = w(t) − w (t) gives 2 ds.Interchanging the differentiation and integration, we obtain 2 ds, i.e., For sufficiently small , for any t ∈ [t i , t i + ), equation ( 23) indicates Since E(x, x ) is the minimal energy of a path joining x and x , we have Hence, taking → 0, and, since t i was arbitrary, we have for all t ∈ [0, ∞) Integrating the above equation from 0 to t yields 11) by [c s , w s ] and its transpose from the left and right, respectively, gives Applying the Cauchy-Schwarz inequality, we have which, together with (27), leads to for any t.Note that the preceding inequality holds for any path c(t) connecting x(t) and x (t).If we choose the path with minimal energy, i.e. γ(t), then (28) becomes Plugging (26) into the above inequality yields for any t.Therefore, for any T > 0, where β(x, x ) = αλE(x, x ).The proof is complete.Remark 4. From the proof of Theorem 1, one can see that W (x) in ( 15) and ( 16) is connected with M (x) in ( 10) and ( 11) by M (x) = W −1 (x).This is similar to the LTI case where a matrix equal to the inverse of a Lyapunov matrix is introduced for state-feedback control design [23].We term W (x) as a dual RCCM.
Removal of synthesis conditions' dependence on u: Condition (15) may depend on u and w due to the presence of terms A and Ẇ .Dependence on w is not a significant issue as a bound on w can usually be pre-established and incorporated in solving the optimization problem involving (15).Since a bound on u is not easy to obtain (before a controller is synthesized), the dependence of (15) on u is undesired.To remove the dependence on u, we need the following condition: (C1) For each i = 1, . . ., m, ∂ bi W − ∂bi ∂x W = 0. Formally, condition (C1) states that b i is a Killing vector for the metric W [17, Section III.A].In particular, if B is in the form of [0, I m1 ] , condition (C1) requires that W must not depend on the last m 1 state variables.Remark 5. Due to the product term λW in (15), conditions (15) and ( 16) are not convex.However, since λ is a constant, one can perform a line or bi-section search for λ.In such case, verifying the conditions ( 15) and ( 16) becomes a statedependent LMI problem, which can be solved by gridding of the state space or using sum of square (SOS) techniques (see [1] for details).

B. Offline search of RCCM for L ∞ gain minimization
The constant α, which is an upper bound on the universal L ∞ gain, appears linearly in the condition (16) of Theorem 1.Therefore, one can minimize α when searching for W (x) and Y (x).To make the optimization problem feasible, one often needs to limit the states to a compact set, i.e., considering x ∈ X , where X is a compact set.Additionally, since calculating the inverse of W (x) is needed for constructing the control law due to M (x) = W −1 (x) (detailed in Section III-D), one may also want to enforce a lower bound, β, on the eigenvalues of W (x). Therefore, in practice, one could solve the optimization problem OPT RCCM : subject to Conditions ( 15) and ( 16), (29b) x ∈ X .
Note that OPT RCCM just needs to be solved once offline.

C. Offline optimization for refining state and input tubes
In formulating the optimization problem OPT RCCM to search for W (x) and Y (x), the z vector often contains weighted states and inputs to balance the tracking performance and control efforts.For instance, we could have z = [(Qx) , (Ru) ] , where Q and R are some weighting matrices.After obtaining W (x) and Y (x), one can always derive refined L ∞ -gain bounds for some specific state and input variables, ẑ ∈ R l , by re-deriving the C and D matrices in (6) for ẑ = ĝ(x, u), and then solving the optimization problem OPT REF : subject to Conditions ( 15) and ( 16), (30b) For instance, by solving OPT REF , we get an L ∞ -gain bound for the deviation of some states (i.e., x I − x I L∞ , where I is the index set) with ẑ = x I , and a L ∞ -gain bound for the deviation of all inputs (i.e., u − u L∞ ) with ẑ = u.With an L ∞ -gain bound α (from solving OPT REF ) and a bound w on the disturbances, i.e., w − w L∞ ≤ w, the actual variable ẑ is guaranteed to stay in a tube around the nominal variable ẑ , i.e., Following this idea, we can easily get the tube for all or part of the states or inputs.
Remark 6.The tubes obtained through (31) hold for any trajectories that satisfy the nominal dynamics (2), and are particularly suitable to be incorporated into online planning and predictive control schemes, e.g., tube MPC.

D. Online computation of the control law
Geodesic computation: Similar to other CCM or RCCM based control [1], [17], [22], the most computationally expensive part of the proposed control law (14) lies in online computation of the geodesic γ(t) according to (14a) at each time instant t, which necessitates solving a nonlinear programming (NLP) problem.However, since the NLP problem does not involve dynamic constraints, it is much easier to solve than a nonlinear MPC problem.Following [24], such a problem can be efficiently solved by applying a pseudospectral method, i.e., by discretizing the interval [0, 1] using the Chebyshev-Gauss-Lobatto nodes and using Chebyshev interpolating polynomials up to degree D to approximate the solution.The integral in (14a) is approximated using the Clenshaw-Curtis quadrature scheme with N > D nodes.
Control signal computation: Given the solution to the geodesic problem (14a) parameterized by a set of values {γ(s k )} N k=0 and {γ s (s k )} N k=0 , s k ∈ [0, 1], the control signal can be computed according to (14b) with the integral again approximated by the the Clenshaw-Curtis quadrature scheme.
The control law in (14b) is just one way to construct a control signal achieving the universal L ∞ -gain bound, but it is not the only one and others may be preferable.We now show how to construct a set of robustly stabilizing controls, following [22].
From the formula for first variation of energy [25], we have for the derivative of energy functional at any point x that is not on the cut locus of x : 1 2 From the proof of Theorem 1, one can see that the control law in ( 14) essentially tries to ensure (25).Obviously, the set of control inputs for which (25) holds is non-empty, i.e., we have where the dependence of M , f , B and B w on x has been omitted.The worst-case w for such case is independent of u: . So, for each state x we could construct a set of control inputs: It should be noted that when using the preceding equation to construct the control inputs, one cannot solve OPT REF to compute the L ∞ -gain bound if the output variables, z, depend on some control inputs.

IV. APPLICATION TO FEEDBACK MOTION PLANNING
Thanks to the certificate tubes in (31), the RCCM controller presented in Section III can be conveniently incorporated as a low-level tracking or ancillary controller into a feedback motion planning or nonlinear tube MPC framework.We demonstrate an application to the former in this section.The core idea is to compute nominal motion plans (x , u ) using the nominal dynamics (2) and tightened constraints.Denote the tubes for x − x and u − u obtained through solving OPT REF in Section III-C as Ωx {x ∈ R n : x ≤ α x w} and Ωu {ũ ∈ R m : ũ ≤ α u w}, where α x and α u are the universal L ∞ -gain bounds for the states and control inputs, respectively, and w is bound on the disturbances, i.e., w − w L∞ ≤ w.Then, the tightened constraints are given by where U represents the control constraints, and denotes the Minkowski set difference.One can simply use the tightened constraints in (34) and the nominal dynamics (2) to plan a target trajectory.Then, with the proposed RCCM controller, the actual states and inputs are guaranteed to stay in X and U, respectively, in the presence of disturbances bounded by w.Remark 7. Dependent on the tasks, one may want to focus on some particular states when designing the RCCM controller through solving OPT RCCM .For instance, for motion planning with obstacle-avoidance requirements, one may want to focus on minimizing the tube size for position states.This often leads to tight tubes for position states, enabling planning more aggressive yet safe motions, as demonstrated in Section VI.

V. COMPARISONS WITH AN EXISTING CCM-BASED APPROACH
In [1], for the same system (1) considered here, the authors designed a tracking controller based on CCM without considering disturbances and then derived a tube where the actual states are guaranteed to stay in the presence of disturbances using input-to-state stability (ISS) analysis.In comparison, our method explicitly incorporates disturbance rejection property in designing the RCCM controller and produces tubes for both states and inputs together with the controller (if we include the tube refining process in Section III-C as a part of the controller design process).In this section, under mild assumptions, we will prove that the tube yielded by our method is tighter than that from applying the idea of [1].To be consistent with the problem setting in [1], for this section, we set w ≡ 0 in defining the nominal (i.e., un-disturbed) system (2), which leads to the nominal dynamics: The main technical ideas from [1] (mainly related to Theorem 3.5, Lemma 3.7 and Section 4.2 of [1]) can be summarized as: (1) searching a (dual) CCM metric, Ŵ , for the nominal system (35), which yields a nonlinear controller guaranteeing the incremental stability of the nominal close-loop system; (2) deriving a tube to quantify the actual state in the presence of disturbances, i.e., subject to the dynamics (1), based on ISS analysis.Unlike our approach, in [1] the search of the CCM metric is not jointly done with search of a matrix function (i.e., Y (x) in Theorem 1, used to construct a differential feedback controller).Instead, [1] uses a min-norm type control law computed using only the CCM metric.To facilitate a rigorous comparison, we slightly modify the condition for the CCM metric search to include another matrix function (analogous to Y (x) in Theorem 1).Indeed, a joint search of a CCM metric W (x) and a matrix function Y (x) is adopted in [17], which [1] builds upon.Such modification only influences the control signal determination, and does not change the essential ideas of [1].With such modifications, the main results of [1] can be summarized in the following lemma using the notations of this paper.
We also need the following assumption.
Assumption 2. The metric Ŵ in (36) satisfies both of the following conditions: Condition (C2) is similar to condition (C1), and is also imposed in [1] to simplify the verification of (36) and get a controller with a simple differential feedback form (see [17, III.A]).Condition (C3) states that each b w,i forms a Killing vector for Ŵ , which essentially ensures that the condition (36), evaluated using the perturbed dynamics (i.e., replacing Â in (36) with A below (6)), does not depend on w.Now we are ready to build a connection between the CCM-based approach in [1] and our approach.Lemma 3. Assume there exists a metric Ŵ (x), a matrix function Ŷ (x), and a constant λ > 0 satisfying (36) and Assumption 2.Then, (15) and ( 16) with C = I n and D = 0 (corresponding to g(x, u) = x) can be satisfied with where a sup x∈X σ(B w (x))/ ββ , and α is defined in (38).
According to Lemma 3, if we can find matrices Ŵ and Ŷ and constants λ satisfying the inequality (36), which guarantees the contraction of the nominal closed-loop system and ensures an L ∞ -gain bound α from disturbances to states, we can obtain the same L ∞ -gain bound using our approach (Theorem 1), if we choose W (x) and Y (x) in ( 15) and ( 16) to be scaled versions of Ŵ (x) and Ŷ (x) in (36), i.e., enforcing the constraints in (39).However, if we relax such constraints in the optimization problem OPT RCCM , we are guaranteed to obtain a less conservative bound α, i.e., α ≤ α.This observation is summarized in the following theorem with the straightforward proof omitted.Theorem 2. Assume there exist a metric Ŵ (x), a matrix function Ŷ (x), and a constant λ > 0 satisfying (36) and Assumption 2.Then, we can always find W (x), Y (x), λ > 0, µ > 0 and α ≤ α satisfying (15) and ( 16) with C = I n and D = 0, where α is defined in (38).
Remark 8. Theorem 2 indicates that our proposed RCCM approach is guaranteed to yield a tighter tube for the actual states than the CCM approach in [1], under Assumption 2.

VI. SIMULATION RESULTS
In this section, we apply the proposed approach to a planar VTOL vehicle (illustrated in Fig. 1) and a 3D quadrotor and perform extensive comparisons with the CCM-based approach in [1].All the subsequent computations and simulations were done in Matlab R2021a 1 .A video for visualizing the simulation results is available at youtu.be/mrN5iQo7NxE. 1 Matlab codes are available at github.com/boranzhao/robustccm tube.

A. Planar VTOL vehicle
The state vector is defined as x = [p x , p z , φ, v x , v z , φ] , where p = [p x , p z ] is the position in x and z directions, respectively, v x and v z are the slip velocity (lateral) and the velocity along the thrust axis in the body frame of the vehicle, φ is the angle between the x direction of the body frame and the x direction of the inertia frame.The input vector u = [u 1 , u 2 ] contains the thrust force produced by each of the two propellers.The dynamics of the vehicle are given by where m and J denote the mass and moment of inertia about the out-of-plane axis and l is the distance between each of the propellers and the vehicle center, and w denotes the disturbance in x direction of the inertia frame.Following [1], the parameters were set as m = 0.486 kg, J = 0.00383 Kg m 2 , and l = 0.25 m. 1) Computation of CCM/RCCM and associated tubes: We parameterized both the RCCM W and the CCM Ŵ as polynomial matrices in (φ, v x ) with up to degree 4 monomials.When searching for CCM/RCCM, we also imposed the following bounds: s, which can be concatenated as the vector constraint h(x) ≥ 0. For a fair comparison of the proposed RCCM-based approach and the CCM-based approach in [1], we used same parameters when searching CCM and RCCM whenever possible.For instance, we used the same basis functions for parameterizing W and Ŵ when applying the SOS techniques to solve the optimization problems, and imposed the same lower bound for W and Ŵ : W ≥ 0.01I 6 and Ŵ ≥ 0.01I 6 .
We first considered the optimization of the tube size for all the states, on which [1] is focused.For simplicity, we did not use weights for the states.For RCCM synthesis, we included a penalty for large control efforts when solving OPT RCCM by setting g(x, u) = [x , u ] , and denote the resulting controller as RCCM.Additionally, we designed another RCCM controller with a focus on optimizing the tubes for the position states and inputs, denoted as RCCM-P, by setting g(x, u) = [p x , p z , u ] .We denote the controller designed using the CCM approach in [1] as CCM.
We considered a cross-wind disturbance along the x direction of the inertia frame with effective acceleration up to 1 m/s (i.e., w = 1), which is 10 times as large as the disturbance considered in [1].We swept through a range of values for λ (setting λ = λ) and solved the OPT RCCM in Section III-B to search for the RCCM and the optimization problem in [1, Section 4.2] to search for the CCM, using SOS techniques with YALMIP [26] and Mosek solver [27].After obtaining the RCCM, we further solved OPT REF in Section III-C by gridding the state space to get refined tubes for different variables.The results are shown in Fig. 2. According to the top plot, while both controllers focused on optimizing the tube  size for all states without using weights, RCCM yielded a much smaller tube than CCM.RCCM-P yielded a tube of similar size for all states compared to CCM, which came as no surprise since RCCM-P focused on minimizing the tube size for position states only, i.e., (p x , p z ).From the middle and bottom plots, one can see that RCCM-P yielded much smaller tubes for both position states and inputs than RCCM, which further outperforms CCM by a large margin.For subsequent tests and simulations, we selected a best λ value for each of the three controllers in terms of tube size for (p x , p z ), since the vehicle position is of more importance in tasks with collisionavoidance requirements.The best values for CCM, RCCM, RCCM-P are determined to be 0.8, 1.4 and 1.2, respectively.Figure 3 depicts the input tube, and the projection of the state tube onto different planes, yielded by each of the three controllers with the best λ value.It is no surprise that RCCM-P, while yielding much smaller tubes for (p x , p z ) and inputs, results in relatively larger tubes for (v x , v z ) and (φ, φ).

2) Trajectory tracking and verification of tubes:
To test the trajectory tracking performance of the three controllers in scenarios and evaluate the conservatism with the derived tubes, we considered a task of navigation from the origin to target point (10,10).We first planed a nominal trajectory with the objective of minimal force and minimal travel time, using OptimTraj [28], where the state constraint h(x) ≥ 0, used in searching for CCM/RCCM, was enforced.With the nominal state and input trajectories, we simulated the performance of controllers in the presence of a wind disturbance, artificially simulated by w(t) = 0.8 + 0.2 sin(2πt/10).OPTI [29] and Matlab fmincon solvers were used to solve the geodesic optimization problem at each sampling instant for all the three controllers (see Section III-D for details).With Matlab 2021a running on a PC with Intel i7-4790 CPU and 16 GB RAM and generated C codes for evaluating the cost function and gradient, it took roughly 20 ∼ 30 milliseconds to solve the optimization problem for computing the geodesic once.
The results of the position trajectories along with the tubes projected to the (p x , p z ) plane are shown in Fig. 4. First, it is clear that the actual trajectory under each controller always stays in the associated tube.Second, in terms of tracking performance, RCCM-P and CCM perform the best and worst, respectively.
3) Feedback motion planning and tracking in the presence of obstacles: We now consider a joint trajectory planning and tracking problem for the same task considered in Section VI-A2 but in the presence of obstacles, illustrated as black circles in Fig. 5.We followed the feedback motion planning framework and incorporated the tubes for both position states and inputs when planning the trajectory.For simplicity, we ignored the tubes for other states (i.e., v x , v z , φ, φ) in the planning.The planned trajectory and tube associated with each controller are denoted by a black dotted line and a shaded area in Fig. 5.As expected, the trajectory optimizer found different trajectories for the three controllers due to different tube sizes.The travel time associated with the planned trajectories under CCM, RCCM, and RCCM-P are 18.0, 11.8 and 10.1 seconds, respectively, with RCCM-P yielding the shortest travel time.The actual trajectories in the presence of the disturbances are also included in Fig. 5.It is clear that the actual trajectory under each of three controllers always stays in the tube around its associated nominal trajectory and remains collision-free.Once again, RCCM-P yielded the smallest tracking error.

B. 3D quadrotor
The 3D quadrotor model is taken from [1] and has the state-space representation given by x = [p x , p y , p z , ṗx , ṗy , ṗz , τ, φ, θ, ψ] , where the position p = [p x , p y , p z ] ∈ 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 τ > 0 is the total (normalized by mass) thrust generated by the four rotors.For controller design, we consider u [ τ, φ, θ] as the control input.The actual implementation embeds the τ term within an integrator, and the resulting thrust and angular velocity reference (after being converted to body rate reference) are passed to a lower-level controller that is assumed to operate at a much faster time-scale.Given this parameterization, the dynamics of the quadrotor may be written as where g is the local gravitational acceleration, e 3 = [0, 0, 1] , bz is the body-frame z-axis, and w = [w 1 , w 2 , w 3 ] ∈ R 3 denotes the disturbance.The dynamics of (τ, φ, θ, ψ) reduce trivially to first-order integrators.We impose the following bounds: and τ ∈ [−5, 5]g/s, which are sufficient for fairly aggressive maneuvers.Since yaw control is not a focus here, we simply set ψ = 0.
1) Computation of CCM/RCCM and associated tubes: We parameterize both the RCCM W and the CCM Ŵ as polynomial matrices in (τ, φ, θ) with up to degree 3 monomials.Additionally, the top left 6 × 6 block of Ŵ was imposed to Fig. 6: Tube size gain for all states (top) and position states (bottom) versus λ ( λ) value be constant, i.e., independent of (τ, φ, θ) to ensure that the resulting synthesis condition did not depend on u (see [1] for details).The RCCM synthesis condition (15), however, depends on ( τ, φ, θ), which is why we impose the bounds on ( τ, φ, θ) mentioned above.Similar to Section VI-A, we first consider the optimization of the tube size for all the states.For simplicity, we do not use weights for the states, which can also be considered as using equal weights for all the states.For RCCM synthesis, we include a penalty for large control efforts required when solving OPT RCCM by setting g(x, u) = [x , 0.02 τ, 0.05 φ, 0.05 θ] , and denote the resulting controller as RCCM.Additionally, we design another RCCM controller with a focus on optimizing the tubes for the position states.For this, we set g(x, u) = [p x , p y , p z , 0.02 τ, 0.05 φ, 0.05 θ] and denote the resulting controller as RCCM-P.We denote the controller designed using the CCM-based approach in [1] as CCM.Through numerical experimentation, we found that imposing the lower bound constraint W ≥ 0.01I 9 yielded good performance for searching W , while imposing the constraint Ŵ ≥ I 9 yielded good performance when searching Ŵ .We swept through a range of values from 0.4 to 3.6 for λ (setting λ = λ) and solved the OPT RCCM in Section III-B to search for the RCCM and the optimization problem in [1, Section 4.2] to search for the CCM.We first tried the SOS technique used in Section VI-A to solve the involved optimization problems, but found it was not reliable especially for RCCM synthesis, taking notoriously long time while still yielding unsatisfactory results.Therefore, we eventually chose to grid the set of (τ, φ, θ) (and additionally ( τ, φ, θ) for RCCM search), and solved the resulting optimization problem with a finite number of LMIs with YALMIP [26] and Mosek solver [27].After obtaining the RCCM, we further solved OPT REF in Section III-C using the gridding technique to get refined tubes for different variables.The results in terms of tube size gain are shown in Fig. 6.As shown in the top plot, while both controllers focused on optimizing the tube size for all states without using weights, RCCM yielded a much smaller tube than CCM.RCCM-P yielded a tube of similar size for all states compared to CCM, which came as no surprise since RCCM-P focused on minimizing the tube size for position states only.From the bottom plots, one can see that RCCM-P yielded much smaller tubes for both position states and inputs than RCCM, which further outperforms CCM when λ is less than 2. Note that Assumption 2 does not hold anymore for this example, as the condition (C2) cannot be satisfied.Therefore, Theorem 2 does not hold, which indicates that we cannot guarantee that RCCM will yield tighter tubes for all the states than CCM.2) Feedback motion planning and tracking in cluttered environments: To verify the controller performance, we randomly initialized the obstacle environments for the quadrotor, depicted in Fig. 9.The task for the quadrotor is to navigate from the start point [0, 0, 0] to the goal region, depicted by the light green box, while avoding collisions.We considered a wind disturbance of up to 1 m/s 2 , i.e., w ≤ w 1, under which RCCM-P yielded a tube of a 0.32 m radius ball (i.e., α p w = 0.32) for position coordinates at λ = 3.4.For comparison, we chose the CCM controller obtained at λ = 3.4, which yielded the smallest tube size gain of 0.58 (and thus a tube of a 0.58 m radius ball) for the positions state among all CCM controllers.Trajectory planning was performed by first computing a waypoint path using geometric FMT * [30], and then smoothing this path using polynomial splines with the min-snap algorithm in [31], with the Matlab codes in [32].Finally, differential flatness was leveraged to recover the open-loop (i.e., state and control trajectories.Collision checking was performed by leveraging the configuration space representation of the obstacles, i.e., polytopes, inflated by the projection of the tube bound onto position coordinates, i.e., α p w .For simulation, CCM and RCCM-P were implemented using at 250 Hz.The Euler angular rates ( φ, θ) computed by the CCM/RCCM-P, and ψ (which was set to constant zero) were converted to desired body rates, which were then sent to a low-level proportional controller.The P controller computed the three moments to track the desired body rates.The moments and the total thrust (from integrating τ ) were then applied as ultimate inputs to the quadrotor, which of 12 states.With CCM and RCCM-P, the planned and actual trajectories together with the projected tubes for position coordinates under a wind disturbance, simulated by w(t) = (0.8 + 0.2 sin(0.2πt))[sin(45 • ), − cos(45 • ), 0], are depicted in Fig. 9.One can see that the actual position trajectories were fairly close to the nominal ones and consistently  VII.CONCLUSION For nonlinear control-affine systems subject to bounded disturbances, this paper presents robust contraction metrics (RCCM) for designing trajectory tracking controllers with explicit disturbance rejection properties and certificate tubes for both states and inputs.The tubes are valid for any feasible nominal trajectories, and are guaranteed to contain the actual trajectories despite disturbances.Both the RCCM controller and the tubes can be computed, offline, by solving convex optimization problems and conveniently incorporated into a feedback motion planning framework.Simulation results for a planar VTOL vehicle and a 3D quadrotor verify the effectiveness the proposed approach.
Future work includes testing of the proposed method on real hardware and leveraging the proposed method to deal with unmatched uncertainties within an adaptive control framework [20].

1 0
c s M (c(s))c s (s)ds.We also use the notation E(a, b) to denote the minimal energy of a path joining a and b, i.e., E(a, b) inf c∈Γ(a,b) E(c).

Fig. 2 :
Fig. 2: Tube size for all states (top), position states (middle) and inputs (bottom) versus λ ( λ) value in the presence of disturbances bounded by w = 1

Fig. 3 :
Fig. 3: Projection of state and input tubes under wind disturbances with effective acceleration up to 1 m/s 2

Fig. 4 :
Fig. 4: Tracking of a nominal trajectory by different controllers: full (top) and zoomed-in (bottom) view

WindFig. 5 :
Fig. 5: Planning and tracking of a nominal trajectory by different controllers incorporating safety tubes: full (top) and zoomed-in (bottom) view.Dotted lines denote planned trajectories.Shaded areas denote the tubes for the position states.

Fig. 7 :
Fig. 7: Tracking error for the position states under RCCM-P.The green dashed line denotes the theoretical bound associated with w = 1.

Fig. 8 :
Fig. 8: Nominal and actual rotational angles under RCCM-P.The green dashed lines denote the theoretical bounds associated with w = 1.

Fig. 9 :
Fig. 9: Planned nominal and actual trajectories in an obstacle-rich environment under the RCCM-P and CCM controllers.Actual trajectories consistently stay in the (light blue shaded) ellipsoidal tubes around the nominal trajectories.

Fig. 10 :EnergyFig. 11 :
Fig. 10: Nominal and actual derivatives of the total thrust (one of the control inputs) under RCCM-P