# Predictive Path Following and Collision Avoidance of Autonomous Connected Vehicles
## 3 Controller Synthesis
### Optimization Problem Implementation  
In order to utilize the aforementioned infinite dimensional OCP of the NMPC, a proper formulation of the optimization problem implementation is required. There are two possible categories when it comes to dynamic optimization problems; indirect and direct methods. Indirect methods, such as calculus of variations and dynamic programming, consider the continuous NMPC as it is and does not require discretization of the problem. They provide exact analytic solution of the optimal control problem [22], but they are not common to be used for nonlinear optimal control problems due  
Figure 3: Elliptic Disk. to their inability to handle a wide class of systems because they require a guess of the optimal control structure [22]. Direct methods, such as collocation [23] and multiple shooting [24], are the most desirable technique for formulating and parameterizing NMPC optimization problem. The main idea behind them is introducing the discretization and finite parameterization into the optimal control problem formulation which results in a Nonlinear Programming Problem (NLP), and then solve it directly with numerical methods. In the multiple shooting technique, the one used in this paper, the nonlinear continuous ordinary differential equation (ODE) and the OCP are discretized at a proper sampling interval \(T_{s}=\frac{T_{p}}{N},N=2,3,\cdots\)[24]. Consider a general OCP of the form  
\[\min_{\mathbf{x}(\cdot),\mathbf{u}(\cdot)}= \int_{t=t_{k}}^{T_{p}+t_{k}}\ell(\bar{\mathbf{x}}(t),\bar{\mathbf{u}}(t))dt\] (14) subject to: \[\bar{\mathbf{x}}(t_{k})-\bar{\mathbf{x}}_{k} =0\] \[\hat{\mathbf{x}}(t)-f(\bar{\mathbf{x}}(t),\bar{\mathbf{u}}(t)) =0, t\in[t_{k},t_{k}+T_{p}],\] \[r(\bar{\mathbf{x}},\bar{\mathbf{u}}) =0,\] \[h(\bar{\mathbf{x}},\bar{\mathbf{u}}) \geq 0, t\in[t_{k},t_{k}+T_{p}],\]  
where \(\hat{\mathbf{x}}(t)=f(\bar{\mathbf{x}}(t),\bar{\mathbf{u}}(t))=0\) is the ODE model of the system, \(\bar{\mathbf{x}}(t_{k})\) is the initial condition, and \(h(\bar{\mathbf{x}},\bar{\mathbf{u}})\) is a vector function which represents the optimization state and control constraints. The control input \(\bar{\mathbf{u}}(t)\) is discretized at the time instants  
\[T_{d}=\{t_{k+1},t_{k+2},\cdots,t_{k+N}=t_{k}+T_{p}\},\]  
such that  
\[\bar{\mathbf{u}}(t)=\mathbf{q}_{i}\quad\forall t\in[t_{i},t_{i+1}].\]  
The nonlinear ODE, which can not be solved analytically, must be solved numerically on each subinterval \([t_{i},t_{i+1}]\) using a suitable integration technique, e.g., fourth-order Runge-Kutta, and starting with an artificial initial value \(\mathbf{s}_{i}\). The trajectory pieces \(\bar{\mathbf{x}}(t;\mathbf{s}_{i},\mathbf{q}_{i})\) are obtained by solving the initial value problem (IVP)  
\[\hat{\mathbf{x}}(t;\mathbf{s}_{i},\mathbf{q}_{i}) =f(\bar{\mathbf{x}}_{i}(t;\mathbf{s}_{i},\mathbf{q}_{i}),\mathbf{q}_{i}),\qquad t \in[t_{k},t  
where (16b) represents the initial condition, Equation (16c) represents the continuity condition between each two consecutive subintervals, Equations (16d) and (16e) represent the discretized constraints, and \(\bar{\mathbf{x}}_{i}(t_{i+1})\
eq\mathbf{s}_{i+1}\) is the solution of the IVP (15). The NLP (16) is solved using sequential quadratic programming (SQP), with the aid of the method of Lagrange multipliers where the Hessian matrix is approximated using Gauss-Newton [25]. Each quadratic problem (QP) is then solved using active-set method [26]. The open-source ACADO Code Generation toolkit [26] is used in the implementation of the NMPC algorithm. Based on a symbolic representation of optimal control problems, ACADO generates an optimized and self-contained C code for the SQP formulation with static memory for a good real-time performance. The C code can be integrated into MATLAB as MATLAB executable (mex) files.