Robust Nonlinear Tracking Control with Exponential Convergence Using Contraction Metrics and Disturbance Estimation

Abstract

This paper presents a tracking controller for nonlinear systems with matched uncertainties based on contraction metrics and disturbance estimation that provides exponential convergence guarantees. Within the proposed approach, a disturbance estimator is proposed to estimate the pointwise value of the uncertainties, with a pre-computable estimation error bounds (EEB). The estimated disturbance and the EEB are then incorporated in a robust Riemannian energy condition to compute the control law that guarantees exponential convergence of actual state trajectories to desired ones. Simulation results on aircraft and planar quadrotor systems demonstrate the efficacy of the proposed controller, which yields better tracking performance than existing controllers for both systems.

1. Introduction

Robotic systems generally have nonlinear dynamics and are subject to model uncertainties and disturbances. Moreover, many robotic systems are underactuated, i.e., having fewer independent control inputs than degrees of freedom, including fixed-wing aircraft, quadrotors and dynamic walking robots. The design of tracking controllers for underactuated robotic systems is a much more challenging problem compared to that for fully-actuated systems. Recently, the concept of a control contraction metric (CCM) was introduced in [1] to synthesize trajectory tracking controllers for general nonlinear systems, including underactuated ones. The CCM extends contraction theory [2] from analysis to constructive control design, while contraction theory is focused on analyzing nonlinear systems in a differential framework by studying the convergence between pairs of state trajectories toward each other. It was shown in [3] that CCM reduces to conventional sliding and energy-based designs for fully-actuated systems. On the other hand, for underactuated systems, compared to prior approaches based on local linearization [4], the CCM approach leads to a convex optimization problem for controller synthesis and generates controllers that stabilize every feasible trajectory in a region, instead of just a single target trajectory that must be known a priori [3].

On the other hand, control design methods to deal with dynamic uncertainties in the deterministic setting can be roughly classified into adaptive and robust approaches. Robust approaches, such as $H_{\infty }$ control [5], $\mu$ synthesis [6] and robust/tube model predictive control (MPC) [7,8], usually consider parametric uncertainties or bounded disturbances and aim to find controllers with performance guarantees for the worst case of such uncertainties. The consideration of worst-case scenarios associated with robust approaches often leads to conservative nominal performance. Disturbance–observer (DOB) based control and related methods such as active disturbance rejection control (ADRC) [9] lump all uncertainties that may include parametric uncertainties, unmodeled dynamics and external disturbances, together as a “total disturbance”, estimate it via an observer and then compute control actions to compensate for the estimated disturbance [10] to recover the nominal performance. However, for state-dependent uncertainties, DOB-based control methods usually ignore the dependence of “disturbance” on system states and rely on assumptions on the derivative of the “disturbance” that are difficult to verify for theoretical guarantees [10,11]. Alternatively, adaptive control methods such as model reference adaptive control (MRAC) [12] usually need a parametric structure for the uncertainties, rely on online estimation of the parameters for control law construction and provide asymptotic performance guarantees in most cases. One of the exceptions is ${\mathcal{L}}_1$ adaptive control [13] that does not need a parameterization of the uncertainties (similar to DOB-based control) and focuses on transient performance guarantees in terms of uniformly bounded error between the ideal and uncertain systems.

Both robust and adaptive control approaches have been explored in the context of CCM-based control in the presence of uncertainties and disturbances. In particular, adaptive control was combined with CCM to handle nonlinear control-affine systems with both parametric [14] and non-parametric uncertainties [15]. The case of bounded disturbances in CCM-based control was addressed by leveraging input-to-state stability analysis [16] or robust CCM [17,18]. CCM for stochastic systems was developed in [19] to minimize the mean squared tracking error in the presence of stochastic disturbances. Closely relevant to this paper, [15] designed an ${\mathcal{L}}_1$ adaptive controller to augment a baseline CCM-based controller to compensate for matched nonlinear non-parametric uncertainties that can depend on both time and states. The authors of [15] proved that transient tracking performance was guaranteed in the sense that the actual state trajectory exponentially converges to a neighborhood or a tube around the desired one. Compared to [15], our approach relies on a disturbance observer that yields an estimation error bound and robust Riemannian energy condition and ensures that the actual state trajectory exponentially converges to the nominal one.

Statement of Contributions: We present a tracking controller for nonlinear systems subject to matched uncertainties that can depend on both time and states based on contraction metrics and disturbance estimation. Our controller leverages a disturbance estimator to estimate the pointwise value of the uncertainties, with a pre-computable estimation error bound. The estimated disturbance and the estimation error bound are then incorporated into a robust Riemannian energy condition to compute the control law that guarantees exponential convergence of actual state trajectories to nominal ones. We validate the efficacy of our controller on two simulation examples and demonstrate its advantages over existing controllers.

The idea presented in this paper is leveraged in [20] for safe learning of uncertain dynamics using deep neural networks. Compared to [20], this paper is not relevant to learning and allows the uncertainty to be dependent on both time and states, as opposed to the dependence on states only in [20]. Additionally, this paper includes an additional aircraft example for performance illustration and conducts extensive comparisons with existing adaptive approaches in simulations that are not available in [20].

