Perception-Based Sampled-Data Optimization of Dynamical Systems

A major challenge in controlling complex autonomous systems consists of incorporating rich data from perceptual and sensing sensor data. The performance of feedback control systems critically relies on the information extracted from perceptual sensing, which may require the processing of high-dimensional data only available at given spatio-temporal granularities; see, e.g. Dean and Recht (2021); Al Makdah et al. (2020); Xu et al. (2021); Dawson et al. (2022). This paper investigates how to integrate perceptual information into controllers inspired by optimization algorithms where the goal is to steer a dynamic system toward solutions of an optimization problem with costs associated with the system’s inputs and states. For example, in autonomous driving, the optimization problem may formalize way-point tracking and obstacle avoidance, whose information is provided by images from a dashboard camera. Other examples include robotics and power systems (the latter leveraging pseudo-measurements).

The line of research on feedback optimization goes back to earlier concepts of KKT-type controllers in Jokic et al. (2009); Brunner et al. (2012); Hirata et al. (2014), and it was recently expanded to include new classes of controllers inspired by first-order optimization methods in Colombino et al. (2020); Zheng et al. (2020); Hauswirth et al. (2021); Lawrence et al. (2020); Bianchin et al. (2021, 2022); Belgioioso et al. (2021); Agarwal et al. (2022); Simpson-Porco (2021); also see references therein. In this paper, we provide contributions relative to existing works by considering a setup where the cost of the optimization problem evolves over time (for instance, for way-point tracking and to avoid moving obstacles) and the state of the system cannot be directly measured. The latter is a distinctive feature of this work: we address the case in which optimization-based controllers must leverage perceptual information available at given temporal granularities and learning mechanisms to estimate the system state.

We develop a sampled-data feedback controller that is based on an adaptation of a projected gradient descent method. Based on the specified time-varying costs, the gradient-based controller generates inputs for the system which are then passed through a zero-order hold. Importantly, the controller leverages a trained neural network that maps perceptual information into estimates of the state of the system. We derive sufficient conditions to guarantee (local) input-to-state stability (ISS) of the control loop. In particular, we show that the interconnected system tracks the optimal solution trajectory of the optimization problem up to an error that depends on the approximation errors of the neural network and the time-variability of the cost and unknown disturbance. The ISS bounds are derived by leveraging the fundamental results of Jiang and Wang (2001) and Nešić et al. (1999).

We note that a similar perception-based regulation problem was considered in our prior work in Cothren et al. (2022a); however, in Cothren et al. (2022a) controllers operate at continuous time and thus do not account for the sample-data nature of the feedback information. A sampled-data controller was developed in Belgioioso et al. (2021); with respect to Belgioioso et al. (2021), we consider cases where the optimization problem is time-varying and the state is estimated via perception maps.

We test our controller on an autonomous driving application where vehicles are modeled via unicycle dynamics; the controller acquires the position of the vehicle from a neural network that estimates positions from images.

In the following, we formalize our research problem and discuss the necessary assumptions.¹¹1Notation. We denote by $\mathbb{N},\mathbb{Z}_{+},\mathbb{R},\mathbb{R}_{>0},\text{ and }\mathbb{R}_{\geq 0}$ the set of natural numbers, positive integers, real numbers, positive real numbers, and non-negative real numbers. For vectors $x\in\mathbb{R}^{n}$ and $u\in\mathbb{R}^{m}$, $\|x\|$ is the Euclidean norm of $x$, $\|x\|_{\infty}$ is the supremum norm, and $(x,u)\in\mathbb{R}^{n+m}$ is their vector concatenation. $x^{\top}$ denotes transposition, and $x_{i}$ denotes the $i$-th element of $x$. For a matrix $A\in\mathbb{R}^{n\times m}$, $\|A\|$ is the induced $2$-norm and $\|A\|_{\infty}$ is the supremum norm. The set $\mathcal{B}_{n}(r):=\{z\in\mathbb{R}^{n}:\|z\|<r\}$ is the open ball in $\mathbb{R}^{n}$ with radius $r>0$; $\mathcal{B}_{n}[r]:=\{z\in\mathbb{R}^{n}:\|z\|\leq r\}$ is the closed ball. Given two sets $\mathcal{X}\subset\mathbb{R}^{n}$ and $\mathcal{Y}\subset\mathbb{R}^{m}$, $\mathcal{X}\times\mathcal{Y}$ is their Cartesian product; $\mathcal{X}+\mathcal{B}_{n}(r)$ is the open set defined as $\mathcal{X}+\mathcal{B}_{n}(r)=\{x+y:x\in\mathcal{X},y\in\mathcal{B}_{n}(r)\}$. $\Pi_{\mathcal{U}}$ is the Euclidean projection of $z\in\mathbb{R}^{n}$ onto $\mathcal{U}\subset\mathbb{R}^{n}$; or, $\Pi_{\mathcal{U}}(z):=\operatorname{arg}\min_{u\in\mathcal{U}}\|u-z\|^{2}$. A function $\gamma:\mathbb{R}_{\geq 0}\rightarrow\mathbb{R}_{\geq 0}$ is of class $\mathcal{K}$ if it is continuous, $\gamma(0)=0$, and strictly increasing; it is of class $\mathcal{K}_{\infty}$ if it is additionally unbounded. A function $\beta:\mathbb{R}_{\geq 0}\times\mathbb{R}_{\geq 0}\to\mathbb{R}_{\geq 0}$ is of class $\mathcal{KL}$ if for each fixed $t$ the function $\beta(r,t)$ is of class $\mathcal{K}$, and if for each fixed $r$ the function $\beta(r,t)$ is decreasing with respect to $t$ and is s.t. $\beta(r,t)\rightarrow 0$ for $t\rightarrow\infty$. We consider systems that can be modeled using dynamics of the form:

