## 4 MPC Scheme Design
### MPC Problem Formulation  
The primary task of the stabilization control of high-speed AGVs is to ensure safe vehicle operation within the previously-defined constraints. This is realized by expressing the stabilization control as an optimal control problem to be evaluated over a finite prediction horizon, formulated as:  
\[\underset{\xi,\delta_{f},k=0,\cdots,N_{p}}{\text{minimize}} J= \sum_{k}\left(W_{e\psi}\left(e_{\psi}^{(k)}\right)^{2}+W_{e\psi} \left(e_{y}^{(k)}\right)^{2}\right) \tag{20a}\] \[+\sum_{k}W_{\delta f}\Big{(}\delta_{f}^{(k)}-\delta_{f}^{(k-1)} \Big{)}^{2}\] (20b) \[+\sum_{k}W_{\text{sh}}^{T}S_{sh}^{(k)}W_{\text{sh}}\] (20c) s.t. \[\xi^{(k+1)}=A_{s}\xi^{(k)}+B_{s1}\mathbf{u}_{1}^{(k)}+B_{s2}\mathbf{u}_{2}^ {(k)}\] (20d) \[\xi^{(k+1)}=A_{i}\xi^{(k)}+B_{1}\mathbf{u}_{aug}{}^{(k)}+B_{12}\mathbf{u }_{aug}{}^{(k+1)}\] (20e) \[\left|\mathbf{H}_{sh}^{k}\xi^{(k)}+\mathbf{P}_{sh}\mathbf{u}_{2}^{(k)}\right| \leq\mathbf{G}_{sh}^{k}+S_{sh}^{(k)}\] (20f) \[|g_{ZMP}|\leq g_{ZMP,max}\] (20g) \[\mathbf{H}_{\text{env}}\xi^{(k)}\leq\mathbf{G}_{\text{env}}^{(k)}\] (20h) \[\left|\delta_{f}\right|\leq\delta_{f,\max}\] (20i) \[\left|\Delta\delta_{f}\right|\leq\Delta\delta_{f,\max} \tag{20j}\]  
In this formulation, the variables to be optimized are the front wheel steering angle (\(\delta_{f}\)) and the slack variables on the constraints \(S_{sh}\). The tunable parameters are \(W\delta_{f}\), \(W_{e\psi}\), \(W_{e\psi}\) and the slack variable costs factor \(W_{sh}\). The cost function consists of three terms: (20a) enforces penalty on the vehicle states deviation; (20b) establishes the trade-off between fast convergence and a smooth input trajectory; (20c) enforces the penalty for slack variables' violations.  
(20d) and (20e) are the discretized vehicle dynamics models, derived from (9) and (10). Constraints (20f), (20g) and (20h) enforce constraints on the sideslip envelope, ZMP and road boundaries, respectively. (20i) and (20j) enforce the physical limits of the steering system. With the choice of sufficiently large weighting coefficient \(W_{sh}\), the cost term (20c) encourages zero-valued slack variables, resulting in optimal vehicle trajectories that adhere to the sideslip envelopes whenever possible.  
Along with the relative value of \(W\delta_{f}\), \(W_{e\psi}\), \(W_{e\psi}\) and \(W_{sh}\), a hierarchy can be established between the collision, stabilization and smooth steering. Setting \((W_{e\psi},W_{e\psi})\gg W_{sh}\gg W\delta_{f}\) encodes a prioritization of collision avoidance over vehicle stability and then over smooth steering. This prioritization allows the controller to selectively violate stability criteria if necessary to avoid a collision.  
The optimization (20) resolves at each step; as is standard with MPC, only the optimal input for the first step into the prediction horizon, \(\delta_{f}^{(0)}\), is executed. The convexity of the optimization problem  
Figure 7: Lateral safety corridor.  
allows for efficient real-time implementation. CVXGEN [31] generates a solver that is implemented on a Thinkpad W541 stationary computer with a quad-core i7-processor. The resulting controller is capable of solving the optimization problem in less than 50 ms, allowing for an execution rate of 20 Hz.