A Feedback Linearized Model Predictive Control Strategy for Input-Constrained Self-Driving Cars

Abstract

This paper proposes a novel real-time affordable solution to the trajectory tracking control problem for self-driving cars subject to longitudinal and steering angular velocity constraints. To this end, we develop a dual-mode Model Predictive Control (MPC) solution starting from an input-output feedback linearized description of the vehicle kinematics. First, we derive the state-dependent input constraints acting on the linearized model and characterize their worst-case time-invariant inner approximation. Then, a dual-mode MPC is derived to be real-time affordable and ensuring, by design, constraints fulfillment, recursive feasibility, and uniformly ultimate boundedness of the tracking error in an ad-hoc built robust control invariant region. The approach’s effectiveness and performance are experimentally validated via laboratory experiments on a Quanser Qcar. The obtained results show that the proposed solution is computationally affordable and with tracking capabilities that outperform two alternative control schemes.

I INTRODUCTION

The advent of self-driving cars marks a significant step forward in automotive technology, heralding a new era of road safety, traffic efficiency, and environmental sustainability. Central to the autonomous vehicle’s operational integrity is trajectory tracking control, a critical aspect for ensuring the safety, reliability, and comfort of these vehicles [1, 2]. The term reference trajectory refers to a sequence of consecutive waypoints, with associated spatial and temporal information, that the vehicle is required to accurately track [3]. Achieving reliable trajectory tracking enables autonomous vehicles to navigate complex environments with precision, adaptability, and safety, thus accelerating the integration of autonomous technology into everyday transportation systems [4].

Extensive research has been conducted to develop control strategies to solve the trajectory tracking problem for autonomous cars [5], ranging from simple non-model based solutions like the well-established PID controllers [6, 7], to more sophisticated nonlinear control solutions like sliding-mode controllers [8, 9], and adaptive backstep** control [10]. In the last decade, deep learning-based approaches have been applied to the control of autonomous vehicles due to their ability to self-optimize their behavior from data and adapt to complex and dynamic environments. [11, 12] offer an exhaustive review of the most recent developments in the application of machine learning techniques to autonomous vehicle control. Despite their relatively high tracking performance, the biggest challenge pertaining to this class of algorithms remains their dependence on large, annotated datasets for training, which can be expensive and time-consuming to collect and maintain. Additionally, machine learning models often act as “black boxes,” offering limited interpretability regarding how decisions are made, which raises concerns about accountability and safety in critical applications like autonomous driving. One common drawback of the above-discussed tracking strategies is their incapability to address input constraints, i.e., physical limitations of the computed control signal, which may lead to a lack of close-loop stability guarantees [13].

On the other hand, Model Predictive Control (MPC) has emerged as a premier control strategy in this domain, owing to its ability to anticipate the future behavior of the vehicle and handle multiple constraints simultaneously [14, 2]. In recent years, the application of MPC in autonomous vehicles has been extensively studied, highlighting its potential in managing the complex dynamics and uncertainties inherent to vehicular control [2]. Notable works [15, 16] have underscored the efficacy of MPC in navigating autonomous vehicles through dynamic environments. Despite these advancements, the deployment of MPC in real-world scenarios faces significant difficulties. Nonlinear MPC approaches [17, 18], while robust, are computationally demanding, posing challenges to their real-time implementation [19, 20]. Moreover, given the nonconvex nature of the MPC optimization, the solver algorithms may be characterized by uncertain convergence and suboptimality [19, 21]. Conversely, linear MPC techniques [22, 23, 20] offer computational efficiency but at the expense of model fidelity. As a matter of fact, solutions that exploit error dynamics linearized around the reference trajectory have been found to be suboptimal [24].

The proposed literature review highlights the need for an MPC framework that combines the real-time operational feasibility of linear formulations with the precision of nonlinear methods. Existing linear MPC strategies often resort to simplified models, sacrificing accuracy for computational speed. Meanwhile, the precision of nonlinear MPC comes at the cost of computational feasibility, limiting its practical application in autonomous vehicles. A possible way to mitigate the computational burdens of nonlinear approaches while preserving the accuracy of their model prediction is to resort to Feedback Linearization, a well-established technique to recast a nonlinear system into an equivalent linear one. One of the first attempts to feedback-linearize the car-bicycle kinematics can be found in [25], where both input-output FL and dynamic FL have been proposed. However, it can be shown that if such a linearized model is exploited for predictions, even simple box-like input constraints recast into state-dependent constraints leading to nonconvex MPC formulations [26, 27, 28]. Although the combination of MPC and FL is not a new approach, the literature lacks FL-based MPC solutions for constrained systems. Recently, in [29], such an idea has been used to solve a tracking problem for differential-drive robots. However, to the best of the authors knowledge, there are no existing FL-based MPC solutions for input-constrained autonomous cars.

I-A Paper’s Contribution and Organization

In this paper, we address the above-identified gap by designing a novel dual-mode FL-based MPC strategy for self-driving cars subject to longitudinal and steering angular velocity constraints. The contributions of the paper can be summarized as follows:

• it formally characterizes the car input-output linearized tracking error dynamics and its time-varying and state-dependent input constraint set. Moreover, a worst-case time-invariant approximation of the input constraints is analytically defined;

• it provides a design procedure to compute a stabilizing controller and associated control invariant region for the feedback linearized vehicle model; such a solution generalizes the approach presented in [28].

• it proposes a dual-mode linearized MPC scheme ensuring stable full-state tracking and input constraints fulfillment. Differently from existing nonlinear MPC formulations for car-like vehicles, the proposed approach requires the solution of a Quadratic Programming (QP) problem. On the other hand, contrary to linear MPC formulations, the proposed strategy does not introduce approximated model predictions along the used prediction horizon.

• it experimentally validates the proposed dual-mode MPC using laboratory experiments with a Quanser QCar and performance comparisons with other two competitor schemes. The developed code is available at the following GitHub repository https://github.com/PreCyseGroup/Feedback-Linearized-MPC-for-self-driving-cars.

The remainder of the paper is organized as follows. Section II collects some preliminary definitions from control invariance theory and car’s kinematic modeling. Moreover, it formally states the considered trajectory tracking problem. Section III describes the proposed dual-mode MPC strategies, with a formal proof of the obtained theoretical results. Section IV describes the experimental testbed, the performed experiments, and the obtained results. Finally, Section V concludes the paper with some final remarks.

II Preliminaries and Problem Formulation

Given a matrix $M$ and a vector $v,$ $M{[i,:]}$ denotes the $i-th$ row of $M,$ $M{[i,j]}$ the $(i,j)$ entry of $M,$ and $v{[i]}$ the $i-th$ element of $v.$ A continuous-time function $f(t)$ is said of class $\mathcal{C}^{3}$ if $f(t)$ admits three continuous derivatives in its domain. Given $n>$ scalars $m_{i}\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits$, $i=1,\dots,n,$ $M=diag(\left[m_{1},\dots,m_{n}\right])\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{n\times n}$ defines a diagonal matrix with elements $m_{i}$ on the main diagonal. Given a variable $v,$ $v(k)$ denotes the values of $v$ at the discrete sampling time instant $k\in\mathop{{\rm Z}\mskip-7.0mu{\rm Z}}\nolimits_{+}:=\{0,1,\ldots\}.$ Given a discrete-time index $i\in\mathop{{\rm Z}\mskip-7.0mu{\rm Z}}\nolimits_{+}$ and the signal $v(k),$ $v(k+i|k)$ denotes the $i$-steps ahead prediction of $v$ from the time instant $k.$

Definition 1.

Consider a dynamical system $z(k+1)=f(z(k),u(k))$ subject to the input constraints $u(k)\in\mathcal{U},\,\forall k\geq 0.$ A set $\Sigma$ is said to be control invariant if [30]:

$$\forall z(t)\in\Sigma,\,\exists u(k)\in\mathcal{U}\,:\,f(z(k),u(k))\in\Sigma,\,\forall k\geq 0$$

Definition 2.

Consider a dynamical system $z(k+1)=f_{a}(z(k),w(k))$ subject to bounded disturbance $w(k)\in\mathcal{W},\,\forall\,k\geq 0.$ A set $\Sigma$ is said Robust Positive Invariant (RPI) if [30]:

$$\forall z(0)\in\Sigma,\,z(k)\in\Sigma,\,\forall w\in\mathcal{W},\,\forall k>0$$

Definition 3.