where $x:\mathbb{R}_{\geq 0}\to\mathcal{X}\subseteq\mathbb{R}^{n_{x}}$ is the state, $u:\mathbb{R}_{\geq 0}\to\mathcal{U}\subseteq\mathbb{R}^{n_{u}}$ is the control input, $w:\mathbb{R}_{\geq 0}\to\mathcal{W}\subseteq\mathbb{R}^{n_{w}}$ is a time-varying unknown exogenous disturbance, and where the vector field $f:\mathcal{X}\times\mathcal{U}\times\mathcal{W}\to\mathbb{R}^{n_{x}}$ is continuously differentiable on the open and connected domain $\mathcal{X}\times\mathcal{U}\times\mathcal{W}$ and is Lipschitz-continuous in its arguments with constants $L_{x},L_{u}$, and $L_{w}$ respectively. In this paper, motivated by practical hardware and operational requirements, we restrict our attention to cases where $u\in{\mathcal{U}}_{\textup{c}}$ at all times, where ${\mathcal{U}}_{\textup{c}}\subset\mathcal{U}$ is compact. We impose the following additional assumptions on the system (1).

Assumption 1 guarantees that, for constant inputs $\bar{u}$ and $\bar{w}$, system (1) admits the unique equilibrium point $\bar{x}:=h(\bar{u},\bar{w})$. Note that when $\nabla_{x}f(x,\bar{u},\bar{w})$ is invertible for any $\bar{u}$ and $\bar{w}$, then the existence of $h(\bar{u},\bar{w})$ is always guaranteed. Furthermore, by the implicit function theorem, $h(\bar{u},\bar{w})$ is differentiable since $f(x,\bar{u},\bar{w})$ is differentiable. We also note that the equilibrium set $\mathcal{X}_{eq}:=\{h(\bar{u},\bar{w}):\bar{u}\in\mathcal{U}_{c},\,\,\bar{w}\in\mathcal{W}_{c}\}$ is compact; this is due to $\mathcal{U}_{c}\times\mathcal{W}_{c}$ being compact, $h(u,w)$ being continuously differentiable, and the result (Rudin, 1976, Thm. 4.14). For any $u\in\mathcal{U}_{c}$, we have that $\|\nabla_{u}h(u,\bar{w})\|\leq\ell_{h_{u}}$ since $\mathcal{U}_{c}$ is compact Rudin (1976).

Next, as customary in the context of feedback optimization (see, e.g., Colombino et al. (2020); Hauswirth et al. (2021), we require a stability condition on the system to control. To this end, let $\mathcal{X}_{r}=\mathcal{X}_{eq}+\mathcal{B}_{r}(r)\subseteq\mathcal{X}$, $r>0$, be a set for which the following assumption holds.

From Khalil (2002), Assumption 3 implies that there exists constants $k,a,\gamma>0$ such that the following holds:

for some constant $\gamma>0$, and for $x(0)\in\mathcal{X}_{0}:=\mathcal{X}_{eq}+\mathbb{B}_{n}(r_{0}),\,\,r_{0}<(r-\text{diam}({\mathcal{X}}_{\textup{eq}}))/k-\bar{\gamma}$, $\bar{\gamma}:=\gamma\sigma_{w}\left(\sup_{t\geq 0}\|\dot{w}(t)\|)\right)$.