Notations: Let ${\mathbb{R}}^n$, ${\mathbb{R}}^{+}$ and ${\mathbb{R}}^{m\times 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 and 0 denote an identity matrix, and  a zero matrix of compatible dimensions, respectively; ‖·‖ denotes the 2-norm of a vector or a matrix. For a vector y, $y_i$ denotes its ith element. For a matrix-valued function $M:{\mathbb{R}}^n\to {\mathbb{R}}^{n\times n}$ and a vector $y\in {\mathbb{R}}^n$, ${\partial }_{y}M\left(x\right)\triangleq {\sum }_{i=1}^n\frac{\partial M\left(x\right)}{\partial x_i}{y}_i$ denotes the directional derivative of $M\left(x\right)$ along y. For symmetric matrices P and Q, $P>Q$ ($P\ge Q$) means $P-Q$ is positive definite (semidefinite). $\langle X\rangle$ is the shorthand notation of $X+X^{\top }$. Finally, ⊖ denotes the Minkowski set difference.

2. Problem Statement and Preliminaries

Consider a nonlinear control-affine system with uncertainties

$$\dot{x}\left(t\right)=f\left(x\left(t\right)\right)+B\left(x\left(t\right)\right)(u\left(t\right)+d(t,x\left(t\right))),$$

(1)

where $x\left(t\right)\in \mathcal{X}\subset {\mathbb{R}}^n$ is the state vector, $u\left(t\right)\in \mathcal{U}\subset {\mathbb{R}}^m$ is the control input vector, $f:{\mathbb{R}}^n\to {\mathbb{R}}^n$ and $B:{\mathbb{R}}^n\to {\mathbb{R}}^m$ are known and locally Lipschitz continuous functions, and $d(t,x)$ represents the matched model uncertainty that can depend on both time and states. We assume that $B\left(x\right)$ has full column rank for any $x\in \mathcal{X}$. Suppose $\mathcal{X}$ is a compact set that contains the origin, and  the control constraint set $\mathcal{U}$ is defined as $\mathcal{U}\triangleq \{u\in {\mathbb{R}}^m:\underline{u}\le u\le \overline{u}\}$, where $\underline{u},\overline{u}\in {\mathbb{R}}^m$ denote the lower and upper bounds of all control channels, respectively. Furthermore, we make the following assumptions on $B\left(x\right)$ and $d(t,x)$.

Assumption 1.

There exist known positive constants $L_B$, $L_d$, $l_d$ and $b_d$ such that for any $x,y\in \mathcal{X}$ and $t,\tau \ge 0$, the following inequalities hold:

$$\begin{array}{cc}\hfill ∥B\left(x\right)-B\left(y\right)∥& \le L_B∥x-y∥,\hfill \end{array}$$

(2)

$$\begin{array}{cc}\hfill ∥d(t,x)-d(\tau ,y)∥& \le L_d∥x-y∥+l_d\left|t-\tau \right|,\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill ∥d(t,x)∥& \le b_d.\hfill \end{array}$$

(4)

Remark 1.

Assumption 1 indicates that the uncertain function $d(t,x)$ is locally Lipschitz in both t and x with known Lipschitz constants and is uniformly bounded by a known constant in the compact set $\mathcal{X}$.

In fact, given the local Lipschitz constants $L_d$ and $l_d$, a uniform bound on $d(t,x)$ in $\mathcal{X}$ can always be derived by using Lipschitz continuity properties if the bound on $d(t,x^{*})$ for an arbitrary $x^{*}$ in $\mathcal{X}$ and any $t\ge 0$ is known. For instance, assuming $\Vert d(t,0)\Vert \le b_d^0$, from (3), we have $\Vert d(t,x)\Vert \le b_d^0+L_d{max}_{x\in \mathcal{X}}\Vert x\Vert$ for any $x\in \mathcal{X}$ and $g\ge 0$. In practice, some prior knowledge about the actual system and the uncertainty may be leveraged to obtain a tighter bound than the one based on the Lipschitz continuity explained earlier, which is why we directly make an assumption on the uniform bound. With Assumption 1, we will show (in Section 3.3) that the pointwise value of $d(t,x(t\left)\right)$ at any time t can be estimated with a pre-computable estimation error bound.

For the system in (1), assume we have a nominal state and input trajectory, $x^{\star }(·)$ and $u^{\star }(·)$, which satisfy the nominal, i.e., uncertainty-free, dynamics:

$${\dot{x}}^{\star }=f\left(x^{\star }\right)+B\left(x^{\star }\right)u^{\star }.$$

(5)

We would like to design a state-feedback controller in the form of

$$u\left(t\right)=k(t,x\left(t\right),x^{\star }\left(t\right))+u^{\star }\left(t\right),$$

(6)

so that the actual state trajectory $x(·)$ exponentially converges to the nominal one $x^{\star }(·)$. Our solution is based on CCM and disturbance estimation. Next, we briefly review CCM for uncertainty-free systems.

Control Contraction Metrics (CCMs)

We first introduce some notations related to Riemannian geometry, most of which are from [1]. A Riemannian metric on ${\mathbb{R}}^n$ is a symmetric positive-definite matrix function $M\left(x\right)$, smooth in x, which defines a “local Euclidean” structure for any two tangent vectors ${\delta }_1$ and ${\delta }_2$ through the inner product ${\langle {\delta }_1,{\delta }_2\rangle }_x\triangleq {\delta }_1^{\top }M\left(x\right){\delta }_2$ and the norm $\sqrt{{\langle {\delta }_1,{\delta }_2\rangle }_x}$. A metric is called uniformly bounded if $a_{1}I\le M\left(x\right)\le a_{2}I$ holds $\forall x$ and for some scalars $a_2\ge a_1>0$. Let $\mathsf{\Gamma }(a,b)$ be the set of smooth paths connecting two points a and b in ${\mathbb{R}}^n$, where each $c\in \mathsf{\Gamma }(a,b)$ is a piecewise smooth mapping, $c:[0,1]\to {\mathbb{R}}^n$, satisfying $c\left(0\right)=a,c\left(1\right)=b$. We use the notation $c\left(s\right),s\in [0,1]$, and $c_s\left(s\right)\triangleq \frac{\partial c}{\partial s}$. Given a metric $M\left(x\right)$ and a curve $c\left(s\right)$, we define the Riemannian energy of $c\left(s\right)$ as $E\left(c\right)\triangleq {\int }_0^1{c}_s^{\top }M\left(c\left(s\right)\right)c_s\left(s\right)ds$. The Riemannian energy between a and b is defined as $E(a,b)\triangleq {inf}_{c\in \mathsf{\Gamma }(a,b)}E\left(c\right)$.

Contraction theory [2] draws conclusions on the convergence between pairs of state trajectories toward each other by studying the evolution of the distance between any two infinitesimally close neighbouring trajectories. CCM generalizes contraction analysis to the controlled dynamics setting in which the analysis jointly searches for a controller and a metric that describes the contraction properties of the resulting closed-loop system. Following [1,14], we now briefly review CCMs by considering the nominal, i.e., uncertainty-free, system:

$$\dot{x}=f\left(x\right)+B\left(x\right)u,$$

(7)

where $x\left(t\right)\in {\mathbb{R}}^n$ and $u\left(t\right)\in {\mathbb{R}}^m$. The differential form of (7) is given by ${\dot{\delta }}_x=A(x,u){\delta }_x+B\left(x\right){\delta }_u,$ where $A(x,u)\triangleq \frac{\partial f}{\partial x}+{\displaystyle \sum _{i=1}^m}\frac{\partial b_i}{\partial x}{u}_i$ with $b_i\left(x\right)$ denoting the ith column of $B\left(x\right)$. Consider a function $V(x,{\delta }_x)={\delta }_x^{\top }M\left(x\right){\delta }_x$ for some positive definite metric $M\left(x\right)$, which can be viewed as the Riemannian squared differential length at point x. Differentiating and imposing that the squared length decreases exponentially with rate $2\lambda$, one obtains

$$\dot{V}(x,{\delta }_x)={\delta }_x^{\top }(〈MA〉+\dot{M}){\delta }_x+2{\delta }_x^{\top }MB{\delta }_u\le -2\lambda {\delta }_x^{\top }M{\delta }_x,$$

(8)

where $\dot{M}={\partial }_{f+Bu}M={\partial }_{f}M+{\sum }_{i=1}^m{\partial }_{b_i}M{u}_i$. We first recall some basic results related to CCM.

Definition 1

([1]). The system (7) is said to be universally exponentially stabilizable if, for any feasible desired trajectory $x^{\star }\left(t\right)$ and $u^{\star }\left(t\right)$, a feedback controller can be constructed that for any initial condition $x\left(0\right)$, a unique solution to (7) exists and satisfies $\Vert x\left(t\right)-x^{\star }\left(t\right)\Vert \le R\Vert x\left(0\right)-x^{\star }\left(0\right)\Vert e^{-\lambda t},$ where λ and R are the convergence rate and overshoot, respectively, independent of the initial conditions.

Lemma 1

([1]). If there exists a uniformly bounded metric M(x), i.e., ${\alpha }_{1}I\le M\left(x\right)\le {\alpha }_{2}I$ for some positive constants ${\alpha }_1$ and ${\alpha }_2$, such that for all x and ${\delta }_x\ne 0$ satisfying ${\delta }_x^{\top }MB=0$,

$$\begin{array}{cc}\hfill & {\delta }_x^{\top }\left(〈M\frac{\partial f}{\partial x}〉+{\partial }_{f}M+2\lambda M\right){\delta }_x\le 0,\hfill \end{array}$$

(9a)

$$\begin{array}{cc}\hfill & {\delta }_x^{\top }\left(〈M\frac{\partial b_i}{\partial x}〉+{\partial }_{b_i}M\right){\delta }_x=0,i=1,\cdots ,m\hfill \end{array}$$

(9b)

hold, then the system (7) is universally exponentially stabilizable in the sense of Definition 1 via continuous feedback defined almost everywhere, and everywhere in the neighborhood of the target trajectory with the convergence rate λ and overshoot $R=\sqrt{\frac{{\alpha }_2}{{\alpha }_1}}$.

The condition (9) ensures that the dynamics orthogonal to the input are contracting, i.e., (8) holds in the presence of ${\delta }_x^{\top }MB=0$ and is often termed as the strong CCM condition [1]. In particular, the condition (9b) can be satisfied by enforcing that each column of $B\left(x\right)$ forms a killing vector field for the metric $M\left(x\right)$, i.e., $〈M\frac{\partial b_i}{\partial x}〉+{\partial }_{b_i}M=0$ for all $i=1,\cdots ,m$. The CCM condition (9) can be transformed into a convex constructive condition for the metric $M\left(x\right)$ by a change of variables. Let $W\left(x\right)=M^{-1}\left(x\right)$ (commonly referred to as the dual metric), and $B_{\perp }\left(x\right)$ be a matrix whose columns span the null space of the input matrix B (i.e., $B_{\perp }^{\top }B=0$). Then, condition (9) can be cast as convex constructive conditions for $W\left(x\right)$:

$$\begin{array}{cc}\hfill & B_{\perp }^{\top }\left(〈\frac{\partial f}{\partial x}W〉-{\partial }_{f}W+2\lambda W\right)B_{\perp }\le 0\hfill \end{array}$$

(10a)

$$\begin{array}{cc}\hfill & 〈\frac{\partial b_i}{\partial x}W〉-{\partial }_{b_i}W=0,\mathrm{for}i=1,\cdots ,m.\hfill \end{array}$$

(10b)

The existence of a contraction metric $M\left(x\right)$ is sufficient for stabilizability via Lemma 1. What remains is constructing a feedback controller that achieves the universal exponential stabilizability (UES). As mentioned in [1,16], one way to derive the controller is to interprete the Riemann energy, $E(x^{\star }\left(t\right),x\left(t\right))$, as an incremental control Lyapunov function and use it to construct a min-norm controller that renders for any time t

$$\dot{E}(x^{\star }\left(t\right),x\left(t\right))\le -2\lambda E(x^{\star }\left(t\right),x\left(t\right)).$$

(11)

Specifically, at any time $t>0$, given the metric $M\left(x\right)$ and a desired/actual state pair $(x^{\star }\left(t\right),x\left(t\right))$, a minimum-energy path, i.e., a geodesic, $\gamma (·,t)$ connecting these two states (i.e., $\gamma (0,t)=x^{\star }\left(t\right)$ and $\gamma (1,t)=x\left(t\right)$), can be computed (e.g., using the pseudospectral method in [21] to solve a nonlinear programming problem). Consequently, the Riemannian energy of the geodesic is defined as $E(x^{\star }\left(t\right),x\left(t\right))={\int }_0^1{\gamma }_s{(s,t)}^{\top }M(\gamma (s,t))){\gamma }_s(s,t)ds$, where ${\gamma }_s\left(s\right)\triangleq \frac{\partial \gamma }{\partial s}$, can be calculated. As noted in [16], from the formula for the first variation of energy [22], $\dot{E}(x^{\star }\left(t\right),x\left(t\right))=2{\gamma }_s^{\top }(1,t)M(x\left(t\right))\dot{x}\left(t\right)-2{\gamma }_s^{\top }(0,t)M(x^{\star }\left(t\right)){\dot{x}}^{\star }\left(t\right)$. Therefore, (11) can be rewritten as

