Improved Quintic Polynomial Autonomous Vehicle Lane-Change Trajectory Planning Based on Hybrid Algorithm Optimization

Abstract

A trajectory planning method is proposed to address the lane-changing problem in intelligent vehicles. The method is based on quintic polynomial improvement. The transit position is determined according to the position and state of motion of the vehicle and the obstacle vehicle; the lane-changing process is divided into two segments. The quintic polynomials are commonly applied in trajectory planning, respectively, in the two segments. According to the different characteristics of the lane-changing paths in the front and rear segments, a multi-objective optimization function with different weight coefficients is established. A safe and comfortable lane-changing trajectory is achieved through the improved particle swarm optimization algorithm. Real-time simulation tests of lane-changing method are conducted on the hardware-in-the-loop platform. The method can be used in different scenarios to plan safe and comfortable trajectories.

1. Introduction

Self-driving vehicles can fulfill most of the operations of human drivers and reduce traffic accidents caused by improper operation of human drivers [1]. According to relevant studies, it is predicted that the popularization of self-driving technology can reduce the rate of automobile accidents by more than 90% and significantly improve road traffic safety [2,3]. As a prevalent driving action when intelligent vehicles drive automatically, lane-changing behavior that is safe, fast and smooth can improve the efficiency of passage and reduce the incidence of traffic accidents [4].

Reasonable lane-changing trajectory is a prerequisite for intelligent vehicles to accomplish the lane-changing task safely [5]. Trajectory planning for self-driving vehicles is the core technology to ensure safe and efficient driving, and with the increase in complexity and dynamics of the traffic environment, trajectory planning algorithms need to be efficient, robust and adaptive [6]. The current mainstream methods are systematically categorized into five groups: graph search methods search for optimal paths by constructing environment maps (e.g., raster maps) and search algorithms (e.g., A* algorithms), which are suitable for static environments but have high computational complexity [7]; sampling methods (e.g., RRT, PRM) generate paths by random sampling, which are suitable for high-dimensional spaces and dynamic environments, but the paths may not be smooth enough [8]; geometric methods use geometric curves (e.g., polynomial curves) to generate smooth trajectories that satisfy dynamical constraints [9]; optimal control-based methods generate high-quality trajectories by solving optimization problems, but are computationally burdensome [10]; and learning-based methods use machine learning (e.g., reinforcement learning) to learn planning strategies from data, which is adaptive, but data acquisition and model generalization capabilities are still challenges [11,12]. The performance comparison of different planning algorithms is summarized in

Table 1. Comparison of planning algorithms.

Category	Advantages	Disadvantages
Graph Search	Global optimal, structured env.	High-dimension slow, static env.
Sampling	Fast in high-dim, dynamic env.	Suboptimal, needs post-processing
Geometric	Smooth, low compute	Struggles with dynamics
Optimization	High precision, multi-objective	Heavy compute, model-dependent
Learning	Adapts to chaos, no explicit model	Data-hungry, safety risks

.

Out of the techniques previously described, the geometric method has the easiest computational process compared to the other methods, which can generate smooth trajectories that satisfy the kinematic constraints through simple computations, and thus polynomial curves are a popular choice for trajectory planning. Low-order polynomials (e.g., cubic polynomials) can satisfy the continuity of position and velocity and are often used in simple trajectory planning problems. The literature [13] proposes a trajectory planning method via discrete cubic polynomial curves to identify the optimal path by adjusting the vehicle’s location in a defined area. Higher order polynomials (e.g., sixth or higher order) can satisfy higher order dynamics constraints and are suitable for high-speed motion or scenarios that require very high trajectory smoothness, but the computation becomes complicated as the order increases. The literature [14] proposes a trajectory planning algorithm using a sixth-order polynomial for obstacle avoidance based on vehicle kinematics equations. However, this algorithm suffers from high computational complexity and slow convergence due to higher order and more coefficients to be determined. The lane-changing trajectory planned using quintic polynomials can effectively balance the smoothness of the trajectory and computational efficiency. Compared with the low-order polynomials, quintic polynomials can satisfy the continuity requirements of position, velocity and acceleration, and generate smoother trajectories. Meanwhile, a quintic polynomial has lower computational complexity compared with higher-order polynomials, which can quickly generate trajectories in scenes with high real-time requirements. The literature [15] proposes a path planning algorithm via polynomial functions, which sets the acceleration constraints and establishes a polynomial parameterized simulation model with velocity boundaries and path time optimization, which provides a reference for the subsequent research on dynamics models. The literature [16] proposes a real-time path planning method via quintic polynomial optimization, which generates smooth and dynamically constrained trajectories by quintic polynomials and combines the optimization algorithm to achieve real-time and safety, applied to the trajectory generation of self-driving vehicles in complex traffic environments.

Quintic polynomials are commonly applied in lane-changing path planning due to unique advantages; however, the traditional quintic polynomial model is commonly used to increase the order of the constraint equations in obstacle avoidance and lane changing to satisfy the demands of vehicle dynamics limitations and driving performance. The method is computationally cumbersome, which can lead to an increase in planning time and a decrease in safety [17]. To solve the above problems, an intelligent vehicle lane-changing path planning method via improved quintic polynomial is proposed. The process of lane-changing is segmented into two parts before and after by setting a suitable transit position. Then, according to the different characteristics of the two paths, a multi-objective optimization objective function with different weight coefficients is established, and a secure and comfortable lane-changing trajectory is obtained through optimization algorithm. The proposed framework for lane-changing path planning in self-driving cars is graphically represented in Figure 1.

Contributions of this article:

(1) Via the traditional quintic polynomial planning for lane-changing trajectory planning, it is proposed to increase the staging position and change it into segmented planning for lane-change trajectory. By analyzing the critical collision state of the intelligent car and the obstacle car in front, and at the same time considering the state of the two cars to determine the transit position, it is  ensured that there is no collision risk between the intelligent car and the obstacle car when changing lanes.

(2) Establish multi-objective optimization functions with different weighting coefficients according to the different characteristics of the paths at the front and rear ends, which are used to adjust the lane-changing time. Improve the optimization algorithm by integrating genetic algorithm in particle swarm algorithm to accelerate the convergence process, and enhance the diversity of solutions and global search ability, so that the optimization algorithm has stronger adaptability and robustness.

(3) In addition to static and dynamic obstacle vehicle scenarios, the validation algorithm adds the validation in low adhesion coefficient roadway scenarios, which ensures that the lane-changing model has wide applicability.

2. Problem and Scenario Descriptions

