# Trajectory-Tracking Control of Unmanned Vehicles Based on Adaptive Variable Parameter MPC
## 4 Adaptive Trajectory-Tracking Controller Design  
### Unmanned Vehicle Dynamics Model  
In this study, the trajectory-tracking controller is designed based on the MPC algorithm to discretize Equation (26):  
Discretization of the trajectory-tracking model:  
\[\left\{\begin{array}{l}x(k+1)=A_{d}x(k)+B_{d}u(k)+Tw(k)\\ y(k)=C_{d}x(k)\end{array}\right. \tag{27}\]  
\[A_{d}=I+\hat{A}T,B_{d}=\hat{B}T,C_{d}=C\]  
Combining the discrete state variables \(x(k)\) with the control variables \(u(k)\) gives the new state vector as  
\[\xi(k)=\begin{bmatrix}x(k)\\ u(k-1)\end{bmatrix} \tag{28}\]  
Combining Equations (27) and (28) yields the new equation of state as  
\[\left\{\begin{array}{l}\xi(k+1)=\tilde{A}_{d}\xi(k)+\tilde{B}_{d}\Delta u(k )+Tw(k)\\ y(k)=\tilde{C}_{d}\xi(k)\end{array}\right. \tag{29}\]  
\[\tilde{A}_{d}=\begin{bmatrix}A_{d}&B_{d}\\ 0&I\end{bmatrix},\tilde{B}_{d}=\begin{bmatrix}B_{d}\\ I\end{bmatrix},\tilde{C}_{d}=(C_{d},0),w=\left[\begin{array}{cccc}\frac{hK _{d}\phi_{t}}{l_{x}}&0&\frac{K_{d}\phi_{t}}{l_{x}}&0&0&-k_{d}v_{x}\end{array} \right]^{T}\]Let the prediction time domain be \(N_{p}\) and the control time domain be \(N_{c}\). Iterative calculation of the state quantities in the prediction time domain can be obtained:  
\[Y(k)=C_{p}X(k)+D_{p}\Delta U(k)+E_{p}W(k) \tag{30}\]  
\[\begin{split} Y(k)=\begin{bmatrix}y(k+1)\\ \vdots\\ y(k+N_{p})\end{bmatrix},\Delta U(k)=\begin{bmatrix}\Delta u(k)\\ \vdots\\ \Delta u(k+N_{c}-1)\end{bmatrix},X(k)=\begin{bmatrix}\xi(k)\\ \vdots\\ \xi(k+N_{p}-1)\end{bmatrix}\\ D_{p}=\begin{bmatrix}\tilde{C}_{d}\tilde{B}_{d}&0&\cdots&0\\ \tilde{C}_{d}\tilde{A}_{d}\tilde{B}_{d}&\tilde{C}_{d}\tilde{B}_{d}&\cdots&0\\ \tilde{\vdots}&\vdots&\cdots&0\\ \tilde{\mathcal{C}}_{d}\tilde{A}_{d}^{N_{c}}\tilde{B}_{d}&\tilde{\mathcal{C}}_ {d}\tilde{A}_{d}^{N_{c}-1}\tilde{B}_{d}&\cdots&\tilde{\mathcal{C}}_{d}\tilde{B} _{d}\\ \vdots&\ddots&\vdots\\ \tilde{\mathcal{C}}_{d}\tilde{A}_{d}^{Np}\tilde{B}_{d}&\tilde{\mathcal{C}}_ {d}\tilde{A}_{d}^{Np-1}\tilde{B}_{d}&\cdots&\tilde{\mathcal{C}}_{d}\tilde{A}_{ d}^{Np-Nc}\tilde{B}_{d}\end{bmatrix}W(k)=\begin{bmatrix}Tw(k)\\ \vdots\\ Tw(k+N_{c}-1)\end{bmatrix}\\ E_{p}=\begin{bmatrix}\tilde{\mathcal{C}}_{d}&0&\cdots&0\\ \vdots&\vdots&\vdots&\vdots\\ \tilde{\mathcal{C}}_{d}\tilde{A}_{d}^{Np-1}&\tilde{\mathcal{C}}_{d}\tilde{A}_{ d}^{Np-2}&\cdots&\tilde{\mathcal{C}}_{d}\end{bmatrix}\\ C_{p}=\begin{bmatrix}\tilde{\mathcal{C}}_{d}&\tilde{C}_{d}\tilde{A}_{d}^{2}& \cdots&\tilde{\mathcal{C}}_{d}\tilde{A}_{d}^{Nc}&\cdots&\tilde{\mathcal{C}}_{ d}\tilde{A}_{d}^{Np}\end{bmatrix}^{T}\end{split}\]  
Define the MPC cost function as  
\[J=Y^{T}QY+\Delta U^{T}R\Delta U+\rho\epsilon^{2} \tag{31}\]  
where \(Q,R\) is the weight matrix; \(\epsilon\) is the relaxation factor; \(\rho\) is the weight coefficient of the relaxation factor. In order to ensure the feasibility of the MPC controller, the control quantity and control increment are constrained:  
\[\left\{\begin{array}{l}\Delta U_{\min}\leq\Delta U(t)\leq\Delta U_{\max}\\ U_{\min}\leq U(t)+A\Delta U(t)\leq U_{\max}\end{array}\right.\]  
The above Equation (31) is transformed into a quadratic optimization problem, and the optimal control sequence that satisfies the trajectory-tracking accuracy and body stability is derived through real-time solving [31].  
\[\begin{split}\min\,J=\sum\limits_{k}\begin{bmatrix}W_{e_{\varphi} }{(e_{\varphi}{}^{(k)})}^{2}+W_{e_{y}}{(e_{y}{}^{(k)})}^{2}\end{bmatrix}\\ +\sum\limits_{k}W_{\delta_{f}}\left[\delta_{f}{}^{(k)}-\delta_{f}{}^{(k-1)} \right]^{2}\\ s.t\,\,\left\{\begin{array}{l}\mathcal{T}\Delta\delta_{\min}\leq \Delta\delta_{f}\leq T\Delta\delta_{f\max}\\ \delta_{f\min}\leq\delta_{f}\leq\delta_{f\max}\\ \alpha_{\min}-\epsilon\leq\alpha\leq\alpha_{\max}+\epsilon\end{array}\right.\end{split} \tag{32}\]  
Equation (32) suppresses the lateral error and heading angle error in the trajectory tracking of the unmanned vehicle to ensure the accuracy of trajectory tracking [27; 28], and the optimal control sequence of the front wheel angle can be obtained by solving.