$${\gamma }_s^{\top }(1,t)M\left(x\left(t\right)\right)\dot{x}\left(t\right)-{\gamma }_s^{\top }(0,t)M(x^{\star }\left(t\right)){\dot{x}}^{\star }\left(t\right)\le -\lambda E(x^{\star }\left(t\right),x\left(t\right)),$$

(12)

where $\dot{x}\left(t\right)=f\left(x\left(t\right)\right)+B\left(x\left(t\right)\right)u\left(t\right)$ and ${\dot{x}}^{\star }\left(t\right)=f(x^{\star }\left(t\right))+B(x^{\star }\left(t\right))u^{\star }\left(t\right)$. Therefore, the control signal with a minimum norm for $u\left(t\right)-u^{\star }\left(t\right)$ can then be obtained by solving the following quadratic programming (QP) problem:

$$u\left(t\right)=\underset{k\in {\mathbb{R}}^m}{argmin}\Vert {k-u^{\star }\left(t\right)\Vert }^2\mathrm{subject}\mathrm{to}(12)$$

(13)

at each time t, which is guaranteed to be feasible under condition (9) [1]. The minimization problem (13) is often termed as the pointwise min-norm control problem and has an analytic solution [23]. The above discussions can be summarized in the following theorem. The proof is trivial by following Lemma 1 and the subsequent discussions and is thus omitted.

Theorem 1

([1]). Given a nominal system (7), assume that there exists a uniformly bounded metric $W\left(x\right)$ that satisfies (10) for all $x\in {\mathbb{R}}^n$. Then, the control law constructed by solving (13) with $M\left(x\right)=W^{-1}\left(x\right)$, universally exponentially stabilizes the system (7) in the sense of Definition 1, where $R=\sqrt{\frac{{\alpha }_2}{{\alpha }_1}}$ with ${\alpha }_1$ and ${\alpha }_2$ being two positive constants satisfying ${\alpha }_{1}I\le M\left(x\right)\le {\alpha }_{2}I$.

Remark 2.

