Optimal Vehicle Lane Change Trajectory Planning in Multi-Vehicle Traffic Environments

Featured Application

This paper is mainly dedicated to research on a trajectory planning strategy for intelligent vehicle autonomous lane changing in the V2V scenario of an urban multi-vehicle traffic environment.

Abstract

Autonomous driving technology in urban environments is a very important avenue of research. Notably, the question of how to plan safe lane-changing trajectories is a challenge in multi-vehicle traffic environments. In our research, three kinds of polynomial lane changing mathematical models were analyzed and compared. It was found that the fifth polynomial is the most suitable for lane changing trajectories; it is defined as a generalized lane-changing trajectory cluster, whereby the minimum lane change time is determined by the vehicle lateral stability threshold. Here, a collision avoidance algorithm is proposed to eliminate unsafe trajectories. Finally, the TOPSIS algorithm is used to solve the multi-objective optimization problem, and the optimal lane-changing expected trajectory is obtained from the safe trajectory cluster. The simulation results showed improvements in lane-changing efficiency of 6.67% and no collisions in the overtaking condition. In general, the proposed method of identifying the optimal lane changing trajectory can achieve safe, efficient and stable lane changing.

1. Introduction

With rapid economic growth, artificial intelligence will be more widely applied to automobile development. At present, Automatic Driving Systems (ADSs) are being developed to prevent accidents and reduce carbon emissions [1]. If automatic driving can be achieved, traffic congestion will be alleviated. In addition, if accidents and fuel consumption are reduced [2], the annual social benefits of autonomous driving systems are expected to reach nearly $800 billion by 2050 [3]. However, automatic driving systems are not yet mature for use in urban environments, and have already resulted in many traffic accidents, making many people wary of using them [4]. As early as 2005, in the DARPA competition, five teams completed the cross-country project. The biggest difficulty of automatic driving technology is not cross-country, but rather, urban environments [5]. On structured roads, the traffic environments are very complex, and vehicles need to interact with multiple vehicles during the lane-changing process [6]. Therefore, the development of a safe and feasible active lane-changing system remains a challenge.

A complete automatic lane change system includes environmental perception, behavior decision, trajectory planning and trajectory tracking control [7]. Trajectory planning comprises the calculation of a lane change trajectory after receiving a lane change signal [8]. Path planning only plans a reference trajectory without considering traffic conditions. Trajectory planning should take into account the complex traffic environment. In this respect, there are five types of methods: the Graph Search Algorithm, the Sampling-based Algorithm, the Interpolating Curve Algorithm and the Optimization Algorithm [9].

Sampling-based algorithms may be divided into two kinds: Probabilistic Road Maps (PRMs) and Rapidly Explore Random Tree (RRTs) [10]. The basic principle of the sampling-based algorithm is to collect the path points between the initial point and the end point, thereby connecting the points to form a trajectory. In this way, a safe and smooth path is proposed as a reference trajectory [11]. Liu et al. from Nanchang University of Aeronautics and Astronautics [12] improved the probabilistic road map algorithm and applied the genetic algorithm to optimal path planning. The biggest advantage of their approach was the reduced algorithm operation time. Lan et al. from Boston University [13] planned a safe overtaking trajectory using a second-order RRT algorithm. Although it was computationally efficient, the planned path was not smooth enough [14], which presented a major challenge for trajectory tracking control.

The graph search algorithm comprises the A* and Dijkstra algorithms. The optimized Dijkstra algorithm was used by researchers at the Korea University of Science and Technology to plan the shortest lane change trajectory [15]. The A* algorithm was optimized by Wang C (Sun Yat-sen University) et al. [16], allowing it to solve the collision avoidance problem and plan the shortest trajectory that can be applied to curves. The biggest drawback of the graph search algorithm is that it cannot avoid collisions with moving obstacles, and it is ineffective in multi-vehicle traffic environments.

The basic principle of the optimization algorithm, e.g., artificial potential field algorithm, model prediction algorithm, numerical optimization algorithm, is to establish an optimization index and constraint, and then to solve the optimal control quantity to calculate the optimal reference trajectory [17]. The artificial potential field algorithm is widely used in the field of robotics. The principle is that the potential field of the target position is low, that of the obstacle is high, and that of vehicles tends to be low [18]. Professor Zheng L of Chongqing University [19] solved the collision avoidance problem of vehicles in dynamic environments based on an artificial potential field. Yokoyama et al. from the National Defense Academy, Kanagawa [20] proposed a model predictive control algorithm for autopiloted aircraft, allowing multiple aircraft to fly without collisions in three-dimensional space. The algorithm requires high accuracy, and while it was difficult and time-consuming to solve the optimization problem, it was more difficult to target multiple models. Numerical optimization algorithms generally belong to online trajectory planning. Li B. et al. of Zhejiang University used a numerical optimization algorithm to solve the problem of multi-vehicle cooperative lane change trajectory planning [21]. Those authors noted that it was difficult to solve the numerical optimization problem, which will be a significant challenges in subsequent research.

At present, the interpolation curve algorithm is the most popular in lane-changing scenarios involving intelligent vehicles. Professor Luo YG of Tsinghua University proposed the replanning trajectories with the fifth polynomial, whereby the vehicle returns to the original lane in dangerous scenarios [22].