Consider a set $\mathcal{Q}$ neighborhood of the origin. The dynamical system $z(k+1)=f_{a}(z(k),w(k))$ is said to be Uniformly Ultimately Bounded (UUB) in $\mathcal{Q}$ if [31] $\forall\mu>0,\,\exists T(\mu)>0$, such that $\forall\|z(0)\|\leq\mu,\,z(k)\in\mathcal{Q},\,\forall w\in\mathcal{W},\,\forall k\geq T(\mu)$.

II-A Car-like vehicle modeling

Let’s consider a rear-driven car-like vehicle whose continuous-time kinematics is described by [25]:

Figure 1: Car-like vehicle

$$\dot{q}(t)\!=\!\left[\begin{array}[]{c}\dot{x}(t)\\ \dot{y}(t)\\ \dot{\theta}(t)\\ \dot{\varphi}(t)\end{array}\right]\!=\!\left[\begin{array}[]{c}\cos\theta(t)\\ \sin\theta(t)\\ \frac{1}{l}\tan(\varphi(t))\\ 0\end{array}\right]\!\!v(t)\!+\!\left[\begin{array}[]{c}0\\ 0\\ 0\\ 1\end{array}\right]\omega(t)$$ (1)

where $q=\left[x,y,\theta,\varphi\right]^{T}$ is the car’s state, i.e., the $x$ and $y$ are the Cartesian coordinates of the rear axis’ midpoint, the heading angle $\theta$ of the vehicle, and the steering angle $\varphi\in\left[-\overline{\varphi},\overline{\varphi}\right],\,0<\overline{\varphi}<\frac{\pi}{2}$, respectively. On the other hand, $u=\left[v,\omega\right]^{T}$ are the control inputs, i.e., the longitudinal velocity of the vehicle and the steering angular velocity respectively. Moreover, $l$ represents the car’s wheelbase, i.e, the distance in meters between the front and rear wheels, and

$$x_{f}=x+l\cos\theta\quad y_{f}=y+l\sin\theta$$

are the Cartesian coordinates of the front axis’ midpoint. We assume the car’s model to be subject to symmetrical box-like constraints representing the admissible longitudinal and steering angular velocities that the car can perform, i.e,

$$\begin{array}[]{c}u(t)\in\mathcal{U}_{car}:=\{u\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{2}:\,Tu\leq g\}\end{array}$$ (2)

where

$$T=\left[\begin{array}[]{rr}-1&0\\ 0&-1\\ 1&0\\ 0&1\\ \end{array}\right],\quad g=\left[\begin{array}[]{c}\overline{v}\\ \overline{\omega}\\ \overline{v}\\ \overline{\omega}\end{array}\right]$$

and $\overline{v},\overline{\omega}>0$ are given upper bounds.

By defining a sampling time $T_{s}>0$, and resorting to forward Euler discretization method, the following discrete-time kinematics is obtained:

$$\begin{cases}x(k+1)&=x(k)+T_{s}v(k)\cos(\theta(k))\\ y(k+1)&=y(k)+T_{s}v(k)\sin(\theta(k))\\ \theta(k+1)&=\theta(k)+T_{s}\frac{v(k)}{l}\tan(\varphi(k))\\ \varphi(k+1)&=\varphi(k)+T_{s}\omega(k)\end{cases}$$ (3)

In what follows, for the sake of simplicity, we refer to (3) using the following compact representation:

$$q(k+1)=f_{car}(q(k),u(k))$$

II-B Problem’s statement

Let’s consider a smooth reference trajectory described in terms of Cartesian positions $x_{r}(t)$, $y_{r}(t)$, velocities $\dot{x}_{r}(t)$, $\dot{y}_{r}(t)$ and accelerations $\ddot{x}_{r}(t)$, $\ddot{y}_{r}(t)$ and jerks $\dddot{x}_{r}(t)$, $\dddot{y}_{r}(t)$ of the rear axis center of the car. The corresponding reference car’s state is denoted as $q_{r}(t)=\left[x_{r}(t),y_{r}(t),\theta_{r}(t),\varphi_{r}(t)\right]^{T}$, where $\theta_{r}(t)$ and $\varphi_{r}(t)$ are the heading and steering angles associated to the given trajectory which can be computed as [25]:

$$\begin{array}[]{rcl}\theta_{r}(t)=&\text{ATAN}_{2}\left(\frac{\dot{y}_{r}(t)}{v_{r}(t)},\frac{\dot{x}_{r}(t)}{v_{r}(t)}\right)\\ \varphi_{r}(t)=&\arctan\left(\frac{l(\ddot{y}_{r}(t)\dot{x}_{r}(t)-\ddot{x}_{r}(t)\dot{y}_{r}(t))}{v_{r}(t)^{3}}\right)\end{array}$$ (4)

On the other hand, the reference inputs associated with the trajectory are given by $u_{r}(t)=\left[v_{r}(t),\,\omega_{r}(t)\right]^{T}$, where

$$\begin{array}[]{rcl}v_{r}(t)=&\sqrt{\dot{x}_{r}(t)^{2}+\dot{y_{r}}(t)^{2}}\\ \omega_{r}(t)=&lv_{r}\frac{\left(\dddot{y}_{r}\dot{x}_{r}-\dddot{x}_{r}\dot{y}_{r}\right)v_{r}^{2}-3\left(\ddot{y}_{r}\dot{x}_{r}-\ddot{x}_{r}\dot{y}_{r}\right)\left(\dot{x}_{r}\ddot{x}_{r}+\dot{y}_{r}\ddot{y}_{r}\right)}{v_{r}^{6}+l^{2}\left(\ddot{y}_{r}\dot{x}_{r}-\ddot{x}_{r}\dot{y}_{r}\right)^{2}}\\ \end{array}$$ (5)

Notice that the time dependency has been omitted on the right-hand side for compactness.

Assumption 1.

The reference trajectory $q_{r}(t)=\left[x_{r}(t),y_{r}(t),\theta_{r}(t),\varphi_{r}(t)\right]^{T}$ for (1) is uniformly bounded and smooth, i.e., $\exists\varGamma>0:\,\|q_{r}(t)\|<\varGamma,\,\forall t\geq 0,\,\,q_{r}(t)\in\mathcal{C}^{3}.$ $\Box$

Problem 1.

Design a constrained state feedback controller

$$u(t)=\phi(t,q(t),q_{r}(t),u_{r}(t))$$ (6)

such that $u(t)\in\mathcal{U}_{car},\forall\,t\geq 0$ and stable full-state tracking is achieved, i.e.,

$$\exists\delta>0,\,t_{0}\geq 0\,\text{ s.t. }\|\tilde{q}(t_{0})\|<\delta\implies\|\tilde{q}(t)\|<\varepsilon,\,\forall t\geq t_{0}$$

where $\tilde{q}(t)=q(t)-q_{r}(t)$ is the tracking error.

III Proposed Solution

In this section, the considered problem is addressed by combining feedback-linearization and MPC arguments. First, the control problem is described as a standard nonlinear MPC over a finite prediction horizon. Then, the nonconvex nature of the underlying MPC optimization is analyzed, and a novel predictive framework based on feedback linearization is proposed to recover a convex optimization problem that fulfills constraints while guaranteeing a bounded tracking error.

III-A Nonlinear MPC

Let’s define $\tilde{u}=u-u_{r}$ and a Linear-Quadratic (LQ) cost

$$\begin{array}[]{lcr}J_{N}(k,\tilde{q}(k),\tilde{u}(k))=\displaystyle\sum_{i=0}^{N-1}&\!\!\!\!\!\!\tilde{q}(k+i+1|k)^{T}Q\tilde{q}(k+i+1|k)+\\ &+\tilde{u}(k+i|k)^{T}R\tilde{u}(k+i|k)\end{array}$$

where $N>0$ is the prediction horizon, and $Q=Q^{T}\geq 0,\,Q\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{n}$, $R=R^{T}>0,\,R\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{m}$ are weighting matrices for the state and control input tracking errors, respectively. Then, the optimal control law that minimizes the defined cost function over the prediction horizon $N$ can be computed as:

$$u(k)=\displaystyle\operatorname*{arg\,min}_{u(k),\dots,u(k+N-1)}J_{N}(k,\tilde{q}(k),\tilde{u}(k))\,\,s.t.$$ (7a)

$$q(k+i+1|k)=f_{car}(q(k+i|k),u(k+i))$$ (7b)

$$u(k+i)\in\mathcal{U}_{car}$$ (7c)

$$\tilde{q}(k+N|k)\in\mathcal{Q}_{N}$$ (7d)

$$\quad i=0,1\dots N-1$$