We point out that when the (physical) system does not satisfy Assumption 3, then (1) models the pre-stabilized physical system.

To address Problem 1, we consider the design of feedback controllers that are inspired by projected-gradient-type methods as in Bianchin et al. (2021); Cothren et al. (2022a). However, to acknowledge the fact that the state estimates are available only at given time intervals (for example, images from a camera are captured at a given frequency), our controller design is based on a sampled data mechanism. The controller is equipped with a supervised learning method to estimate the perception map.

Towards this, let $\tau>0$ represent the period between two consecutive arrivals of perceptual data (for example, images) and let $k\in\mathbb{Z}_{+}$ be the sampling index, so that $\mathbb{S}=\{k\tau:k\in\mathbb{Z}_{+}\}$ is the set of times where perceptual information arrives and control inputs are updated. Accordingly, denote as $x_{k}=x(k\tau)$ and $w_{k}=w(k\tau)$ the sampled states and disturbance at time $k\tau$, and let $\phi_{k}(u):=\phi(u,k\tau)$ and $\psi_{k}(x):=\psi(x,k\tau)$ for notational brevity. We propose the following projected-gradient-type controller to generate inputs $\{u_{k}\}$ at each time $k\tau$, $k\in\mathbb{Z}_{+}$:

where $\eta>0$ is a tunable parameter (also known as step size in the gradient descent literature), and

where $H(u)$ is the Jacobian of $h_{u}$ evaluated at $u$.

The controller (4) is of the form of a projected gradient-type algorithm; here, we have modified it by including the gradient evaluated at the instantaneous system state $x_{k}$ to circumvent the need to measure the exogenous input $w_{k}$. We also note that the map $\Psi_{k}$ is applied to the current state $x_{k}$, and the previous control input $u_{k-1}$, which is applied to the system over the interval $[(k-1)\tau,k\tau)$. Critically, the controller relies on the knowledge of the system state $x(t)$ at time $\tau k$, which cannot be observed directly. To address this, we consider training a neural network to obtain an estimate $\hat{p}$ of the perception map $p$ (as in Assumption 4) which gives estimates of state $x(t)$ from the perceptual data $\zeta(t)$.

Towards this, we consider a set of training points $\{x^{(i)},\zeta^{(i)}=q(x^{(i)})\}_{i=1}^{N}$ to guarantee that the network training is well-posed as stated next.

Hereafter, let $\mathcal{Q}:=q(\mathcal{X}_{\text{tr}})$ denote the perception set associated with the training set, which is a compact set. Assumption 6 allows us to leverage existing results on the bounds on the approximation error $\sup_{\zeta\in\mathcal{Q}}\|\hat{p}(\zeta)-p(\zeta)\|$ of feedforward neural networks and residual neural networks over the compact set $\mathcal{Q}$; see Hornik et al. (1989) and Marchi et al. (2022); Tabuada and Gharesifard (2020).

With an estimate $\hat{p}$ of the perception map $p$ obtained via a neural network, the proposed perception-based controller is shown in Figure 1 and is tabulated in Algorithm 1.

In the training phase, the operation $\operatorname{NN-learning}(\cdot)$ refers to a generic training procedure for the neural network via empirical risk minimization, which results in the approximate map $\hat{p}(\cdot)$. In the proposed controller, $\hat{p}(\cdot)$ is utilized to obtain estimates of the state of the dynamical system $\hat{x}_{k}=\hat{p}(\zeta_{k})$, which is subsequently utilized to compute the gradient map $\Psi_{k}(u,\hat{p}(\zeta))$. Note that, as in sampled-data systems, the input $u(t)$ is computed based on the control iterates $\{u_{k}\}$ as the piece-wise constant signal $u(t)=u_{k},t\in[k\tau,(k+1)\tau)$, $k\in\mathbb{Z}_{+}$.

To analyze the performance of the closed-loop system (5), recall that $(u^{*}_{k},x^{*}_{k})$ is the sequence of optimizers of the time-varying problem (2) at the times in $\mathbb{S}$. As proposed in Belgioioso et al. (2021), we consider a discrete-time counterpart of (5); sampling (5) at times in $\mathbb{S}$ yields:

