## 3 Collision Avoidance Strategy
### Two-Layer Model Predictive Control (MPC) Modeling
#### 3.2.2 Reference Trajectory Tracking Layer
The vehicle nonlinear dynamic model is:  
\[F_{yr} =\overline{C}_{ar}\alpha_{r} \tag{19}\]1  
Firstly, to form an MPC problem, the bicycle model is linearized by Taylor formula at the reference point and discretized by the Forward-Euler method as follows:  
\[F_{yr} =\overline{C}_{ar}\alpha_{r} \tag{19}\]2  
\[F_{yr} =\overline{C}_{ar}\alpha_{r} \tag{19}\]3  
where \(A_{k}=I+A_{t},B_{k}=T_{s}B_{t}\) and \(d(k)=\xi_{ref}(k+1)-A_{k}\xi_{ref}(k)-B_{k}u(k)\). \(A_{t}=\left.\frac{\partial f}{\partial\xi}\right|_{\xi_{t},u_{t}}\), \(B_{t}=\left.\frac{\partial f}{\partial u}\right|_{\xi_{t},u_{t}}\). \(T_{s,2}=0.01\) s is the simulation step. \(\eta_{2}(k)=[\varphi\ Y]^{T}\) is the output vector and The predictive state is defined as:  
\[F_{yr} =\overline{C}_{ar}\alpha_{r} \tag{19}\]4  
where \(\widetilde{A}_{k}=\left[ \begin{array}{cc}A_{k}&B_{k}\\ 0_{m\times n}&I_{m}\end{array} \right];\widetilde{B}_{k}=\left[ \begin{array}{c} B_{k}\\ I_{m}\end{array} \right];\widetilde{C}_{k}=\left[ \begin{array}{c}C_{k}\,0 \end{array} \right];\widetilde{d}(k)=\left[ \begin{array}{c}d(k)\\ 0_{m}\end{array} \right];\Delta u(k)=u(k)-u(k-1)\)\(m\) is the dimension of control amount while \(n\) is the dimension of state amounts. Therefore, the final output expression can be expressed as:  
\[\gamma(k+1)=\psi\xi(k)+\Theta\Delta u(k)+\Gamma\Phi(k) \tag{34}\]  
where  
\[\gamma(k+1)=\left[ \begin{array}{c}\eta_{2}(k+1)\\ \vdots\\ \eta_{2}(k+N_{c,2})\\ \vdots\\ \eta_{2}(k+N_{p,2})\end{array} \right],\psi=\left[ \begin{array}{c} \widetilde{C}_{k}\widetilde{A}_{k}\\ \vdots\\ \widetilde{C}_{k}\widetilde{A}_{k}^{N_{c,2}-2}\\ \vdots\\ \widetilde{C}_{k}\widetilde{A}_{k}^{N_{p,2}}\end{array} \right],\ \Delta U(k)=\left[ \begin{array}{c} \Delta u(k)\\ \Delta u(k+1)\\ \vdots\\ \Delta u(k+N_{c,2}-1)\end{array} \right],\] \[\Theta=\left[ \begin{array}{c}\widetilde{C}_{k}\widetilde{B}_{k} &0&0&0\\ \vdots&\vdots&\vdots&\vdots\\ \widetilde{C}_{k}\widetilde{A}_{k}^{N_{c,2}-1}\widetilde{B}_{k}&\widetilde{C} _{k}\widetilde{A}_{k}^{N_{c,2}-2}\widetilde{B}_{k}&\cdots&\widetilde{C}_{k} \widetilde{B}_{k}\\ \vdots&\vdots&\ddots&\vdots\\ \widetilde{C}_{k}\widetilde{A}_{k}^{N_{p,2}-1}\widetilde{B}_{k}&\widetilde{C} _{k}\widetilde{A}_{k}^{N_{p,2}-2}\widetilde{B}_{k}&\cdots&\widetilde{C}_{k} \widetilde{A}_{k}^{N_{p,2}-N_{c,2}}\widetilde{B}_{k}\end{array} \right],\] \[\Gamma=\left[ \begin{array}{c}\widetilde{C}_{k}&\cdots&0\\ \vdots&\ddots&\vdots\\ \widetilde{C}_{k}\widetilde{A}_{k}^{N_{p,2}-1}&\cdots&C\end{array} \right],\ \Phi(k)=\left[ \begin{array}{c}\widetilde{d}(k)\\ \vdots\\ \widetilde{d}\big{(}k+N_{p,2}-1\big{)}\end{array} \right],\]  
and \(N_{p,2}\), \(N_{c,2}\) are the prediction horizon in tracking layer and the control horizon in tracking layer, respectively. At each time step we solve the following optimization problem:  
\[\min_{\Delta U_{t},\varepsilon}J_{2}\left(\xi(t),u(t),\Delta U(t),\varepsilon\right)\] \[s.t.\] \[\Delta U_{\min}\leq\Delta U_{t}\leq\Delta U_{\max}\] \[U_{\min}\leq A\Delta U_{t}+U_{t}\leq U_{\max} \tag{35}\] \[y_{hc,\min}\leq y_{hc}\leq y_{hc,\max}\] \[y_{sc,\min}-\varepsilon\leq y_{sc}\leq y_{sc,\max}+\varepsilon\] \[\varepsilon>0\]  
where \(y_{hc,[\min,\max]}\) is the [upper, lower] bound on output variables to be hard constrained; \(y_{sc,[\min,\max]}\) is the [upper, lower] bound on output variables to be soft constrained. As it is a convex optimization problem, the function can be reconstructed as a standard quadratic form:  
\[J_{2}(\xi(t),u(t-1),\Delta U(t),\varepsilon)=\left[\Delta U(t)^{T},\varepsilon \right]^{T}H_{t}\Big{[}\Delta U(t)^{T},\varepsilon\Big{]}+G_{t}\Big{[}\Delta U( t)^{T},\varepsilon\Big{]}+P_{t} \tag{36}\]  
where \(H=\left( \begin{array}{cc}\Theta^{T}Q_{\varepsilon}\Theta+R_{\varepsilon}&0\\ 0&\rho\end{array} \right)\), \(G_{t}=\left[ \begin{array}{cc}2E^{T}Q_{\varepsilon}\Theta&0\end{array}