where $\mathcal{Q}_{N}$ is a predefined set, PI with respect to an offline-designed feedback terminal control law $u_{N}(k)=\phi_{N}(k,q(k),q_{r}(k),u_{r}(k))\in\mathcal{U}_{car},\,\forall k\geq N$. The above is known as dual-mode MPC, i.e., for the first $N$ steps, the control law is obtained by solving the above optimization and applying the optimal solution in a receding horizon fashion, i.e., only the first sample $u(k)$ is applied to the system (1) and the optimization is solved at any sampling time. Then, once the error trajectory $\tilde{q}(k)$ reaches $\mathcal{Q}_{N}$, the control law $u_{N}(k)$ associated to $\mathcal{Q}_{N}$ is used.

Remark 1.

The above dual-mode MPC strategy guarantees stability and input constraint fulfillment, for any initial condition $\tilde{q}(0)$ such that the optimization problem (7) is feasible [32]. Consequently, under the effect of the dual-mode MPC control law, the tracking error is bounded with respect to any trajectory complying with Assumption 1.

Remark 2.

Although appealing, optimization (7) suffers from the following drawbacks:

• The optimization problem is highly nonconvex due to the presence of the constraints (7b) and (7d). Moreover, the obtained solutions may suffer from local minima problems [21];

• The computational burden associate to (7), especially for large prediction horizon $N$, may not allow the real-time implementation of the control scheme;

• The computation of $\mathcal{Q}_{N}$ and associated state-feedback controller $u_{N}$ is not trivial for the nonlinear vehicle kinematic model (1).

Motivated by the above drawbacks, in what follows a novel MPC formulation based on feedback linearization arguments is proposed. In particular, first, the input-output linearization proposed in [25] is exploited to obtain a linear description of the car’s kinematics. Then, inspired by the idea introduced in [28], the time-varying input constraints acting on the linearized model and their worst-case realization are analytically characterized. Finally, the obtained constrained model and worst-case arguments are used to design a tracking control strategy that ensures stability, recursive feasibility, and input constraint fulfillment.

III-B Input-Output Feedback Linearization

Here, the input-output feedback-linearization introduced in [25] is used to obtain a linearized description of the car’s kinematic model.

Let’s define two new outputs

$$z=\left[\begin{array}[]{c}z_{1}\\ z_{2}\end{array}\right]=\left[\begin{array}[]{c}x+l\cos(\theta)+\Delta\cos(\theta+\varphi)\\ y+l\sin(\theta)+\Delta\sin(\theta+\varphi)\end{array}\right]$$ (8)

representing the Cartesian position of a point $P$ at a distance $\Delta>0$ from the center of front wheels’ axis, and a new state vector $\eta=\left[\eta_{1},\,\eta_{2}\right]^{T}=\left[\theta,\,\varphi\right]^{T}$. Then, by resorting to the following input transformation depending on $\eta$:

$$\begin{array}[]{c}w=M(\eta)u,\\ M(\eta)=\left[\!\!\begin{array}[]{cc}\cos(\eta_{1})-\tan(\eta_{2})(\sin(\eta_{1})+\frac{\Delta}{l}s_{1})&-\Delta s_{1}\\ \sin(\eta_{1})+\tan(\eta_{2})(\cos(\eta_{1})+\frac{\Delta}{l}c_{1})&\Delta c_{1}\end{array}\!\!\right]\end{array}$$ (9)

where $s_{1}=\sin(\eta_{1}+\eta_{2})\quad c_{1}=\cos(\eta_{1}+\eta_{2}),$ the kinematic model (1) is recast into

$$\dot{z}=w$$ (10a)

$$\dot{\eta}=O(\eta)w$$ (10b)

where

$$O(\eta)=\left[\begin{array}[]{cc}\frac{\sin(\eta_{2})c_{1}}{l}&\frac{\sin(\eta_{2})s_{1}}{l}\\ \frac{-\sin(\eta_{2})c_{1}}{l}-\frac{s_{1}}{\Delta}&\frac{-\sin(\eta_{2})s_{1}}{l}+\frac{c_{1}}{\Delta}\end{array}\right]$$

Notice that (10a) defines a two-single-integrator model subject to a decoupled nonlinear internal dynamics (10b). The above decoupled system can be discretized via forward Euler discretization method, obtaining:

$$z(k+1)=Az(k)+Bw(k),\quad A=I_{2\times 2},\quad B=T_{s}I_{2\times 2}$$ (11a)

$$\eta(k+1)=\eta(k)+T_{s}O(\eta(k))w(k)$$ (11b)

Property 1.

The input output feedback-linearization (8)-(10) and forward Euler discretization method commute for the car’s kinematic model (1).

Proof.

Let’s consider the output transformation (8) and its first-order derivative

$$\begin{array}[]{rcl}\dot{z}_{1}&=&\dot{x}-l\sin(\theta)\dot{\theta}-\Delta\sin(\theta+\varphi)(\dot{\theta}+\dot{\varphi})\\ \dot{z}_{2}&=&\dot{y}+l\cos(\theta)\dot{\theta}+\Delta\cos(\theta+\varphi)(\dot{\theta}+\dot{\varphi})\end{array}$$

Then, under forward Euler discretization arguments one obtains

$${\scriptsize\begin{split}\frac{z_{1}(k+1)-z_{1}(k)}{T_{s}}&=\frac{x(k+1)-x(k)}{T_{s}}-l\sin(\theta)\frac{\theta(k+1)-\theta(k)}{T_{s}}-\\ &-\Delta\sin(\theta+\varphi)\left[\frac{\theta(k+1)-\theta(k)}{T_{s}}+\frac{\varphi(k+1)-\varphi(k)}{T_{s}}\right]\\ \frac{z_{2}(k+1)-z_{2}(k)}{T_{s}}&=\frac{y(k+1)-y(k)}{T_{s}}+l\cos(\theta)\frac{\theta(k+1)-\theta(k)}{T_{s}}+\\ &+\Delta\cos(\theta+\varphi)\left[\frac{\theta(k+1)-\theta(k)}{T_{s}}+\frac{\varphi(k+1)-\varphi(k)}{T_{s}}\right]\end{split}}$$ (12)

By substituting $x(k+1)$, $y(k+1)$, $\theta(k+1).$ $\varphi(k+1)$ with the right-hand sides of (3) and rewriting the equation in a compact form, the resulting discrete-time evolution of $z_{1}$ and $z_{2}$ is:

$$z(k+1)=z(k)+T_{s}M(\eta(k))u(k)$$ (13)

Finally, by using the input transformation (9), the discrete-time feedback linearized system (11) is obtained, which is equal to the discrete-time system obtained by discretization of (10a). Similarly, the nonlinear internal dynamics (10b) can be discretized obtaining (11b). Hence, input-output linearization and discretization commute. ∎

III-C Tracking Error Model and Input Constraint Characterization

Here, the feedback-linearized tracking error model is formally derived. By applying the output transformation (8), the reference output for the input-output linearized system (10) is given by

$$z_{r}=\left[\begin{array}[]{c}x_{r}+lcos(\theta_{r})+\Delta\cos(\theta_{r}+\varphi_{r})\\ y_{r}+lsin(\theta_{r})+\Delta\sin(\theta_{r}+\varphi_{r})\end{array}\right]$$ (14)

Similarly, reference inputs for (10) can be computed via (9), obtaining

$$w_{r}(t)=M(\theta_{r}(t),\varphi_{r}(t))u_{r}(t)$$ (15)

By defining the error vectors $\tilde{z}=z-z_{r}$ and $\tilde{w}=w-w_{r}$, $\tilde{\eta}=\eta-\eta_{r},\,\eta_{r}=\left[\theta_{r},\phi_{r}\right]^{T}$, the input-output linearized and internal tracking error dynamics are given by:

$$\dot{\tilde{z}}(t)=\tilde{w}(t)$$ (16a)

$$\dot{\tilde{\eta}}(t)=\kappa(\tilde{\eta},\tilde{w},\eta_{r},w_{r},t)=O(\eta(t))w(t)-O(\eta_{r}(t))w_{r}(t)$$ (16b)

which can be discretized by resorting to the Euler forward method and re-written as

$$\tilde{z}(k+1)=A\tilde{z}(k)+Bw(k)-Bw_{r}(k),$$ (17a)

$$A=I_{2\times 2},\quad B=T_{s}I_{2\times 2}$$ (17b)

$$\tilde{\eta}(k+1)=\tilde{\eta}(k)+T_{s}\kappa(\tilde{\eta}(k),\tilde{w}(k),\eta_{r}(k),w_{r}(k),k)$$ (17c)

Remark 3.