This paper mainly centers on the lane-changing and obstacle-avoiding behavior of self-driving vehicles. When the self-driving vehicle cruises at a certain speed in the lane, when there is an obstacle vehicle in front of the lane that affects the normal passage of the vehicle, the self-driving vehicle takes the initiative to change lanes according to a safe and comfortable lane-changing trajectory that is planned so as to avoid the obstacle vehicle in front of it and continue to drive normally.

A typical scenario of a self-driving vehicle changing lanes to avoid obstacles is shown in Figure 2:

Figure 2 shows a typical unidirectional two-lane highway lane-changing scenario; two same direction lanes are a super lane (fast lane) and an ordinary lane, both lane widths are 3.75 m. Intelligent vehicle A, the front obstacle vehicle B and the rear vehicle C are all driving in the right ordinary lane, and the left front vehicle D and the left rear vehicle E are all driving in the left lane. According to traffic rules as well as actual driving habits, vehicle lane-changing to avoid obstacles generally chooses to change lanes from the current lane to the neighboring lane on the left side, so the left overtaking lane is the target lane for the self-driving vehicle A. As the relative longitudinal distance between intelligent vehicle A and vehicle B decreases, intelligent vehicle A has to perform the lane-changing operation to avoid the potential collision danger with the vehicle in front of it, B.

The focus of this paper is to study the lane-changing trajectory planning for self-driving vehicles to avoid the obstacle vehicles in front of them, so it is assumed that the lane-changing vehicles are only affected by the obstacle vehicles in front of them, except that they are not affected by other vehicles in the surrounding area, and the scenarios are simplified as follows.

(1) Vehicle A has sufficient space to change lane to the target lane (vehicle D and vehicle E will not affect vehicle A’s lane change).

(2) Rear vehicle C has also been travelling at a safe distance from vehicle A and will not affect vehicle A’s lane-changing operation.

(3) Vehicle A is travelling at a safe distance from vehicle B in front of it before changing lanes, and the effect of longitudinal speed on transverse speed is ignored during the lane change.

Eventually, the simplified lane-changing collision avoidance scenario is shown in Figure 3.

Via the simplified scenario, we plan lane-changing trajectories of smart car A that encounters obstacle car B in front of it.

3. Modeling of Lane-Change Trajectory Planning

3.1. Conventional Quintic Polynomial Lane-Change Model

After the above simplification of scenario, when the intelligent vehicle A needs to change lanes to avoid the obstacle vehicle B in order to continue normal driving, the lane-changing path planning mainly considers the vehicle A to avoid collision with the obstacle vehicle B during the lane-changing process. The intelligent vehicle A can choose a quintic polynomial curve based on time as a parameter of the lane-changing trajectory when it executes instruction to change lane.

In Figure 3, vehicle A is about to perform the lane-changing operation. Vehicle A drives from the centerline position of the lane it is in to the target position on the centerline of the adjacent target lane. When using the quintic polynomial curve as the planning trajectory curve of the vehicle lane change, we only need to know the start state $S_{\mathrm{start}}$ and the end state $S_{\mathrm{end}}$ of the intelligent vehicle A. After calculation, we can get a smooth quintic polynomial curve as the obstacle avoidance trajectory of the intelligent vehicle.

The start state $S_{\mathrm{start}}$ and the goal state $S_{\mathrm{end}}$ of smart vehicle A are

