## 3 Collision Avoidance Strategy
### Two-Layer Model Predictive Control (MPC) Modeling  
After behavior is re-planned, the global reference path should be re-planned based on the real-time relative position of the ego vehicle and obstacles. Then the re-planned path will be tracked. When building a vehicle mathematical model, it is necessary to comprehensively consider factors such as performance and calculation speed. Due to the large amount of calculation bring by the algorithm of the re-planning layer, the main purpose of introducing MPC is to ensure the re-planning results meet the constraints of vehicle dynamics and kinematics. Therefore, in order to reduce the calculation, there is no advantage when choosing a complex model. A comparison conducted proved that adopting a relatively lower precision model at the re-planning layer and a relatively higher precision model at the tracking layer can balance control performance and calculation speed. Therefore, a bicycle model and a point mass model are adopted at the re-planning layer and the tracking layer, respectively. The core work of this layer is to design a reasonable evaluation function to realize the obstacle avoidance and reduce the deviation between the re-planned path and global reference path while satisfying the constraints. Then, the local reference path fitting function and the reference longitudinal acceleration are output to the tracking layer. In this layer, we define the cost function as follows:  
\[J_{1}=\sum_{i=1}^{N_{p,1}}\|\eta_{1}(t+i|t)-\eta_{ref,global}(t+i|t)\|_{Q_{1}}^ {2}+\sum_{i=1}^{N_{c,1}}\|U_{i}(t+i|t)\|_{R_{1}}^{2}+J_{ob} \tag{3}\]  
where \(\eta_{1}(t+i|t)=Y(t+i|t)\) is the output vector. \(\eta_{ref,global}(t+i|t)\), with \(i=1,\ldots,N_{p,1}\), is the initial trajectory reference, for the output tracking variable. \(U_{i}= \begin{bmatrix}a_{x}&a_{y}\end{bmatrix} ^{T}\) is control vector. \(N_{p,1}\), \(N_{c,1}\) are the prediction horizon in replanning layer and the control horizon in re-planning layer, respectively. \(Q_{1}\in\mathbb{R}^{p_{y,1}\times p_{y,1}},R_{1}\in\mathbb{R}^{m_{1}\times m_ {1}}\) are weighting matrices. \(J_{ob}=J_{Pariking}+J_{road}+J_{OnC}\) is the total cost of obstacles avoidance. The above variables and constraints will be explained in detail in the following subsections. In this function, the first term suppresses the lateral deviation and heading deviation of the ego vehicle relative to the reference trajectory. The second item requires ego vehicles to keep speed unchanged as far as possible, which also ensures the comfort of the vehicle. In this layer, the vehicle is treated as a point with a given mass \(m\), i.e., no orientation is defined and the yaw dynamics is neglected [29], shown as Figure 7.  
Figure 7: Point mass vehicle model for trajectory re-planning layer. The state of motion and the state of force can be expressed as:  
\[\dot{X}=\dot{x}\cos\varphi-\dot{y}\sin\varphi \tag{4}\]  
\[\dot{Y}=\dot{x}\sin\varphi+\dot{y}\cos\varphi \tag{5}\]  
\[ma_{y}=F_{y} \tag{6}\]  
\[ma_{x}=F_{x}-F_{d} \tag{7}\]  
where subscript \([x,y]\) is the [longitudinal, lateral] axle in the vehicle body frame. \(X,Y\) are the Cartesian coordinates that belong to the world axis frame. \(F_{x}\), \(F_{y}\) are the resultant force in longitudinal and lateral axles in the vehicle body frame, respectively. \(F_{d}=ma_{d}\) represents the resultant force of resistance in the longitudinal axle. The maximum tire forces should be constrained by the maximum adhesion provided by ground. In particular, for a given friction coefficient \(\mu\) and the tire normal force \(F_{z}\), the lateral and longitudinal forces are constrained as follows:  
\[F_{x}^{2}+F_{y}^{2}\leq(\mu F_{z})^{2} \tag{8}\]  
Therefore the vehicle dynamics described by the Equations (4)-(7) can be rewritten in the following compact from:  
\[ \begin{array}{l}\dot{\kappa}(t)=f^{pm}(\kappa(t),u(t))\\ \qquad\qquad\qquad\qquad\qquad s.t.\\ u_{min}\leq u(t)\leq u_{max}\\ \qquad\qquad a_{x}^{2}+a_{y}^{2}\leq(\mu g)^{2}\end{array}