## 3 Obstacle Avoidance Motion Planning Framework
### Optimization  
A model predictive controller is used for iterative optimization of vehicle dynamics, compliance and obstacle avoidance paths, and trajectory tracking over the predicted time horizon.  
Before engaging in trajectory planning using MPC, it is essential to establish a global path that outlines the desired lane and desired speed for the MPC. The desired output to be tracked is defined as:  
\[y_{des}=\left[Y_{des}\ u_{des}\right]^{T} \tag{35}\]  
where \(y_{des}\) is the output matrix; \(Y_{des}\) and \(u_{des}\) are the desired lateral position and desired longitudinal velocity.  
The driving behaviors of the vehicle are categorized into two scenarios according to the planning framework, each corresponding to a distinct optimization equation. The optimization problem of motion planning is:  
\[\underset{u,\epsilon}{\text{min}}\sum_{k=1}^{Np}U_{\text{com}}{}^{(t+k,t)}+U_{ \text{obs}}^{(t+k,t)}+\left\|y^{(t+k,t)}-y_{\text{des}}^{(t+k,t)}\right\|_{Q}^{ 2}+\left\|u^{(t+k-1,t)}\right\|_{R}^{2}+\left\|u^{(t+k-1,t)}-u^{(t+k-2,t)} \right\|_{S}^{2}+\left\|\epsilon_{k}\right\|_{P}^{2} \tag{36}\]  
\[\underset{u,\epsilon}{\text{min}}\sum_{k=1}^{Np}U_{\text{vio}}{}^{(t+k,t)}+U_{ \text{obs}}^{(t+k,t)}+\left\|y^{(t+k,t)}-y_{\text{des}}^{(t+k,t)}\right\|_{Q}^ {2}+\left\|u^{(t+k-1,t)}\right\|_{R}^{2}+\left\|u^{(t+k-1,t)}-u^{(t+k-2,t)} \right\|_{S}^{2}+\left\|\epsilon_{k}\right\|_{P}^{2} \tag{37}\]  
\[x^{(t+k,t)}=A_{d}x^{(t+k-1,t)}+B_{d}u^{(t+k-1,t)} \tag{38}\]  
\[y^{(t+k,t)}=Cx^{(t+k,t)}+Du^{(t+k,t)} \tag{39}\]  
\[u_{c\_\text{min}}<u_{c}^{(t+k-1,t)}<u_{c\_\text{max}} \tag{40}\]  
\[\Delta u_{c\_\text{min}}<u_{c}^{(t+k-1,t)}-u_{c}^{(t+k-2,t)}<\Delta u_{c\_ \text{max}} \tag{41}\]  
\begin{table}
\begin{tabular}{c c c} \hline \hline
**Violation Object** & **Violation Type** & **Penalties** \\ \hline Lane line & & \\ Solid write line & Lane markings violation & 1 \\ Double solid yellow line & Wrong-way driving & 3 \\ Broken line & Long-time drive on line & 0 \\ Virtual line & Improper turn & 0 \\ Stop line & & \\ Marked stop line & Run a red light & 6 \\ Virtual stop line & Fail to yield & 3 \\  & Tailgating & 0 \\ speed & & \\ \(vx_{upper}<vx(ego)\) & Speeding & 0–6 \\ \(vx(ego)<0\) & Reverse at intersection & 0 \\ \(vx(ego)=0\) & Stop at no-parking zone & 0 \\ \hline \hline \end{tabular}
\end{table}
Table 4: Penalties for violations.  
where \(N_{p}\) is the prediction horizon, set to 20; \(N_{c}\) is the control horizon, set to 5; \(\epsilon_{k}\) is the vector of slack variables at the \(k\)th step. The states are predicted through Equation (38), where \(A_{d}\) and \(B_{d}\) are the discrete state and input matrices by discretizing Equation (12). The tracking outputs are calculated by Equation (39), in which \(C\) and \(D\) are the output and feedforward matrices. The control inputs and their changes are constrained in Equations (40) and (41). \(u_{c\_\min}\) and \(u_{c\_\max}\) are the matrices of the lower and upper bounds of the control input; \(\Delta u_{c\_\min}\) and \(\Delta u_{c\_\max}\) are the matrices of the lower and upper bounds of the control input changes; \(Q\), \(R\), \(S\), and \(P\) are weighting matrices.  
In normal driving scenarios, Equation (36) is utilized as the cost function within the MPC for compliance motion planning. In unanticipated obstacle scenarios, Equation (37) is utilized for obstacle avoidance motion planning. The PFs integrated with the optimization problem are nonlinear and non-convex. The method proposed by [PERSON] et al. [37] is employed to convert the optimization problem into an approximately quadratic convex.