$$\left\{\begin{array}{c}{S}_{\mathrm{start}}=\left(x_{\mathrm{start}},{\dot{x}}_{\mathrm{start}},{\ddot{x}}_{\mathrm{start}},y_{\mathrm{start}},{\dot{y}}_{\mathrm{start}},{\ddot{y}}_{\mathrm{start}}\right)\hfill \\ S_{\mathrm{end}}=\left(x_{\mathrm{end}},{\dot{x}}_{\mathrm{end}},{\ddot{x}}_{\mathrm{end}},y_{\mathrm{end}},{\dot{y}}_{\mathrm{end}},{\ddot{y}}_{\mathrm{end}}\right)\hfill \end{array}\right.$$

(1)

where x, $\dot{x}$, $\ddot{x}$, y, $\dot{y}$ and $\ddot{y}$ are the displacement, speed and acceleration of vehicle A in longitudinal as well as transversal direction, respectively.

The commutative trajectory functions in the x- and y-directions are computed as follows:

$$\left\{\begin{array}{c}f(x,t)=\sum _{i=0}^5{a}_i{t}^i\hfill \\ f(y,t)=\sum _{i=0}^5{b}_i{t}^i\hfill \end{array}\right.$$

(2)

where $a_i$ and $b_i$ are the coefficients to be determined for the trajectory function in the x and y directions, respectively; t is time; $f\left(x,t\right)$ means longitudinal state function; $f\left(y,t\right)$ is transverse state function.

Combining Equations (1) and (2), the quintic polynomial commutative channel model can be expressed as

$$\left\{\begin{array}{c}{\left[x_{\mathrm{start}},{\dot{x}}_{\mathrm{start}},{\ddot{x}}_{\mathrm{start}},x_{\mathrm{end}},{\dot{x}}_{\mathrm{end}},{\ddot{x}}_{\mathrm{end}}\right]}^{\mathrm{T}}=T_{6\ast 6}\ast A^{\mathrm{T}}\hfill \\ {\left[y_{\mathrm{start}},{\dot{y}}_{\mathrm{start}},{\ddot{y}}_{\mathrm{start}},y_{\mathrm{end}},{\dot{y}}_{\mathrm{end}},{\ddot{y}}_{\mathrm{end}}\right]}^{\mathrm{T}}=T_{6\ast 6}\ast B^{\mathrm{T}}\hfill \end{array}\right.$$

(3)

where T is the time parameter matrix and $A^{\mathrm{T}}$ and $B^{\mathrm{T}}$ are the longitudinal and transverse state vectors, respectively.

Define the time parameter matrix T as follows:

$$T_{6\ast 6}=\left[\begin{array}{cccccc}{t}_{\mathrm{start}}^5& t_{\mathrm{start}}^4& t_{\mathrm{start}}^3& t_{\mathrm{start}}^2& t_{\mathrm{start}}& 1\\ 5t_{\mathrm{start}}^4& 4t_{\mathrm{start}}^3& 3t_{\mathrm{start}}^2& 2t_{\mathrm{start}}& 1& 0\\ 20t_{\mathrm{start}}^3& 12t_{\mathrm{start}}^2& 6t_{\mathrm{start}}& 2& 0& 0\\ t_{\mathrm{end}}^5& t_{\mathrm{end}}^4& t_{\mathrm{end}}^3& t_{\mathrm{end}}^2& t_{\mathrm{start}}& 1\\ 5t_{\mathrm{end}}^4& 4t_{\mathrm{end}}^3& 3t_{\mathrm{end}}^2& 2t_{\mathrm{end}}& 1& 0\\ 20t_{\mathrm{end}}^3& 12t_{\mathrm{end}}^2& 6t_{\mathrm{end}}& 2& 0& 0\end{array}\right]$$

(4)

where $t_{\mathrm{start}}$ is the initial moment of channel change; $t_{\mathrm{end}}$ is the moment of channel change completion.

Define the coefficient matrices of the longitudinal and transverse state functions A and B, respectively, as follows:

$$\left\{\begin{array}{c}A=\left[\begin{array}{cccccc}{a}_5\hfill & a_4\hfill & a_3\hfill & a_2\hfill & a_1\hfill & a_0\hfill \end{array}\right]\hfill \\ B=\left[\begin{array}{cccccc}{b}_5\hfill & b_4\hfill & b_3\hfill & b_2\hfill & b_1\hfill & b_0\hfill \end{array}\right]\hfill \end{array}\right.$$

(5)

At the starting moment of lane-changing, the displacement, speed and acceleration of vehicle A in the transverse direction are zero. After the longitudinal distance L, vehicle A completes the lane change, and its target state has a displacement of W in the transverse direction and zero velocity and acceleration in the transverse direction. The trajectory function should satisfy the following boundary conditions:

$$\left\{\begin{array}{c}x(t=0)=x_{\mathrm{start}}=0\hfill \\ \dot{x}(t=0)={\dot{x}}_{\mathrm{start}}=v_{\mathrm{start}}\hfill \\ \ddot{x}(t=0)={\ddot{x}}_{\mathrm{start}}=0\hfill \\ x(t=T)=x_{\mathrm{end}}=L\hfill \\ \dot{x}(t=T)={\dot{x}}_{\mathrm{end}}=v_{\mathrm{end}}\hfill \\ \ddot{x}(t=T)={\ddot{x}}_{\mathrm{end}}=0\hfill \end{array};\left\{\begin{array}{c}y(t=0)=y_{\mathrm{start}}=0\hfill \\ \dot{y}(t=0)={\dot{y}}_{\mathrm{start}}=0\hfill \\ \ddot{y}(t=0)={\ddot{y}}_{\mathrm{start}}=0\hfill \\ y(t=T)=y_{\mathrm{end}}=W\hfill \\ \dot{y}(t=T)={\dot{y}}_{\mathrm{end}}=0\hfill \\ \ddot{y}(t=T)={\ddot{y}}_{\mathrm{end}}=0\hfill \end{array}\right.\right.$$

(6)

where $t=0$ for the starting moment; $t=T$ for the ending moment; $v_{\mathrm{start}}$, $v_{\mathrm{end}}$ for the beginning and end of vehicle A speed, respectively; W is usually taken as the standard high-speed lane width of 3.75 m; L for the longitudinal displacement; assuming that the longitudinal speed of vehicle A is unchanged when changing lanes, then $L=v_{\mathrm{start}}\ast T$.

By setting the boundary conditions, it can be ensured that not only the speed is continuous without sudden change between the starting and ending positions of the planning path, but also the acceleration is continuous without sudden change, which improves the smoothness of traveling when vehicle A is changing lanes, and enhances passenger comfort.

Combining the boundary conditions and other constraints of Equation (6), and then combining Equations (3)–(5), the coefficient matrices A and B in Equation (5) can be found:

$$\left\{\begin{array}{c}{a}_0=0,a_1=v_s,a_2=0,a_3={\displaystyle \frac{1}{t_T^3}}\left(10L-10v_s{t}_T\right),\hfill \\ a_4={\displaystyle \frac{1}{t_T^4}}\left(-15L+15v_s{t}_T\right),a_5={\displaystyle \frac{1}{t_T^5}}\left(6L+6v_s{t}_T\right),\hfill \\ b_0=b_1=b_2=0,b_3={\displaystyle \frac{10W}{t_T^3}},b_4=-{\displaystyle \frac{15W}{t_T^4}},b_5=-{\displaystyle \frac{6W}{t_T^5}}\hfill \end{array}\right.$$

(7)

After obtaining the coefficient matrices A and B, the final lane-changing trajectory function is achieved through combining Equations (2) and (3).

In traditional quintic polynomial planning, if the initial speed $v_{\mathrm{start}}$ and the final speed $v_{\mathrm{end}}$ of the lane-changing vehicle are determined, the lane-changing time T is the only remaining variable to be considered, and the final lane-changing trajectory function can be obtained via the boundary conditions and constraints [18].

3.2. Improved Quintic Polynomial Lane-Change Model

Traditional lane-change trajectory planning using quintic polynomial curves not only requires a large safety distance, but also takes a long time to change lanes. In order to solve the defects of using a single quintuple polynomial, we propose a double-quintic polynomial lane-changing trajectory planning algorithm via the quintic polynomial. By adding a transitional state in the lane-changing process, the original lane-changing process is segmented into two parts. The transit position not only acts as the end point of the first planning, but also the starting point of the second planning, thus obtaining the lane-changing trajectory composed of two quintuple polynomial curves. The lane-changing process is shown in Figure 4, where vehicle A is the intelligent vehicle that needs to perform lane-changing, and vehicle B is the obstacle vehicle in front of the original lane.

Vehicle A arrives at the expected staging position at the end of the first planning, at which time vehicle A maintains a safe distance from the right front obstacle vehicle B in the transverse direction. Thereafter, when the lane-changing vehicle A performs the second planning, not only is it no longer intersecting with the obstacle vehicle B in the longitudinal direction, but also the distance in the transverse direction is increasing, and there is no risk of collision with the obstacle vehicle B in the transverse and longitudinal directions.

After smart vehicle A reaches the transit location of the transition, it performs five times polynomial path planning again via the transit state and the final goal condition at this time, as shown in Figure 5:

Adding a suitable transition staging location to divide the lane-changing process into two phases significantly improves the safety of intelligent vehicles during the lane-changing phase.

3.2.1. Solving for Transit Position

Selecting the appropriate transition staging location is the difficulty. The selection of the transit position needs to consider the speeds of the lane-changing vehicle A and the obstacle vehicle B ahead, the distance between the two vehicles, the displacement of the lane-changing vehicle A, and the width of the obstacle vehicle B ahead, in conjunction with lane-change safety distance modeling.

It is assumed that during the local planning cycle of lane-changing path planning, the intelligent vehicle A is cruising normally at a certain speed. The perception system module of the intelligent vehicle obtains the motion status information of the obstacle vehicle B ahead, as well as the position state information through the sensors. A right-angle coordinate system is constituted with the center of mass of the vehicle in the initial state of lane-changing of the intelligent vehicle as the coordinate origin and the centerline of the initial lane as the X-axis, from which the state of the obstacle vehicle B ahead in the following period of time can be determined.

Taking the transit position as the demarcation point, the lane-changing process of the intelligent vehicle is segmented into two successive stages. In the first stage, the intelligent vehicle travels from the initial position of lane-changing to the mid-turn position, and only needs to move the appropriate lateral displacement to complete the obstacle avoidance task for the obstacle vehicle in front; in the second stage, the intelligent vehicle travels from the mid-turn position to the target position, and since there is no risk of collision between the lane-changing vehicle and the obstacle vehicle in this stage, the second lane-changing path planning can be regarded as the lane-changing path planning under the barrier-free situation.

At time $t=m$, at this time the lane-changing vehicle A has just completed the first stage of lane-changing planning and arrived at the transit position. At this time, vehicle A and the obstacle vehicle B are in the critical collision state, as shown in Figure 6a. In order to make the two vehicles have no risk of collision, the following needs to be satisfied:

$$x_{B_m}⩾x_{A_m}+a_A+{\displaystyle \frac{W_A}{2}}sin\theta \left(t_m\right)+b_B$$

(8)

where $x_{B_m}$, $x_{A_m}$ are the horizontal coordinates of the center of mass of the obstacle vehicle B and the intelligent vehicle A at $t=m$, respectively; $a_A$ is the distance from the center of mass of the intelligent vehicle A to the front of the vehicle; $b_B$ is the distance from the center of mass of the obstacle vehicle B to the rear of the vehicle; $W_A$ is the width of the vehicle A; and $\theta$ is the angle between the axis of the body of the intelligent vehicle A and the direction of the road at the end of the first planning.

In the actual process, the intelligent vehicle at the end of the first lane change has a very small angle between its body axis and the road, and its body is nearly parallel to the road. Therefore, the heading angle of vehicle A at the end of the first stage of planning can be ignored, assuming that the position of vehicle A at this time is at the transit position shown in Figure 6b.

That is, Equation (8) becomes the following:

$$x_{B_m}⩾x_{A_m}+a_A+b_B$$

(9)

Therefore, the state of smart vehicle A vehicle in the transit position is

$$\left(x_m,{\dot{x}}_m,{\ddot{x}}_m,y_m,{\dot{y}}_m,{\ddot{y}}_m\right)=\left(L_m,v_m,0,W_m,0,0\right)$$

(10)

where $W_m$, $L_m$, and $v_m$ are the transverse displacement, longitudinal displacement, and longitudinal velocity, respectively, of intelligent vehicle A when it is located in the transit position at the moment $t=m$.

The $W_m$ is marginally broader than the width of the obstacle vehicle B ahead. According to the actual width of the obstacle vehicle B, adjustments are made, and the value is generally between 1.8 m and 2 m to ensure that the cars have a certain safety distance in the lateral direction. From this, we can get $W_m$:

$$W_m=\lambda W_B$$

(11)

where $\lambda$ is a constant between 1.1 and 1.2, the value is related to the condition of the two vehicles; $W_B$ is the width of the vehicle B.

The longitudinal displacement $L_m$ is determined via the lane-changing time in the first stage and the starting speed $v_s$ of vehicle A. The vehicle’s speed is hypothesized to remain constant while a short period of time is planned for. From this, $L_m$ is obtained:

$$L_m=t_1{v}_s$$

(12)

The speed $v_m$ in the transition position is usually slightly greater than $v_s$. In order to complete the lane change expeditiously, the vehicle making the lane change generally adopts the strategy to accelerate to make the lane change. There is a special situation, that is, if the front obstacle vehicle is in the stationary state, the lane change vehicle may be required to decelerate appropriately to make the longitudinal lane change; at this time, the transit speed needs to be slightly less than $v_m$. From this, we can get $v_m$:

$$v_m=\mu v_s$$

(13)

where $v_m$ and $v_s$ denote the longitudinal velocity of car A at the transit moment and the initial moment, respectively, $\mu$ is a constant taking values between 0.8 and 1.4.

3.2.2. Modeling of Double-Quintic Polynomial Lane Change

After determining the appropriate transition staging position state function, and then combining the initial and goal states described in the previous section, the double quintuple polynomial path planning state can be obtained, as shown in Equation (13):

$$\left\{\begin{array}{c}{S}_{\mathrm{start}}=\left(x_{\mathrm{start}},{\dot{x}}_{\mathrm{start}},{\ddot{x}}_{\mathrm{start}},y_{\mathrm{start}},{\dot{y}}_{\mathrm{start}},{\ddot{y}}_{\mathrm{start}}\right)=\left(0,v_s,0,0,0,0\right)\hfill \\ S_m=\left(x_m,{\dot{x}}_m,{\ddot{x}}_m,y_m,{\dot{y}}_m{\ddot{y}}_m\right)=\left(x_m,v_m,0,y_m,0,0\right)\hfill \\ S_{\mathrm{end}}=\left(x_{\mathrm{end}},{\dot{x}}_{\mathrm{end}},{\ddot{x}}_{\mathrm{end}},y_{\mathrm{end}},{\dot{y}}_{\mathrm{end}},{\ddot{y}}_{\mathrm{end}}\right)=\left(x_e,v_e,0,y_e,0,0\right)\hfill \end{array}\right.$$

(14)

where $S_{\mathrm{start}}$, $S_m$, $S_{\mathrm{end}}$ are the initial state, the transitional transit state, and the final target state of the lane change, respectively; $v_s$ is the speed of vehicle A in the initial state of the lane change; $x_m$, $v_m$, $y_m$, and $x_e$, $v_e$, $y_e$ are the longitudinal displacement, longitudinal velocity, and transverse displacement in the transit state and the final target state of the lane change.

The first phase of the lane change involves smart vehicle A traveling to the mid-turn position to avoid the obstacle vehicle in front of it, and the second phase involves smart vehicle A traveling from the mid-turn position to the final target position.

To acquire the coefficient matrices A and B in Equation (3), the longitudinal speed $v_m$ at the staging position and the lane-changing time $T_1$ at the first stage can be used as the optimization variables. $v_m$ and $T_1$ can be taken in a range of values according to the condition of the two vehicles.

The starting condition $S_B$ of the front obstacle vehicle B vehicle is shown in Equation (15):

$$S_B=\left(x_B,v_B,a_B,0,0,0\right)$$

(15)

where $v_B$ is the speed of the vehicle B ($v_B$ is less than $v_s$); $x_f$ is the distance between the two vehicles; $a_B$ is the acceleration of the vehicle B ($a_B⩽0$).

Assuming that the center of mass of the two vehicles moves to the same position in the longitudinal direction after t time and the intelligent vehicle A maintains a constant speed within the shorter planning cycle, Equations (16) and (17) can be obtained:

$$v_{Bt}=v_B+a_{B}t$$

(16)

$$v_{s}t=\left\{\begin{array}{cc}{v}_{B}t+x_B,\hfill & a_B=0\hfill \\ {\displaystyle \frac{v_{Bt}^2-v_B^2}{2a_B}}+x_B,\hfill & a_B\ne 0\hfill \end{array}\right.$$

(17)

where $v_{Bt}$ is the vehicle speed of the obstacle vehicle B after time t.

In real driving situations, the speed of the smart vehicle A usual varies, so solving Equation (16) yields the maximum lane-change time $T_{max}$:

$$T_{max}=\left\{\begin{array}{cc}{\displaystyle \frac{x_f-x_a}{v_s-v_f}},\hfill & a_B=0\hfill \\ {\displaystyle \frac{p-{\left(p^2-2a_{f}q\right)}^{1/2}}{a_f}},\hfill & a_B\ne 0\hfill \end{array}\right.$$

(18)

where p is the speed difference between the two vehicles; q is the distance of the two vehicles B ($x_B{-x}_S$).

When the front obstacle vehicle B is not stationary and has a certain speed to travel, the first quintic polynomial lane-changing process is able to be subdivided into two phases. The first phase is the intelligent vehicle A traveling to the starting location of the obstacle vehicle B. In the second phase, the vehicle A travels to the end of the first quintic polynomial planning (i.e., the transition mid-range position).

Two times $t_1$ and $t_2$ are calculated based on the states of the lane-changing vehicle A and the obstacle vehicle B ahead:

$$\left\{\begin{array}{c}{t}_1={\displaystyle \frac{x_B}{v_s}}\hfill \\ t_2=\left\{\begin{array}{cc}{\displaystyle \frac{t_1{v}_{Bt}}{v_s}},\hfill & a_B=0\hfill \\ {\displaystyle \frac{{\left(v_{Bt}+a_B{t}_1\right)}^2-v_{Bt}^2}{2a_{Bt}{v}_s}},\hfill & a_B\ne 0\hfill \end{array}\right.\hfill \end{array}\right.$$

(19)

Then, the total time for the first stage of lane-changing is

$$T_1=t_1+t_2$$

(20)

Determine the optimal range of values for $T_1$ depending on whether the front vehicle is stationary or in motion, respectively:

$$T_1=\left\{\begin{array}{cc}{\eta }_1{T}_{max},\hfill & v_f=0\hfill \\ {\eta }_2\left(t_1+t_2\right),\hfill & v_f>0\hfill \end{array}\right.$$

(21)

where, ${\eta }_1$, ${\eta }_2$ are real numbers less than 1. When the obstacle vehicle in front is stationary, the lane-changing time $T_1$ must have a certain safety margin, which should be less than the theoretical maximum lane-changing time. In addition, to ensure safety, $T_1$ should also satisfy

$$T_1<T_{max}$$

(22)

When the front obstacle vehicle is stationary (i.e., both $v_B$ and $a_B$ are 0), at this time, the front obstacle vehicle stays where it is and does not continue to move forward. If the intelligent vehicle wants to avoid collision with it, it needs to satisfy Equation (9): $x_m<x_B-a_A-b_B$.

In the second segment of the quintic polynomial trajectory planning, both the initial state and the final goal state are determined. Similar to traditional quintic polynomial programming, only the lane-changing time $T_2$ and longitudinal displacement $x_e$ need to be solved by the optimization algorithm.

4. Trajectory Optimization Based on GA-PSO Hybrid Algorithm

4.1. GA-PSO Hybrid Algorithm

Particle Swarm Optimization (PSO) is an optimization algorithm via group intelligence, which explores the optimal solution by simulating a group of “particles” interacting and collaborating in a search space [19]. The main advantage of PSO is its fast convergence speed and simple and easy-to-implement update mechanism. However, when the solution space is multi-peaked or of high complexity, the local search ability of PSO is weak.

The Genetic Algorithm (GA) is an optimization method based on heuristic principles, drawing inspiration from natural selection and biological evolution. It functions by emulating evolutionary mechanisms, wherein a group of potential solutions undergoes iterative refinement across multiple generations to identify optimal or near-optimal outcomes for complex problems. The process starts with an initial set of randomly generated individuals, each encoded as a chromosome representing a possible solution. Through repeated cycles of selection, crossover, and mutation, the population gradually evolves, enhancing fitness as measured by an objective function. A notable strength of GA lies in its capability to address non-linear, multi-modal, and high-dimensional optimization challenges, particularly in scenarios where traditional gradient-based techniques may struggle. Unlike deterministic methods, GA maintains a diverse population, enabling broader exploration of the search space and reducing the likelihood of getting trapped in local optima.

Compared with PSO, genetic algorithm GA performs more outstandingly in global search and can effectively avoid the search from falling into local optimum. Therefore, the genetic algorithm is embedded within the particle swarm optimization framework to enhance its capabilities:

(1) Selection operation: use roulette selection to retain particles with higher adaptation and eliminate particles with lower adaptation.

(2) Crossover operation: use arithmetic crossover to perform crossover operation on the selected particles to generate new particles:

$$X_{\mathrm{new}}=\alpha ·X_i+(1-\alpha )·X_j$$

(23)

where $\alpha$ is a random number and $X_i$ and $X_j$ are selected particles.

(3) Mutation operation: using Gaussian variation, particles are randomly mutated to increase the diversity of the population.

(4) Retention of elite particles: in each iteration, the best-adapted particles are retained to avoid losing the optimal solution.

(5) Stagnation detection: if the global optimal solution $P_g$ does not change in multiple iterations, reset $P_g$ to avoid the algorithm from falling into local optimality.

In the proposed GA-PSO hybrid algorithm (Algorithm 1), the Gaussian variation probability is set to 0.1 ($P_m=0.1$). This means that each selected particle has a 10% chance to undergo Gaussian mutation during each generation. The Gaussian perturbation is defined as follows:

$$X_{\mathrm{new}}=X_i+\mathcal{N}(0,\sigma )$$

(24)

where $\sigma$ is the Gaussian standard deviation set to 5% of the search space length. The convergence of the GA-PSO algorithm is determined based on two conditions: (1) The algorithm terminates if the iteration number reaches a predefined maximum, $N_{\max }=50$. (2) Early termination when the global optimal solution does not improve significantly (i.e., $\Delta P_g<{10}^{-6}$) within 20 consecutive generations.

By fusing genetic algorithms, the performance of PSO will be significantly improved. It can not only accelerate the convergence process, but also enhance the diversity of solutions and global search ability, which makes the algorithm more adaptable and robust.

Algorithm 1. GA-PSO hybrid optimization algorithm.

1: Initialize: population size m, learning factors $c_1$, $c_2$, inertia weights ${\omega }_{max}$, ${\omega }_{min}$, max iterations $K=50$   2: Set: GA parameters: crossover probability $p_c$, mutation probability $p_m$   3: for each particle $X_i$ do   4:     Randomly initialize position $X_i$ and velocity $V_i$   5:     Set personal best $P_i\leftarrow X_i$   6:     if $X_i$ violates constraint Equation (33) then   7:         Set fitness$\left(X_i\right)\leftarrow$ MAX   8:     end if   9: end for 10: Set global best $P_g\leftarrow min\left(P_i\right),i=1,\dots ,m$ 11: for $k=1$ to K do 12:     for each particle $X_i$ do 13:         Update $V_i$, $X_i$ using PSO rule (Equation (26)) 14:         if fitness$\left(X_i\right)<$ fitness$\left(P_i\right)$ then 15:            $P_i\leftarrow X_i$ 16:         end if 17:         if fitness$\left(P_i\right)<$ fitness$\left(P_g\right)$ then 18:            $P_g\leftarrow P_i$ 19:         end if 20:     end for 21:     Apply GA operators: 22:        Selection: select particles based on fitness 23:        Crossover: for selected pairs, with probability $p_c$ 24:           $X_{\mathrm{new}}=\alpha ·X_i+(1-\alpha )·X_j,\alpha \in [0,1]$ 25:        Mutation: with probability $p_m$, perform: 26:           $X_{\mathrm{new}}=X_i+\mathcal{N}(0,\sigma )$ 27:        Replace old population with new individuals 28:     if $P_g$ has not improved for 5 iterations then 29:         Reset or perturb $P_g$ 30:     end if 31: end for 32: Output: final global best $P_g\to T_1$, $v_c$, $T_2$, $x_c$

4.2. Optimization Problem Construction

To generate an optimal lane-changing trajectory that ensures driving safety, passenger comfort, and lane-changing efficiency, a constrained multi-objective optimization model is established based on vehicle dynamics and evaluation metrics. The decision variables, objective function, and constraints are clearly defined as follows.

4.2.1. Optimization Variable Analysis

With Equation (2), it can be seen that the vehicle trajectory is represented as a two-dimensional function over time where the decision variables are vectors that are combinations of two sets of polynomial coefficients.

$$z={[a_0,a_1,\dots ,a_5,b_0,b_1,\dots ,b_5]}^T\in {\mathbb{R}}^{12}$$

(25)

The optimization objective is to find z such that the vehicle lane-changing trajectory is optimal in terms of safety, comfort and efficiency combined.

4.2.2. Multi-Objective Optimization Function

In order to obtain the optimal lane-changing trajectory, combined with the vehicle safety, passenger comfort and vehicle lane-changing efficiency of the three evaluation factors, the multi-objective optimization evaluation function on the lane-changing trajectory can be established [20]; the evaluation function is as follows:

$$J={\omega }_1{J}_s\left(z\right)+{\omega }_2{J}_c\left(z\right)+{\omega }_3{J}_e\left(z\right)$$

(26)

where the weighting coefficients of the objective functions $J_s$, $J_c$, and $J_e$ are ${\omega }_1$, ${\omega }_2$, and ${\omega }_3$, respectively, and the sum of the three weighting coefficients should be one.

Due to the use of three cost terms $J_s$, $J_c$ and $J_e$ to evaluate the merits of lane-changing trajectories, the overall weights assigned to vehicle lane-changing efficiency, vehicle safety and passenger comfort need to be rationalized. Different weight assignments are used depending on the characteristics of the two successive lane-change situations. The first section focuses on completing the obstacle avoidance task for the obstacle vehicle B in front, and the vehicle A needs to travel to the staging position as soon as possible, so this section needs to give more consideration to vehicle safety. In the latter section, the vehicle has no obstacle vehicle to avoid, and only needs to travel to the final target position, so this part needs to give more consideration to the comfort of the passengers. Therefore, the weight coefficients of the objective function of the two segments are different: the weight coefficients of the vehicle safety in the front segment are larger, and the weight coefficients of the passenger comfort in the back segment are larger. Based on the empirical principles and simulation tuning results in real road scenarios, different weight coefficients of the multi-objective function on the front and back trajectories are obtained. The weight coefficients of the first section are ${\omega }_1=0.4$, ${\omega }_2=0.3$, ${\omega }_3=0.3$; the weight coefficients of the second section are ${\omega }_1=0.3$, ${\omega }_2=0.4$, ${\omega }_3=0.3$.

$J_s$ is the vehicle safety evaluation function. Vehicle safety is reflected in the body’s stability, and the body stability is in turn correlated with the vehicle’s swing angular velocity ${\omega }_R$ and side deflection angle $\beta$ of the two parameters; the smaller the value of the two indicates that the vehicle is more stable. On the contrary, the vehicle may easily produce a side-slip phenomenon, so the vehicle is unsafe to change lanes. Establish $J_s$ evaluation function as follows:

$$J_s={\displaystyle \frac{max\left({\omega }_r\right)}{{\omega }_{rmax}}}+{\displaystyle \frac{max\left(\beta \right)}{{\beta }_{max}}}$$

(27)

where $max\left({\omega }_r\right)$ and $max\left(\beta \right)$ denote the optimization process that computes the maximum ${\omega }_r$ and $\beta$, ${\omega }_{rmax}$ and ${\beta }_{max}$ denote the maximum values of the two parameters that are allowed when the vehicle does not undergo lateral deflection, respectively.

The angular velocity of the transverse pendulum can be expressed as follows:

$${\omega }_r={\displaystyle \frac{\ddot{y}\left(t\right)}{v_x\left(t\right)}}$$

(28)

Depending on the two-degree-of-freedom vehicle dynamics model, center-of-mass lateral deflection can be represented by

$$\beta =arctan{\displaystyle \frac{\dot{y}\left(t\right)}{\dot{x}\left(t\right)}}$$

(29)

$J_c$ is the function used to assess passenger comfort. Passenger comfort reflects the passenger’s riding experience, which can be expressed by the vehicle’s lateral acceleration $a_y$ and lateral leap $J_y$; the smaller its value, the better the passenger comfort, and too large a value will negatively affect the passenger riding comfort. The $J_c$ evaluation function is established as follows:

$$J_c={\displaystyle \frac{max\left(a_y\right)}{a_{ymax}}}+{\displaystyle \frac{max\left(J_y\right)}{J_{ymax}}}$$

(30)

where $a_{ymax}$ and $J_{ymax}$ mean the maximum values of lateral acceleration and lateral leap allowed by the vehicle.

$J_e$ is the evaluation function of lane-changing efficiency. The shorter the time t used, the higher the efficiency of lane-changing. The evaluation function of $J_e$ is established as follows:

$$J_e={\displaystyle \frac{T}{T_{max}}}+{\displaystyle \frac{L}{L_{max}}}$$

(31)

where $T_{max}$ indicates the longest time allowed for lane-changing, and $L_{max}$ indicates the maximum length of longitudinal displacement allowed for lane-changing.

4.2.3. Analysis of Constraints

(1) Safety constraints analysis:

In order to prevent the risk of sideslip or rollover of the vehicle changing lanes, it is necessary to set critical conditions for the occurrence of vehicle skidding. The values of the three parameters of vehicle lateral acceleration $a_y$, vehicle swing angular velocity ${\omega }_R$ and vehicle center of mass lateral deviation angle $\beta$ need to be controlled within certain limits. When the vehicle is changing lanes, its longitudinal velocity is much larger than the transverse velocity, and $\beta$ is smaller, where the focus is on considering ${\omega }_R$ constraint:

$${\omega }_r={\displaystyle \frac{F_{yr}+F_{yf}cos\delta }{m{v}_x}}⩽{\displaystyle \frac{F_{yr}+F_{yf}}{m{v}_x}}⩽{\displaystyle \frac{mg\mu }{m{v}_x}}={\displaystyle \frac{g\mu }{v_x}}$$

(32)

where $\mu$ denotes the coefficient of road friction. g denotes the gravitational acceleration. $F_{yr}$ and $F_{yf}$ are the lateral forces operating on the main vehicle’s front and rear wheels.

(2) Comfort constraints analysis:

While guaranteeing the vehicle’s safety, the ride comfort of the passengers in the vehicle also needs to be considered. Due to the road surface undulation generated by the bumps and vehicle acceleration in the process of changing lanes, normal vehicle driving will affect the passenger’s ride comfort [19]. During the vehicle lane change, the vehicle passengers are increasingly sensitive to the lateral motion of the vehicle compared to the longitudinal motion of the vehicle. When the vehicle changes lanes to produce lateral motion, the passengers will feel obvious center of gravity deflection. If the lateral movement is more intense, the vehicle occupants will also be squeezed with other occupants in the narrow space inside the vehicle, which seriously reduces the comfort of the passengers. According to the national standard document GB/T13441.1-2007 [21] on the approximate description of occupant comfort and the current consensus of the automotive industry, and combined with the constraints of the vehicle dynamics in the actual movement process [20], passenger comfort is related to the vehicle’s transverse acceleration and transverse leap. Smaller values of both mean that the change in velocity and acceleration is smooth and the vehicle occupant comfort is higher. Therefore, we use these two parameters as the main coefficients for measuring comfort.

(3) lane-changing efficiency constraints:

The lane-changing efficiency is mainly reflected in the two parameters of commutation time and commutation longitudinal displacement.

In summary, the following constraints are established on the quintic polynomial commutation trajectory:

$$\left\{\begin{array}{c}{\omega }_r⩽{\displaystyle \frac{g\mu }{v_x}}\hfill \\ \beta ⩽{\beta }_{max}\hfill \\ \left|a_y\right|⩽0.25\mathrm{g}\hfill \\ \left|J_y\right|⩽1\hfill \\ t<t_{max}\hfill \\ L<L_{max}\hfill \end{array}\right.$$

(33)

5. Simulation Test and Result Analysis

We use a hardware-in-the-loop (HIL) simulation device to validate the above lane-changing trajectory planning algorithm. Unlike the traditional simulation that is only virtualized within the software, the HIL simulation platform can make the simulation results more accurate and convincing by combining real-time hardware and software for real-time simulation [22]. The structure of the HIL simulation platform is illustrated in Figure 7.

The platform integrates a real-time controller, vehicle dynamics simulation model, traffic environment simulation and algorithm deployment modules to realize a complete closed-loop testing process. The upper processor is equipped with an Intel Core i9-12900K processor (Intel Corporation, Santa Clara, CA, USA) and an NVIDIA RTX 4080 GPU (NVIDIA Corporation, Santa Clara, CA, USA); the real-time controller is used to deploy the trajectory planning and control algorithms; the simulation modeling platform is based on the co-simulation of CarSim and Simulink; the communication interface module simulates the sensor inputs and actuator outputs through the CAN bus and the I/O interfaces; the trajectory planning algorithms are integrated into the Simulink controller in a modular form and run in real time on the HIL platform. The main experimental parameters are summarized in

Table 2. Experiment-related parameters.

Unit	Name	Value	Unit	Name	Value
kg	Vehicle mass	1270	m	Moment of inertia	3610
m	Vehicle length	4.5	m/s−2	$a_{y max}$	2.0
m	Vehicle width	1.75	m/s−2	$a_{x max}$	2.5

.

5.1. Scenario Configuration

(1) s1: static scenario

When vehicle A is cruising at a certain speed, a stationary obstacle vehicle B suddenly appears in front of the lane where it is located, and it needs to change lanes to avoid it. The speed of vehicle A is set to be 17 m/s, and vehicle B in front of vehicle A is about 45 m. The optimized and improved double-quintic polynomial lane-changing model is simulated with the unoptimized double-quintic polynomial model and the traditional quintic polynomial model for the lane-changing paths.

(2) s2: dynamic scenario

The obstacle vehicles in front of us cannot be all stationary, but are more in dynamic motion, i.e., the obstacle vehicles in front of us have a certain initial velocity and are still decelerating. Set the speed of the vehicle A to 14 m/s; the front obstacle vehicle B is 30 m ahead of the vehicle A; the speed is 12 m/s, and the acceleration of $-2$ m/s² is the deceleration.

(3) s3: low coefficient of adhesion pavement scenario

The low traction coefficient road surface will significantly reduce the friction between the tires and the ground, and it is easy for the vehicle to skid and lose control. Therefore, it is necessary to verify the safety and stability in the condition of a low adhesion coefficient environment. Setting the road surface adhesion coefficient to 0.6, the speed of the vehicle 11 m/s, the distance of the obstacle vehicle in front to 40 m, the speed of the vehicle 9 m/s.

5.2. Analysis of Results

(1) s1: static scenario

A comparison of planning times across three methods is presented to evaluate computational efficiency. The proposed trajectory planning algorithm, enhanced by the GA-PSO hybrid optimization, achieved the shortest average computation time of 22.3 ms. In contrast, the unimproved version without hybridization took 30.4 ms, and the traditional quintic polynomial method required 25.7 ms on average. All methods satisfy the real-time control cycle requirement (100 ms). The traditional quintic polynomial lane-changing model plans trajectory only via the start and final goal states. It is common to set the longitudinal velocity constant and utilize a larger transverse velocity and transverse acceleration as a way to avoid collision with the vehicle B during the lane-changing process. Figure 8 and Figure 9 show that the time required for lane-changing is shorter and the longitudinal displacement is smaller than that of the two quintic polynomials. However, the vehicle moves violently in the transverse direction, especially in Figure 9a, which shows that its transverse velocity is much larger than that of the other two models. Therefore, the traditional quintic polynomial model is less stable and comfortable.

The double-quintic polynomial lane-changing model separates the lane-changing process into two phases by adding transition states. The vehicle only needs to move a certain distance laterally in the first half of the process to avoid the obstacle vehicle in front of it. Since the vehicle in front is stationary and will not move forward, the lane-changing vehicle takes a deceleration in the longitudinal direction to reserve more time for lane-changing in the first phase, and the longitudinal acceleration is shown in Figure 9c. Even so, the front vehicle is stationary compared to the front vehicle with a certain speed; the lane-changing time in the first phase is still a little shorter, so the vehicle’s transverse velocity and transverse acceleration are also slightly larger, and the main vehicle’s transverse motion parameters are shown in Figure 9a,b. After the lane-changing vehicle passes through the staging position, the remaining second phase of lane-changing is equivalent to the barrier-free lane-changing, and there is no obstacle avoidance task in this phase, so it only needs to travel to the final target position. As can be seen through Figure 9a,b,d, the vehicle’s transverse angular velocity as well as the values of lateral velocity and lateral acceleration are smaller in this phase, in which the stability of the lane-changing vehicle and the passenger’s ride comfort are a bit better. In particular, our proposed optimized double-quintic polynomial commutation model not only satisfies the constraints in terms of velocity, acceleration, and angular velocity of the pendulum, but also has smaller fluctuations in the range of values compared to the unoptimized double-quintic polynomial model. Therefore, the optimized model is more stable and comfortable.

(2) s2: dynamic scenario

In a second test under a dynamic environment, our proposed hybrid algorithm achieved an average planning time of 35.6 ms, compared to 49.5 ms without the hybrid improvement and 45.3 ms using the traditional quintic polynomial method. All three methods meet the real-time requirement of the control cycle. The planned trajectories and the dynamic response results under different methods are shown in Figure 10 and Figure 11, respectively. In Figure 11, the peak value of the transverse velocity profile of the improved double quintuple permutation trajectory is lower than 1.0 m/s. Compared with the other two models, the improved permutation model trajectory satisfies the constraints in terms of transverse acceleration and longitudinal acceleration as well as transverse pendulum angular velocity, and the range of fluctuation of the values is lower compared with the other two models, so that the optimized and improved double quintuple polynomial permutation model is better in terms of stability and comfort. Therefore, the optimized and improved double-quintic polynomial commutation model is more stable and comfortable.

(3) s3: low coefficient of adhesion pavement scenario

In the third group of experiments conducted under low-adhesion road conditions, the proposed hybrid planning algorithm achieved an average computation time of 27.3 ms, compared to 36.8 ms for the non-hybrid version. Both algorithms met the real-time requirements of the control cycle. Based on Figure 12 and Figure 13, it can be seen that the optimized and improved lane-changing model, compared with the unoptimized model, improves the performance in many aspects, such as transverse velocity, transverse acceleration, and transverse angular velocity. Not only do the values all satisfy the constraints, but also the range of numerical fluctuations is a bit smaller, so the improved model is suitable for roads with low coefficient of adhesion.

6. Conclusions and Future Work

A lane-changing trajectory planning method via improved quintic polynomials for intelligent cars is proposed. For the unidirectional two-lane scenario, first, by analyzing the critical collision state between the two cars, and at the same time, considering the state of the two cars to determine the transit position, the lane-changing process is segmented into the front and rear parts, and the quintic polynomials are utilized for trajectory planning, respectively. Then, according to the different characteristics of the lane-changing paths in the front and rear segments, a multi-objective optimization function with different weight coefficients is established. A safe and comfortable lane-changing trajectory is achieved through the improved particle swarm optimization algorithm. Finally, the robustness and adaptability of the algorithm in different environments are verified through hardware-in-the-loop simulation tests. The results show that the lane-changing trajectories planned by the proposed model have significant advantages in terms of safety, comfort and real-time performance, and have good suitability in the face of static and dynamic obstacle vehicles, as well as on low-traction surfaces.

Due to limitations in experimental conditions, the scenarios selected in this paper are relatively simple and have not been systematically compared with other advanced trajectory planning methods. To further improve the adaptability and practicality of the proposed model, we plan to introduce predictive interaction models, such as the Intelligent Driver Model (IDM) or game-theoretic planning, to more effectively handle the uncertain behaviors of surrounding vehicles. The decision-making system will be based on the prediction information of surrounding vehicles and combined with a lane-change safety distance model to determine whether the conditions for a safe lane change are met. Once the feasibility and risk assessment criteria are satisfied, the lane-change command will be issued to the planning system. In future work, we also intend to extend the current highway-focused setting to urban or mixed-traffic environments, and conduct side-by-side comparisons with other state-of-the-art trajectory planning algorithms to more comprehensively evaluate the effectiveness of the proposed method.