## 2 Dynamic Model Predictive Control
### Trajectory Tracker Design for Dynamic Model Predictive Control  
#### 2.2.1 Model Predictive Control  
Combined with the kinematics model of the 3 WIDWIS mobile robot derived above, the state quantity \(X=\left[X^{W},\ y^{W},\ \theta\right]^{T}\) and the control quantity \(u=\left[v_{cx},v_{cy},\ \omega\right]^{T}\) are taken. In trajectory tracking, the trajectory of the robot is composed of a series of discrete reference positions \(X_{r}\), and the control variable \(u_{r}\) can be obtained by substituting the reference position \(X_{r}\) into Equation (12). The state equation of the robot at the reference point is:  
\[\dot{X}_{r}=f(X_{r},u_{r}) \tag{15}\]  
The above formula is a nonlinear equation, which is not conducive to the optimization solution. Suppose any reference point on the desired trajectory satisfies, where \(X_{r}=\left[x_{r},y_{r},\theta_{r}\right]^{T}\), \(u_{r}=\left[v_{xx},v_{yr},\omega_{r}\right]^{T}\). The nonlinear vector function \(f(X_{r},u_{r})\) is expanded into a Taylor series in the vicinity of any reference point \((X_{r},u_{r})\), where \(R(X,u)\) represents the higher-order derivative terms in the series expansion, resulting in the following equation:  
\[\dot{X}=\frac{\partial f}{\partial X}(X-X_{r})+\frac{\partial f}{\partial u}( u-u_{r})+R(X,u) \tag{16}\]  
The linear error model can be obtained by subtracting the Equation (16) from the Equation (15):  
\[\dot{X}_{e}=A(k)X_{e}+B(k)u_{e} \tag{17}\]  
where \(\dot{X}_{e}\) represents the dynamic speed error of the system, \(X_{e}\) represents the dynamic state variable error of the system, and \(u_{e}\) represents the dynamic control variable error of the system. \(A(k)\) is the Jacobian matrix of the equation of state \(f(X_{r},u_{r})\) in \(X_{r}\), and \(B(k)\) is the Jacobian matrix of the equation of state \(f(X_{r},u_{r})\) in \(u_{r}\). All of the above derivations are carried out in the case of a continuous system. To facilitate the calculation and control, the system needs to be discretized. The Euler method is used to discretize the Equation (17), which is:  
\[ \begin{array}{l}\dot{X}_{e}(k)=\frac{X_{e}(k+1)-X_{e}(k)}{I}\\ =A(k)X_{e}(k)+B(k)u_{e}(k)\end{array}  \tag{18}\]  
\[X_{e}(k+  
The output equation of the system in the future is:  
\[Y=\Psi\xi(k)+\Theta\Delta U \tag{23}\]  
where  
\[Y=\left[ \begin{array}{c}\eta(\mathrm{k}+1)\\ \eta(\mathrm{k}+2)\\ \vdots\\ \eta(\mathrm{k}+N_{p})\end{array} \right],\Psi=\left[ \begin{array}{c}\widetilde {C}_{k}\widetilde{A}_{k}\\ \widetilde{C}_{k}\widetilde{A}_{k}^{2}\\ \widetilde{C}_{k}\widetilde{A}_{k}^{N_{c}}\\ \widetilde{C}_{k}\widetilde{A}_{k}^{N_{c}}\\ \cdots\\ \widetilde{C}_{k}\widetilde{A}_{k}^{N_{p}}\end{array} \right],U_{\varepsilon}= \left[ \begin{array}{c}u_{\varepsilon}(k)\\ u_{\varepsilon}(k+1)\\ u_{\varepsilon}(k+2)\\ \cdots\\ u_{\varepsilon}(k+N_{c}-1)\end{array} \right]\]  
\[\Theta=\left[ \begin{array}{cccc}\widetilde{C}_{k}\widetilde{B}_{k}&0&0&\cdots &0\\ \widetilde{C}_{k}\widetilde{A}_{k}\widetilde{B}_{k}&\widetilde{C}_{k}\widetilde {B}_{k}&0&\cdots&0\\ \cdots&\cdots&\cdots&\ddots&\cdots\\ \widetilde{C}_{k}\widetilde{A}_{k}^{N_{c}-1}\widetilde{B}&\widetilde{C}_{k} \widetilde{A}_{k}^{N_{c}-2}\widetilde{B}_{k}&\widetilde{C}_{k}\widetilde{A}_{ k}^{N_{c}-3}\widetilde{B}_{k}&\cdots&\widetilde{C}_{k}\widetilde{A}_{k}^{0} \widetilde{B}_{k}\\ \cdots&\cdots&\cdots&\cdots&\cdots\\ \widetilde{C}_{k}\widetilde{A}_{k}^{N_{p}-1}\widetilde{B}_{k}&\widetilde{C}_{k }\widetilde{A}_{k}^{N_{p}-2}\widetilde{B}_{k}&\widetilde{C}_{k}\widetilde{A}_ {k}^{N_{p}-3}\widetilde{B}_{k}&\cdots&\widetilde{C}_{k}\widetilde{A}_{k}^{N_{ p}-N_{c}}\widetilde{B}_{k}\end{array} \right]\]  
From Equation (23), if the current state variable and control increment in the control horizon \(N_{C}\) are known, the system output in \(N_{P}\) can be predicted. The design objective function is:  
\[J=(Y-Y_{r})^{T}Q(Y-Y_{r})+\Delta u^{T}R\Delta u \tag{24}\]  
where \(Y_{r}=\left[ \begin{array}{c}\eta_{r}(k+1)\\ \eta_{r}(k+2)\\ \cdots\\ \eta_{r}(k+N_{p})\end{array} \right]=\left[