## 3 Collision Avoidance Strategy
### Two-Layer Model Predictive Control (MPC) Modeling
 \tag{9}\]  
where the state vector is \(\kappa=\left[\dot{y}\;\dot{x}\;\varphi\;Y\;X\right]^{T}\) and the input vector is \(u=\left[a_{x}\;a_{y}\right]^{T}\). Since the point-mass vehicle model ignores the information of body size, the obstacles should be expanded. At the same time, the obstacle needs to be segmented according to a certain resolution to prevent the vehicles passing through the obstacle when its size is too large, shown as Figure 8.  
The principles of processing of road boundary and the parked vehicle are as follows:  
1. The actual boundary of the road should be preprocessed according to the width of the ego vehicle;
2. The parked vehicle is decomposed into several discrete small obstacles;
3. The cost of the function is adjusted by the vehicle speed and the distance deviation between the obstacle points and the vehicle. Figure 8: Obstacle expansion and the cost caused by parked vehicle. The cost function of the distance between the ego vehicle and parked vehicle is established as:  
\[J_{Parking}=\sum_{i=1}^{N_{p,1}}\sum_{n=1}^{N_{1}}\frac{S_{1}}{\left(X_{i}-X_{0,i }\right)^{2}+\left(Y_{i}-Y_{0,i}\right)^{2}+\zeta_{1}} \tag{10}\]  
where \(S_{1}\) is the weight coefficient. \(\zeta_{1}\) is a small positive number that prevents the denominator from being zero. \(N_{1}\) is total number of copies after the parked vehicle is segmented. \(X_{0,i}\) is the predictive position of the ego vehicle at step \(i\). Moreover, we consider that the road is divided into two opposite lanes and the global reference path is in one of them, shown as Figure 9. Therefore, in order to eliminate the deviation between the actual trajectory and the reference trajectory, the cost function of the road part is adjusted by the relative position between the ego vehicle and the road bounds. \[J_{road}=\left\{ \begin{array}{c}0,D_{sc2 Road,i}\geq D_{\min};\\ \sum\limits_{i=1}^{N_{p,1}}\frac{S_{2}}{D_{sc2 Road,i}+\zeta_{2}},D_{sc2 Road,i}<D_{\min} \end{array} \right. \tag{11}\]  
where \(S_{2}\) is the weight coefficient. \(\zeta_{2}\) is a small positive number that prevents the denominator from being zero. \(D_{\min}\) is the allowable minimum distance between the ego vehicle and the road bounds. \(D_{sc2 Road,i}\) is defined as:  
\[D_{sc2 Road,i}=\left\{ \begin{array}{c}\left|Y_{sf,j}-R_{ub}\right|,Y_{sf}-R_ {ub}\leq Y_{sf}-R_{lb};\\ \left|Y_{sf,j}-R_{lb}\right|,Y_{sf}-R_{ub}\geq Y_{sf}-R_{lb}\end{array} \right. \tag{12}\]  
where \(Y_{sf}\) is the longitudinal coordinate of the ego vehicle in world axis frame. \(R_{[ub,lb]}\) is the [upper, lower] bound constraint of the road. Similar to a parked vehicle and taking the position prediction of the oncoming vehicle into account, the cost function of oncoming vehicle part is defined as:  
\[J_{OnC}=\sum_{i=1}^{N_{p,1}}\sum_{n=1}^{N_{2}}\frac{S_{3}}{D_{sc2 moi}|_{n,T=i} \ +\zeta_{3}} \tag{13}\]  
Figure 9: Cost caused by road bounds. where \(S_{3}\) is the weight coefficient. \(\zeta_{3}\) is a small positive number that prevents the denominator from being zero. \(N_{2}\) is total number of copies after the oncoming vehicle is segmented. \(D_{sc2 mo}\big{|}_{n,T=i}\) is defined as:  
\[D_{sc2 mo}\big{|}_{n,T=i}\ =(X_{n,i}-X_{0,i})^{2}+(Y_{n,i}-Y_{0,i})^{2} \tag{14}\]  
where \(X_{n,i}\) is the predictive position of oncoming vehicle's \(n\)th block after being segmented at step \(i\). When combining road, parked vehicle and the oncoming vehicle, shown as the Figure 10, we solve the following optimization problem at each time step:  
\[ \begin{split}&\min_{U_{t}}\ J_{1}\ (\kappa(t),u(t))\\ &\ s.t.\\ & u_{min}\leq u(t)\leq u_{max}\\ & a_{x}^{2}+a_{y}^{2}\leq(\mu_{g})^{2}\end{split}  \tag{15}\]  
With the non-linear problem solver, the solution \(U_{t}^{*}=\left[u_{t,t}^{*},\ldots,u_{t+N_{c},2,t}^{*}\right]^{T}\) is obtained.