# Optimal Vehicle Lane Change Trajectory Planning in Multi-Vehicle Traffic Environments
## 2 Methods
### Collision Avoidance Algorithm
In Formula (17), \(x_{k1,\ldots,4}(t)\) and \(y_{k1,\ldots,4}(t)\) are the coordinates of each vertex of the rectangle, \(\tau_{tHk}\) is the end time of the subject vehicle lane change, \(\tau_{i}\) is the end time of the traffic vehicle lane change, \(l_{H}\) and \(w_{H}\) are the length and width of the subject vehicle respectively, \((x_{k}(t),y_{k}(t))\) is the coordinate point of the subject vehicle's center of mass. The heading angle of the vehicle is \(\varphi_{k}(t)\) as follows:  
\[\varphi_{k}(t)=\arctan\frac{y_{k}(t+T)-y_{k}(t)}{x_{k}(t+T)-x_{k}(t)},0\leq t \leq\tau=\max\{\tau_{tHk},\tau_{i}\},i=1,\ldots,4 \tag{18}\]  
The collision between vehicles is approximately the intersection of two rectangles. According to geometric principles, the following Algorithm 1 is constructed to determine whether the rectangles intersect. ```
function[out] = \(ColL(x_{Mt}(t),y_{Mt}(t),x_{Tl}(t),y_{Tl}(t))\) \(A=(x_{1t}(t),y_{1t}(t));B=(x_{2t}(t),y_{2t}(t));C=(x_{3t}(t),y_{43}(t));D=(x_{ 44}(t),y_{44}(t))\) \(A_{i}=(x_{Tl}(t),y_{Tl}(t));B_{i}=(x_{Tl2}(t),y_{Tl2}(t));C_{i}=(x_{Tl3}(t), y_{Tl3}(t));D_{i}=(x_{Tl4}(t),y_{Tl4}(t))\) \(S_{A}=\left|\overset{\leftarrow}{A}A_{i}\times\overset{\leftarrow}{B}_{i} \right|+\left|\overset{\leftarrow}{A}B_{i}\times\overset{\leftarrow}{A}C_{i} \right|+\left|\overset{\leftarrow}{A}C_{i}\times\overset{\leftarrow}{A}D_{i} \right|+\left|\overset{\leftarrow}{D}_{i}\times\overset{\leftarrow}{A}A_{i} \right|\right)/2\) \(S_{B}=\left(\left|\overset{\leftarrow}{B}A_{i}\times\overset{\leftarrow}{B} B_{i}\right|+\left|\overset{\leftarrow}{B}B_{i}\times\overset{\leftarrow}{B}C_{i} \right|+\left|\overset{\leftarrow}{B}C_{i}\times\overset{\leftarrow}{B}D_{i} \right|+\left|\overset{\leftarrow}{B}D_{i}\times\overset{\leftarrow}{B}A_{i} \right|\right)/2\) \(S_{C}=\left(\left|\overset{\leftarrow}{A}A_{i}\times\overset{\leftarrow}{C} B_{i}\right|+\left|\overset{\leftarrow}{C}B_{i}\times\overset{\leftarrow}{C}C_{i} \right|+\left|\overset{\leftarrow}{C}C_{i}\times\overset{\leftarrow}{C}D_{i} \right|+\left|\overset{\leftarrow}{C}D_{i}\times\overset{\leftarrow}{C}A_{i} \right|\right)/2\) \(S_{D}=\left(\left|\overset{\leftarrow}{A}A_{i}\times\overset{\leftarrow}{B} B_{i}\right|+\left|\overset{\leftarrow}{B}B_{i}\times\overset{\leftarrow}{D}C_{i} \right|+\left|\overset{\leftarrow}{D}C_{i}\times\overset{\leftarrow}{D}D_{i} \right|+\left|\overset{\leftarrow}{D}D_{i}\times\overset{\leftarrow}{D}A_{i} \right|\right)/2\) \(S_{i}=\left|\overset{\leftarrow}{A}_{i}B_{i}\times A_{i}D_{i}\right|\); if\((S_{A}-S_{i})>0\epsilon(S_{B}-S_{i})>0\epsilon(S_{C}-S_{i})>0\epsilon(S_{D}-S_{i})>0\) \(\text{out}=0\); %Collision - free  else \(\text{out}=1\); %Collision end
```  
**Algorithm 1**Principle of \(Col\) function  
According to \(Col\) function, when the following rules are observed, whether the subject vehicle collides with the traffic vehicle can be judged:  
\[\left\{ \begin{matrix}x_{Tl}(t)=\left[x_{Tl1}(t)\right.&x_{Tl2}(t)&x_{Tl3}(t)&x_ {Tl4}(t)\right]\\ y_{Tl}(t)=\left[y_{Tl1}(t)&y_{Tl2}(t)&y_{Tl3}(t)&y_{Tl4}(t)\right]\end{matrix} \right. \tag{19}\]  
\[\left\{ \begin{matrix}x_{Hk}(t)=\left[x_{k1}(t)\right.&x_{k2}(t)&x_{k3}(t)&x_{k4 }(t)\\ y_{Hk}(t)=\left[y_{k1}(t)&y_{k2}(t)&y_{k3}(t)&y_{k4}(t)\right]\end{matrix} \right. \tag{20}\]  
\[\sum_{t=0}^{\tau}J_{k}(t)=\sum_{t=0}^{\tau}[CoL(x_{Tl}(t),y_{Tl}(t),x_{Hk}(t), y_{Hk}(t))]=0 \tag{21}\]  
In the formula, \(i=1,2,3,4\) are respectively represented as the four traffic vehicles in different lanes, the coordinates of each vertex of the rectangle and the schematic diagram  
Figure 6: (a) Point P is outside the rectangle; (b) Point P is inside the rectangle. of whether there is a collision are shown in Figure 7, where \([x_{T\_in H}(t),y_{T\_in H}(t)]\) is the vertex coordinate of the traffic vehicle, \(i\) represents the traffic vehicle, \(n\) represents a certain vertex, The lower left corner of the rectangle is marked as 1, and the remaining numbers increase counterclockwise along the rectangle.