# Trajectory Tracking Control Strategy of 20-Ton Heavy-Duty AGV Considering Load Transfer
## 4 Design of Trajectory Tracking Control Strategy Considering Load Transfer
### Lateral Load Transfer IMPC Control Strategy
#### 4.2.3 Constraint Condition Design Based on Double Ackerman Angle Constraint  
In order to avoid the drastic change in control input and maintain the stability of the system, it is necessary to set the maximum and minimum constraints on the input control variable \(U_{e}\) and the control increment \(\Delta\)U and set the equality constraint of \(\delta_{f}=-\delta_{r}\) for the front and rear corners according to the (9). The relationship between the input control variable \(U_{e}\) and the control increment \(\Delta U\) can be obtained by derivation:  
\[U_{e} = \begin{bmatrix}u_{e}(k)\\ u_{e}(k+1)\\ \vdots\\ u_{e}(k+N_{c}-1)\end{bmatrix}=\begin{bmatrix}u_{e}(k-1)\\ u_{e}(k-1)\\ \vdots\\ u_{e}(k-1)\end{bmatrix}+\begin{bmatrix}I_{N_{c}}&0&0&\cdots&0\\ I_{N_{c}}&I_{N_{c}}&0&\cdots&0\\ \vdots&\vdots&\vdots&\ddots&\vdots\\ I_{N_{c}}&I_{N_{c}}&I_{N_{c}}&\cdots&I_{N_{c}}\end{bmatrix}\begin{bmatrix} \Delta u(k)\\ \Delta u(k+1)\\ \Delta u(k+2)\\ \vdots\\ \Delta u(k+N_{c}-1)\end{bmatrix} \tag{37}\] \[= U_{t}+A_{I}\Delta U\]  
Therefore, the inequality constraint of control increment can be obtained:  
\[\begin{cases}A_{I}\Delta U\leq U_{\max}-U_{t}\\ -A_{I}\Delta U\leq-U_{\min}+U_{t}\\ \Delta U\leq\Delta U_{\max}\\ \Delta U\geq\Delta U_{\min}\end{cases} \tag{38}\]  
where \(U_{t}\) is the actual input control variable at time \(k-1\), \(U_{\max}\) and \(U_{\min}\) are the maximum and minimum values of the input control variable \(U_{e}\), \(\Delta U_{\max}\) and \(\Delta U_{\min}\) are the maximum and minimum values of the control increment.  
From (9), \(\delta_{f}=-\delta_{r}\) can be obtained, and equality constraints need to be added. From (37), we can obtain  
\[U = \begin{bmatrix}u(k)\\ u(k+1)\\ \vdots\\ u(k+N_{c}-1)\end{bmatrix}=\begin{bmatrix}u_{e}(k-1)+u_{r}(k)\\ u_{e}(k-1)+u_{r}(k+1)\\ \vdots\\ u_{e}(k-1)+u_{r}(k+N_{c}+1)\end{bmatrix}+\begin{bmatrix}I_{N_{c}}&0&0&\cdots &0\\ I_{N_{c}}&I_{N_{c}}&0&\cdots&0\\ \vdots&\vdots&\vdots&\ddots&\vdots\\ I_{N_{c}}&I_{N_{c}}&I_{N_{c}}&\cdots&I_{N_{c}}\end{bmatrix}\begin{bmatrix} \Delta u(k)\\ \Delta u(k+1)\\ \Delta u(k+2)\\ \vdots\\ \Delta u(k+N_{c}-1)\end{bmatrix}\] \[= U_{t}+U_{r}+A_{I}\Delta U\]  
Therefore, the following formulas can be obtained:\[Eq=\begin{bmatrix}\delta_{f}(k)+\delta_{r}(k)\\ \delta_{f}(k+1)+\delta_{r}(k+1)\\ \vdots\\ \delta_{f}(k+N_{c}-1)+\delta_{r}(k+N_{c}-1)\end{bmatrix}=\begin{bmatrix}\kappa&0& \cdots&0\\ 0&\kappa&\cdots&0\\ \vdots&\vdots&\ddots&\vdots\\ 0&0&0&\kappa\end{bmatrix}U=K(U_{t}+U_{r}+A_{I}\Delta U) \tag{40}\]  
where \(\kappa=[1,1,0]\), \(U_{r}\) is the reference value of rotation angle and velocity at different moments.  
Thus, the equality constraint of control increment can be obtained:  
\[Eq=0 \tag{41}\]  
So far, the optimization problem of model predictive control can be transformed into a standard QP problem:  
\[\left\{\begin{aligned} \underset{\Delta U}{\min}J&=\frac{1}{2} \Delta V^{T}H\Delta V+G\Delta V\\ \text{s.t.}&\begin{cases}A_{I}\Delta U_{I}&\leq U_{\text{max}}-U_{t}\\ -A_{I}\Delta U_{I}&\leq-U_{\text{min}}+U_{t}\\ \Delta U&\leq\Delta U_{\text{max}}\\ \Delta U&\geq\Delta U_{\text{min}}\\ \quad\quad\quad\quad\quad Eq=0\end{cases}\end{aligned}\right. \tag{42}\]  
In each control cycle, a series of control increment \(\Delta\)U values can be obtained by solving the Equation (42). In order to improve the accuracy of the prediction, only the first result of the obtained control sequence is used as input to the system, and the same operation is performed at the next moment; the cycle is repeated until the trajectory tracking is completed.