According to Definition 1 and Theorem 1, under the conditions of Theorem 1, given any feasible trajectory ($x^{\star }((·),u^{\star }(·)$) of (7), a controller can always be constructed to ensure that the actual state trajectory $x(·)$ exponentially converges to $x^{\star }(·)$.

3. Robust Trajectory Tracking Using CCM and Disturbance Estimation

In Section 2, we have shown that existence of a CCM for a nominal (i.e., uncertainty-free) system can be used to construct a feedback control law to guarantee the universal exponential stabilizability (UES) of the system. In this section, we present a controller based on CCM and disturbance estimation to ensure the UES of the uncertain system (1), whose architecture is depicted in Figure 1.

3.1. CCMs for the Actual System

To apply the contraction method to design a controller to guarantee the UES of the uncertain system (1), we need to first search a valid CCM for it. Following Section 2, we can derive the counterparts of the strong CCM condition (9) or (10). Due to the particular structure with (1) attributed to the matched uncertainty assumption, we have the following lemma. A similar observation has been made in [14] for the case of matched parametric uncertainties. The proof is straightforward and thus omitted. One can refer to [14] for more details.

Lemma 2.

The strong (dual) CCM condition for the uncertain system (1) is the same as the strong (dual) CCM condition, i.e., (9) and (10), for the nominal system.

Remark 3.

As a result of Lemma 2, a metric $M\left(x\right)$ (dual metric $W\left(x\right)$) satisfying the condition (9) and (10) for the nominal system (7) is always a CCM (dual CCM) for the true system (1).

Define $\mathcal{D}=\{y\in {\mathbb{R}}^m:\Vert y\Vert \le b_d\}$, where $b_d$ is introduced in Assumption 1. Assumption 1 indicates $d(t,x)\in \mathcal{D}$ for any $t\ge 0$ and $x\in \mathcal{X}$. As mentioned in Section 2, given a CCM and a desired trajectory $x^{\star }\left(t\right)$ and $u^{\star }\left(t\right)$ for a nominal system, a control law can be constructed to ensure exponential convergence of the actual state trajectory $x\left(t\right)$ to the desired state trajectory $x^{\star }\left(t\right)$. In practice, we have access to only the nominal dynamics (5) instead of the true dynamics to plan a trajectory $x^{\star }\left(t\right)$ and $u^{\star }\left(t\right)$. The following lemma gives the condition when $x^{\star }\left(t\right)$, planned using the nominal dynamics (5), is also a feasible state trajectory for the true system.

Lemma 3.

Given a desired trajectory $x^{\star }\left(t\right)$ and $u^{\star }\left(t\right)$ satisfying the nominal dynamics (5) with $x^{\star }\left(t\right)\in \mathcal{X}$, if 

$$u^{\star }\left(t\right)\in \mathcal{U}\ominus \mathcal{D},\forall t\ge 0,$$

(14)

then $x^{\star }\left(t\right)$ is also a feasible state trajectory for the true system (1).

Proof.

Define ${\overline{u}}^{\star }\left(t\right)\triangleq u^{\star }\left(t\right)-d(t,x^{\star }\left(t\right))$. Since $u^{\star }\left(t\right)\in \mathcal{U}\ominus \mathcal{D}$ and $-d(t,x^{\star }\left(t\right))\in \mathcal{D}$, which is due to $x^{\star }\left(t\right)\in \mathcal{X}$ and Assumption 1, we have ${\overline{u}}^{\star }\left(t\right)\in \mathcal{U}$. By comparing the dynamics in (1) and (5), we conclude that $x^{\star }\left(t\right)$ and ${\overline{u}}^{\star }\left(t\right)$ satisfy the true dynamics (1) and thus are a feasible state and input trajectory for the true system. □

Lemma 3 provides a way to verify whether a trajectory planned using the nominal dynamics is a feasible trajectory for the true system in the presence of actuator limits. In the absence of such limits, any feasible trajectory for the learned dynamics is also a feasible trajectory for the true dynamics due to the particular structure of (1) associated with the matched uncertainty assumption.

3.2. Robust Riemannian Energy Condition

Section 2 shows that, given a nominal system and a CCM for such a system, a control law can be constructed via solving a QP problem (13) with a condition to constrain the decreasing rate of the Riemannian energy, i.e., condition (12). When considering the uncertain dynamics in (1), the condition (12) becomes

$${\gamma }_s^{\top }(1,t)M(x\left(t\right))\dot{x}\left(t\right)-{\gamma }_s^{\top }(0,t)M(x^{\star }\left(t\right)){\dot{x}}^{\star }\left(t\right)\le -\lambda E(x^{\star }\left(t\right),x\left(t\right)),$$

(15)

where $\dot{x}\left(t\right)=f(x\left(t\right))+B(x\left(t\right))(u\left(t\right)+d\left(x\left(t\right)\right))$ represents the true dynamics evaluated at $x\left(t\right)$, and ${\dot{x}}^{\star }\left(t\right)=f(x^{\star })+B(x^{\star })u^{\star }$ as defined in (5). Several observations follow immediately. First, it is clear that (15) is not implementable due to its dependence on the true uncertainty $d\left(x\right(t\left)\right)$ through $\dot{x}\left(t\right)$. Second, if we could have access to the pointwise value of $d\left(x\right(t\left)\right)$ at each time t, (15) will become implementable even when we do not know the exact functional representation of $d\left(x\right)$. Third, if we could estimate the pointwise value of $d\left(x\right(t\left)\right)$ at each time t with a bound to quantify the estimation error, then we could derive a robust condition for (15). Specifically, assume $d\left(x\right(t\left)\right)$ is estimated as $\widehat{d}\left(t\right)$ at each time t with a uniform estimation error bound (EEB) $\delta$, i.e., $\Vert \widehat{d}\left(t\right)-d\left(x\left(t\right)\right)\Vert \le \delta ,\forall t\ge 0.$ Then, we could immediately get the following sufficient condition for (15):

$$\begin{array}{cc}\hfill {\gamma }_s^{\top }(1,t)& M(x)\dot{\check{x}}\left(t\right)-{\gamma }_s^{\top }(0,t)M(x^{\star }){\dot{x}}^{\star }+\Vert {\gamma }_s^{\top }(1,t)M(x)B\left(x\right)\Vert \delta \le -\lambda E(x^{\star },x),\hfill \end{array}$$

(16)

where

$$\dot{\check{x}}\left(t\right)\triangleq f\left(x\right)+B(x)(u\left(t\right)+\widehat{d}\left(t\right)).$$

(17)

Moreover, since $M\left(x\right)$ satisfies the CCM condition (9), $u\left(t\right)$ that satisfies (16) is guaranteed to exist for any $t\ge 0$, regardless of the size of $\delta$, if the input constraint set $\mathcal{U}$ is sufficiently large. We term condition (16) the robust Riemannian energy (RRE) condition.

3.3. Disturbance Estimation with a Computable EEB

We now introduce a disturbance estimation scheme to estimate the pointwise value of the uncertainty $d\left(x\right)$ with a pre-computable EEB, which can be systematically improved by tuning a parameter in the estimation law. The estimation scheme is based on the piecewise-constant estimation (PWCE) law in [24], which was originally from [25]. The PWCE law consists of two elements, namely a state predictor and a piecewise-constant update law. The state predictor is defined as:

$$\dot{\widehat{x}}\left(t\right)=f\left(x\left(t\right)\right)+B\left(x\left(t\right)\right)u\left(t\right)+\widehat{\sigma }\left(t\right)-a\tilde{x}\left(t\right),\widehat{x}\left(0\right)=x\left(0\right),$$

(18)

where $\tilde{x}\left(t\right)\triangleq \widehat{x}\left(t\right)-x\left(t\right)$ is the prediction error, and a is an arbitrary positive constant. The estimation, $\widehat{\sigma }\left(t\right)$, is updated in a piecewise-constant way:

$$\left\{\begin{array}{cc}\hfill \widehat{\sigma }\left(t\right)& =\widehat{\sigma }\left(iT\right),t\in [iT,(i+1)T),\hfill \\ \hfill \widehat{\sigma }\left(iT\right)& =-\frac{a}{e^{aT}-1}\tilde{x}\left(iT\right),\hfill \end{array}\right.$$

(19)

where T is the estimation sampling time, and $i=0,1,2,\cdots$. Finally, the pointwise value of $d\left(x\right(t\left)\right)$ at time t is estimated as

$$\widehat{d}\left(t\right)=B^{†}\left(x\left(t\right)\right)\widehat{\sigma }\left(t\right),$$

(20)

where $B^{†}\left(x\left(t\right)\right)$ is the pseudoinverse of $B\left(x\right(t\left)\right)$. The following lemma establishes the EEB associated with the estimation scheme in (18) and (19). The proof is similar to that in [24]. For completeness, it is given in Appendix A.

Lemma 4.

Given the dynamics (1) subject to Assumption 1, and the estimation law in (18) and (19), if $x\in \mathcal{X}$ and $u\in \mathcal{U}$ for any $t\ge 0$, the estimation error can be bounded as   

$$\begin{array}{cc}\hfill \Vert \widehat{d}\left(t\right)-d(t,x\left(t\right))\Vert & \le \delta (t,T)\triangleq \left\{\begin{array}{c}{b}_d,\forall 0\le t<T,\hfill \\ \alpha \left(T\right)\underset{x\in \mathcal{X}}{max}{B}^{†}\left(x\right),\forall t\ge T,\hfill \end{array}\right.\hfill \end{array}$$

(21)

where

$$\begin{array}{cc}\hfill \alpha \left(T\right)& \triangleq \left(2\sqrt{n}T(L_d\varphi +l_d)+(1-e^{-aT})\sqrt{n}{b}_d\right)\underset{x\in \mathcal{X}}{max}\Vert B\left(x\right)\Vert +2\sqrt{n}T{L}_B{b}_d,\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill \varphi & \triangleq \underset{x\in \mathcal{X},u\in \mathcal{U}}{max}\Vert f\left(x\right)+B\left(x\right)u\Vert +b_d\underset{x\in \mathcal{X}}{max}\Vert B\left(x\right)\Vert ,\hfill \end{array}$$

(23)

with constants $L_B$, $L_d$ and $b_d$ from Assumption 1, and ϕ defined in (23). Moreover, ${lim}_{T\to 0}\delta (t,T)=0,$ for any $t\ge T$.

Proof.

See Appendix A. □

Remark 4.

Lemma 4 implies that theoretically, for $t\ge T$, the disturbance estimation after a single sampling interval can be made arbitrarily accurate by reducing T, which further indicates that the conservatism with the RRE condition can be arbitrarily reduced after a sampling interval.

In practice, the value of T is subject to the limitations related to computational hardware and sensor noise. Additionally, using a very small T tends to introduce high frequency components in the control loop, potentially harming the robustness of the closed-loop system, e.g., against time delay. This is similar to the use of a high adaptation rate in model reference adaptive control schemes as discussed in [13]. Therefore, one should avoid the use of a very small T for the sake of robustness unless a low-pass filter is used to filter the estimated disturbance before fed into (16), as suggested by the ${\mathcal{L}}_1$ adaptive control theory [13].

Remark 5.

The estimation in $[0,T)$ cannot be arbitrarily accurate. This is because the estimation in $[0,T)$ depends on $\tilde{x}\left(0\right)$ according to (19). Considering that $\tilde{x}\left(0\right)$ is purely determined by the initial state of the system, $x\left(0\right)$, and the initial state of the predictor, $\widehat{x}\left(0\right)$, it does not contain any information of the uncertainty. Since T is usually very small in practice, lack of a tight estimation error bound for the interval $[0,T)$ will not cause an issue from a practical point of view. Additionally, the estimation of ϕ defined in (23) could be quite conservative. Further considering the frequent use of Lipschitz continuity and inequalities related to matrix/vector norms in deriving the constant $\alpha \left(T\right)$, $\alpha \left(T\right)$ can be overly conservative. Therefore, for practical implementation, one should leverage some empirical study, e.g., performing simulations under a few user-selected functions of $d(t,x)$ and determining a bound for $\delta (t,T)$. In our experiments, we found the theoretical bound $\delta (t,T)$ computed according to (21) was usually at least 10 and could be ${10}^4$ times more conservative.

3.4. Exponentially Convergent Trajectory Tracking

Based on the review of contraction control in Section 2 and the discussions in Section 3.2 and Section 3.3, the control law can be obtained by solving the following QP problem at each time t:

$$u\left(t\right)=\underset{k\in {\mathbb{R}}^m}{argmin}\Vert {k-u^{\star }\left(t\right)\Vert }^2$$

(24)

subject to

$${\gamma }_s^{\top }(1,t)M(x)\dot{\check{x}}-{\gamma }_s^{\top }(0,t)M(x^{\star }){\dot{x}}^{\star }+\Vert {\gamma }_s^{\top }(1,t)M(x)B\left(x\right)\Vert \delta (t,T)\le -\lambda E(x^{\star },x),$$

(25)

where $\dot{\check{x}}\left(t\right)=f\left(x\right)+B\left(x\right)(k+\widehat{d}\left(t\right))$, according to (17), depends on $\widehat{d}\left(t\right)$, which is from the disturbance estimation law defined by (18) to (20), $\delta (t,T)$ as defined in (21), and ${\dot{x}}^{\star }\left(t\right)=f(x^{\star })+B(x^{\star })u^{\star }$ as defined in (5). Similar to (13),  problem (24) is a pointwise min-norm control problem and has an analytic solution [23]. Specifically, denoting ${\varphi }_0(t,x^{\star },x)\triangleq {\gamma }_s^{\top }(1,t)M\left(x\right)(f\left(x\right)+B\left(x\right)(u^{\star }\left(t\right)+\widehat{d}\left(t\right))+\Vert {\gamma }_s^{\top }(1,t)M\left(x\right)B\left(x\right)\Vert \delta (t,T)-{\gamma }_s^{\top }(0,t)M\left(x^{\star }\right){\dot{x}}^{\star }+\lambda E(x^{\star },x)$ and ${\varphi }_1(x^{\star },x)\triangleq B^{\top }\left(x\right)M\left(x\right){\gamma }_s(1,t)$, (25) can be written as ${\varphi }_0(t,x^{\star },x)+{\varphi }_1^{\top }(x^{\star },x)(k-u^{\star }\left(t\right))\le 0$, and the solution for (24) is given by

$$u\left(t\right)=k^{\star }=\left\{\begin{array}{cc}{u}^{\star }\left(t\right)\hfill & \mathrm{if}{\varphi }_0(t,x^{\star },x)\le 0,\\ u^{\star }\left(t\right)-\frac{{\varphi }_0(t,x^{\star },x){\varphi }_1(x^{\star },x)}{{\Vert {\varphi }_1(x^{\star },x)\Vert }^2}\hfill & \mathrm{if}{\varphi }_0(t,x^{\star },x)>0.\end{array}\right.$$

(26)

To move forward with analysis, we need to verify that when $x\left(t\right),x^{\star }\left(t\right)\in \mathcal{X}$, the control signal $u\left(t\right)$ resulting from solving the QP problem (24) satisfies $u\left(t\right)\in \mathcal{U}$. Deriving verifiable conditions to ensure this set bound is outside the scope of this paper and will be addressed as future work. We are now ready to state the main result of the paper.

Theorem 2.

Given an uncertain system represented by (1) satisfying Assumption 1, assume that there exists a metric $W\left(x\right)$ such that for all $x\in \mathcal{X}$, (10) holds and ${\alpha }_{1}I\le M\left(x\right)=W^{-1}\left(x\right)\le {\alpha }_{2}I$ holds for positive constants ${\alpha }_1$ and ${\alpha }_2$. Furthermore, suppose that a nominal trajectory ($x^{\star }(·),u^{\star }(·)$) planned using the nominal dynamics (5) and the initial actual states $x\left(0\right)$ satisfy (14) and

$$\mathsf{\Omega }\left(t\right)\triangleq \left\{y\in {\mathbb{R}}^n:y\le \Vert x^{\star }\left(t\right)\Vert +\sqrt{\frac{{\alpha }_2}{{\alpha }_1}}\Vert x\left(0\right)-x^{\star }\left(0\right)\Vert e^{-\lambda t}\right\}\subset \mathcal{X},$$

(27)

for any $t\ge 0$. Then, if $u\left(t\right)$ from solving (24) satisfies $u\left(t\right)\in \mathcal{U}$ for any $t\ge 0$, the control law constructed by solving (24) ensures $x\left(t\right)\in \mathcal{X}$ for any $t\ge 0$, and furthermore, universally exponentially stabilizes the uncertain system (1) in the sense of Definition 1 with $R=\sqrt{\frac{{\alpha }_2}{{\alpha }_1}}$, i.e.,

$$\Vert x\left(t\right)-x^{\star }\left(t\right)\Vert \le \sqrt{\frac{{\alpha }_2}{{\alpha }_1}}\Vert x\left(0\right)-x^{\star }\left(0\right)\Vert e^{-\lambda t},\forall t\ge 0.$$

(28)

Proof.

We use contradiction to show $x\left(t\right)\in \mathcal{X}$ for all $t\ge 0$. Assume this is not true. According to (27), $x\left(0\right)\in \mathcal{X}$. Since $x\left(t\right)$ is continuous, there must exist a time $\tau$ such that

$$x\left(t\right)\in \mathcal{X},\forall t\in [0,{\tau }^{-}]\mathrm{and}x\left(\tau \right)\notin \mathcal{X}.$$

(29)

Now let us consider the system evolution in $[0,{\tau }^{-}]$. Since $u\left(t\right)\in \mathcal{U}$ by assumption and $x\left(t\right)\in \mathcal{X}$ for any t in $[0,{\tau }^{-}]$, the EEB in (21) holds in $[0,{\tau }^{-}]$. As a result, the control law obtained from solving (24) ensures satisfaction of the RRE condition (16) and thus satisfaction of the Riemannian energy condition (15) for the uncertain system (1), and thereby universally exponentially stabilizes the uncertain system (1) in $[0,{\tau }^{-}]$, in the sense of Definition 1 with $R=\sqrt{\frac{{\alpha }_2}{{\alpha }_1}}$, according to Theorem 1. On the other hand, satisfaction of (14) implies that $x^{\star }\left(t\right)$ is a feasible state trajectory for the uncertain system (1) according to Lemma 3. Further considering Theorem 1, we have $\Vert x\left(t\right)\Vert \le \Vert x^{\star }\left(t\right)\Vert +\sqrt{\frac{{\alpha }_2}{{\alpha }_1}}\Vert x\left(0\right)-x^{\star }\left(0\right)\Vert e^{-\lambda t}$ for any t in $[0,{\tau }^{-}]$. Due to (27), the preceding inequality indicates that $x\left(t\right)$ remains in the interior of $\mathcal{X}$ for t in $[0,{\tau }^{-}]$. This, together with the continuity of $x\left(t\right)$, immediately implies $x\left(\tau \right)\in \mathcal{X}$, which contradicts (29). Therefore, we conclude that $x\left(t\right)\in \mathcal{X}$ for all $t\ge 0$. From the development of the proof, it is clear that with the control law given by the solution of (24), the UES of the closed-loop system in the sense of Definition 1 with $R=\sqrt{\frac{{\alpha }_2}{{\alpha }_1}}$ for all $t\ge 0$ is achieved, which is mathematically represented by (28). The proof is complete. □

3.5. Discussion

Theorem 2 essentially states that under certain assumptions, the proposed controller guarantees exponential convergence of the actual state trajectory $x\left(t\right)$ to a desired one $x^{\star }\left(t\right)$. With the exponential guarantee, if the actual trajectory meets the desired trajectory at certain time $\tau$, then these two trajectories will stay together afterward. While the exponential convergence guarantee is stronger than the performance guarantees provided by existing adaptive CCM-based approaches [14,15] that deal with similar settings (i.e., matched uncertainties), the proposed method requires the knowledge of the Lipschitz bound of the uncertainty $d(t,x)$ and the input matrix function $B\left(x\right)$ to be in a compact set known a priori (see Assumption 1), and the actual control inputs to stay in a compact set known a priori, which cannot be verified at this moment due to the lack of a bound on the control inputs. These requirements are not needed in [14,15].

The approach here is related to the robust control Lyapunov-based approaches [23] which provide robust stabilization around an equilibrium point (as opposed to a trajectory considered in this paper) in the presence of uncertainties.

Remark 6.

The exponential convergence guarantee stated in Theorem 2 is based on a continuous-time implementation of the controller. In practice, a controller is normally implemented on a digital processor or controller with a fixed sampling time. As a result, the property of exponential convergence may be slightly violated.

Computational cost: As can be seen from Section 2 and Section 3.2, Section 3.3 and Section 3.4, computation of the control signal at each time t includes three steps: (i) updating the estimated disturbance $\widehat{d}\left(t\right)$ via (18) to (20), (ii) computing the geodesic $\gamma (·,t)$ connecting the actual and nominal states (see the discussion below (11)), and (iii) computing the control signal $u\left(t\right)$ via (26). The computation costs of steps (i) and (iii) are quite low as they only involve integration and algebraic calculation. In comparison, step (ii) has a relatively high computational cost as it 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 model predictive control (MPC) problem [21]. Following [21], such a problem can be efficiently solved by applying a pseudospectral method.

4. Simulation Results

In this section, we illustrate the performance of our proposed tracking controller based on the RRE condition and disturbance estimation, denoted as DE-CCM, using aircraft and planar quadrotor examples. For both examples, we perform comparisons of DE-CCM with standard CCM controllers that ignore the uncertainties and adaptive CCM (Ad-CCM) controllers considering parametric uncertainties designed using the approach in [14]. All the computations and simulations were performed in Matlab R2021b.

4.1. Longitudinal Dynamics of an Aircraft

We first implement our method on the simplified pitch dynamics of an aircraft borrowed from [26]:

$$\dot{x}\triangleq \left[\begin{array}{c}\dot{\theta }\\ \dot{\alpha }\\ \dot{q}\end{array}\right]=\left[\begin{array}{c}q\\ q-\overline{L}\left(\alpha \right)\\ -k_{q}q+\overline{M}\left(\alpha \right)\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]u,$$

(30)

where $\theta$, $\alpha$ and q are the pitch angle (in rad), angle of attack (in rad), and pitch rate (in rad/s). Here $\overline{L}\left(\alpha \right)$ and $\overline{M}\left(\alpha \right)$ are aerodynamic lift and moment, respectively. Using the flat plat theory, these two aerodynamic terms are approximated by $\overline{L}\left(\alpha \right)=0.8sin\left(2\alpha \right)$ and $\overline{M}\left(\alpha \right)=-l_{\alpha }\overline{L}\left(\alpha \right)$ with unknown parameters $k_q\in \left[0.10.8\right]$ and $l_{\alpha }\in [-31]$. For all the simulations, the true values are chosen to be $k_q=0.8$, $l_{\alpha }=-3$, while the nominal values of these parameters used in designing all the tested controllers are limited to $k_q^{nom}=0.2$, $l_{\alpha }^{nom}=-1$. As a result, the dynamics can be recast in the form of (30) with $f\left(x\right)={[q,q-\overline{L}\left(\alpha \right),-k_q^{nom}q-l_{\alpha }^{nom}\overline{L}\left(\alpha \right)]}^{\top }$, $B\left(x\right)={[0,0,1]}^{\top }$ and $d(t,x)=-(k_q-k_q^{nom})q-(l_{\alpha }-l_{\alpha }^{nom})\overline{L}\left(\alpha \right)$. The control objective is to drive the system from nominal initial states ${[0,0,0]}^T$ to terminal states ${[180,0,0]}^T$. For CCM search and trajectory planning, the following constraints are enforced: $x\in \mathcal{X}=[-10°,180°]\times [-5°,40°]\times [-10,50]°/$s, $u\in \mathcal{U}=[-15,15]°$/s${}^2$.

We set the convergence rate $\lambda$ to 1. By gridding the set of $\alpha$ and evaluating the constraints (9) in those grid points, we found a CCM $W\left(x\right)$ as a quadratic function of $\alpha$ with the SPOT toolbox [27] (to formulate the convex optimization problem) and Mosek solver [28]. Additionally, the constants ${\alpha }_1$ and ${\alpha }_2$ in (28) such that ${\alpha }_{1}I\le M\left(x\right)=W^{-1}\left(x\right)\le {\alpha }_{2}I$ for all $x\in \mathcal{X}$ were found to be ${\alpha }_1=0.1$ and ${\alpha }_2=396.5$. We planned a nominal trajectory ($x^{\star }(·)$, $u^{\star }(·)$) using OptimTraj [29,30], to drive the system from the initial states ${[0,0,0]}^T$ to the terminal states ${[180°,0,0]}^T$, while minimizing the task completion time ($T_a$) and energy consumption characterized by the cost function $J={\int }_0^{T_a}{u\left(t\right)}^{2}dt+5T_a$. For simulation, OPTI [31] and Matlab fmincon solvers were used to solve the geodesic optimization problem (see Section 2). The initial states of the actual system were chosen to be ${[5°,5°,0]}^T$, slightly deviated from that planned ones to better illustrate the tracking performance. We implemented our proposed DE-CCM, Ad-CCM from [14] and a standard CCM which neglects all the uncertainty. For Ad-CCM design, the adaptation gain was chosen to be diag $\left([{10}^3,{10}^3]\right)$ to achieve a relatively good tracking result, while further increasing it did not help much with the tracking performance. The design procedure for Ad-CCM in [14] requires a parametric structure for the uncertainty, which is given by

$$d(t,x)=\underset{\Delta (t,x)}{\underbrace{\left[\begin{array}{cc}q& \overline{L}\left(\alpha \right)\end{array}\right]}}\underset{\theta }{\underbrace{\left[\begin{array}{c}{k}_q^{nom}-k_q\\ l_{\alpha }^{nom}-l_{\alpha }\end{array}\right]}}=\left[\begin{array}{cc}q& 0.8sin\left(2\alpha \right)\end{array}\right]\left[\begin{array}{c}-0.6\\ 2\end{array}\right],$$

(31)

where $∆(t,x)$ is the known base function and $\theta$ is the unknown parameter vector to be estimated by the adaptive law proposed in [14]. The control signals under all three controllers were updated at 200 Hz.

It is easy to notice from Assumption 1 that $L_B=0$ and $l_d=0$ since the input matrix is constant, and the uncertainty is time-invariant. We can also verify that the disturbance is bounded by $b_d=2.12$ and has a Lipschitz constant $L_d=3.80$. By gridding the space $\mathcal{X}$ and making use of the control input bound, the system derivative can also be bounded by a constant $\varphi =2.11$. According to (21), if we want to achieve an EEB $\delta (t,T)=0.05$ for all $t\ge T$, the maximum value for the estimation sample time T is $7.76\times {10}^{-4}$ s. However, as mentioned in Remark 5, the way to compute the EEB is quite conservative. In the simulations we found that $T_s=0.005$ was more than enough to ensure the EEB and therefore used $T_s=0.005$ for implementing the DE-CCM controller.

As shown in Figure 2 and Figure 3, due to ignoring the uncertainties, CCM yielded a large tracking error between 2 and 6 s. The state trajectories under Ad-CCM had some oscillations, which lasted roughly up to 8 s. All three states yielded by DE-CCM achieve good tracking performance without large deviations from the planned trajectories, unlike the performance yielded by Ad-CCM and CCM. From Figure 3 we notice that the tracking error represented by $x-x^{\star }$ under DE-CCM monotonically decreases and achieves the smallest steady-state error. The small non-zero tracking error at the end under DE-CCM, which is inconsistent with the performance guarantee in (25), is due to the limited control update frequency, while the performance guarantee in Lemma 4 holds under continuous update of the control signal, i.e., corresponding to an infinitely high update frequency.

Table 1. MSE for state trajectory tracking for the aircraft example.

	CCM	Ad-CCM	DE-CCM
MSE	$2.994\times {10}^{-3}$	$1.397\times {10}^{-3}$	$1.369\times {10}^{-3}$

shows the mean squared error (MSE) for state trajectory tracking defined by

$$\mathrm{MSE}=\frac{1}{N}\sum _{i=1}^N\Vert {x\left(t_i\right)-x^{\star }\left(t_i\right)\Vert }^2,$$

(32)

where N is the number of data points, under DE-CCM, Ad-CCM and CCM. We observe that DE-CCM outperforms CCM and Ad-CCM in terms of MSE by 54% and 2%, respectively.

From Figure 4, we observe that the input of DE-CCM is smoother than Ad-CCM. The small oscillations between 2 s to 8 s in DE-CCM input are due to the finite tolerance in the geodesic optimization. Decreasing the tolerance and the sample time will reduce the oscillations but request more iterations (and thus more time) to compute the control signal at each time step.

4.2. Planar Quadrotor

A planar quadrotor system is borrowed from [16]. The state vector is defined as $x={[p_x,p_z,\varphi ,v_x,v_z,\dot{\varphi }]}^{\top }$, where $p_x$ and $p_z$ are the positions 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, $\varphi$ 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

$$\dot{x}=\left[\begin{array}{c}\dot{}{p}_x\\ \dot{}{p}_z\\ \dot{\varphi }\\ \dot{}{v}_x\\ \dot{}{v}_z\\ \ddot{\varphi }\end{array}\right]=\left[\begin{array}{c}{v}_{x}cos\left(\varphi \right)-v_{z}sin\left(\varphi \right)\\ v_{x}sin\left(\varphi \right)+v_{z}cos\left(\varphi \right)\\ \dot{\varphi }\\ v_z\dot{\varphi }-gsin\left(\varphi \right)\\ -v_x\dot{\varphi }-gcos\left(\varphi \right)\\ 0\end{array}\right]+\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ \frac{1}{m}& \frac{1}{m}\\ \frac{l}{J}& -\frac{l}{J}\end{array}\right](u+d(t,x)),$$

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 $d(t,x)$ denotes the unknown disturbances exerted on the propellers. The parameters were set as $m=0.486$ kg, $J=0.00383\mathrm{K}\mathrm{g}{\mathrm{m}}^2$, and $l=0.25$ m. The uncertainty $d(t,x)$ was set to be $d(t,x)=0.15(v_x^2+v_z^2)[-1+0.3sin\left(2t\right);-1+0.3cos\left(2t\right)]$. We imposed the following constraints: $x\in \mathcal{X}\triangleq [0,10]\times [0,10]\times [-\frac{\pi }{3},\frac{\pi }{3}]\times [-2,2]\times [-1,-1]\times [-\frac{\pi }{3},\frac{\pi }{3}]$, $u\in \mathcal{U}\triangleq [0,\frac{3}{2}mg]\times [0,\frac{3}{2}mg]$.

When searching for CCM, we parameterized the CCM W by $\varphi$ and $v_x$ and imposed the constraint $W\ge 0.01I$. The convergence rate $\lambda$ was chosen to be 0.8. More details about synthesizing the CCM can be found in [17]. For estimating the disturbance using (18) to (20), we set $a=10$. It is easy to verify that $L_d=1.23$, $l_d=0.64$, $b_d=1.38$, and $L_B=0$ (due to the fact that B is constant) satisfy (2). By gridding the space $\mathcal{X}$, the constant $\varphi$ in (23) can be determined as $\varphi =584.6$. According to (21), if we want to achieve an EEB $\delta (t,T)=0.1$ for all $t\ge T$, then the estimation sampling time needs to satisfy $T\le 6.42\times {10}^{-7}$ s. However, as noted in Remark 5, the EEB computed according to (21) could be overly conservative. In the simulations, we found that the estimation sampling time of $0.002$ s was more than enough to ensure the desired EEB and therefore simply set $T=0.002$ s.

We consider the task of navigation from $(2,0)$ to $(8,8)$ while avoiding three obstacles depicted by black circles in Figure 5. A nominal trajectory ($x^{\star }(·)$, $u^{\star }(·)$) was generated using OptimTraj [29] to minimize the cost $J={\int }_0^{T_a}{u\left(t\right)}^{2}dt+5T_a$, where $T_a$ is the arrival time. OPTI [31] and Matlab fmincon solvers were used to solve the geodesic optimization problem (see Section 2). The actual start point was set to be $(0,0)$, which was different from the planned start point, to reveal the trajectory convergence pattern.

For comparison, we also designed a standard CCM controller by completely ignoring the uncertainty and two adaptive CCM (Ad-CCM) controllers following the approach in [14]. To apply the approach in [14] which can only handle parametric uncertainties, we parameterized the uncertainty as

$$d(t,x)=\underset{\Delta (t,x)}{\underbrace{\left[\begin{array}{cc}{v}_x^2(-1+0.3sin\left(2t\right))& v_z^2(-1+0.3sin\left(2t\right))\\ v_x^2(-1+0.3cos\left(2t\right))& v_z^2(-1+0.3cos\left(2t\right))\end{array}\right]}}\underset{\theta }{\underbrace{\left[\begin{array}{c}0.15\\ 0.15\end{array}\right]}},$$

(33)

where $\Delta (t,x)$ is the basis function that is assumed to be known, and $\theta$ is th vector of unknown parameters. With the parametric structure (33), we designed two adaptive CCM controllers using $\mathsf{\Gamma }=10$ and $\mathsf{\Gamma }=100$, respectively, where $\mathsf{\Gamma }$ denotes the adaptive gain. Figure 5 shows the planned and actual trajectories under the CCM, Ad-CCM, and our proposed controller based on the RRE condition and disturbance estimation, denoted as DE-CCM, while Figure 6 and Figure 7 show the control inputs and Riemannian energy. One can see that the actual trajectories yielded by the CCM controller deviated quite a lot from the planned ones and collided with one obstacle. On the other hand, the actual trajectories yielded by the DE-CCM controller converged to the desired trajectory as expected and almost overlapped with it afterward. In fact, the slight deviations of actual trajectories from the desired ones under the DE-CCM controller were due to the finite step size associated with the ODE solver used for the simulations (see Remark 6).

Table 2. MSE for state trajectory tracking for the planar quadrotor example. For Ad-CCM, only the result for $\mathsf{\Gamma }=100$, which corresponds to better tracking performance compared to $\mathsf{\Gamma }=10$, is included.

	CCM	Ad-CCM ($\mathsf{\Gamma }=100$)	DE-CCM
MSE	1.175	0.738	0.634

shows the MSE for state trajectory tracking defined in (32). We observe that DE-CCM outperforms CCM and Ad-CCM in terms of MSE by 46% and 14%, respectively.

From Figure 6, one can see that the magnitude of $E(x^{\star },x)$ under the RD-CCM controller decreased exponentially, and the magnitude was bounded by the curve $E(x^{\star }\left(0\right),x\left(0\right))e^{-2\lambda t}$ from above except at the very end when the energy is close to zero. In comparison, Ad-CCM with $\mathsf{\Gamma }=100$ yielded similar tracking performance to DE-CCM, while the tracking performance of Ad-CCM with $\mathsf{\Gamma }=10$ was relatively worse and characterized by larger oscillations. Additionally, from Figure 7, one can see that the control inputs generated by both of the Ad-CCM controllers have high-frequency oscillations before 3 s, which is undesired for practical deployment. Finally, Figure 8 shows the actual and estimated disturbances as well as the estimation error. One can see that the estimated disturbance is quite close to the actual one for both channels, and the EEB of 0.1 is respected throughout the simulation.

5. Conclusions

This paper presents a robust trajectory tracking controller with exponential convergence for uncertain nonlinear systems based on control contraction metrics (CCM) and disturbance estimation. The controller uses a disturbance estimator to estimate the pointwise value of the uncertainty with a pre-computable estimation error bound (EEB). The estimated disturbance and the EEB are then incorporated into a robust Riemannian energy condition, which guarantees exponential convergence of actual trajectories to desired ones. The efficacy of the proposed controller is validated in simulations. In particular, the proposed controller outperforms an existing adaptive CCM controller in terms of tracking performance by 2% for the aircraft example and 14% for the planar quadrotor example, while not needing to know the basis functions to parameterize the uncertainties that are needed by the adaptive CCM controller.

This paper considers only matched uncertainties, which are added to the system through the same channels as control inputs. In the future, we would like to address unmatched uncertainties that widely exist in practical systems. Additionally, we would like to experimentally validate the proposed controller on real hardware.