# MPC and PSO Based Control Methodology for Path Tracking of 4 WS4 WD Vehicles
## 3 Control Methodology
### MPC-SQP Method
#### 3.1.2 Model Predictive Control
It is obvious that the objective function value of non-optimal inputs sequence has to be no less than \(V(x_{ki+1})\), which can be expressed as  
\[V(x_{k+1})\leq\bar{V}, \tag{35}\]  
thereby,  
\[V(x_{k+1})-V(x_{k})\leq\bar{V}-V(x_{k}). \tag{36}\]  
Since \(\bar{V}\) shares the same future inputs sequence and the predictive outputs sequence with \(V(x_{k})\) for the sample time \(k+1,...,k+N_{p}-1\), it can be easily derived that the difference between these two functions is  
\[\bar{V}-V(x_{k})=y_{k+N_{p}}^{T}y_{k+N_{p}}-y_{k+1}^{T}y_{k+1}-\Delta u_{k}^{ T}r_{w}\Delta u_{k}. \tag{37}\]  
As is given in Equation (32), the optimization problem is subjected to the constraint \(y_{k+N_{p}}=0\). Then, it can be obtained that  
\[\bar{V}-V(x_{k})\leq-y_{k+1}^{T}y_{k+1}-\Delta u_{k}^{T}r_{w}\Delta u_{k}. \tag{38}\]  
Then, the monotonicity of the Lyapunov function can be obtained by  
\[V(x_{k+1})-V(x_{k})\leq-y_{k+1}^{T}y_{k+1}-\Delta u_{k}^{T}r_{w}\Delta u_{k}<0, \tag{39}\]  
which can prove the asymptotic stability of the system. The model predictive control algorithm is realized by receding optimization. In order to apply the MPC efficiently, we assume that the predicted outputs sequence is in a finite prediction horizon \(N_{p}\) and the inputs sequence is in a control horizon \(N_{c}\), which is less than \(N_{p}\). The sequences mentioned above can be expressed in the matrix form:  
\[ \begin{split}\mathbf{Y}&=\left[y(k+1|k)\quad y(k+2|k) \quad...\quad y(k+N_{p}|k)\right]^{T},\\ \Delta\mathbf{U}&=\left[\Delta u(k)\quad\Delta u(k+1) \quad...\quad\Delta u(k+N_{c}-1)\right]^{T},\end{split}  \tag{40}\]  
where \(y(k+n|k)\) denotes the predicted outputs at time \(k+n\) based on the states at time \(k\). Based on Theorem 1 and the assumption above, the corollary about finite-time unconstrained MPC can be obtained as follows. **Corollary 1**. : _Given the system without input and output constraints, the prediction and control horizon are \(N_{p}\) and \(N_{c}\), respectively. Then, the following feedback control law \(\Delta u(k)=Kx(k)\) can asymptotically stabilize the closed-loop system, where_  
\[K=\overbrace{[10\...\ 0]^{T}}^{N_{c}}(-\Xi[\Gamma-(\Psi\Xi)^{-1}(CA^{N_{p}}+ \Psi\Xi\Gamma)]), \tag{41}\]  
_and,_  
\[ \begin{split} F&=\left[\begin{matrix}CA\\ CA^{2}\\ \vdots\\ CA^{N_{p}}\end{matrix}\right],\quad\Psi=\left[\begin{matrix}CA^{N_{p}-1}B\\ CA^{N_{p}-2}B\\ \vdots\\ CA^{N_{p}-N_{c}}B\end{matrix}\right]^{T},\\ \Phi&=\left[\begin{matrix}CB&0&\cdots&0\\ CAB&CB&\cdots&0\\ CA^{2}B& CAB&\cdots&0\\ \vdots\\ CA^{N_{p}-1}B&CA^{N_{p}-2}B&\cdots&CA^{N_{p}-N_{c}}B\end{matrix}\right],\\ \Xi&=(\Phi^{T}\Phi+\bar{R})^{-1},\\ \Gamma&=\Phi^{T}F.\end{split}  \tag{42}\]  
Proof of Corollary 1.: According to Equations (31) and (40), the predicted output sequence \(Y\) can be expressed by  
\[\mathbf{Y}=Fx(k)+\Phi\Delta\mathbf{U}. \tag{43}\]  
Meanwhile, the final item of \(Y\) can be expressed as  
\[y(k+N_{p}|k)=CA^{N_{p}}x(k)+\Psi\Delta\mathbf{U}. \tag{44}\]  
By substituting Equations (43) and (44) into Theorem 1, the optimization problem turns into the following form:  
\[ \begin{split}\arg\min_{\Delta\mathbf{U}}J&=(\mathbf{R }_{s}-Fx(k_{i})-\Phi\Delta\mathbf{U})^{T}(\mathbf{R}_{s}-Fx(k_{i})-\Phi\Delta \mathbf{U})\\ &\qquad+\Delta\mathbf{U}^{T}\bar{R}\Delta\mathbf{U},\\ s.t.&\Psi\Delta\mathbf{U}+CA^{N_{p}}x(k_{i})=0,\end{split}  \tag{45}\]  
where \(\bar{R}\) is a weighting matrix. Note that in the application of offset model based path tracking, the target vector \(R_{s}\) should be zero all the time. It can be seen that \(J\) meets an equality constrained quadratic programming. Then, the objective function is expanded by Lagrange expression and simplified by omitting the constant term,  
\[ \begin{split} J&=2\Delta\mathbf{U}^{T}\Phi^{T}Fx(k)+ \Delta\mathbf{U}^{T}(\Phi^{T}\Phi+\bar{R})\Delta\mathbf{U}\\ &\quad+\xi^{T}(\Psi\Delta\mathbf{U}+CA^{N_{p}}x(k)),\end{split}  \tag{46}\]  
where \(\xi\) is the Lagrange multiplier.