Since the reference trajectory is assumed to be bounded, then also $w_{r}(k)$ is bounded and the set of admissible $w_{r}(k)$ can be over-approximated by a ball $\mathcal{W}_{r}\subset\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{2}$ of radius $r_{d},$ i.e.,

$$w_{r}\in\mathcal{W}_{r}=\{w_{r}\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{2}:w_{r}^{T}W_{r}^{-1}w_{r}\leq 1\},\,\,W_{r}=r_{d}^{2}I_{2\times 2}$$ (18)

Given the feedback linearized tracking error dynamics, the following lemma establishes sufficient conditions for bounded internal dynamics.

Lemma 1.

If the reference trajectory $q_{r}(t)$ complies with Assumption 1, $v_{r}(t)$ and $\omega_{r}(t)$ satisfies (5) and $0<v_{r}(t)\leq V>0,\forall t$ and $\forall|\varphi_{r}(t)|\leq\frac{\pi}{2},\,\forall t,$ then the tracking-error zero dynamics $\dot{\tilde{\eta}}=\kappa(\eta,0,\eta_{r},w_{r},t)$ are asymptotically stable [33, Theorems 1-3]. Consequently, if (16a) is stable, stable full-state tracking is achieved [33]. $\Box$

By applying the transformation (9) to the input constraints (2), the tracking-error dynamics (17) are subject to the following time-varying polyhedral input constraints, depending on the internal dynamics state $\eta$ i.e.,

$$w\!\in\!\mathcal{U}(\eta)\!=\!\{w\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{2}\!:L(\eta)w\leq g\},\,L(\eta)=TM^{-1}(\eta)$$ (19)

The following lemma analytically characterizes the polyhedral set $\mathcal{U}(\eta),$ which rotates and resizes in function of $\eta.$

Figure 2: Time-varying input constraint set and its worst-case approximation

Lemma 2.

The polyhedral input constraint set (19) is a time-varying parallelogram that admits the following worst-case circular inner approximation (see Fig. 2):

$$\begin{array}[]{lcr}\hat{\mathcal{U}}=\displaystyle\bigcap_{\forall\eta}\mathcal{U}(\eta)=\{w\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{2}|w^{T}w\leq\hat{r}^{2}\},\\ \hat{r}=\min\left(\frac{\Delta l\overline{\omega}}{\sqrt{\Delta^{2}+l^{2}}},\overline{v}\right)\end{array}$$ (20)

Figure 3: Possible side length configurations for $\mathcal{U}(\eta)$

Proof.

By defining $s_{2}=\sin(\eta_{1}+2\eta_{2})$ and $c_{2}=\cos(\eta_{1}+2\eta_{2}),$ the sha** matrix of the polyhedral set $\mathcal{U}(\eta)$ can be re-written as:

$$\begin{array}[]{c}L(\eta)\!=\!\!\left[\!\!\begin{array}[]{cc}-\frac{\cos(\eta_{1})+c_{2}}{2}&-\frac{\sin(\eta_{1})+s_{1}}{2}\\ \frac{\Delta s_{1}-\Delta\sin(\eta_{1})+2ls_{2}}{2\Delta l}&\frac{-\Delta c_{2}+\Delta\cos(\eta_{1})-2lc_{1}}{2\Delta l}\\ \frac{\cos(\eta_{1})+c_{2}}{2}&\scriptstyle\frac{\sin(\eta_{1})+s_{1}}{2}\\ -\frac{\Delta s_{1}-\Delta\sin(\eta_{1})+2ls_{2}}{2\Delta l}&-\frac{-\Delta c_{2}+\Delta\cos(\eta_{1})-2lc_{1}}{2\Delta l}\end{array}\!\!\right]\end{array}$$

By intersecting the four hyperplanes, the four vertices have the following analytical expression:

$$V_{1}(\eta)=\left[\begin{array}[]{c}\frac{\Delta s_{1}l\overline{\omega}+\Delta\sin(\eta_{1})l\overline{\omega}+\Delta\overline{v}\cos(\eta_{1})-\Delta\overline{v}c_{2}-2c_{1}l\overline{v}}{(2l\cos(\eta_{2}))}\\ \frac{-\overline{\omega}\Delta l\cos(\eta_{1})-\overline{\omega}\Delta lc_{2}-\Delta s_{1}\overline{v}+\Delta\sin(\eta_{1})\overline{v}-2s_{2}l\overline{v}}{2l\cos(\eta_{2})}\end{array}\right]$$ (21c)

$$V_{2}(\eta)=\left[\begin{array}[]{c}\frac{-\Delta s_{1}l\overline{\omega}-\Delta\sin(\eta_{1})l\overline{\omega}+\Delta\overline{v}\cos(\eta_{1})-\Delta\overline{v}c_{2}-2c_{1}l\overline{v}}{2l\cos(\eta_{2})}\\ \frac{\overline{\omega}\Delta l\cos(\eta_{1})+\overline{\omega}\Delta lc_{2}-\Delta s_{1}\overline{v}+\Delta\sin(\eta_{1})\overline{v}-2s_{2}l\overline{v}}{2l\cos(\eta_{2})}\end{array}\right]$$ (21f)

$$V_{1}(\eta)=-V_{3}(\eta),\quad V_{2}(\eta)=-V_{4}(\eta)$$ (21g)

By computing the Cartesian distances between the vertices, each side of the parallelogram has the following length:

$$\begin{array}[]{c}l_{1}=l_{3}=2\sqrt{\Delta^{2}\overline{\omega}^{2}}\\ l_{2}(\eta_{2})=l_{4}(\eta_{2})=2\sqrt{\frac{v_{1}^{2}(-\Delta^{2}\cos(2\eta_{2})+\Delta^{2}+2l^{2})}{l^{2}(\cos(2\eta_{2})+1)}}\end{array}$$ (22)

It can also be noted that the angular coefficients of the four lines $L_{1},\,L_{2},\,L_{3},\,L_{4}$ defining the polyhedron, namely $m_{1}(\eta_{1},\eta_{2}),\,m_{2}(\eta_{1},\eta_{2}),m_{3}(\eta_{1},\eta_{2}),\,m_{4}(\eta_{1},\eta_{2}),$ are such that $m_{1}(\eta_{1},\eta_{2})=m_{3}(\eta_{1},\eta_{2})$, and $m_{2}(\eta_{1},\eta_{2})=m_{4}(\eta_{1},\eta_{2})$, $\forall\eta_{1}\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits,\eta_{2}\in\left[-\overline{\varphi},\overline{\varphi}\right]$. Consequently, $\mathcal{U}(\theta)$ is a time-varying parallelogram whose side lengths depend on the state variable $\eta_{2}$, and on the car’s parameters $\Delta,\,l,\,\overline{v},\,\overline{\omega}$.

In order to find the radius of the smallest circle inscribed in the polyhedral set, we resort to geometric arguments. By referring to Fig. 3, two different cases must be considered (a) $l_{1},l_{3}<l_{2},l_{4}$ and (b) $l_{1},l_{3}>l_{2},l_{4}$. Depending on the specific case, the diameter of the inscribed circular set can be found either as the distance of vertex $V_{2}$ from the point $I_{1}$, or the distance of vertex $V_{1}$ from the point $I_{2}$. Note that $I_{1}$ is the intersection of the line $L_{2}$ and the orthogonal to $L_{2}$ crossing $V_{2}$ (case (a)), whereas $I_{2}$ is the intersection of the line $L_{3}$ and the orthogonal to $L_{3}$ crossing $V_{1}$ (case (b)).

Formally, the lines $L_{2}$ and $L_{3}$ crossing $(V_{1},\,V_{4})$ and $(V_{4},\,V_{3})$, respectively are

$$\begin{array}[]{c}L_{2}:\quad w_{2}=m_{2}(\eta_{1},\eta_{2})w_{1}+h_{2}(\eta_{1},\eta_{2})\\ m_{2}(\eta_{1},\eta_{2})=-\frac{L\left[2,1\right]}{L\left[2,2\right]},\quad h_{2}(\eta_{1},\eta_{2})=\frac{g\left[2\right]}{L\left[2,2\right]}\end{array}$$ (23)

$$\begin{array}[]{c}L_{3}:\quad w_{2}=m_{3}(\eta_{1},\eta_{2})w_{1}+h_{3}(\eta_{1},\eta_{2})\\ m_{3}(\eta_{1},\eta_{2})=-\frac{L\left[3,1\right]}{L\left[3,2\right]},\quad h_{3}(\eta_{1},\eta_{2})=\frac{g\left[3\right]}{L\left[3,2\right]}\end{array}$$ (24)

∎

Moreover, by resorting to simple geometric arguments, the equations of the lines $L_{h1}$ and $L_{h2}$ are:

$$L_{h1}:\quad w_{2}-V_{2}\left[2\right]=-\frac{1}{m_{2}(\eta_{1},\eta_{2})}(w_{1}-V_{2}\left[1\right])$$ (25)

$$L_{h2}:\quad w_{2}-V_{1}\left[2\right]=-\frac{1}{m_{3}(\eta_{1},\eta_{2})}(w_{1}-V_{1}\left[1\right])$$ (26)

Then, the points $I_{1}$ and $I_{2}$ can be computed intersecting $L_{2}$ with $L_{h1}$ and $L_{3}$ with $L_{h2}$, obtaining

$$I_{1}=\left[\begin{array}[]{cc}-m_{2}(\eta_{1},\eta_{2})&1\\ \frac{1}{m_{2}(\eta_{1},\eta_{2})}&1\end{array}\right]^{-1}\left[\begin{array}[]{c}h_{2}(\eta_{1},\eta_{2})\\ \frac{V_{2}\left[1\right]}{m_{2}(\eta_{1},\eta_{2})}+V_{2}\left[2\right]\end{array}\right]$$ (27)

$$I_{2}=\left[\begin{array}[]{cc}-m_{3}(\eta_{1},\eta_{2})&1\\ \frac{1}{m_{3}(\eta_{1},\eta_{2})}&1\end{array}\right]^{-1}\left[\begin{array}[]{c}h_{3}(\eta_{1},\eta_{2})\\ \frac{V_{1}\left[1\right]}{m_{3}(\eta_{1},\eta_{2})}+V_{1}\left[2\right]\end{array}\right]$$ (28)

By noticing that the inscribed circles have diameters equal to $d_{1}=\overline{V_{2}I_{1}}$ (case (a)) and $d_{2}=\overline{V_{1}I_{2}}$ (case (b)), the radii, namely $r_{1}(\eta_{2})$ and $r_{2}(\eta_{2}),$ are

$$\begin{array}[]{c}r_{1}(\eta_{2})=\frac{1}{2}d=\frac{1}{2}\sqrt{(I_{1}-V_{2})^{T}(I_{1}-V_{2})}=\\ \vspace{-0.3cm}\hfil\\ =\frac{\Delta l\overline{\omega}}{\sqrt{\Delta^{2}-\Delta^{2}\cos(\eta_{2})^{2}+l^{2}}}\end{array}$$ (29)

$$\begin{array}[]{c}r_{2}(\eta_{2})=\frac{1}{2}d=\frac{1}{2}\sqrt{(I_{2}-V_{1})^{T}(I_{2}-V_{1})}=\\ \vspace{-0.3cm}\hfil\\ =\sqrt{\frac{\overline{v}^{2}}{\cos(\eta_{2})^{2}}}\end{array}$$ (30)

Furthermore, since $\eta_{2}\in\left[-\overline{\eta_{2}},\overline{\eta_{2}}\right],\overline{\eta_{2}}<\frac{\pi}{2},$ the minimum value of $r_{1}(\eta_{2})$ and $r_{2}(\eta_{2})$ is obtained for $\eta_{2}=0,$ that it is equals to

$$r_{1}=r_{1}(0)=\frac{\Delta l\overline{\omega}}{\sqrt{\Delta^{2}+l^{2}}},\quad r_{2}=r_{2}(0)=\overline{v}$$

Consequently, (20) defines the worst-case circle inscribed in $\mathcal{U}(\eta),\forall\eta,$ concluding the proof.

III-D Robust Invariant Control Design

By using similar arguments to the ones exploited in [28], the linearized tracking error dynamics can be exploited to design a state feedback controller that fulfills the prescribed time-varying and state-dependent input constraints in a properly defined robust invariant region.

Proposition 1.

The circular set

$$\Sigma_{N}=\{\tilde{z}\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{2}|\tilde{z}^{T}S\tilde{z}\leq 1\},\quad S=\frac{1}{\hat{r}^{2}}K^{T}K$$ (31)

is RPI for (17a) under the state-feedback controller

$$w(k)=K\tilde{z}(k)+\hat{w}_{r}(k)$$ (32)

where $\hat{w}_{r}(k)$ is the optimal solution of the following Quadratic Programming (QP) problem:

$$\hat{w}_{r}(k)=\displaystyle\arg\min_{\hat{w}_{r}}\|\hat{w}_{r}-w_{r}(k)\|_{2}^{2}\quad s.t.$$ (33a)

$$K\tilde{z}(k)+\hat{w}_{r}\in\mathcal{U}(\eta)$$ (33b)

and $K$ is such that

$$\lambda^{-1}A_{cl}^{T}S^{-1}A_{cl}+(1-\lambda)^{-1}B^{T}W_{r_{d}}^{-1}B\leq S^{-1}.$$ (34)

where $A_{cl}=A-BK$, $\lambda=1-\sqrt{\xi}$ and $\xi$ is the only repeated eigenvalue of the matrix $G^{T}B^{T}W_{r_{d}}^{-1}BG$, with $G$ such that $G^{T}S^{-1}G=I_{2\times 2}.$

Proof.

For the disturbance-free model (i.e. obtained from (17a) when $w_{r}(k)=0,\forall\,k$), any stabilizing controller $w(k)=K\tilde{z}(k)$ fulfills the input constraint for any $\tilde{z}\in\Sigma_{N}$ where $\Sigma_{N}$ is as in (31). Specifically, the set $\Sigma_{N}$ is obtained by plugging the state-feedback controller $w(k)=K\tilde{z}(k)$ into the circular region (20), representing the worst-case set of admissible input for (17). Moreover, since in the disturbance-free case, (34) reduces to a standard Lyapunov inequality $(A-BK)^{T}S^{-1}(A-BK)-S^{-1}\leq 0,$ then if $K$ fulfills (34) then $\Sigma_{N}$ is also a positively invariant region.

On the other hand, in the presence of $w_{r}(k)\neq 0$ and under the control law $w(k)=K\tilde{z}(k)+\hat{w}_{r}(k),$ the closed-loop system is

$$\begin{array}[]{rcl}\tilde{z}(k+1)&=&(A-BK)\tilde{z}(k)+B(\hat{w}_{r}(k)-w_{r}(k))\\ &=&A_{cl}\tilde{z}(k)+Bw_{d}(k)\end{array}$$ (35)

with $w_{d}=\hat{w}_{r}(k)-w_{r}(k).$ If $\hat{w}_{r}(k)$ is given by the solution of (33), then the control law $w(k)=K\tilde{z}(k)+\hat{w}_{r}(k)$ fulfils the input constraints for any $\tilde{z}\in\Sigma_{N}$. Moreover, $w_{d}$ is bounded inside the set $\mathcal{W}_{r}$ (with the worst-case happening when $\hat{w}_{r}(k)=0$). Finally, as proven in [34, Section 3], if $K$ fulfils (34), then $\Sigma_{N}$ is RPI for (35), concluding the proof.

∎

Remark 4.

In [28], the authors have proposed an analytical design of the state feedback controller such that it is optimal for a given linear quadratic cost. Also, it is worth mentioning that (34) represents a sufficient condition to ensure RPI. For a more exhaustive discussion on necessary and sufficient conditions, the interested reader may refer to [34].

III-E Feedback Linearized Model Predictive Control

Under input-output linearization arguments, optimization (7) can be equivalently rewritten as follows:

$$\displaystyle\min_{w(k),\dots,w(k+N-1)}\displaystyle J_{N}(k,\tilde{z}(k),\tilde{w}(k))$$ (36a)

$$\tilde{z}(k+i+1|k)=A\tilde{z}(k+i|k)+Bw(i)-Bw_{r}(i)$$ (36b)

$$\eta(k+i+1|k)=\eta(k+i|k)+T_{s}O(\eta(k+i|k))w(k+i)$$ (36c)

$$L(\eta(k+i|k))w(k+i)\leq g$$ (36d)

$$\forall i=0,1,\dots N-1$$

$$\tilde{z}^{T}(k+N|k)S\tilde{z}(K+N|k)\leq 1$$ (36e)

Optimization (36) is still nonconvex due to constraints (36c)-(36d). Indeed, $\forall i\geq 1,$ the input constraints depend on the predicted state of the internal dynamics which is a nonlinear and nonconvex function of the control inputs. One possible way to convexify the optimization problem is to substitute the polyhedral constraint (36d) with its quadratic worst-case approximation (20) $\forall i\geq 1$, which is independent of the nonlinear dynamics state $\eta$. On the other hand, to mitigate the conservativeness of the MPC controller, for $i=0$, since $\eta(k)$ can be measured, the actual polyhedral constraint can be used. Consequently, optimization (36) can be rewritten as:

$$\displaystyle\min_{w(k),\dots,w(k+N-1)}\displaystyle J_{N}(k,\tilde{z}(k),\tilde{w}(k))$$ (37a)

$$\tilde{z}(k+i+1|k)=A\tilde{z}(k+i|k)+Bw(i)-Bw_{r}(i)$$ (37b)

$$\forall i=0,1,\dots N-1$$

$$L(\eta(k))w(k)\leq g$$ (37c)

$$w(k+i)^{T}w(k+i)\leq\hat{r}^{2},\,i=1,\dots N-1$$ (37d)

$$\tilde{z}^{T}(k+N|k)S\tilde{z}(K+N|k)\leq 1$$ (37e)

which is a Quadratically Constrained Quadratic Programming (QCQP) problem.

Proposition 2.

The QCQP problem (37) can be rewritten in the following standard form:

$$\mathbf{w^{*}}=\displaystyle\arg\min_{\mathbf{w}}\displaystyle\frac{1}{2}\mathbf{w}^{T}H\mathbf{w}+p^{T}\mathbf{w}\,s.t.$$ (38a)

$$\hat{L}(\eta(k))\mathbf{w}\leq g$$ (38b)

$$\mathbf{w}^{T}\hat{Q}_{u}\mathbf{w}\leq 1$$ (38c)

$$\mathbf{w}^{T}\Theta^{T}_{N}S\Theta_{N}\mathbf{w}\!+\!2\tilde{z}^{T}(k)\Psi^{T}_{N}S\Theta_{N}\mathbf{w}\leq 1\!-\!\tilde{z}^{T}(k)\Psi_{N}^{T}S\Psi_{N}\tilde{z}(k)$$ (38d)

where

$$H=\Theta^{T}\hat{Q}\Theta+\hat{R}$$

and

$$p=\Theta^{T}\hat{Q}\Psi\tilde{z}(k)-\Theta^{T}\hat{Q}\Theta\mathbf{w_{r}}-\hat{R}\mathbf{w_{r}}$$

with

$$\Psi=\left[\begin{array}[]{c}A\\ A^{2}\\ \vdots\\ A^{N}\end{array}\right],\quad\displaystyle\begin{array}[]{lcr}\Theta=\left[\begin{array}[]{cccc}B&0&\dots&0\\ AB&B&\dots&0\\ \vdots&\vdots&\ddots&\vdots\\ A^{N-1}B&A^{N-2}B&\dots&B\end{array}\right]\end{array}$$

$$\Psi_{N}=A^{N},\quad\Theta_{N}=\left[A^{N-1}B,\,A^{N-2}B,\dots,AB,\,B\right]$$

$$\hat{Q}=\left[\begin{array}[]{cccc}Q&0&\dots&0\\ 0&Q&\dots&0\\ \vdots&\vdots&\ddots&\vdots\\ 0&0&\dots&Q\end{array}\right],\quad\hat{R}=\left[\begin{array}[]{cccc}R&0&\dots&0\\ 0&R&\dots&0\\ \vdots&\vdots&\ddots&\vdots\\ 0&0&\dots&R\end{array}\right]$$

$$\hat{Q}_{u}=\left[\begin{array}[]{cccc}0&0&\dots&0\\ 0&\frac{1}{\hat{r}^{2}}I_{2\times 2}&\dots&0\\ \vdots&\vdots&\ddots&\vdots\\ 0&0&\dots&\frac{1}{\hat{r}^{2}}I_{2\times 2}\end{array}\right]$$

$$\hat{L}(\eta(k))=\left[L(\eta(k)),\,0,\dots 0\right]$$

Proof.