The lane-changing trajectory must satisfy not only the safety constraints, but also the efficiency and lateral stability of lane-changing in a multi-vehicle traffic environment. Single collision avoidance and optimization algorithms have been applied to lane-changing trajectory planning by many scholars, but in this paper, a new research method combining lateral stability constraints, a collision avoidance algorithm and an optimization algorithm is proposed. Our research method is divided into four steps: the first is to analyze and compare the polynomial lane changing model; the second defines the vehicle lateral stability constraints; the third step involves the online collision avoidance algorithm, designed to search for safe trajectory clusters; and in the final step, the multi-objective optimization problem is solved by the TOPSIS algorithm to obtain the optimal lane changing trajectory. Three typical working conditions were verified by simulations. The results show that the proposed algorithm can be used to solve the problem of collision-free trajectory planning problem in multi-vehicle traffic environments.

2. Methods

Environmental perception is based on V2V transmission information, whereby behavior decisions directly output lane changing instructions. The overall framework of this paper is shown in Figure 1.

2.1. Lane-Changing Trajectory Model

The vehicle is assumed to be travelling in a straight line and at the same longitudinal speed before changing lanes. To improve comfort, it is necessary to reduce longitudinal and transverse fluctuations. As such, the vehicle’s transverse velocity, longitudinal displacement, lateral displacement and longitudinal acceleration are all zero at the initial moment. The first step is to set up a lane-changing model. Conventional lane-changing trajectory models include cubic, fifth and seventh-degree polynomials [23]. The initial expression of the cubic polynomial is:

$$y_{\mathrm{des}3}={\displaystyle \sum _{i=0}^3{c}_i}{x}^i=c_0+c_1{x}^1+c_2{x}^2+c_3{x}^3$$

(1)

where $x$ is the longitudinal displacement of the vehicle; $y_{\mathrm{des}}$ is the ideal lateral displacement of the vehicle, and $c_i$ is the fitting coefficient. The initial position of the vehicle is $(x_0,y_0)$, and the end position is $(x_t,y_t)$. The vehicle is traveling at a constant speed, so its lateral displacement is $y_0=0$, its lateral speed is $\dot{y}{|}_{x=x_0}=0$ and its lateral acceleration is $\ddot{y}{|}_{x=x_0}=0$ at this time. The lateral displacement $y_t=a$ of the vehicle at the end of collision avoidance and the longitudinal travel distance $x_t=b$. The constraint of the reference trajectory is summarized in the following formula.

