# MPC and PSO Based Control Methodology for Path Tracking of 4 WS4 WD Vehicles
## 2 Vehicle System Modelling
### Offset Model  
An offset error model is proposed for the evaluation of the performance of the controllers. The reference position (RP) of the vehicle on the reference path is defined as the normal projection of the heading of the vehicle at the centre of gravity on to the reference path, as shown in Figure 5. The tracking error in vehicle coordinate frame consists of lateral offset error \(l_{os}\) and heading error \(\theta_{os}\), expressed as  
\[\widetilde{\mathbf{p}}\overline{\mathbf{o}}\overline{\mathbf{s}}=\begin{bmatrix}0&l_{os}& \theta_{os}\end{bmatrix}^{T} \tag{17}\]  
The position errors are always in the lateral direction of the vehicle. The longitudinal position error is set to 0. The heading error can be found by  
\[\theta_{os}=\theta-\theta_{ref}, \tag{18}\]  
Figure 5: Offset model.  
where \(\theta_{ref}\) is the reference heading direction of the RP, tangential to the reference path.  
The velocity error states \(\widetilde{\mathbf{vel}}\) is the difference between vehicle velocity \(\mathbf{vel}\) and reference velocity states \(\mathbf{vel}_{ref}\) in the vehicle coordinate frame. \(\widetilde{\mathbf{vel}}\) can be expressed as  
\[\widetilde{\mathbf{vel}}=\begin{bmatrix}V_{lerr}&V_{rerr}&\Omega_{err}\end{bmatrix}^ {T}=\mathbf{vel}-\mathbf{vel}_{ref}, \tag{19}\]  
where \(V_{lerr}\), \(V_{rerr}\) and \(\Omega_{err}\) are the errors in longitudinal, lateral, and angular velocity, respectively. Reference velocity states are defined as  
\[\mathbf{vel}_{ref}=\begin{bmatrix}V_{lref}&V_{rref}&\Omega_{ref}\end{bmatrix}^{T}, \tag{20}\]  
where reference angular velocity \(\Omega_{ref}\) is the rate of change of the heading of RP on the reference path.  
The reference longitudinal velocity \(V_{lref}\) and lateral velocity \(V_{rref}\) in vehicle coordinate frames can be derived from the reference path coordinate frame by the equations:  
\[\begin{split} V_{lref}&=V_{lp}\cos(\theta_{os})-V_{rp} \sin(\theta_{os}),\\ V_{ref}&=V_{lp}\sin(\theta_{os})+V_{rp}\cos(\theta_{os}), \end{split} \tag{21}\]  
where \(V_{lp}\) is the reference longitudinal velocity, taken as tangential to the reference path. \(V_{rp}\) is the reference radial velocity normal to the reference path at RP.  
Finally, the acceleration error states \(\widetilde{\mathbf{acc}}\) are written as:  
\[\widetilde{\mathbf{acc}}=\begin{bmatrix}a_{lerr}&a_{rerr}&\gamma_{err} \end{bmatrix}^{T}=\mathbf{acc}-\mathbf{acc}_{ref}, \tag{22}\]  
where \(\mathbf{acc}\) is the difference between vehicle acceleration \(\mathbf{acc}\) and reference acceleration \(\mathbf{acc}_{ref}\). The reference acceleration vector \(\mathbf{acc}_{ref}\) is defined as  
\[\mathbf{acc}_{ref}=\begin{bmatrix}a_{lref}&a_{rref}&\gamma_{ref}\end{bmatrix}^{T}, \tag{23}\]  
where \(a_{lref}\) and \(a_{ref}\) are, respectively, the reference longitudinal and lateral acceleration at point RP, measured in the direction and normal to the vehicle heading, and \(\gamma_{ref}\) is the reference angular acceleration of the point RP.  
The reference acceleration in vehicle coordinate frame can be derived from the path coordinate frame by  
\[\begin{split} a_{lref}&=a_{lp}\cos(\theta_{os})-a_{rp} \sin(\theta_{os}),\\ a_{rref}&=a_{lp}\sin(\theta_{os})+a_{rp}\cos(\theta_{ os}),\end{split} \tag{24}\]  
where \(a_{lp}\) and \(a_{rp}\) are the reference acceleration tangential and normal to the reference path.