# Trajectory Planning with Time-Variant Safety Margin for Autonomous Vehicle Lane Change +
## 2 Methodology
### Trajectory Planning with Time Variant Safety Margin
#### 2.2.1 Trajectory Planning  
During the trajectory planning of vehicle, safety and comfort are two influential factors for the performance evaluation. The safety mainly refers to avoid potential collisions, which keep a distance  
Figure 3: Diagram of the parameters used in calculating each gap’s score. away from other vehicles; and the comfort refers to minimize the variation of vehicle movement, such as the changing rate of velocity and acceleration. In order to consider these factors during the trajectory planning, a cost function for the optimum programming is designed in this section. The basic cost function for trajectory planning is designed to minimize the changes of the motion status of host vehicle \(V_{H}\), such as vehicle speed, acceleration and jerk, while keeping them within the longitudinal and lateral constraints. Here the longitudinal and lateral trajectory planning employ similar cost functions by utilizing a Quadratic Programming (QP) method. The function for the longitudinal trajectory planning is defined as  
\[ \begin{split} J_{x}&=q_{1x}\left|vx_{H}-vx_{H,des} \right|^{2}+q_{2x}\left|ax_{H}\right|^{2}\\ &+q_{3x}\left|jx_{H}\right|^{2}+q_{ex}\left(\left|e_{x1}\right|^{ 2}+\left|e_{x2}\right|^{2}\right)\\ s.t.&\begin{bmatrix}vx_{min}\\ ax_{min}\\ jx_{min}\end{bmatrix}-\epsilon_{x1}\leqslant\begin{bmatrix}vx_{H}\\ ax_{H}\\ jx_{H}\end{bmatrix}\leqslant\begin{bmatrix}vx_{max}\\ ax_{max}\\ jx_{max}\end{bmatrix}+\epsilon_{x2}\\ &\left[x_{H}\left(1\right),vx_{H}\left(1\right),ax_{H}\left(1\right)\right]= \left[x_{H1},vx_{H1},ax_{H1}\right]\\ &0\leqslant\epsilon_{xi}\leqslant\begin{bmatrix}M_{xxi}E\\ M_{xi}E\\ M_{jxi}E\end{bmatrix},\quad i=1,2\end{split}  \tag{12}\]  
where the longitudinal position, speed, acceleration and acceleration jerk of \(V_{H}\) are represented as \(x_{H}\), \(vx_{H}\), \(ax_{H}\) and \(jx_{H}\), they are discrete series formulated as the column vectors with length \(k_{end}\), and \(k_{end}\) is the length of the discretization for prediction horizon. \(E=[1,1,\ldots,1]^{T}\) is a column vector also with length \(k_{end}\); \(vx_{H,des}\) is the desired longitudinal speed of \(V_{H}\); \(q_{1x}\), \(q_{2x}\), \(q_{3x}\) and \(q_{ex}\) are weight coefficients. The initial parameter values \(x_{H1}\), \(vx_{H1}\) and \(ax_{H1}\) are obtained through measurement at the start of lane change. To improve the success rate of trajectory planning, slack variables \(\epsilon_{x1}\) and \(\epsilon_{x2}\) are employed to loosen the constraints. The slack variables are only applied during trajectory re-planning due to unexpected traffic events. The upper bounds of the slack variables are determined by \(M_{yxi}\), \(M_{axi}\) and \(M_{jxi}\). The cost function of the lateral trajectory planning can be obtained by change the parameters of Equation (12) with \(y_{H}\), \(vy_{H}\), \(ay_{H}\), \(jy_{H}\), \(vy_{H,des}\), \(q_{1y}\), \(q_{2y}\), \(q_{3y}\), \(q_{ey}\), \(M_{yyi}\), \(M_{ayi}\) and \(M_{yi}\). All of the predefined longitudinal and lateral constraints are listed in Table 1. The velocities of all traffic vehicles will be varied within the range of the constraints. For the algorithm of trajectory planning, we need to specify a planning horizon. Based on the prediction horizon for the traffic vehicles, it is reasonable to set the trajectory planning horizon the same as \(t_{p}\). Since the actual time horizon for LC process is less than the planning horizon.