$$\left\{\begin{array}{c}{x}_0=0\\ y_0=0\\ \dot{y}{|}_{x=0}=0\\ \ddot{y}{|}_{x=0}=0\\ y_t=w\\ x_t=l\end{array}\right.$$

(2)

Based on the above formula, the reference path of the cubic polynomial is calculated as:

$$y_3=3\frac{w}{l^2}{x}^2-2\frac{w}{l^3}{x}^3$$

(3)

The lateral velocity and lateral acceleration constraints of the vehicle at the end moment of collision avoidance are taken into account in the fifth polynomial reference path, as shown in the following formula:

$$y_{des5}={\displaystyle \sum _{i=0}^5{c}_i{x}^i=c_0}+c_1{x}^1+c_2{x}^2+c_3{x}^3+c_4{x}^4+c_5{x}^5$$

(4)

This reference path constraint is as follows:

$$\left\{\begin{array}{c}{x}_0=0\\ y_0=0\\ \dot{y}{|}_{x=0}=0\\ \ddot{y}{|}_{x=0}=0\\ y_t=w\\ x_t=l\\ \dot{y}{|}_{x=t}=0\\ \ddot{y}{|}_{x=t}=0\end{array}\right.$$

(5)

where $\dot{y}{|}_{x=t}$ and $\ddot{y}{|}_{x=t}$ are lateral vehicle speed and lateral acceleration at the end of the lane change, respectively. From the above formula, the reference path of the fifth polynomial is calculated as:

$$y_5=10\frac{w}{l^3}{x}^3-15\frac{w}{l^4}{x}^4+6\frac{w}{l^5}{x}^5$$

(6)

The rate of change of lateral acceleration at the initial and end moments of the vehicle is further considered for the planning of the seventh-degree polynomial reference path:

$$y_{des7}={\displaystyle \sum _{i=0}^7{c}_i{x}^i=c_0}+c_1{x}^1+c_2{x}^2+c_3{x}^3+c_4{x}^4+c_5{x}^5+c_6{x}^6+c_7{x}^7$$

(7)

The reference path constraint conditions are as follows:

$$\left\{\begin{array}{c}{x}_0=0\\ y_0=0\\ \dot{y}{|}_{x=0}=0\\ \ddot{y}{|}_{x=0}=0\\ y_t=w\\ x_t=l\\ \dot{y}{|}_{x=t}=0\\ \ddot{y}{|}_{x=t}=0\\ \stackrel{⃛}{y}{|}_{x=0}=0\\ \stackrel{⃛}{y}{|}_{x=t}=0\end{array}\right.$$

(8)

where $\stackrel{⃛}{y}{|}_{x=0}$ and $\stackrel{⃛}{y}{|}_{x=t}$ are the lateral jerk at the start and end of the lane change, respectively. From the above formula, the reference path of the seventh-degree polynomial is calculated as:

$$y_7=35\frac{w}{l^4}{x}^4-84\frac{w}{l^5}{x}^5+70\frac{w}{l^6}{x}^6-20\frac{w}{l^7}{x}^7$$

(9)

Comparing the lateral displacements under the constraints of a longitudinal vehicle speed of 72 km/h, a longitudinal distance of the obstacle ahead of $l=50$ m and a lateral deflection of the vehicle $w=3.75$ m, the lateral displacements under the third, fifth and seventh order polynomial planning are determined. The result is shown in Figure 2.

The reference transverse angular velocity and lateral acceleration of the fifth polynomial are relatively smaller and simpler to calculate than those of the seventh order polynomial. In summary, the fifth polynomial provides both comfortable and efficient trajectories for changing lanes. Figure 2d shows fifth polynomial reference lane change trajectories at different speeds. If the longitudinal speed of the vehicle is constant, the only variable of this expression is $t_f$, so the analytic formula can be rewritten as:

$$\left\{\begin{array}{l}x(t)=ut\hfill \\ y(t)=10\frac{w}{t_f{}^3}{t}^3-15\frac{w}{t_f{}^4}{t}^4+6\frac{w}{t_f}{t}^5\hfill \end{array}\right.$$

(10)

In the geodetic coordinate system, the longitudinal speed of the vehicle remains constant when changing lanes, that is: $u_{x0}=u_x(t)=u$. Due to $x^n=u^n{t}^n,l^n=u^n{t}_f^n$, divided by $u^n$ to get Formula (10), where $w$ is lane width, $t$ is lane change time, and $t_f$ is lane change end time. Different lane changing end times lead to different trajectories. The optimal lane changing trajectory will be selected from the fifth polynomial trajectory cluster.

2.2. Vehicle Lateral Dynamic Stability Constraint

The 2-DOF vehicle model was built to determine the boundary value of vehicle lateral stability, from which some trajectory clusters that exceed the lateral stability constraints can be eliminated, the 2-DOF vehicle model is shown in Figure 3. The remaining trajectory clusters are lane changing trajectory clusters with lateral stability.

According to automobile theory, the relationship between the vehicle yaw rate and the front and rear axle slip angles can be determined using the following formula [24]:

$$\stackrel{·}{\beta }=\frac{F_{yr}+F_{yf}\cos {\delta }_f}{m{v}_x}-\gamma \approx \frac{F_{yr}+F_{yf}}{mu}-\gamma$$

(11)

where $\stackrel{·}{\beta }$ is the side-slip angular velocity of the center of mass, $F_{yf}$ and $F_{yr}$ are the lateral force of the front axle and the lateral force of the rear axle, respectively, ${\delta }_f$ is the front wheel angle of the vehicle (the left and right wheel angles are approximately equal), $m$ is the vehicle mass, and $\gamma$ is the yaw rate. To keep the vehicle lateral dynamics in a stable state, $\stackrel{·}{\beta }$ approaches 0 [25], and the yaw rate limit can be deduced using Formula (12).

$$\left|{\gamma }_s\right|=\frac{F_{yr}+F_{yf}}{mu}\le \frac{mg\mu }{mu}=\frac{g\mu }{u}$$

(12)

The brush tire model can be used to calculate the tire lateral force, and the tire slip angle can be approximated according to the vertical force. Then, the brush tire model is referenced to constrain the side slip angle of the vehicle’s center of mass, as Formula (13):

$$F_{yf,r}=\left\{\begin{array}{l}-C_{\alpha f,r}\tan {\alpha }_{f,r}+\frac{C_{\alpha f,r}^2\left|\tan {\alpha }_{f,r}\right|}{3\mu F_z}\tan {\alpha }_{f,r}-\frac{C_{\alpha f,r}^3\tan {\alpha }_{f,r}^3}{27{\mu }^2{F}_z^2},{\alpha }_{f,r}<\arctan (\frac{3\mu F_{zf,r}}{C_{\alpha f,r}})\hfill \\ -\mu F_z\mathrm{sgn}{\alpha }_{f,r},{\alpha }_{f,r}\ge \arctan (\frac{3\mu F_{zf,r}}{C_{\alpha f,r}})\hfill \end{array}\right.$$

(13)

where $F_y$ is tire lateral force, ${\alpha }_{f,r}$ is the side slip angle between the rear axle and the front axle, $F_{zf,r}$ is the vertical force of the rear or front axle of the vehicle, and $C_{\alpha f,r}$ is the cornering stiffness of the rear and front axles. Based on the assumption of a small angle, the range of stability is:

$${\alpha }_r\approx \beta -\frac{b{\gamma }_s}{u}\le {\tan }^{-1}\left(\frac{3mg\mu }{C_{\alpha r}}\cdot \frac{a}{a+b}\right)$$

(14)

where $a$ and $b$ are the distances from the front and rear axles to the center of mass of the vehicle, derived from Formulas (13) and (14). The limit of the side slip angle of the center of mass can be obtained as:

$$\left|{\beta }_s\right|\le {\tan }^{-1}\left(\frac{3mg\mu }{C_{\alpha r}}\cdot \frac{a}{l}\right)+\frac{b{\gamma }_s}{u}$$

(15)

In summary, the stability limits of the vehicle yaw rate and the of sideslip angle of centroid can be determined using Formulas (12) and (14) respectively. From the above vehicle dynamics stability limit, in order to accurately quantify the lateral dynamic stability limit, it is necessary to know the vehicle model and other relevant parameters (such as the parameters of the steering and tire systems. The D-class vehicle in CarSim is used as the research object; it is equipped with a 150 kw engine, a six-speed automatic transmission, an independent suspension, and an ABS (Anti-lock Braking System) system.

The vehicle changes lanes at a speed of 72 km/h. Different expected trajectories are calculated by changing the end time of different lane changes. At the same time, the road adhesion coefficient and vehicle speed are constantly changed to complete the lane change. The time of each lane change can be measured. Through data fitting, the following three-dimensional diagram can be obtained:

It can be found from Figure 4 that within the vehicle dynamic stability limit, $t_f$ becomes larger, the vehicle speed increases, and the adhesion coefficient decreases; when the road adhesion coefficient is larger, x decreases significantly; this is also in line with the driver’s daily driving habits. Therefore, for a specific vehicle, when the road environment and vehicle speed are determined, there must be a minimum lane change completion time, denoted as $t_{f\min }$, at this time, the non-stable lane change trajectory cluster can be eliminated, and the remaining trajectory clusters are called stable trajectory clusters [26].

2.3. Collision Avoidance Algorithm

The traditional method for studying safe lane change is the minimum safe distance algorithm, which is most widely used in the forward warning of automatic emergency braking system. Classic safety distance models include the Berkeley model, the Honda safe distance model and the Mazda safe distance model; The forward warning system is too conservative, which leads to frequent automatic braking, and the driver and passenger will feel uncomfortable. There are few application scenarios for the safety distance model algorithm: it is applied in a one-way two lane environment, and the surrounding vehicles have no intention to change lanes, in the multi-vehicle traffic environment, the minimum safe distance method is ineffective because it can not be applied to the scenario where other vehicles have to change lanes. Therefore, heuristic collision avoidance algorithm is used to deal with the problem of lane changing in multi-vehicle traffic environment. The algorithm has a safety rule, all dangerous trajectory clusters are eliminated based on safety rules, and the remaining lane-changing trajectory clusters meet the requirements of safe lane-changing which are called safe trajectory clusters.

Then the collision avoidance algorithm includes two rules: first, if the traffic vehicle is in a straight-line driving state, the subject vehicle will not collide with the traffic vehicle when changing lanes; Second, if the traffic vehicle also needs to change lanes, the subject vehicle will not collide with the traffic vehicle. Whether two vehicles collide is equivalent to whether two rectangles intersect at the same time. As shown in Figure 5: the intersection of two rectangles includes the following four situations.

Assuming that there are vehicles $i$ and $j$ in the same scene, the first situation is that a vertex of the two rectangles is inside each other, and the second situation is that all the vertices of the two rectangles do not fall inside the rectangle, but penetrate each other. Assuming that rectangle $i$ keeps going straight, and rectangle $j$ collides with rectangle $i$ when changing lanes, it is certain that rectangle $i$ has a vertex inside rectangle $j$. If the collision continues, the two rectangles will pass through. none of the vertices are inside the rectangle $j$, according to the judgment, it can be seen that the second situation must occur after the first situation.

In order to make the point p outside the rectangle ABCD, an algebraic inequality based on the area of the graph can be used to establish a constraint condition. If the following area conditions are satisfied, it can be proved that the two vehicles will not collide.

$$S_{\Delta \mathrm{PAB}}+S_{\Delta \mathrm{PBC}}+S_{\Delta \mathrm{PCD}}+S_{\Delta \mathrm{PDA}}>S_{▭\mathrm{ABCD}}$$

(16)

In Formula (16), $S_{\Delta }$ is the area of the triangle, and $S_{▭}$ is the area of the rectangle. The geometric description is shown in Figure 6.

The lane-changing trajectory of the vehicle is determined by the end time of the lane-changing [27], and the position of the vertex of the rectangle at any time in formula (17).

$$\left[\begin{array}{cc}{x}_{k1}(t)& y_{k1}(t)\\ x_{k2}(t)& y_{k2}(t)\\ x_{k3}(t)& y_{k3}(t)\\ x_{k4}(t)& y_{k4}(t)\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}-l_H& -w_H& 0& 0\\ l_H& -w_H& 0& 0\\ l_H& w_H& 0& 0\\ -l_H& w_H& 0& 0\end{array}\right]\left[\begin{array}{cc}\cos {\phi }_k(t)& \cos {\phi }_k(t)\\ -\sin {\phi }_k(t)& \sin {\phi }_k(t)\\ 0& 0\\ 0& 0\end{array}\right]+\left[\begin{array}{cc}{x}_k(t)& y_k(t)\\ x_k(t)& y_k(t)\\ x_k(t)& y_k(t)\\ x_k(t)& y_k(t)\end{array}\right]$$

(17)

In Formula (17), $x_{k1,\dots 4}(t)$ and $y_{k1,\dots 4}(t)$ are the coordinates of each vertex of the rectangle, ${\tau }_{Hk}$ 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 ${\phi }_k(t)$ as follows:

$${\phi }_k(t)=\arctan \frac{y_k(t+T)-y_k(t)}{x_k(t+T)-x_k(t)},0\le t\le \tau =\max \left\{{\tau }_{Hk},{\tau }_i\right\},i=1,\dots ,4$$

(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.

Algorithm 1. Principle of $CoL$ function

$$\begin{array}{l}\mathrm{function}\left[\mathrm{out}\right]=CoL(x_{M\tau }(t),y_{M\tau }(t),x_{Ti}(t),y_{Ti}(t))\\ A=(x_{k1}(t),y_{k1}(t));B=(x_{k2}(t),y_{k2}(t));C=(x_{k3}(t),y_{k3}(t));D=(x_{k4}(t),y_{k4}(t))\\ A_i=(x_{Ti1}(t),y_{Ti1}(t));B_i=(x_{Ti2}(t),y_{Ti2}(t));C_i=(x_{Ti3}(t),y_{Ti3}(t));D_i=(x_{Ti4}(t),y_{Ti4}(t))\\ S_A=\left(\left|{\stackrel{\rightharpoonup }{AA}}_i\times {\stackrel{\rightharpoonup }{AB}}_i\right|+\left|{\stackrel{\rightharpoonup }{AB}}_i\times {\stackrel{\rightharpoonup }{AC}}_i\right|+\left|{\stackrel{\rightharpoonup }{AC}}_i\times {\stackrel{\rightharpoonup }{AD}}_i\right|+\left|{\stackrel{\rightharpoonup }{AD}}_i\times {\stackrel{\rightharpoonup }{AA}}_i\right|\right)/2\\ S_B=\left(\left|{\stackrel{\rightharpoonup }{BA}}_i\times {\stackrel{\rightharpoonup }{BB}}_i\right|+\left|{\stackrel{\rightharpoonup }{BB}}_i\times {\stackrel{\rightharpoonup }{BC}}_i\right|+\left|{\stackrel{\rightharpoonup }{BC}}_i\times {\stackrel{\rightharpoonup }{BD}}_i\right|+\left|{\stackrel{\rightharpoonup }{BD}}_i\times {\stackrel{\rightharpoonup }{BA}}_i\right|\right)/2\\ S_C=\left(\left|{\stackrel{\rightharpoonup }{CA}}_i\times {\stackrel{\rightharpoonup }{CB}}_i\right|+\left|{\stackrel{\rightharpoonup }{CB}}_i\times {\stackrel{\rightharpoonup }{CC}}_i\right|+\left|{\stackrel{\rightharpoonup }{CC}}_i\times {\stackrel{\rightharpoonup }{CD}}_i\right|+\left|{\stackrel{\rightharpoonup }{CD}}_i\times {\stackrel{\rightharpoonup }{CA}}_i\right|\right)/2\\ S_D=\left(\left|{\stackrel{\rightharpoonup }{DA}}_i\times {\stackrel{\rightharpoonup }{DB}}_i\right|+\left|{\stackrel{\rightharpoonup }{DB}}_i\times {\stackrel{\rightharpoonup }{DC}}_i\right|+\left|{\stackrel{\rightharpoonup }{DC}}_i\times {\stackrel{\rightharpoonup }{DD}}_i\right|+\left|{\stackrel{\rightharpoonup }{DD}}_i\times {\stackrel{\rightharpoonup }{DA}}_i\right|\right)/2\\ S_i=\left|{\stackrel{\rightharpoonup }{A_{i}B}}_i\times {\stackrel{\rightharpoonup }{A_{i}D}}_i\right|;\\ \mathrm{if}(S_A-S_i)>0\&(S_B-S_i)>0\&(S_C-S_i)>0\&(S_D-S_i)>0\\ \mathrm{out}=0;\%\mathrm{Collision}-\mathrm{free}\\ \mathrm{else}\\ \mathrm{out}=1;\%\mathrm{Collision}\\ \mathrm{end}\end{array}$$

According to $CoL$ function, when the following rules are observed, whether the subject vehicle collides with the traffic vehicle can be judged:

$$\left\{\begin{array}{c}{x}_{Ti}(t)=\left[\begin{array}{cccc}{x}_{Ti1}(t)& x_{Ti2}(t)& x_{Ti3}(t)& x_{Ti4}(t)\end{array}\right]\\ y_{Ti}(t)=\left[\begin{array}{cccc}{y}_{Ti1}(t)& y_{Ti2}(t)& y_{Ti3}(t)& y_{Ti4}(t)\end{array}\right]\end{array}\right.$$

(19)

$$\left\{\begin{array}{c}{x}_{Hk}(t)=\left[\begin{array}{cccc}{x}_{k1}(t)& x_{k2}(t)& x_{k3}(t)& x_{k4}(t)\end{array}\right]\\ y_{Hk}(t)=\left[\begin{array}{cccc}{y}_{k1}(t)& y_{k2}(t)& y_{k3}(t)& y_{k4}(t)\end{array}\right]\end{array}\right.$$

(20)

$${\displaystyle \sum _{t=0}^{\tau }{J}_{ik}(t)}={\displaystyle \sum _{t=0}^{\tau }\left[CoL(x_{Ti}(t),y_{Ti}(t),x_{Hk}(t),y_{Hk}(t))\right]}=0$$

(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 of whether there is a collision are shown in Figure 7, where $\left[x_{T\_in}(t),y_{T\_in}(t)\right]$ 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.

For the subject vehicle, when the following criteria are complied with, it proves that the vehicle traveling along the lane-changing trajectory cluster will not collide.

$${\displaystyle \sum _{i=1}^4\left({\displaystyle \sum _{t=0}^{\tau }{J}_{ik}(t)}\right)}=0$$

(22)

After the safe collision avoidance trajectory is planned, the remaining trajectory meets both vehicle dynamics stability and safe collision avoidance, but it is still a group of trajectory clusters, so it is still necessary to plan the next step in order to solve the optimal lane change trajectory (Figure 8).

2.4. TOPSIS Algorithm for Solving Multiple Indexes

Clusters of safe trajectories are multi-objectively optimised to solve for optimal lane change trajectories. Autonomous vehicles change lanes not only for dynamic stability and safety, but also for comfort and lane change efficiency. Four evaluation indicators are proposed for this question, including Index of trajectory deviation, Index of rollover hazard, Index of Side Sliping and Index of Efficiency [28,29].

1.

Index of trajectory deviation

$$J_T=\sqrt{\left[{\left({\displaystyle {\int }_0^{\tau }{\left[\frac{f(t)-y(t)}{\stackrel{\wedge }{E}}\right]}^{2}dt}\right)}^2+{\left({\displaystyle {\int }_0^{\tau }{\left[\frac{u-\stackrel{·}{\beta }(t)}{\stackrel{\wedge }{\beta }}\right]}^{2}dt}\right)}^2\right]/2}$$

(23)

where $f(t)$ is the reference trajectory, $y(t)$ is the actual vehicle trajectory, $\stackrel{·}{\beta }(t)$ is the mass-side eccentricity speed, $\stackrel{\wedge }{E}$ is the trajectory deviation threshold and $\stackrel{\wedge }{\beta }$ is the mass-side eccentricity threshold.

2.

Index of rollover hazard

$$J_R=\sqrt{\left[{\left({\displaystyle {\int }_0^{\tau }{\left[\frac{a_y(t)}{\stackrel{\wedge }{a_y}}\right]}^{2}dt}\right)}^2+{\left({\displaystyle {\int }_0^{\tau }{\left[\frac{\varphi (t)}{\stackrel{\wedge }{\varphi }}\right]}^{2}dt}\right)}^2\right]/2}$$

(24)

where $\stackrel{\wedge }{a_y}$ is the lateral acceleration threshold, $a_y(t)$ is the vehicle lateral acceleration, $\varphi (t)$ is the vehicle side camber and $\stackrel{\wedge }{\varphi }$ is the vehicle side camber threshold.

3.

Index of Side Slip

$$J_s=\max \left({\displaystyle {\int }_0^{\tau }{[\frac{F_{yf,r}(t)/F_{zf,r}(t)}{\stackrel{\wedge }{\mu }}]}^{2}dt}\right)$$

(25)

where $F_{yf,r}(t)$ is the lateral force on the front and rear axles respectively, $F_{zf,r}(t)$ is the vertical force on the front and rear axles respectively and $\stackrel{\wedge }{\mu }$ is the road friction coefficient threshold value.

4.

Index of Efficiency

$$J_E=t_f$$

(26)

The less time it takes to change lanes, the higher the efficiency of changing lanes [30].

The attributes of each evaluation index are defined as:

$$N=\left[\begin{array}{cccc}1/J_T& 1/J_R& 1/J_S& J_E\end{array}\right]$$

(27)

Benefit indicators include Index of trajectory deviation, Index of rollover hazard and Index of Side Sliping, the cost index is Index of Efficiency. Then the vector $N$ is expanded into a decision matrix, which is the first step of the TOPSIS algorithm.

$$M_1=\left[\begin{array}{cccc}{(1/J_S)}_{11}& {(1/J_T)}_{12}& {(1/J_R)}_{13}& {(J_E)}_{14}\\ \bullet & \bullet & \bullet & \bullet \\ \bullet & \bullet & \bullet & \bullet \\ {(1/J_S)}_{q1}& {(1/J_T)}_{q2}& {(1/J_R)}_{q3}& {(J_E)}_{q4}\end{array}\right]$$

(28)

Among them, the number of rows of the matrix is calculated as Formula (28).

$$q=\frac{t_{f\max }-t_{f\min }(u)}{\Delta t}+1$$

(29)

where $q$ is the number of rows in the decision matrix, $t_{f\max }$ represents the maximum lane change time, which is 9 s, $t_{f\min }(u)$ is the minimum lane-changing time. Previous studies have shown that the minimum lane change time can be calculated when the longitudinal speed of the vehicle and the road adhesion coefficient are determined. $\Delta t$ represents unit interval, which can be expressed as follows.

$$Q={\left[\begin{array}{cccccc}{t}_{f\min }(u)& t_f(\Delta t)& t_f(2\Delta t)& \bullet & \bullet & k_{\max }\end{array}\right]}_{1\times q}$$

(30)

Each element of this set corresponds to each row of the matrix $M_1$. In order to be able to assign weight values, the elements in matrix $M_1$ need to be normalized in the way shown below.

$$b_{ij}=a_{ij}/\sqrt{{\displaystyle \sum _{i=1}^q{a}_{ij}^2}},i=1,2,\dots ,q;j=1,2,3,4$$

(31)

One lane changing track ($t_f$) corresponds to one sample, and the weights of four evaluation indicators in each sample are recorded as:

$$w={\left[w_T,w_R,w_S,w_E\right]}^T$$

(32)

A reasonable weight value is calculated through a large number of repetitions: 2: 3: 3: 2.. Where $w_T$, $w_R$, $w_S$ and $w_E$ denote the relative weights of $1/J_T$, $1/J_R$, $1/J_S$ and $J_E$, respectively; they have the following relation: The weighted matrix is shown in formula (2):

$$H_T={(h_{ij})}_{q\times 4}={(w_j\times b_{ij})}_{q\times 4}$$

(33)

Among them, $i=1,2,3,\dots ,q;j=1,2,3,4.$ To calculate the positive ideal solution based on the above formula, the specific method is to select the maximum value of the benefit index in the optimization index as the positive ideal solution, and the minimum value of the cost index in the optimization index as the positive solution. Negative solution is the same as above.

$$\begin{gathered} h_j^p=\left\{\begin{array}{c}\max h_{ij},j\in \cos \mathrm{t}\begin{array}{cc}& \mathrm{index}\end{array}\\ \min h_{ij},j\in \mathrm{benefit}\begin{array}{cc}& \mathrm{index}\end{array}\end{array}\right.,j=1,2,3,4. \\ {h}_j^n=\left\{\begin{array}{c}\max h_{ij},j\in \mathrm{benefit}\begin{array}{cc}& \mathrm{index}\end{array}\\ \min h_{ij},j\in \cos \mathrm{t}\begin{array}{cc}& \mathrm{index}\end{array}\end{array}\right.,j=1,2,3,4. \end{gathered}$$

(34)

According to the above formula, the positive solution and the negative solution can be calculated after assigning the weight values. The Euclidean distance to the positive solution and negative solution are:

$$\begin{gathered} d_i^{*}=\sqrt{{\displaystyle \sum _{j=1}^4{(h_{ij}-h_j^p)}^2}},i=1,2,\dots ,q. \\ {d}_i^0=\sqrt{{\displaystyle \sum _{j=1}^4{(h_{ij}-h_j^n)}^2}},i=1,2,\dots ,q. \end{gathered}$$

(35)

$d_i^{*}$ is the Euclidean distance of positive solution, $d_i^0$ is the Euclidean distance of positive solution. The evaluation function is used to calculate the optimal lane change trajectory. The evaluation function formula is as follows:

$$Q_i=\frac{d_i^0}{d_i^0+d_i^{*}},i=1,2,\dots ,q.$$

(36)

After calculating the sample score, each sample should be sorted; The sample with the highest score is regarded as the optimal trajectory planning scheme, and the lane change trajectory is the optimal reference lane change trajectory. The above ranking and evaluation are performed in the stability region, In other words, $t_f$ is quantitatively ranked according to the score in the stability region.

3. Results

The lane change trajectory planning strategy not only considers the dynamic stability of vehicles, but also meets the requirements of collision avoidance. At the same time, the lane change trajectory also takes into account the lane change efficiency. Next, the feasibility and effectiveness of the lane change trajectory were verified in three simulation scenarios with increasing complexity. The multi-vehicle traffic environment is shown in Figure 9.

3.1. Overtaking and Lane-Changing Scenario

Overtaking is one of the most common reasons to change lanes. In this section, the longitudinal speed of the subject vehicle is 80 km/h, the speed of the vehicle in front is 72 km/h, the longitudinal distance between the two vehicles in the initial position is 15 m, and the road adhesion coefficient is 0.7. The minimum vehicle lane change time, as shown in Figure 6, is 1.906 s, while the maximum change time is generally defined as 9 s. Matlab and CarSim were used for the simulations, and the results are shown below.

As shown in Figure 10a, the lane changing track of VM is not a single trajectory. but the trajectory clusters. As shown in Figure 10b, the safe lane-changing time for the subject vehicle ranged from 1.906 s to 5.9 s. As shown in Figure 10c, the multi-objective optimization results show that the best lane change end time was 2.8 s. The results of the Matlab and CraSim simulation are shown in Figure 10d. It can be seen that the two vehicles did not collide, and both vehicles followed their expected trajectories without skidding or overturning. In engineering, it is generally stipulated that the time of an automatic auxiliary lane change is 3 s. Our trajectory planning algorithm improves the efficiency of lane-changing while also reducing the interference time of vehicle lane-changing behavior on traffic flow.

3.2. Lane-Exchanging Scenario

Lane-exchanging is more complex than overtaking and is in urban traffic environments. Assume that two vehicles exchange information through V2V and then change lanes, and the initial times of changing lanes are the same.

In this section, the longitudinal speed of the subject vehicle is 100 km/h, the speed of the Lead vehicle is 72 km/h, the longitudinal distance between the two vehicles in the initial position is 18 m, and the road adhesion coefficient is 0.7; the vehicle model parameters are shown in

Table 1. Selected parameters of the simulation vehicle.

Parameter	Meaning	Numerical Value
$m$	Vehicle mass	1370 kg
$w_H$	Vehicle width	2131 mm
$H$	Center of mass height	520 mm
$a$	Distance from center of mass to front axle	1110 mm
$b$	Distance from center of mass to rear axle	1670 mm
$R_m$	Main gear ratio	4.1
$P_E$	Power of the engine	150 kw
$I_z$	Rotational inertia of the pendulum	2315.3 kg/m2
$d$	Wheelbase	1550
-	Tire model	215/55 R17

. The minimum vehicle lane change time, as shown in Figure 6, is 2.013 s, while maximum lane change time is generally defined as 9 s. Matlab and CarSim were applied for the simulation, and the results are shown below:

As shown in Figure 11a, lane change trajectories for both VM and VLe are trajectory clusters. As shown in Figure 11b, the safe lane-changing time for the Subject vehicle ranged from 2.013 s to 6.4 s. As shown in Figure 11c, the multi-objective optimization results showed that the best lane change end time was 5.6 s. The results of the Matlab and CraSim simulation are shown in Figure 11d. It can be seen that the two vehicles did not collide and both vehicles followed the expected trajectories without skidding or overturning.

3.3. Cut-In of a Vehicle from Lane 3

At the beginning of the lane change of the Subject vehicle, the Separate vehicle changed from the third lane to the middle lane. This scenario was considered to verify whether the above planning algorithm was suitable for complex lane changing scenarios.

In this section, the longitudinal speed of the subject vehicle is 90 km/h, the speed of the Lag vehicle and Separate vehicle are 72 km/h, the initial longitudinal displacement between the Subject vehicle and Lag vehicle is 15 m, the initial longitudinal displacement between the Subject vehicle and Separate vehicle is 10 m, and the road adhesion coefficient is 0.7; the vehicle model parameters are shown in Table 1. The minimum vehicle lane change time, as shown in Figure 6, was 1.996 s, while the maximum lane change time was generally defined as 9 s. Matlab and CarSim were applied for these simulations; the results are shown below:

As shown in Figure 12a, different lane change end times correspond to different lane change trajectories. As shown in Figure 12b, the safe lane-changing time for the Subject vehicle ranged from 1.996 s to 7.0 s. It can be seen from Figure 12c, the multi-objective optimization results showed that the optimal vehicle lane change end time was 5.8 s. The results of the Matlab and CraSim simulation are shown in Figure 12d. It can be seen that the two vehicles did not collide and that both vehicles followed their expected trajectories, with no sideslip or rollover.

4. Discussion

University and corporate research methods are mostly end-to-end for intelligent driving, whereby vehicles are directly controlled by training driving behavior data. However, this research method requires a large amount of experimental data, and as a result, it poses a challenge in terms of computational requirements. When a vehicle changes lanes, lateral stability is a key point. Because vehicle dynamics are considered in the trajectory planning module, it is unnecessary to design a vehicle lateral stability controller, which simplifies the research. In a multi-vehicle traffic environment, it is inevitable that other vehicles will change lanes. In this case, it is necessary to propose a safe collision avoidance algorithm so that even if two vehicles change lanes at the same time, there will be no collision. However, in this paper, it was assumed that the speed of the vehicle was constant. Real-time changes of vehicle speed need to be tested in an experimental environment. This will be done in future research.

Many trajectories result from the application of vehicle lateral stability constraints and safe collision avoidance algorithms. These processes seek to determine the optimal lane change trajectory. Most previous research has tended to establish the optimization objective function and constraints; subsequently, the optimization variables are calculated by the algorithm. The four indexes and simulation test data can be used for quantitative analyses of lane change performance. In other words, the four optimization indexes corresponding to each lane-changing trajectory can be obtained with specific values. Multi-objective optimization belongs to the multi-attribute decision making problem, and the best way to solve this kind of problem is by using the TOPSIS algorithm.

For traditional auxiliary lane changing system, decisions made by engineers regarding lane change times are based on driver habits; generally, the minimum lane change time is 3 s, which lacks flexibility. In contrast, our simulation results show that a lane-changing time of 2.8 s would be acceptable, improving lane-changing efficiency by 6.67% in overtaking scenarios. In addition, in lane-exchanging and cut-in scenarios, the general auxiliary lane-changing system is unable to plan the trajectory. The traditional RRT algorithm (Rapidly Exploring Random Tree) and APF algorithm (Artificial Potential Field) can be used to perform simulations in overtaking scenarios, but they cannot handle the latter two complex scenarios; for example, the APF algorithm is generally unable to build an attractive area in a random environment, while the RRT algorithm will report an error. The absence of collisions, as well as efficiency and comfort, are the advantages of the optimized reference trajectory. The above proposed planning strategies provide ideas and methods for intelligent driving research. However, our planning strategy is a non-game system, which does not take the psychological factors of the driver into account. The typical scenarios cannot represent all the scenarios, more complex lane changing scenarios need to be researched in the future.

5. Conclusions

In this paper: the multi-vehicle traffic environment as the research context and intelligent vehicle as the research object; the research on the autonomous lane-changing trajectory planning of intelligent vehicles is based on vehicle-to-vehicle. The fifth polynomial as a mathematical model for lane-changing trajectories; the center of mass slip angle and yaw rate are limited to improve the lateral stability of the vehicle; Collision avoidance algorithm is used to eliminate potentially dangerous trajectories, and TOPSIS algorithm is used to calculate the optimal lane-changing trajectory. Simulation results show that compared with the conventional auxiliary lane-changing system, the above algorithm can improve the lane changing efficiency, and it can be applied to more complex traffic scenarios, such as Lane-exchanging and Cut-in scenarios. In a word, optimal lane changing trajectory is a collision free, comfortable and efficient lane-changing trajectory.

Please note that the proposed trajectory planning algorithm cannot address the interaction of multiple vehicles at different times, hence, our future work will be to provide reasonable planning for more complex interactive scenarios. Uninterrupted vehicle-to-vehicle communication is an ideal situation, If the lane change intention of other vehicles cannot be perceived, trajectory correction strategy is a key issue in lane-changing trajectory planning. In the future research, trajectory correction and trajectory re-planning will be considered in the trajectory planning framework in order to achieve a safe and smooth lane-changing.