# Predictive Path Following and Collision Avoidance of Autonomous Connected Vehicles
## 3 Controller Synthesis  
### Nonlinear Predictive Path-Following Control  
Nonlinear Model Predictive Control (NMPC), also known as Receding Horizon Control and Moving Horizon Optimal Control, is a famous control technique for vehicle control systems that has roots in optimal control. It has been widely adopted in industry as an effective way to deal with multi-variable constrained control problems [21]. The basic concept of MPC is to optimize the forecast of the system, generated by a nominal dynamic model, and optimize it to produce the best decision, i.e., the control signal at the current time slot, while planning the nominal future control actions. This is achieved by employing the prediction of the system model to formulate and solve a dynamic optimization problem online where tracking errors, namely the difference between the predicted output and the desired reference, is minimized over a future horizon, possibly subject to constraints on the control inputs (the manipulated variables) and the states. These constraints could be nonlinear and time-varying over the prediction horizon.  
This optimization problem is solved in discrete control intervals suitable for the system dynamics. At each step, the MPC algorithm computes an open-loop sequence of control input adjustments. The first input in the optimal sequence is injected into the system, and the entire optimization is repeated for the subsequent control intervals.  
Consider a nonlinear state space Model (5) with \(\mathbf{x}\in\mathbb{R}^{n}\) and \(\mathbf{u}\in\mathbb{R}^{m}\) subject to generic state and control constraints  
\[\mathbf{x}(t)\in\mathcal{X}\subset\mathbb{R}^{n} \tag{9}\] \[\mathbf{u}(t)\in\mathcal{U}\subset\mathbb{R}^{m},\]  
where \(\mathcal{X}\) and \(\mathcal{U}\) define the admissible state and control sets, respectively. These admissible sets will be designed later to achieve comfort driving, respect physical constraints of the vehicle, and perform collision avoidance.  
In order to include the Path (6) into the control problem, we treat its parameter \(\theta\) as an extra virtual state whose time evolution \(t\mapsto\theta(t)\) is controlled by an extra virtual input [18]. Therefore, an additional differential equation  
\[\dot{z}=u_{z}, \tag{10}\]  
is added to the system where \(z=\theta\) is the extra state that represents the path time evolution, and \(u_{z}\) is the virtual control input that adds an extra degree of freedom in the controller design.  
Figure 2: Bézier Curve (cubic, \(n=3\)).  
The optimal control problem (OCP) of the NMPC used for path following will be  
\[\min_{\mathbf{x}(t),\mathbf{u}(t),\mathbf{z}(t),u_{z}(t)}J(\mathbf{x},\mathbf{u},z,u_{z}) =\int_{t=t_{k}}^{T_{p}+t_{k}}\ell(\mathbf{x}(t),\mathbf{u}(t),z(t),u_{z}(t) )dt+F(\mathbf{x}(t_{k}+T_{p}))\] (11) subject to: \[\dot{\mathbf{x}}(t) =f(\mathbf{x},\mathbf{u})\] \[\dot{z}(t) =u_{z}(t)\] \[\mathbf{x}(t) \in\mathcal{X},\] \[\mathbf{u}(t) \in\mathcal{U},\] \[z(t) \in\mathcal{Z},\] \[u_{z}(t) \in\mathcal{U},\]  
where \(\ell(\cdot,\cdot,\cdot,\cdot)\) is the stage cost given by a positive definite function, \(T_{p}\) is the prediction horizon, \(\mathcal{Z}\) is the admissible set for the path parameter, and \(\mathcal{U}_{\mathbf{\hat{q}}}\) is the admissible set for the path timing law. The stage cost function for the path following problem of the autonomous vehicle could be defined as  
\[\ell(\mathbf{x},\mathbf{u},z,u_{z})=\left\|\mathbf{h}(\mathbf{x})-\mathbf{p}(z)\right\|_{Q_{1}}+ \left\|V-V^{ref}\right\|_{Q_{2}}+\left\|\mathbf{u}\right\|_{R_{1}}+\left\|u_{z} \right\|_{R_{2}}. \tag{12}\]  
Here, \(Q=\text{diag}(Q1,Q2)\) is positive semi-definite matrix, \(R=\text{diag}(R1,R2)\) is strictly positive-definite, \(\mathbf{h}(\mathbf{x})\) is the output function for the vehicle state space Model (5) and is defined as: \(\mathbf{h}(\mathbf{x})=[x\ y\ \psi]^{T}\), and  
\[\mathbf{p}(z)=\begin{bmatrix}\mathcal{B}(z,2)\\ \psi^{ref}\end{bmatrix}\in\mathbb{R}^{3},\]  
is the reference path which is defined using quadratic Bezier curves.