Let’s define the decision variables vector $\mathbf{w}=\left[w(k),\,w(k+1),\dots w(k+N-1)\right]^{T}$, and the predicted reference input vector $\mathbf{w_{r}}=\left[w_{r}(k),\,w_{r}(k+1),\dots w_{r}(k+N-1\right]^{T}$. Then, using (37b), the model predictions $\mathbf{\tilde{z}}=\left[\tilde{z}(k+1|k),\dots,\tilde{z}(k+N|k)\right]^{T}$ can be rewritten in a compact form as $\mathbf{\tilde{z}}=\Psi\tilde{z}(k)+\Theta(\mathbf{w}-\mathbf{w}_{r})$. As a consequence the cost function

$$\begin{array}[]{lcr}J_{N}(k,\tilde{z}(k),\tilde{w}(k))=\frac{1}{2}\displaystyle\sum_{i=0}^{N-1}\tilde{z}(k+i+1|k)^{T}Q\tilde{z}(k+i+1|k)\\ +\tilde{w}(k+i|k)^{T}R\tilde{w}(k+i|k)\end{array}$$

can be rewritten as $J(\mathbf{w})=\frac{1}{2}[\left(\Phi\tilde{z}(k)+\Theta(\mathbf{w}-\mathbf{w_{r}})\right)^{T}\hat{Q}\left(\Phi\tilde{z}(k)+\Theta(\mathbf{w}-\mathbf{w_{r}})\right)+(\mathbf{w}-\mathbf{w_{r}})^{T}\hat{R}(\mathbf{w}-\mathbf{w_{r}})]=\frac{1}{2}\mathbf{w}^{T}H\mathbf{w}+p^{T}\mathbf{w}+c$. Notice that in optimization (38) the term $c$ has been dropped since it does not affect the optimal solution of the optimization. By applying the same arguments, it is easy to show that $L(\eta(k+i|k))w(k+i)\leq g,\,i=0,1,\dots N-1\iff\hat{L}(\eta(k))\mathbf{w}\leq g$, $w(k+i)^{T}w(k+i)\leq\hat{r}^{2},\,i=1,\dots N-1\iff\mathbf{w}^{T}\hat{Q}_{u}\mathbf{w}\leq 1$, and $\tilde{z}^{T}(k+N|k)S\tilde{z}(k+N|k)\leq 1\iff\mathbf{w}^{T}\Theta^{T}_{N}S\Theta_{N}\mathbf{w}+2\tilde{z}(k)\Psi^{T}_{N}S\Theta_{N}\mathbf{w}\leq 1-\tilde{z}^{T}(k)\Psi_{N}^{T}S\Psi_{N}\tilde{z}(k)$ ∎

Remark 5.

The QCQP problem can be recast into a computationally more affordable QP problem. Specifically, the quadratic constraints (38c) and (38d) can be replaced with polyhedral inner approximations. In particular, by defining two polyhedral sets $\mathcal{P}_{w}=\{w\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{2}:P_{w}w\leq p_{w}\}\subset\hat{\mathcal{U}},\,P_{w}\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{n_{w}\times 2},\,p_{w}\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{n_{w}}$ and $\mathcal{P}_{N}=\{\tilde{z}\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{2}:P_{\tilde{z}_{N}}\tilde{z}\leq p_{\tilde{z}_{N}}\}\subset\Sigma_{N},\,P_{\tilde{z}_{N}}\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{n_{N}\times 2},\,p_{\tilde{z}_{N}}\in\mathop{{\rm I}\mskip-4.0mu{\rm R}}\nolimits^{n_{\tilde{z}_{N}}}$, where $n_{w}$ and $n_{\tilde{z}_{N}}$ are the number of sides of the polyhedral approximations $\mathcal{P}_{w}$ and $\mathcal{P}_{\tilde{z}_{N}}$, respectively. Then, constraint (38c) can be replaced with

$$\hat{P}_{w}\mathbf{w}\leq\hat{p}_{w}$$ (39)

where

$$\hat{P}_{w}=\left[\begin{array}[]{cccc}0&0&\dots&0\\ 0&P_{w}&\dots&0\\ \vdots&\vdots&\ddots&\vdots\\ 0&0&\dots&P_{w}\end{array}\right],\quad\hat{p}_{u}=\left[\begin{array}[]{c}0\\ p_{w}\\ \vdots\\ p_{w}\end{array}\right]$$

Similarly, the quadratic constraint (37e) can be replaced with its polyhedral approximation $P_{\tilde{z}_{N}}\tilde{z}(k+N)\leq p_{\tilde{z}_{N}}$ which can be rewritten as a function of the decision variable $\mathbf{w}$, i.e.,

$$P_{\tilde{z}_{N}}\Theta_{N}\mathbf{w}\leq p_{\tilde{z}_{N}}-P_{\tilde{z}_{N}}\Psi_{N}\tilde{z}(k)$$ (40)

Therefore, replacing (38c)-(38d) with (39)-(40), a QP optimization is obtained. Notice that the conservativeness and computational complexity of the derived QP problem depends on the number of sides of the polyhedra approximations, i.e. $n_{w}$ and $n_{N}$, which are additional design parameters.

All the above developments can be collected into the computable Algorithm 1, which, as proved in the following theorem, provides a solution to Problem 1.

Algorithm 1 Dual-Mode Feedback-Linearized Tracking MPC (Dual-Mode FL-MPC) algorithm

Offline:

1:Find $K$ solving (34) and set $S$ as in (31)

2:Set the prediction horizon $N$ such that (38) is feasible with the initial condition $\tilde{z}(0)=z(0)-z_{r}(0)$

Online:

1:Estimate $x(k),\,y(k),\,\theta(k),\,\varphi(k)$ and compute $\tilde{z}(k)=z(k)-z_{r}(k)$, and $\eta(k)=\left[\theta(k),\varphi(k)\right]^{T}$.

2:Compute $L(\eta(k))$ as in (19) and $w_{r}(k)$ as in (15);

3:if $\tilde{z}(k)\notin\Sigma_{N}$ then

4:     Find $\mathbf{w}^{*}$ solving (38) and set $w(k)={w}^{*}(k)$

5:else

$$w(k)=K\tilde{z}(k)+\hat{w}_{r}(k)$$ (41)

where $\hat{w}_{r}(k)$ is the optimal solution of (33)

6:end if

7:Compute

$$\left[v(k),\omega(k)\right]^{T}=M^{-1}(\eta(k)){w}(k)$$ (42)

and apply it to the car; $k\leftarrow k+1$, go to 1;

Theorem 1.

For any $\tilde{z}(0)$ such that (38) is feasible, the tracking FL-MPC strategy described in Algorithm 1 provides a solution to Problem 1.

Proof.

The proof can be divided into two parts:

(I) Stability and input constraint fulfillment of the linearized tracking error dynamics: First, let’s consider the input-output linearized model (11). If at the generic time $k$, (38) admits a solution for a given initial condition $\tilde{z}(k)$ and for some $N>0$, then the optimal control sequence $\{w^{*}(k),\,w^{*}(k+1),\dots,\,w^{*}(k+N-1)\}$ with $w^{*}(k+i)\in\mathcal{U}(\eta(k)),\forall\eta(k),\,\forall i=0,1,\dots N-1$, is such that $\tilde{z}(k+N)\in\Sigma_{N}$. At time $k+1$, a feasible solution to optimization (38) can be constructed from the optimal solution at time $k$, i.e., $\{w^{*}(k+1),w^{*}(k+2)\dots,\,w^{*}(k+N-1),\,K\tilde{z}(k+N)\}$.

Indeed, the last control move $K\tilde{z}(k+N)$ is, by construction, always constraint-admissible inside the RPI region $\Sigma_{N}.$ As a consequence, the optimization (38) is recursively feasible ensuring that, in at most $N$ steps, $\tilde{z}(k)$ is steered into $\Sigma_{N}.$ Then, given the RPI nature of $\Sigma_{N},$ we can also conclude that $\tilde{z}(k)$ is UUB in $\Sigma_{N}.$ Furthermore, since the used input-output linearization and discretization commutes (see property (1)), the linearized tracking error dynamics (16) is stable under the effect of the proposed dual-mode MPC.

(II) Bounded Tracking Error for (1): As proven in part (I), $w(k)$ computed by Algorithm 1 stabilizes the feedback linearized error dynamics (17a). Therefore, given the result of Lemma 1 and by applying the input transformation (9), the control law (42) solves the considered reference tracking problem with a bounded tracking error $\tilde{q}(k).$ ∎

IV Experimental Results

In this section, the experimental results, obtained using a Quanser Qcar, are presented to show the effectiveness of the proposed FL-MPC tracking controller and compare it with alternative solutions. In particular, the chosen competitors are the nonlinear MPC tracking strategy described in Section III-A (namely “Nonlinear MPC”), and the the constrained adaptive backstep** controller developed in [10] (namely “Backstep**”). Two different versions of the proposed FL-MPC scheme have been tested. The first one exactly follows Algorithm 1 (hereafter referred to as “Dual-mode FL-MPC”). On the other hand, the second one (namely “FL-MPC”) executes Algorithm 1 but it never activates the terminal control law, i.e., it solves (38) for any $k\geq 0.$

Figure 4: Proposed experimental setup

IV-A Experimental Setup

The considered experimental setup is depicted in Fig. 4, and it consists of:

1. a Quanser Qcar;

2. a camera-based Indoor Positioning System (IPS);

3. a ground workstation;

4. a Wifi communication channel between the ground workstation and the car.

The autonomous car-like vehicle is the Quanser Qcar open-architecture prototype, which is designed for academic research experiments. The car has a size of $0.39\,\times\,0.19\,\times\,0.20\,m$, weights $2.7\,kg$, and its wheelbase measures $l=0.256m$. Onboard, the car is equipped with different sensors (encoders gyroscope, accelerometer, magnetometer, lidar, and depth and RGB cameras) an NVIDIA® Jetson™ TX2 with 2 GHz quad-core ARM Cortex-A57 64-bit + 2 GHz Dual-Core NVIDIA Denver2 64-bit CPU and 8GB memory. The considered maximum longitudinal speed is $\overline{v}=1\,m/s$, while the maximum steering angular velocity is $\overline{\omega}=10\,rad/s$. In addition, due to the vehicle’s mechanical construction, the front wheel’s steering angle cannot exceed $\overline{\varphi}=0.6\,rad$. The developed tracking algorithms have been implemented and cross-compiled in C language and run onboard on the Nvidia Jetson CPU. A sampling time $T_{s}=0.01\,s$ has been used for all the performed tests.

The IPS consists of a set of 12 Vicon Vero cameras connected via a wired connection to the ground workstation. The camera system is used to localize the car in the workspace, similar to a standard GPS system. In particular, the cameras detect and track a set of reflective markers placed on the Qcar. The positions of the markers are real-time collected and processed on the ground workstation by the Vicon Tracker software, which accurately reconstructs, via a triangulation algorithm, the position and orientation of car.

The ground workstation is a desktop computer consisting of a 13th Gen Intel(R) Core(TM) i9-13900KF CPU, a NVIDIA GeForce RTX 4070 GPU, and 64GB of RAM. The workstation is used to estimate the pose of the car and broadcast it via a TCP/IP communication channel.

IV-A1 Configuration of the proposed controller

To implement the proposed tracking controller strategy the following parameters have been considered: $\Delta=0.35$, $Q=I_{2\times 2}$, $R=0.01\cdot I_{2\times 2}$, $K=4I_{2\times 2}$. The radius of the worst-case circular input constraint set has been computed as in (20), obtaining $\hat{r}=1$. The reference trajectory is built to comply with (18), with $r_{d}=11.54$. As a consequence, the feedback control gain $K=4I_{2\times 2}$ has been chosen such that RPI condition (34) is satisfied, with $A_{cl}=0.96I_{2\times 2}$, $S=16I_{2\times 2}$, $G=0.25I_{2\times 2}$, $\xi=4.69\cdot 10^{-8}$, $\lambda=0.9998$, computed as outlined in in Proposition 1. The idea described in Remark 5 has been used to obtain a QP formulation of the derived QCQP optimization (38). In particular, the quadratic constraints (38c)-(38d) have been inner approximated using two decahedra, i.e., two polyhedral sets defined as in (39)-(40), with $n_{w}=n_{N}=10$. The control optimization problem has been solved on the car’s processing unit considering a prediction horizon $N=10$ using an Active Set solver algorithm. The Active Set algorithm makes use of a Cholesky decomposition of the hessian matrix $H$, which, being constant for the proposed optimization, has been precomputed to reduce the online computational load. The computational times obtained for the used solver are reported in Section IV-A5.

IV-A2 Car’s state estimation

The state of the car $q(k)$ is onboard estimated by means of an Unscented Kalman Filter (UFK). In particular, the implemented UKF is outlined in [35] and it exploits the nonlinear kinematic model (3) and different collected sensor information: (i) the estimated position and orientation of the Car provided by the Vicon Camera System, (ii) encoder and (iii) IMU (gyroscope and accelerometer) measurements. The UKF has been configured with the following parameters: process and measurement covariance matrices $Q_{UKF}=diag([10^{-3},\,10^{-3},\,10^{-1},\,10])$, $R_{UKF}=diag([2\cdot 10^{-5},\,2\cdot 10^{-5},\,10^{-4},\,10^{-5}])$, initial state estimation covariance matrix $P^{0}_{UKF}=diag([10^{-5},\,10^{-5},\,10^{-6},\,10^{-6}])$, sigma-points parameters $\alpha=0.9$, $\beta=2$, $\kappa=0$ (the interested reader shall refer to [35] for further details about the used parameters).

IV-A3 Reference trajectory generation

A reference trajectory complying with assumption 1 has been generated using a cubic spline interpolation method. The interpolation algorithm receives in input a sequence of waypoints describing the desired path and, in output, it assigns a crossing time based on path curvature and desired average speed. Then, each waypoint is interpolated using quintic splines, obtaining the position $x_{r},\,y_{r}$, velocity $\dot{x}_{r},\,\dot{y}_{r}$, acceleration $\ddot{x}_{r},\,\ddot{y}_{r}$, and jerk $\dddot{x}_{r},\,\dddot{y}_{r}$, needed to compute the reference car’s state $q_{r}$, and inputs $u_{r}$, as in (4)-(5).

IV-A4 Configuration of the competitor schemes

Each competitor scheme has been configured to obtain the best tracking performance in the performed experiments. Specifically, the nonlinear MPC optimization (7) has been solved considering the following LQ cost matrices, $Q=diag(\left[135,\,135,\,65,\,65\right])$, $R=diag(\left[0.3,\,0.1\right])$, and a prediction horizon $N=5$. The nonlinear optimization has been solved using Sequential Quadratic Programming (SQP) method. On the other hand, the backstep** algorithm developed in [10] has been tuned using: $d_{x}=0.3$, $\sigma=0.3$, $\alpha_{c}=0.3$, $k_{3}=3.5$. The interested reader shall refer to [10] for a detailed explanation of the used parameters. It is worth mentioning that the steering command generated by the algorithm is subject to undesired chattering effects, typical of backstep** control algorithms [36]. In order to mitigate such an undesired effect, the control signal has been prefiltered using a low-pass filter with cut-off frequency $\omega_{c}=100\frac{rad}{s}$.

IV-A5 Evaluation of the tracking performance

To evaluate the tracking performance of the proposed controller and alternative schemes, the Integral Square Error (ISE) ($\int_{0}^{T_{f}}e(t)^{2}dt$) and Integral Time Squared Error (ITSE) $(\int_{0}^{T_{f}}te(t)^{2}dt)$ indexes have been used. In particular, for each performed experiment, three different error signals are measured: path distance error $e_{xy}(k)=\|\left[x(k),\,y(k)\right]-\left[x_{r}(k),\,y_{r}(k)\right]\|_{2}$, heading angle error $e_{\theta}(k)=\theta(k)-\theta_{r}(k))$, and steering angle error $e_{\varphi}(k)=\varphi(k)-\varphi_{r}(k))$. Then, for each collected error signal the ISE and ITSE indexes are computed, i.e., $ISE_{x,y}$, $ITSE_{x,y}$, $ISE_{\theta}$, $ITSE_{\theta}$, $ISE_{\varphi}$, $ITSE_{\varphi}$.