where $F(X_{0},v,w)$ denotes the solution of the initial value problem $\dot{X}(t)=f(X(t),v,w(t))$, with $X(t_{0})=X_{0}\in\mathcal{X}_{r}$ at time $t=t_{0}+\tau$, where $\tau$ is the sampling period. The tracking results for (6) will then translate into transient bounds for the sampled-data system (5) by using (Nešić et al., 1999, Theorem 5).

Let $z_{k}:=(x_{k}-x^{*}_{k},u_{k}-u^{*}_{k})$ and define the matrix $M_{1}\in\mathbb{R}^{2\times 2}$ as,

where $c_{w}:=e^{-d_{3}\tau/2}\sqrt{d_{2}/d_{1}}$, $d_{1}>0$ is given in Assumption 3, and $c_{P}:=\sqrt{1-\eta(2\mu-\eta\ell^{2})}$; we further define:

With these definitions, our main result provides transient bounds for the system (6).

Theorem 1 guarantees exponential convergence of the sampled trajectory of the tracking error to a neighborhood of zero. The size of the neighborhood depends on: $\sup_{\zeta\in\mathcal{Q}}\|\hat{p}(\zeta)-p(\zeta)\|+\bar{\varepsilon}$, which corresponds to the error associated with the state estimation, $\sup_{1\leq s\leq k}\|u_{s}^{*}-u_{s-1}^{*}\|$, which captures the time-variability of the optimizer, and $\sigma_{w}(\sup_{0\leq s\leq\tau k}\|\dot{w}(s)\|)$ corresponds to the time-variability of the unknown disturbance. The proof is provided in the extended version of the paper Cothren et al. (2022b).

The transient bound (11) shows that the sampled system (6) is input-to-state stable (ISS), in the sense of Jiang and Wang (2001), with respect to $\|\dot{w}\|$, the drift on the optimal solution $\|u_{k}^{*}-u_{k-1}^{*}\|$, and the error introduced by the estimated perception map. To translate the results of Theorem 1 into transient bounds for the continuous system (5), we leverage Theorem 5 of Nešić et al. (1999). In particular, let $z(t):=(x(t)-x^{*}(t),u_{k}-u^{*}(t))$, where $u^{*}(t)$ is piece-wise constant and such that $u^{*}(t)=u^{*}_{k}$ for $t\in[k\tau,(k+1)\tau)$, and $x^{*}(t)$ is defined similarly. Then, we obtain the following.

Mirroring (11), the bound (12) shows that the system (5) is ISS with respect to $\|\dot{w}\|$, the drift on the optimal solution $\Delta_{u}^{*}$, and the error introduced by the neural network.

To provide a connection with the existing literature, we point out that (12) generalizes the following sub-cases: (i) when the state $x(t)$ can be observed (without errors), then (12) reduces to a bound similar to Bianchin et al. (2021) (where, however, the controller is a continuous-time gradient flow); (ii) when the state $x(t)$ can be observed and the functions $\phi(u)$ and $\psi(x)$ are time invariant, then (12) boils down to the ISS result of Belgioioso et al. (2021) (for sampled-data controllers) and Colombino et al. (2020) (for continuous-time gradient flows) and Zheng et al. (2020).

Theorem 2 follows from the results of Theorem 1 and Theorem 5 of Nešić et al. (1999) by noticing that (5) is uniformly bounded over an interval $\tau$ in the sense of (Nešić et al., 1999, Definition 2). This is because $u(t)$ is piece-wise constant and takes values from the compact set $\mathcal{U}_{c}$, $\mathcal{W}_{c}$ is compact (and $\|\dot{w}\|$ is bounded), and the perception error is bounded; boundedness of $x(t)$ w.r.t. the sampled sequence $\{x_{k}\}$ follows from (Belgioioso et al., 2021, Lemma 1). The proof is omitted due to space limitations.

