# MPC and PSO Based Control Methodology for Path Tracking of 4 WS4 WD Vehicles
## 3 Control Methodology
### MPC-SQP Method
#### 3.1.3 Sequential Quadratic Programming Based Control Allocation  
Considering the vehicle body with the mass \(M\) and inertia \(J\) in this work, the command force vector \(\mathbf{\tau}\) is defined as  
\[\mathbf{\tau}=\begin{bmatrix}\tau_{l}\\ \tau_{r}\\ \tau_{\gamma}\end{bmatrix}=\begin{bmatrix}M&0&0\\ 0&M&0\\ 0&0&J\end{bmatrix}\widetilde{A}, \tag{49}\]  
where \(\tau_{l}\), \(\tau_{r}\) and \(\tau_{\gamma}\) are the longitudinal and lateral forces and the moment about a vertical axis of the vehicle, respectively.  
According to the vehicle body dynamic model given in Equation (1), the actual actuating forces come from the longitudinal forces \(F_{Sl}\) and lateral forces \(F_{SL}\) on each wheel. Let the command forces \(\mathbf{\tau}\) produced jointly by the wheel forces and steering angles be expressed as  
\[\mathbf{\tau}(F_{Sl},F_{SL},\delta)=\mathbf{B}_{u}(\delta)F_{Sl}+\mathbf{B}_{w}(\delta)F_ {SL}, \tag{50}\]  
where the \(i\)-th column of \(\mathbf{B}_{u}(\delta)\) and \(\mathbf{B}_{w}(\delta)\) can be written as  
\[\mathbf{B}_{u}^{i}(u_{gi})=\begin{bmatrix}\cos\delta_{i}\\ \sin\delta_{i}\\ -L_{yi}\cos\delta_{i}+L_{xi}\sin\delta_{i}\end{bmatrix},\ \ \ \ \mathbf{B}_{w}^{i}(u_{gi})=\begin{bmatrix}-\sin\delta_{i}\\ \cos\delta_{i}\\ L_{yi}\sin\delta_{i}+L_{xi}\cos\delta_{i}\end{bmatrix}.\]  
\((L_{xi},L_{yi})\) represents the location of each wheel in a coordinate system with its origin at the centre of gravity and positive \(x\)-axis forward.  
On a 4 WS4 WD vehicle, the drive forces \(F_{Sl}\) and the steering angles \(\delta\) are the direct inputs while the lateral forces \(F_{SL}\) obtained from sensors are considered as a measured disturbance. In this work, the steering angles \(\delta\) are composed of \(\delta_{f}\) and \(\delta_{r}\), which represent the front and rear steering inputs, respectively. Thus, the control problem is reduced to obtaining the feasible solution of Equation (50). In order to facilitate the computation, a slack vector \(\mathbf{s}\) is defined by  
\[\mathbf{s}=\mathbf{\tau}-\mathbf{B}_{u}(\delta)F_{Sl}+\mathbf{B}_{w}(\delta)\hat{F}_{SL}, \tag{51}\]  
which denotes the error between the commanded and actual generalized forces. The slack variable \(s\) guarantees that there always exists a feasible solution in the following optimization [26].  
In order to solve this control allocation problem, the objective function is defined with respect to \(F_{SI}\), \(\delta\) and \(\mathbf{s}\),  
\[J(F_{SI},\delta,\mathbf{s}) =F_{SI}^{T}Q_{f}F_{SI}+(\delta-\delta^{*})^{T}Q_{\delta}(\delta- \delta^{*})+\mathbf{s}^{T}Q_{s}\mathbf{s}, \tag{52}\] \[s.t. \mathbf{s} =\tau-B_{u}(\delta)F_{SI}+B_{w}(\delta)\tilde{F}_{SL},\] \[\mathbf{s} \in\mathfrak{B}_{s},\] \[F_{SI} \in\mathfrak{B}_{F_{SI}},\] \[\delta \in\mathfrak{B}_{\delta},\]  
where \(\mathfrak{B}_{s}\), \(\mathfrak{B}_{F_{SI}}\) and \(\mathfrak{B}_{\delta}\) are the search bounds of each variable.  
In this function, the first term minimizes the magnitudes of the drive forces; the second term is used to ensure the steering angle to search around its previous value. By penalizing the slack variable \(\mathbf{s}\) in the third term, the actual generalized force vector coincides as much as possible with the commanded forces \(\mathbf{\tau}\). The matrix \(Q_{f}\in I^{4\times 4}\), \(Q_{\delta}\in I^{2\times 2}\) and \(Q_{s}\in I^{3\times 3}\) are used to tune the objective. The search bounds of all variables (i.e. \(F_{SI}\), \(\delta\) and \(s\)) are specified by the constraints.  
Based on the objective function presented above, the control allocation is converted to a nonlinear constrained optimization problem. Using the sequential quadratic programming (SQP), the optimal solution can be computed efficiently and reliably by standard numerical software.