IV-A6 Evaluation of computational times

To assess the computation complexity of the approaches Dual FL-MC and FL-MPC” and Nonlinear MPC, the computational times required by each algorithm have been measured. In the performed analysis, only the controller computation has been considered, i.e., the state-estimation algorithm as well as the sensor processing have been neglected.

IV-B Results

Figure 5: Experimental results: Trajectory

Figure 6: Experimental results: Heading and Steering Angles

Figure 7: Experimental results: Control Inputs

The obtained results are collected in Figs. 5-7 and and Tables I-III. For the interested reader, videos of the performed experiments can be found at the following web link: https://youtu.be/aeHZKyRfcEo. The tracking performance has been evaluated considering two reference trajectories generated along the same path. The first requires a maximum speed of $0.6\frac{m}{s}$, while the second, a maximum speed of $0.75\frac{m}{s}$. It is worth mentioning that, for both the considered trajectories, several tests have been run, and the obtained results have been averaged in Table I-II, respectively.

Fig. 5 and Tables I-II show that for both trajectories, the proposed controller achieves better tracking when compared to the Nonlinear MPC and Backstep** controllers. In particular, the Nonlinear MPC showed poor tracking performance in all the performed experiments. This finds justification in the highly nonconvex nature of optimization (7), which converges to local minima and, consequently, to nonoptimal solutions. Moreover, as shown in Table III, the reference tracking performance cannot be improved by increasing the prediction horizon. For example, for $N=10,$ the nonlinear optimization solver for (7) can take up to approximately $40\,ms$ to obtain a solution, which is far above the considered sampling time $T_{s}=10\,ms$. On the other hand, the backstep** controller shows slightly better tracking performance of the Nonlinear MPC. However, a chattering phenomena affects the computed steering angle command (see the rapid and discontinuous switching of the control signal). Moreover, as shown in Fig. 7, the control inputs computed by the backstep** controller are conservative, i.e. the longitudinal and angular steering velocities never reach the prescribed limits, $\overline{v}$ and $\overline{\omega}$, respectively. The two above mentioned drawbacks justify why the tracking performance of the backstep** scheme are slightly worse of the one achieved with the proposed tracking controller degrades.

TABLE I: Comparison of tracking performance: Trajectory 1

Algorithm	Distance Error		Heading Error		Steering Error
	$ISE_{xy}$	$ITSE_{xy}$	$ISE_{\theta}$	$ITSE_{\theta}$	$ISE_{\varphi}$	$ITSE_{\varphi}$
FL-MPC	0.0279	0.3191	0.0201	0.1797	0.0244	0.2834
Dual-mode FL-MPC	0.0323	0.4160	0.0246	0.3195	0.0333	0.3117
Nonlinear MPC	0.2703	4.4197	0.0978	1.4305	0.0135	0.1642
Backstep** [10]	0.1629	2.0904	0.1375	1.6539	1.8797	30.5802

TABLE II: Comparison of tracking performance: Trajectory 2

Algorithm	Distance Error		Heading Error		Steering Error
	$ISE_{xy}$	$ITSE_{xy}$	$ISE_{\theta}$	$ITSE_{\theta}$	$ISE_{\varphi}$	$ITSE_{\varphi}$
FL-MPC	0.0321	0.4718	0.0127	0.1713	0.0132	0.1480
Dual-mode FL-MPC	0.0459	0.6006	0.0250	0.3571	0.0212	0.2439
Nonlinear MPC	0.2458	3.0141	0.0468	0.5247	0.0218	0.2568
Backstep** [10]	0.2634	3.2056	0.2378	2.6428	1.2098	20.9822

On the other hand, the performance obtained with the FL-MPC is slightly superior to the ones obtained with the dual-mode MPC. The reason behind such a result is that FL-MPC uses the RPI region (31) to ensure recursive feasibility, but it never directly relies on the associated controller, which is by nature more conservative. However, the dual-mode MPC implementation shows some computational advantage related to the fact that when the tracking error enters the terminal regions, then a simpler optimization problem (33) is solved. Moreover, for both performed experiments, the proposed FL-MPC controller fulfills the vehicle’s input constraints, as shown in Figs. 5-7.

TABLE III: Comparison of average and maximum computational times (in ms) of Nonlinear and FL-MPC

Algorithm	N=3		N=5		N=10
	MAX $(ms)$	AVG$(ms)$	MAX$(ms)$	AVG$(ms)$	MAX$(ms)$	AVG$(ms)$
FL-MPC	3.3106	0.5416	3.8249	0.6455	3.1471	0.6954
Dual-mode FL-MPC	1.7726	0.3622	3.3520	0.3417	2.0981	0.3638
Nonlinear MPC	7.2283	3.1933	15.6261	5.2227	40.5351	6.8099

Concerning the computational analysis, the proposed Dual-mode FL-MPC and FL-MPC solutions have been compared with the Nonlinear MPC considering the prediction horizons $N=3,N=5,N=10$. The obtained results are collected in Table III. It can be appreciated how the computational times, both maximum and average ones, of the proposed solutions are significantly lower than the ones of the Nonlinear MPC, especially for larger values of $N.$

V Conclusions

In this paper, a novel Feedback Linearized Model Predictive Control strategy for input-constrained self-driving cars has been presented. The proposed strategy combines two main ingredients: 1) an input-output FL technique and 2) a dual-mode MPC framework. The obtained tracking controller has the peculiar capability of efficiently dealing with state-dependent input constraints acting on the feedback linearized car’s model while ensuring recursive feasibility, stability, and velocity constraints fulfillment. Extensive experimental results and comparisons have been carried out to highlight the features and advantages of the proposed tracking controller.