We utilize our controller in Algorithm 1 to control a vehicle to track a set of reference points while avoiding obstacles; the position of the vehicle is accessible only through camera images. The vehicle’s movement is modeled by the unicycle dynamics with state $x=(a,b,\theta)$, where $r:=(a,b)\in\mathbb{R}^{2}$ is the position in the 2D plane and $\theta\in(-\pi,\pi]$ is the orientation with respect to the $a$-axis: $\dot{a}=v\cos(\theta)$, $\dot{b}=v\sin(\theta),\,\,\dot{\theta}=w$, where $v,w\in\mathbb{R}$ are controllable inputs. Importantly, we assume that we do not have direct knowledge of the state $x=(a,b,\theta)$, but instead, camera images, represented by the generative map $\zeta=q(x)$, which return the position $r=(a,b)$. We consider a lower-level stabilizer to stabilize the unicycle dynamics to satisfy Assumption 3. Following (Cothren et al., 2022a, Lemma 2), let $u=(u_{a},u_{b})$ denote the control inputs for each direction $a$ and $b$, and consider the change of coordinates to the error variables, $\xi:=\|u-x\|$, $\phi:=\operatorname{atan}\left(\frac{u_{b}-b}{u_{a}-a}\right)-\theta$. The dynamics of these are $\dot{\xi}=-v\cos(\phi)$ and $\dot{\phi}=\frac{v}{\xi}\sin(\phi)-w$. By setting

for some $\kappa>0$, the unicycle dynamics admit a globally exponentially stable equilibrium point $(\xi,\phi)=(0,0)$. By setting the constants $v,w\in\mathbb{R}$ as in (13), the dynamic plant satisfies Assumption 3.

We consider a sequence of locations that the vehicle would like to follow, denoted as $\mathcal{T}_{d}:=\{r_{d,k}\in\mathcal{R},k\in\mathbb{Z}_{+}\}$, where $\mathcal{R}:=\{r=(a,b):x=(a,b,\theta)\in\mathcal{X}\}$. To avoid obstacles around the vehicle, we consider building at each time $\tau k$, $k\in\mathbb{Z}_{+}$, the free workspace of the vehicle defined as $\mathcal{F}_{k}(r_{k}):=\{r:a_{k}^{(i)}(r_{k})^{\top}r-b^{(i)}(r_{k})\geq 0,i=1,\dots,M_{k}\}$, where $M_{k}$ is the number of obstacles at time $\tau k$, $r^{(i)}$ is the center position of the $i$th obstacle, $a_{k}^{(i)}(r_{k})=r^{(i)}-r_{k}$, $b^{(i)}(r_{k})$ is a scalar computed depending on vehicle and obstacles positions, and $r_{k}\in\mathcal{R}$ is the position at time $\tau k$. The free workspace $\mathcal{F}(r_{k})$ describes a local neighborhood of the vehicle that is guaranteed to be free of obstacles.

To track the desired trajectory $\mathcal{T}_{d}$ while avoiding obstacles, we utilize the following waypoint-tracking formulation with a barrier function

where $\lambda_{k}>0$ is a tuning parameter; in particular, in the simulations we set $\lambda_{k}=1/\exp(0.1k)$. We set $\phi_{k}(u)=0$.

We used a residual neural network to estimate the position of the vehicle from aerial images. specifically, the network returns estimated coordinates in the $(a,b)$. For training, we generate $94,500$ images of size $64\times 64$ pixels depicting a red bot in the roundabout setting in Figure 2. The images were built using the MATLAB Image Processing Toolbox and basic plotting functions therein by setting the background as an aerial view of a roundabout and plotting a red square for the vehicle. We used the resnet50 structure given in the MATLAB Deep Learning Toolbox and tailored the input ($64\times 64\times 3$ sized RGB images) and output (total number of labels) sizes to our specific case. For labels, we selected the pixels that corresponded to parts of the image containing the road so that the network only trains on data corresponding to allowable surfaces for the robot, which totaled to $135$ unique labels. Finally, we select five checkpoints along the road for the robot to follow (denoted by the red stars in Figure 2) during the execution of the algorithm corresponding to $\{r_{d,k}\}$.

Simulation results are given in Fig. 2 for the Algorithm 1. The dashed magenta line corresponds to the trajectory of the unicycle for the neural network-assisted controller, and the yellow line is for the controller with perfect state information. Fig. 3 shows the error $\|r(t)-r^{*}(t)\|$ between the optimal and actual position of the vehicle. Importantly, the trajectory tracks the time-varying reference points within an error and avoids the obstacles.

We proposed an algorithm to regulate dynamical systems towards the solution of a convex optimization problem when we do not have full knowledge of the system states. Specifically, we developed a sampled-data feedback controller that is augmented with a neural network to estimate the state of the system from high-dimensional sensory data. Our results guaranteed exponential convergence of the interconnected system up to an error term dependent on the temporal variability of the problem and the error due to estimating the state from the neural network.