# A New Path Tracking Method Based on Multilayer Model Predictive Control
## 2 Controller Design
### Path Tracking Controller with a Fixed Velocity  
Linear model predictive control (linear MPC) is very powerful in terms of its real-time performance, and it also has high accuracy when the reference velocity is low [26]. Consequently, we chose the linear MPC controller as the path tracking controller with a fixed velocity. In the process of designing  
Figure 1: The framework of multilayer model predictive control (MPC).  
this controller, we also referred to the work of [PERSON] et al. [27] and [PERSON] et al. [28]. The linear MPC controller includes two parts, the predictive model and the rolling optimization. Except for the reference velocities, the three linear MPC controllers are identical.  
The basis of the predictive model is the kinematic model of articulated vehicles. We still use the model in [5]. This model is very mature and its application is very extensive. In this kinematics model, we ignore the force of the tires and consider the bodies of the articulated vehicle as rigid bodies. The kinematics model is presented in Figure 2. The definition of the parameters is presented in [5].  
The matrix form of the kinematic model is:  
\[\left[\begin{array}{c}\dot{x}_{f}\\ \dot{y}_{f}\\ \dot{\theta}_{f}\\ \dot{\gamma}\end{array}\right]=\left[\begin{array}{c}v_{f}\cos\theta_{f}\\ v_{f}\sin\theta_{f}\\ \frac{v_{f}\sin\gamma}{L_{f}\cos\gamma+L_{\gamma}}\\ 0\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ \frac{L_{r}}{L_{f}\cos\gamma+L_{\gamma}}\\ 1\end{array}\right]\omega_{\gamma} \tag{3}\]  
where \(\omega_{\gamma}\) is the articulated angular speed.  
Carrying out Jacobian linearization on Equation (3), the result is:  
\[\widetilde{\mathbf{x}}(k+1|k)=\mathbf{A}\widetilde{\mathbf{x}}(k|k)+\mathbf{ B}\Delta\mathbf{u}(k|k) \tag{4}\]  
where:  
\[\widetilde{\mathbf{x}}(k|k)=\left[\begin{array}{c}x_{f}(k|k)-x_{f}(k-1|k)\\ y_{f}(  
Then Equation (16) can be rewritten as:  
\[J(\overline{\mathbf{x}}(k),\Delta\mathbf{U}(k))=\left\|\overline{\mathbf{v}} \overline{\mathbf{x}}(k|k)+\mathbf{\Theta}\Delta\mathbf{U}(k)-\overline{ \mathbf{v}}\overline{\mathbf{x}}_{ref}(k|k)\right\|_{\mathbf{Q}}^{2}+\left\| \Delta\mathbf{U}(k)\right\|_{\mathbf{R}}^{2}+\rho\varepsilon^{2} \tag{19}\]  
If:  
\[\mathbf{e}(k)=\overline{\mathbf{x}}(k|k)-\overline{\mathbf{x}}_{ref}(k|k) \tag{20}\]  
We can rewrite Equation (19) as:  
\[J(\overline{\mathbf{x}}(k),\Delta\mathbf{U}(k))=\frac{1}{2}\Big{[}\begin{array} []{cc}\Delta\mathbf{U}(k)^{\mathrm{T}}&\varepsilon\end{array}\Big{]}\mathbf{ H}\left[\begin{array}{cc}\Delta\mathbf{U}(k)\\ \varepsilon\end{array}\right]+\mathbf{G}\left[\begin{array}{cc}\Delta \mathbf{U}(k)\\ \varepsilon\end{array}\right]+C \tag{21}\]  
where:  
\[\begin{array}{l}\mathbf{H}=\left[\begin{array}{cc}\mathbf{\Theta}^{ \mathrm{T}}\mathbf{Q}\mathbf{\Theta}+\mathbf{R}&0\\ 0&\rho\end{array}\right]\\ \mathbf{G}=\left[\begin{array}{cc}2\mathbf{\Theta}^{\mathrm{T}}\mathbf{Q} \mathbf{v}\mathbf{e}(k)&0\end{array}\right]\\ C=\left(\mathbf{\Psi}\mathbf{e}(k)\right)^{\mathrm{T}}\mathbf{Q}\mathbf{\Psi} \mathbf{e}(k)+\rho\varepsilon^{2}\end{array} \tag{22}\]  
Combined with the constraints in [5], the final rolling optimization function is:  
\[\begin{array}{l}\min\!\!\!/(\overline{\mathbf{x}}(k),\Delta\mathbf{U}(k))\\ =\frac{1}{2}\Big{[}\begin{array}{cc}\Delta\mathbf{U}(k)^{\mathrm{T}}& \varepsilon\end{array}\Big{]}\mathbf{H}\left[\begin{array}{cc}\Delta \mathbf{U}(k)\\ \varepsilon\end{array}\right]+\mathbf{G}\left[\begin{array}{cc}\Delta \mathbf{U}(k)\\ \varepsilon\end{array}\right]\\ s.t.\begin{array}{l}-0.14\mathrm{rad}/\mathrm{s}\leq\dot{\gamma}\leq 0.14 \mathrm{rad}/\mathrm{s}\\ -0.70\mathrm{rad}\leq\gamma\leq 0.70\mathrm{rad}\end{array} \tag{23}\]  
We can use the quadprog function in MATLAB to solve Equation (23), and the solution algorithm can be specified as active-set. After the solution, the control input increment sequence in the control time domain can be obtained:  
\[\mathbf{u}(k+i|k)=\mathbf{u}(k+i-1|k)+\Delta\mathbf{u}(k+i-1|k) \tag{24}\]