A Dynamic Lane-Changing Trajectory Planning Algorithm for Intelligent Connected Vehicles Based on Modified Driving Risk Field Model

Abstract

A dynamic LC trajectory planning algorithm based on the modified driving risk field is proposed to address the issue of dynamic changes during the lane-changing (LC) process. First, a modified driving risk field (MDRF) model is constructed for LC scenarios. Then, according to the state of the target vehicle and discrete sampling points, a series of LC candidate trajectories were generated based on the quintic polynomial. After eliminating candidate trajectories that do not meet the constraints, the MDRF was utilized as a safety evaluation function. Additionally, comfort and smoothness evaluation functions were combined to evaluate candidate LC trajectories in order to obtain the optimal LC reference trajectory. Then, this paper proposes a dynamic LC trajectory planning algorithm, addressing the challenges of complex traffic scenarios and dynamic changes in adjacent vehicle states. Utilizing the optimal reference trajectory as a basis, a dynamic segmented algorithm is applied to the x–t and y–x curves, constructing an optimized objective function that considers the MDRF. Under multiple constraints, including the continuity and smoothness of the lateral and longitudinal trajectories, the penalty function approach is employed to solve the optimization objective function, yielding the optimal LC trajectory adapted to real-time changes in the traffic state. Finally, the proposed dynamic LC trajectory planning algorithm was validated under four different scenarios by using MATLAB 2023b. The simulation results indicate the safety, continuity, and dynamic feasibility of the proposed algorithm. Moreover, it demonstrates strong adaptability and flexibility in challenging dynamic LC scenarios.

1. Introduction

Lane changing (LC) and following behaviors are the most prevalent driving behaviors. Compared to the following behaviors, LC behaviors require transitioning from the current lane to the target lane, making the process more complex and more prone to collisions with adjacent vehicles [1,2]. Relevant research has demonstrated that 70% of traffic accidents are caused by human error, with LC behavior being one of the primary reasons for various accidents [3]. Based on statistical data, traffic accidents caused by LC maneuvers account for approximately 4% to 10% of all traffic accidents, leading to about 10% of total traffic delays [4]. Intelligent and connected vehicles (ICVs) can enhance travel experience, improve driving safety, reduce traffic accidents, and mitigate the likelihood of traffic congestion [5,6]. With the development of ICVs, it is widely believed in the industry that they provide a crucial solution to address issues such as traffic safety, congestion, and pollution. ICVs have the potential to reduce over 50% of traffic accidents [7]. According to the definitions by the Society of Automotive Engineers (SAE) [8], currently most automobiles are at level 2 (L2). L2 represents partial automation, where auto lane change (ALC) systems are vital components of L2 advanced driving assistance systems (ADAS) used in specific scenarios, enabling vehicles to perform lane changes independently within predefined circumstances [9]. LC trajectory planning is a critical component of ALC systems [10]. It generates an LC path that avoids collisions with adjacent vehicles, providing significant assurance for efficient and safe vehicle maneuvering. It has garnered significant attention from researchers in related fields.

However, in real-road environments, traffic conditions are highly complex and constantly changing. Vehicles equipped with ADAS systems face challenges in assessing dynamic environmental risks, especially in highly dynamic and complex scenarios [11]. The LC trajectory cannot be adjusted based on real-time traffic conditions, which consequently lead to frequent traffic accidents. The National Highway Traffic Safety Administration (NHTSA) reports that in 2023, there were a total of 367 traffic accidents that involved L2-level ADAS in the US. Among which 51.86% involved collisions resulting from lane changing or merging [12]. As a result, the industry has repeatedly postponed the timeline for attaining complete autonomy in highly dynamic and complex scenarios [13]. Consequently, it is imperative for the LC vehicles to adapt to the intricate and fluctuating traffic environments by dynamically adjusting their trajectories based on information such as the positions, speeds, and accelerations of adjacent vehicles, thereby ensuring the safety of LC maneuvers.

Given this, this paper proposes a dynamic LC trajectory planning algorithm for ICVs in response to the dynamically changing surrounding environments. This algorithm aims to provide ICVs with the capability of real-time adaptation to varying traffic environments, accurately anticipate potential collision risks, and quickly make optimal trajectory adjustments. Firstly, referring to the artificial potential field (APF) theory, a modified driving risk field (MDRF) for LC scenarios is constructed, taking into account the size, position, velocity, and acceleration of adjacent vehicles. Next, employing a quintic polynomial, the LC trajectory planning is decomposed into path planning and velocity planning, generating LC trajectories clusters. Utilizing the MDRF, an evaluation function is constructed to select the reference LC trajectory. Subsequently, based on the reference LC trajectory, the optimal trajectory is dynamically segmented and optimized. An optimization objective function based on the MDRF is constructed, which is constrained by start and end points, x–t curve, y–x curve, and smoothness conditions. And the penalty function method is applied to solve this optimization problem, yielding the optimal LC trajectory. Finally, MATLAB simulations are constructed to validate the effectiveness of the proposed algorithm.

The overall structure of this paper is organized as follows: Section 2 summarizes the relevant theoretical methods in the research field of driving risk field (DRF) and LC trajectory optimization. Section 3 constructs an MDRF specifically for LC scenarios, evaluating real-time potential LC risks. In Section 4, a cluster of LC trajectories is generated based on quintic polynomials, from which a reference LC trajectory is selected. Section 5 proposes a dynamic optimizing algorithm for LC trajectories. Finally, Section 6 conducts simulations to validate the proposed algorithm under various scenarios.

2. Literature Review

In recent years, the safety of the traffic participants, especially the ICVs, has attracted significant concern. Trajectory planning and risk assessment methodologies are the main research directions. Risk assessment, a critical component for decision making and ensuring the secure operation of ICVs in complex traffic scenarios, involves evaluating the potential influence of diverse traffic elements within the traffic environment on the operational safety of these vehicles.

The field-based driving risk assessment aligns more closely with the intrinsic nature of how drivers perceive risks [14]. It can assimilate data regarding the vehicles’ dynamic conditions and traffic environment, especially during interactions among multiple vehicles. The concept of artificial potential fields (APFs), introduced by Khatib in 1986, marked a significant advancement in this field. Initially applied to investigate robot path planning and obstacle avoidance, APF has been widely adopted in collision avoidance and path planning for autonomous vehicles [15]. Building upon the foundational principles of APF, Wang et al. innovatively proposed the driving risk field (DRF) model and applied it to vehicle risk assessment in complex road traffic environments [16]. Matinez and Jimenez take into account the forward, side, and diagonal potentials to simulate interactions between the vehicles and environmental elements [17]. The DRF differs from traditional vehicle dynamics and kinematics-based risk assessment methods by quantifying the interactive effects between vehicles and surrounding elements, such as pedestrians, adjacent vehicles, road conditions, and infrastructure, into a risk field distribution during driving [18]. For risk perception, early warning, and decision making in ITS, DRF provides more comprehensive and dynamic theoretical support [19]. Subsequent research has enhanced the DRF by considering factors such as minimum safety distance [20], vehicle motion direction [21], acceleration, and steering angle. Li et al. [22] assessed the LC risks and presented the distribution of the DRF under different motion states, summarizing the safe distance required at the end of the LC process. Mullakkal-Babu et al. [23] developed an improved APF model based on the product of collision probability and expected collision energy to assess the driving risk. They further utilized this model to predict the collision probability with adjacent vehicles and evaluated the risk for different scenarios by selecting either a single time step or multiple time steps. Chen et al. [24] computed the risk at each time point during the LC process based on the risk indicators derived from the DRF. They then generated an LC risk field and applied dynamic time warping (DTW) and K-means clustering algorithms to classify the LC risk profiles into several categories. The DRF contributes to designing and understanding the risk distribution and safety orientation in dynamic traffic environments. It has facilitated the simulation of risk fields for ICVs in intricate traffic scenarios, enabling a deeper analysis and optimization of driving safety. Li et al. [25] suggested a novel dynamic driving risk potential field model that completely accounts for the dynamic influence of the vehicle’s steering angle and acceleration. The distribution of risk fields is impacted by acceleration and steering angle. Son and Kim [26] used a cooperation concept to present an innovative approach to assess the rear-end collision risk for an autonomous LC system, offering a more flexible and proactive method compared to previous methods. Zhang et al. [27] investigated the driving risk pulse field model by risk pulse energy for cars following, which include both basic and random risk energies. The model assesses the overall risk performance and total risk level from vector and scalar perspectives, which includes the properties and motion states of vehicles, pedestrians, obstacles, etc., such as speed and acceleration.

In terms of LC trajectory planning and optimization, a multitude of innovative approaches have been proposed to enhance the safety and efficiency of autonomous vehicles. Yang et al. [28] employed an improved APF method to assess path risk, accounting for the driver’s style and reaction time. Finally, the optimal path was selected based on a comprehensive consideration of LC efficiency and comfort. Zuo et al. [29] proposed a local path planning method that combines Model Predictive Control (MPC) with APF. They incorporated the potential field function into the cost function, transforming the local path planning problem into a typical MPC optimization problem. And the PSO strategy was employed for the optimal path. Matinez et al. [17] proposed the algorithm for lateral and longitudinal planning based on the potential models. The lateral planning algorithm calculates the steering angle based on the total lateral potential, which includes contributions from side and diagonal potentials. While the longitudinal planning algorithm determines the vehicle’s speed based on the obstacles and the road conditions. Li et al. [30] proposed an efficient LC path planning method that leverages prior road information to optimize the selection of sampling points, enhancing the algorithm’s real-time performance and reducing the number of candidate paths. Beyond the sampling points, additional intermediate nodes and target points were introduced to address the issue of continuous LC, thereby enabling ICVs to execute complex maneuvers. Wang et al. [31] proposed a hybrid trajectory planning method based on sampling and numerical optimization methods. Initially, they used sampling methods to decompose path velocity and generate collision-free candidate trajectories. Considering factors such as curve smoothness, collision risk, and travel time, the optimal trajectory was determined. Finally, within the spatiotemporal constraints, sequential quadratic programming (SQP) was applied to optimize the trajectory, guaranteeing safety, stability, and comfort. Ding et al. [32] proposed a time-based quintic polynomial algorithm to derive an LC reference trajectory that satisfies multiple criteria, including safety, LC duration, driving distance, and comfort. They introduced LC constraints induced by adjacent vehicles and ultimately minimized LC duration, driving distance, driving comfort, and fuel consumption to compute the optimal LC trajectory that eliminates potential collisions. Yu et al. [33] proposed the LC decision-making model for ICVs in a mixed traffic environment based on multi-player dynamic game theory. This model introduced an overtaking expectation parameter to measure the utility of following vehicles and employed a hybrid splitting algorithm to solve the Nash equilibrium solution in multi-player games, thereby achieving the optimal solution for LC decisions. Luo et al. [34] employed hidden Markov models (HMMs) to assess adjacent vehicles, calculating the magnitude of obstacle vehicles’ longitudinal and lateral potential field strength. By utilizing HMMs with real-time trajectory prediction methods, the feasible collision avoidance paths were obtained. Wang et al. [35] proposed a combined optimization method based on NSGA-II and TOPSIS to solve the multi-objective optimization problem of LC trajectory algorithms. Under the consideration of LC constraints, feasible trajectory clusters were generated based on the quintic polynomial to ensure the comfort, stability, and effectiveness of the LC process. The NSGA-II algorithm was employed to optimize longitudinal displacement and LC duration. A continuous ordered weighted averaging operator was introduced to calculate weights for three objective optimization functions. Finally, the TOPSIS algorithm was applied to obtain the optimal LC trajectory. Liu et al. [36] aimed to accomplish dependable real-time avoidance of both static and moving obstacles in the situation involving multi-vehicles interacting on a constructed road and provided a least action principle to present a hierarchical lane-changing trajectory planner. Feng et al. [37] addressed the interaction effects between the ego vehicle and adjacent vehicles in the dynamic road environment and suggested an LC decision making and trajectory planning method using the GCNs and multi-segment polynomial curve optimization. Zhu et al. [38] explored the integration of dynamic vehicle trajectories, vehicle safety factors, static traffic environments, and actuator constraints to improve cooperative intent modeling. And proposed a cooperative intent multi-layer graph neural network (CMGNN) model for a more comprehensive approach to trajectory planning. Nie et al. [39] proposed a framework to dynamically plan LC trajectory for autonomous buses. The planning and replanning algorithm is optimized in the safe range of longitudinal length. And the optimization is aimed at generating a real-time reference trajectory that takes into account the buses’ yaw, roll dynamics, and LC efficiency.

In the realm of LC trajectory planning and optimization, significant strides have been made to enhance the rationality and comfort of trajectories, particularly under scenarios where adjacent vehicles are assumed to be in constant conditions. Furthermore, the computational intensity of some methods, such as those utilizing model predictive control (MPC) or sequential quadratic programming (SQP), may exceed the processing capabilities of onboard vehicle systems, thereby limiting their practicality for real-time decision making. The need for dynamic LC trajectory planning that can adapt to the uncertainties and complexities of traffic environments is therefore paramount. In light of these limitations, this paper introduces a dynamic LC trajectory planning algorithm designed to respond to the interactive states between the LC vehicle and adjacent vehicles in real time. The proposed algorithm aims to overcome the aforementioned challenges by adjusting LC strategies across different phases of the LC process, thereby planning trajectories that are not only efficient and smooth but also minimize potential risks. This approach is intended to enable intelligent connected vehicles (ICVs) to better navigate the complexities of real-world road conditions, marking a significant step towards the practical implementation of autonomous driving technologies in varied traffic scenarios.

3. Modeling of Modified Driving Risk Field

3.1. LC Scenario

The typical LC scenario for the ICVs in a multi-lane highway environment is illustrated in Figure 1. TV is the target vehicle intending to change lane, CPV is the current-lane preceding vehicle, TPV is the target-lane preceding vehicle, and TFV is the target-lane preceding vehicle.

3.2. Modified Driving Risk Field Model

The MDRF for LC scenarios includes the road risk field, static risk field, and dynamic risk field. The road risk field mainly consists of the roadside risk field and the lane centerline risk field. The static risk field primarily considers the static attributes of adjacent vehicles, incorporating vehicle size, the distance to the adjacent vehicles, and the direction of vehicles approaching the TV. The dynamic risk field mainly encompasses the impact of the dynamic characteristics of adjacent vehicles and the relative dynamic features of the TV.

3.2.1. Road Risk Field

Highway is the typical structured road, on which the roadside markings are used to regulate and restrict the longitudinal (Y direction) movement of vehicles, ensuring driving safety [40]. Depending on the different impacts of roadside markings and lane centerlines on risk, the road risk field includes the roadside risk field and the lane centerline risk field.

(1)

Roadside Risk Field

Higher risk field strength results from an increase in the potential risk when the TV drives up to the roadside [41]. Since the roadside cannot be crossed and the roadside risk field does not vary dynamically, it is solely determined by the vehicle’s longitudinal position. The roadside risk field can be calculated using the following formulas:

$$U_{road\_b}\left(y\right)=A_{road\_b}\times {\displaystyle \sum _{i=1}^2\exp \left(-\frac{1}{2}{\left(\frac{y-y_{road\_b}}{{\sigma }_{road\_b}}\right)}^{c_1}\right)}$$

(1)

where A_{road_b} is the coefficient of the roadside risk field, y_{road_b} is the longitudinal coordinate of the boundary line, and σ_{road_b} and c₁ are the shape coefficients.

(2)

Lane Centerline Risk Field

The lowest risk for TV is when it remains on the lane centerline, while the highest risk is when it crosses the lane line [42]. The lane centerline risk field does not change over time and is only related to the vehicle’s longitudinal position. Therefore, the lane centerline risk field is:

$$U_{road\_c}\left(y\right)=A_{road\_c}\times {\displaystyle \sum _{i=1}^n\exp \left(-\frac{1}{2}{\left(\frac{y-y_{road\_c}}{{\sigma }_{road\_c}}\right)}^{c_2}\right)}$$

(2)

where n is the number of lanes, A_{road_c} is the coefficient of lane centerline risk field, y_{road_c} is the longitudinal position of the lane centerline, and σ_{road_c} and c₂ are the are the shape coefficients.

(3)

Road Risk Field

Therefore, the road risk field is the combination of the roadside risk field and the lane centerline risk field:

$$U_{road}\left(y\right)=U_{road\_b}\left(y\right)+U_{road\_c}\left(y\right)$$

(3)

3.2.2. Static Risk Field

Three factors primarily influence the static risk field, which focuses on the static attributes of adjacent vehicles: (1) the relative distance between the TV and adjacent vehicles; (2) the direction in which the TV approaches the adjacent vehicles; and (3) the dimensions of the adjacent vehicles.

First, the smaller the relative distance between the TV and adjacent vehicles, the greater the possibility of collision and the stronger the static risk field. Second, considering the actual motion characteristics of vehicles, their traveling direction is limited; that is, the longitudinal speed of vehicles is usually much smaller than the lateral speed [43]. Therefore, laterally, the static risk field of adjacent vehicles has a greater impact range, while longitudinally, the impact range of the static risk field is smaller. In addition, taking into account the influence of the adjacent vehicles’ dimensions, the larger the vehicle size, the relatively higher the risk it poses.

Due to the rapid decrease in field strength with increasing distance in traditional first-order central moment two-dimensional Gaussian functions when describing the static risk field of vehicles [44]. It implies the risk-field strength at the edge of the vehicle will rapidly decrease. However, this situation does not align with reality. In fact, the edge of the vehicle also poses a higher risk to the TV. In contrast, using two-dimensional Gaussian functions with high-order central distance can flatten the peak of the function. This results in a smaller difference in field strength between the edge and the center of the vehicle, better reflecting the distribution of the vehicle’s static risk field.

Furthermore, high-order Gaussian functions can accurately describe the impact of changes in the relative position between TV and adjacent vehicles on the field strength and can control the shape of the risk field by adjusting the high-order terms, making the lateral and longitudinal risk distribution of the vehicle more consistent with the perceived risk during actual driving. In conclusion, high-order Gaussian functions can better reflect the influence of relative position, directional differences, and vehicle dimensions on the risk field. The static risk field can be formulated as Equation (4).

$$U_{sta}\left(x,y\right)=A_{sta}\times \exp \left(-{\left({\displaystyle \frac{{\left(x-x_j\right)}^2}{{\sigma }_{xg}^2}}\right)}^{\beta }-{\left({\displaystyle \frac{{\left(y-y_j\right)}^2}{{\sigma }_{yg}^2}}\right)}^{\beta }\right)$$

(4)

where (x, y) is the coordinate of TV, (x_j, y_j) is the coordinate of the j-th adjacent vehicle, A_sta is the coefficient of static risk field strength, it is a constant; β is the high-order coefficient that influences the distribution of static risk field, which can reflect the distribution in different scenarios by adjusting the value; σ_x and σ_y are the shape function of the adjacent vehicle, σ_x = k_x × L_j, σ_y = k_y × W_j, L_j and W_j are the length and width of the j-th adjacent vehicle, respectively; k_x and k_y are the dimensions coefficient of the adjacent vehicle, which adjust the influence range of the static risk field in the longitudinal and lateral directions, respectively.

3.2.3. Dynamic Risk Field

The dynamic risk field describes the evolution patterns of the potential risk between TV and the moving adjacent vehicles. It takes into account the relative distance, relative velocity, and approaching direction between TV and adjacent vehicles. As the relative distance between vehicles decreases, the risk of collision increases. Furthermore, there is a nonlinear acceleration in the rate of risk rise with decreasing distance; the rate of risk increase shows a nonlinear acceleration. In real traffic environments. The dynamic speed and approach direction of adjacent vehicles significantly affect the LC risk. For instance, the rate of increase in collision risk is faster if the following vehicle approaches at a higher speed than the leading vehicle. Conversely, the rate of decrease in collision risk is also faster if the leading vehicle accelerates away at a speed higher than the following vehicle [45]. Similarly, all other adjacent vehicles constantly changing positions during the LC process will have similar and complex interactive effects on TV.

For the moving adjacent vehicles, the highest point of the risk field strength undoubtedly lies at the vehicle’s center point. Moreover, as the distance from the vehicle’s center point increases, the risk field strength decreases, and the risk field is symmetrical on the left and right sides. That is, regardless of whether other vehicles approach from the left or right side, the risk is the same. Therefore, to comprehensively capture the influence of dynamic factors on LC risk, a dynamic risk field can be constructed using a two-dimensional Gaussian function. The two-dimensional Gaussian function can integrate multiple variables, such as relative distance and relative velocity, to accurately describe the evolution of the risk field along with the dynamics of the vehicle. The dynamic risk field is calculated as Equation (5):

$$U_{dyn}\left(x,y\right)=A_{dyn}\times {\displaystyle \frac{\exp \left(-\frac{{\left(x-x_j\right)}^2}{{\sigma }_v^2}-\frac{{\left(y-y_j\right)}^2}{{\sigma }_{yg}^2}\right)}{1+\exp \left(-re{l}_v\left(x-x_j-\alpha L_{j}re{l}_v\right)\right)}}$$

(5)

where A_dyn is the coefficient of dynamic risk field strength, it is a constant, α is a constant, 0 ≤ α ≤ 1, σ_v is a function relating the relative velocity between the moving adjacent vehicle and TV, σ_v = k_v|v_j − v|, v_j is the velocity of the j-th adjacent vehicle, v is the velocity of the TV, k_v is a constant greater than 0, rel_v is the function describing the direction of relative motion between TV and adjacent vehicle, rel_v = sign|v_j − v|, if v_j > v, rel_v = 1, and v_j ≤ v, rel_v = −1.

3.2.4. Modified Driving Risk Field

In conclusion, the MDRF for TV is the sum of the road risk field, static risk field, and dynamic risk field, namely:

$$U=U_{road}\left(y\right)+{\displaystyle \sum U_{sta,j}\left(x,y\right)}+{\displaystyle \sum U_{dyn,j}\left(x,y\right)}$$

(6)

where U is the LC driving risk field, U_sta,j(x) is the static risk field of the j-th adjacent vehicle, and U_dyn,j(x) is the dynamic risk field of the j-th adjacent vehicle.

3.2.5. The Simulation of MDRF Model

According to Wang [19], and by conducting multiple tests, the values of each parameter in the MDRF model utilized in these simulations are detailed in

Table 1. The values of parameters in MDRF model.

Parameter	Value	Parameter	Value	Parameter	Value	Parameter	Value
Aroad_b	2.12	Aroad_c	0.23	Asta	3	Adyn	3
σroad_b	1	σroad_c	1	β	2	α	0.6
c1	2	c2	2	kx	2.6	kv	6
				ky	0.35

.

To validate the field strength distributions of the road risk field, static risk field, dynamic risk field, and MDRF, simulations were conducted using MATLAB. The lane width is 3.75 m; the location, length, width, and velocity are detailed in

Table 2. Simulation scenario setting.

Vehicle	Location	Length (m)	Width (m)	Velocity (m/s)
TV	(80, 1.875)	5.6	2.2	30
CPV	(100, 1.875)	5.6	2.1	28
CFV	(30, 1.875)	5.6	2.1	26
TPV	(120, 5.625)	5	2	32
TFV	(55, 5.625)	5.2	2	32

.

The road risk field, static risk field, and dynamic risk field are illustrated in Figure 2, Figure 3 and Figure 4.

In Figure 2, the minimum risk field strength is at the centerline of the lane, while the maximum strength is at the road boundary lines. The risk field strength at the lane line is between these two extremes. The distribution of road risk fields ensures the vehicles can smoothly cross the lane line and guides the vehicles to drive near the centerline. And it can be seen in Figure 3 that the static risk field of each adjacent vehicle distributes in a shape approximating a rectangle, indicating that regardless of whether the TV approaches from the left or right, front or rear of the adjacent vehicle, it faces the same strength of static risk field. Furthermore, the coverage range of the risk field along the X-axis is greater than that along the Y-axis. In this scenario, due to the velocities of TPV and TFV being both greater than those of TV, the distribution of the dynamic risk field in Figure 4 shows a pronounced forward concentration propensity, which implies that the dynamic risk of collision is considerably high when vehicles approach from the front. Figure 5 illustrates the distribution of MDRF among the adjacent vehicles. It is evident that the risk field strengths of TPV and TFV exceed those of CPV and CFV, and the spatial extents of TPV and TFV are also greater than those of CPV and CFV. The MDRF model considers the differential impact of the adjacent vehicles, effectively capturing the gradient of risk imparted by different adjacent vehicles to TV, thus providing a nuanced assessment of the adjacent vehicular interactions.

According to the simulation results, the MDRF model comprehensively considers various influential factors such as the road, the morphological attributes, relative distances, and velocities of the adjacent vehicles on the LC risk within complex multi-vehicle motion scenarios. The MDRF model can dynamically reflect the real-time evolution of risk profiles surrounding the TV. By adjusting parameters such as the shape coefficient, the field distribution that more closely aligns with actual traffic conditions can be obtained, which can enhance the model’s applicability across diverse traffic conditions. Moreover, by mapping the risk distribution with specific spatial coordinates within the traffic environment, the MDRF model enables a more accurate and expeditious perception of dynamic changes. This mapping not only provides a visual representation but also serves as a critical input for the LC trajectory planning algorithm.

4. The LC Trajectory Dynamic Planning Algorithm Based on MDRF

4.1. Coordinate System

Highways are a typical structured road, characterized by their rigorous lane demarcations and stringent traffic regulations. These elements collectively establish clear boundaries and constraining conditions for vehicle operation, thereby imparting a distinct regularity and controllability to vehicle movement when traveling on highways. The upper and lower boundaries of the road are defined as B1 and B2, respectively. The coordinate system is established with the starting point of the target lane as the origin, creating a local rectangular coordinate system as shown in Figure 6. The X direction is defined as the forward direction along the road, which corresponds to the vehicle’s travel direction. The Y direction is perpendicular to the X direction. The coordinates (x₀, y₀) represent the position of TV at the beginning of the LC process, and the coordinates (x_end, y_end) represent the vehicle’s position at the end of the LC process.

4.2. Discrete Sampling of LC Space

In order to avoid creating an infinite number of LC trajectories and minimize computational resources, it is necessary to discretized sample the LC space. Based on the research about the LC duration and distance, the LC distance is primarily concentrated within 150 m, and the LC duration is concentrated in 3~10 s [46,47,48]. Therefore, the sampling points are taken at an interval of ΔX = 10 m in the X direction and at an interval of Δt = 1 s in the Y direction. The sampling region ahead of the TV is shown in Figure 7. In the local rectangular coordinate system constructed in Figure 6, the length of the LC trajectory planning area is set to 150 m.

In terms of lateral position, the initial position of TV at the start of the LC is x_ini. The discrete sampling points are as follows:

$$X_{sampling}=\left[\begin{array}{ccccc}{x}_{ini}+10& x_{ini}+20& \cdots & x_{ini}+140& x_{ini}+150\end{array}\right]$$

(7)

In terms of longitudinal position, the vehicle’s outer edge line should remain within the lane; the coordinate range for the vehicle center is as follows:

$$y_{\mathit{min}}={\displaystyle \frac{W}{2}},\hspace{0.33em}\hspace{0.33em}{y}_{\mathit{max}}=2W_r-{\displaystyle \frac{W}{2}}$$

(8)

where W_r is the width of each lane, W is the width of the vehicle.

Therefore, within the position interval [y_min, y_max], with the sampling time interval Δt = 1 s, the longitudinal sampling points are:

$$t_{sampling}=\left[\begin{array}{ccccc}3& 4& \cdots & 9& 10\end{array}\right]$$

(9)

4.3. Generation of LC Trajectory

4.3.1. LC Trajectory Planning Based on Quintic Polynomial

LC trajectory planning is the process of ICVs on highways, that calculates precisely a feasible trajectory from the current lane to the target lane in a safe and smooth manner based on real-time road conditions, vehicle dynamic state parameters, and traffic rules. This complex process not only requires comprehensive consideration of the vehicle’s dynamic characteristics, the current driving state of the vehicle, and the real-time traffic situation in the target lane, but also necessitates a forward-looking assessment of potential risks that may occur in future moments, ultimately ensuring that the planned trajectory meets the requirements for efficient driving while maximizing the safety of the LC maneuver.

Due to its advantages of continuous curvature, smoothness, and ease of computation, polynomial trajectory planning is widely used in trajectory planning research, which provides natural, coherent, and easily controllable motion trajectories for vehicles when performing LC maneuvers. Moreover, the real LC trajectories have a high similarity with polynomial curves. Cubic, quintic, and septic polynomials are commonly used. Among these algorithms, the cubic polynomials, although simple, may not be sufficient to accurately describe complex curve patterns in certain complex LC scenarios, especially when precise control of start and end positions, velocities, and accelerations is required. Septic polynomials can provide fine trajectory descriptions but may lead to overfitting and high computational complexity. Quintic polynomials are more flexible and offer appropriate degrees of freedom than cubic and septic polynomials, which satisfies the requirements of most LC scenarios that necessitate meticulous trajectory control.

Trajectories for lateral and longitudinal motions are generated separately using quintic polynomials for the LC scenario depicted in Figure 1, with the moment of LC initiation serving as the starting point for trajectory planning and the starting point of the road segment as the origin of the X-Y coordinates. A cluster of LC trajectories is generated by combining these trajectories. Trajectories that fail to meet the kinematic constraints and boundary condition constraints are filtered out. Subsequently, based on the evaluation function, the trajectory with the lowest cost is selected as the reference LC trajectory.

4.3.2. Generation of Candidate LC Trajectory Based on Quintic Polynomials

In the X-Y coordinate system, a quintic polynomial represents the variations of lateral position (x) and longitudinal position (y) over time (t). To take into account the dynamic changes in the surrounding traffic environment, this paper converts the trajectories into y–x curves and x–t curves, where y–x curves represent the change in longitudinal position (y) with respect to lateral position (x), and x–t curves depict the change in lateral position (x) over time (t). Essentially, this approach converts the original three-dimensional trajectory planning problem involving x–t and y–t into a two-dimensional problem represented by y–x and x–t.

The quintic polynomials in the X and Y directions are shown in Equation (10):

$$\left\{\begin{array}{l}X\left(t\right)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5\\ Y\left(x\right)=b_0+b_{1}x+b_2{x}^2+b_3{x}^3+b_4{x}^4+b_5{x}^5\end{array}\right.$$

(10)

where X(t) and Y(x) are the positions of TV, a₀, …, a₅ and b₀, …, b₅ are the unknown coefficients of the polynomials, respectively associated with the vehicle’s lateral and longitudinal motion. Column matrix A represents the parameters related to the motion in the X direction, while column matrix B represents those related to the motion in the Y direction:

$$\left\{\begin{array}{l}A={\left[a_0,a_1,a_2,a_3,a_4,a_5\right]}^T\\ B={\left[b_0,b_1,b_2,b_3,b_4,b_5\right]}^T\end{array}\right.$$

(11)

To solve the aforementioned 12 parameters, 12 boundary conditions are required. These boundary conditions are obtained from the vehicle’s state information at t = 0 and t = t_end. By solving the system of equations using these boundary conditions, the parameters in column matrices A and B are determined. For the x–t curve X(t), at the time t = 0 and t = t_end:

$$\left\{\begin{array}{l}X\left(t_0\right)=X_0\left(t\right)\\ \dot{X}\left(t_0\right)=v_{x\_0}\\ \ddot{X}\left(t_0\right)=a_{x\_0}\\ X\left(t_{end}\right)=X_{end}\left(t\right)\\ \dot{X}\left(t_{end}\right)=v_{x\_end}\\ \ddot{X}\left(t_{end}\right)=a_{x\_end}\end{array}\right.$$

(12)

where X₀(t), v_{x_0} and a_{x_0} are the position, velocity and acceleration in the X direction at t = 0, X_end(t), v_{x_end} and a_{x_end} are the position, and velocity and acceleration in the X direction at t = t_end.

Moreover, for the x–y curve Y(x), at the start and end points:

$$\left\{\begin{array}{l}Y\left(x_0\right)=Y_0\left(x\right)\\ \dot{Y}\left(x_0\right)=\tan {\phi }_0\\ \ddot{Y}\left(x_0\right)={\displaystyle \frac{1}{Wb}}{\left(1+\tan {\phi }^2\right)}^{\raisebox{1ex}{$3$}\!\left/ \!\raisebox{-1ex}{$2$}\right.}\tan {\delta }_0\\ Y\left(x_{end}\right)=Y_{end}\left(x\right)\\ \dot{Y}\left(x_{end}\right)=0\\ \ddot{Y}\left(x_{end}\right)=0\end{array}\right.$$

(13)

where Y₀(x) is the position in the Y direction at t = 0, φ₀ and δ₀ are the heading angle and steering angle at t = 0, Wb is the wheelbase, Wb = 2.6 m. And at the end of the LC process, a_{x_end} = 0. Y_end(x) is the position in the Y direction at t = t_end, and ${\dot{Y}}_{end}\left(x\right)=0$, ${\ddot{Y}}_{end}\left(x\right)=0$.

Convert Equations (12) and (13) into matrix form:

$$\left\{\begin{array}{l}\left[X\left(t_0\right),\dot{X}\left(t_0\right),\ddot{X}\left(t_0\right),X\left(t_{end}\right),\dot{X}\left(t_{end}\right),\ddot{X}\left(t_{end}\right)\right]=T\times A\\ \left[Y\left(x_0\right),\dot{Y}\left(x_0\right),\ddot{Y}\left(x_0\right),Y\left(x_{end}\right),\dot{Y}\left(x_{end}\right),\ddot{Y}\left(x_{end}\right)\right]=X\times B\end{array}\right.$$

(14)

where:

$$T=\left[\begin{array}{cccccc}{t}_0^5& t_0^4& t_0^3& t_0^2& t_0& 1\\ 5t_0^4& 4t_0^3& 3t_0^2& 2t_0& 1& 0\\ 20t_0^3& 12t_0^2& 6t_0& 2& 0& 0\\ t_{end}^5& t_{end}^4& t_{end}^3& t_{end}^2& t_{end}& 1\\ 5t_{end}^4& 4t_{end}^3& 3t_{end}^2& 2t_{end}& 1& 0\\ 20t_{end}^3& 12t_{end}^2& 6t_{end}& 2& 0& 0\end{array}\right]$$

(15)

$$X=\left[\begin{array}{cccccc}{x}_0^5& x_0^4& x_0^3& x_0^2& x_0& 1\\ 5x_0^4& 4x_0^3& 3x_0^2& 2x_0& 1& 0\\ 20x_0^3& 12x_0^2& 6x_0& 2& 0& 0\\ x_{end}^5& x_{end}^4& x_{end}^3& x_{end}^2& x_{end}& 1\\ 5x_{end}^4& 4t_{end}^3& 3t_{end}^2& 2x_{end}& 1& 0\\ 20x_{end}^3& 12x_{end}^2& 6x_{end}& 2& 0& 0\end{array}\right]$$

(16)

By using the conditions of positions, velocity, acceleration, heading angle, and steeling angle at t = 0 and t = t_end, the parameters A and B are obtained by Equation (14). This yields a set of coefficients, thus defining a candidate trajectory.

4.3.3. Constraints for LC Trajectories Subsubsection

Not all the candidate LC trajectories generated by quintic polynomials meet the requirements. To ensure the feasibility of the LC trajectories and to minimize the computational workload for subsequent trajectory evaluations, the generated LC trajectories that do not meet the constraints will be filtered. The constraints are as follows:

(1)

Constraints of Lateral Velocity

$$v_{x\_\mathit{min}}<v_x<v_{x\_\mathit{max}}$$

(17)

To guarantee safety, the TV’s lateral velocity must remain above the minimum threshold and below a maximum limit. In this study, the velocity values are set as v_{x_min} = 0 m/s, v_{x_max} = 35 m/s.

(2)

Constraints of Lateral Acceleration

$$a_{x\_\mathit{min}}<a_x<a_{x\_\mathit{max}}$$

(18)

To ensure stability, the TV’s lateral acceleration should also fall within a predefined range of acceleration. In this study, the acceleration values are set as a_{x_min}= −6 m/s², a_{x_max} = 4 m/s².

(3)

Constraints of Road Boundary

$$B_2+{\displaystyle \frac{1}{2}}W<y<B_1-{\displaystyle \frac{1}{2}}W$$

(19)

During the LC process, TV must remain within the boundary lines of the lane. Where W represents the width of the vehicle, B₁ and B₂ represent the coordinates of the upper and lower boundaries of the road, respectively.

(4)

Constraints of Acceleration

$$a=\sqrt{a_x^2+a_y^2}\le \mu g$$

(20)

During the LC process, the acceleration must satisfy the limit of pavement adhesion. Where μ is the coefficient of road adhesion, μ = 0.8, and g is the acceleration of gravity.

By traversing LC trajectory clusters generated by the quintic polynomial, each trajectory will be systematically filtered one by one. Those trajectories that do not meet Equations (17)–(20) will be eliminated, while those that do meet the criterion will be retained, resulting in feasible trajectory clusters that satisfy the constraints.

4.3.4. LC Trajectory Evaluation Function

After eliminating LC trajectories that do not comply with the constraint conditions, there still exist multiple trajectories that satisfy all constraints. To obtain the optimal LC reference trajectory, it is necessary to evaluate the candidate LC trajectories. Generally, the main factors considered in the evaluation of LC trajectories include comfort, smoothness, and safety.

(1)

Comfort Evaluation Function (y–x Curve Evaluation Function)

The comfort evaluation function mainly considers the arc length, curvature, and change rate of curvature of the LC trajectory y–x curve. The degree of comfort increases with decreasing arc length, curvature, and rate of change in curvature.

$$f_1=k_1{\displaystyle {\int }_{x_0}^{x_{end}}{\left|\dot{Y}\left(x\right)\right|}^2}dx+k_2{{\displaystyle {\int }_{x_0}^{x_{end}}\left|\ddot{Y}\left(x\right)\right|}}^{2}dx+k_3{{\displaystyle {\int }_{x_0}^{x_{end}}\left|\stackrel{⃛}{Y}\left(x\right)\right|}}^{2}dx$$

(21)

where k₁, k₂, and k₃ are the influencing coefficients, $\dot{Y}\left(x\right)$ is the arc length, $\ddot{Y}\left(x\right)$ is curvature, and $\stackrel{⃛}{Y}\left(x\right)$ is the change rate of curvature.

(2)

Smoothness Evaluation Function (x–t Curve Evaluation Function)

Research has shown that humans are particularly sensitive to variations in acceleration. Rapid changes in acceleration can negatively affect passenger comfort and cause unstable vehicle motion. Therefore, the smoothness of driving is evaluated from the perspectives of the rates of change in lateral and longitudinal accelerations.

$$f_2=k_4{\displaystyle {\int }_0^{t_{end}}{\left|\dot{X}\left(t\right)-v_{des}\right|}^{2}dt}+k_5{{\displaystyle {\int }_0^{t_{end}}\left|\ddot{X}\left(t\right)\right|}}^{2}dt+k_6{{\displaystyle {\int }_0^{t_{end}}\left|\stackrel{⃛}{X}\left(t\right)\right|}}^{2}dt$$

(22)

where k₄, k₅, and k₆ are the influencing coefficients, $\dot{X}\left(t\right)-v_{des}$ is the difference between velocity and desire velocity, $\ddot{X}\left(t\right)$ is the acceleration, and $\stackrel{⃛}{X}\left(t\right)$ is Jerk_x, the change rate of acceleration. Smoothness requires that within the time interval [0, t_end] of LC trajectory planning, the vehicle should maintain a velocity close to the desired velocity while minimizing acceleration and jerk.

(3)

Safety Evaluation Function (MDRF Evaluation Function):

The MDRF, which is constructed in Section 3.2, is employed as the safety evaluation function. For each candidate LC trajectory, K equidistant discrete points are selected. The risk field strength value is then calculated for each discrete point, and the sum values give the overall risk field strength value for each trajectory, i.e.,:

$$f_3={\displaystyle \sum U_k}$$

(23)

where U_k is the risk field strength value of the k-th sample point.

In conclusion, the comprehensive evaluation function can be obtained by combining the comfort, smoothness, and safety evaluation functions through a weighted summation, as follows:

$$f=w_1{f}_1+w_2{f}_2+w_3{f}_3$$

(24)

where w₁, w₂, and w₃ are the weights of f₁, f₂, and f₃, respectively. In this study, w₁ = 1.2, w₂ = 1, w₃ = 1.3.

Based on the comprehensive evaluation function f, the filtered trajectories are comprehensively evaluated. Employing an exhaustive method, the evaluation function is computed for each trajectory. In accordance with the definitions of f₁, f₂, and f₃, the trajectories X_ref(t) and Y_ref(x) with the minimum f value are referred to as the optimal LC reference trajectory for dynamic optimization.

4.4. Dynamic Optimization of LC Trajectory

LC trajectory clusters were generated, filtered, and evaluated in order to get the reference LC trajectories X_ref(t) and Y_ref(x) for the planning period. However, due to the real-time update in the motion states of adjacent vehicles, the reference LC trajectories X_ref(t) and Y_ref(x) cannot adequately address the dynamically changing needs of LC in real time but serve only as references for the overall trajectory planning process. Hence, this paper utilizes real-time status information of adjacent vehicles, including speed and position, to predict the positions of vehicles in the next moment. The reference LC trajectory is dynamically optimized in real time by dynamic programming, updating the LC trajectories to obtain the optimal one.

4.4.1. Prediction of Vehicle’s Position

In the context of the ICV environment, real-time information about adjacent vehicles can be obtained through V2V communication. Given that the motion states of adjacent vehicles are continuously updating in real time, it is necessary to predict the real-time positions of vehicles and dynamically adjust the feasible area of LC trajectories accordingly. Considering the limitations of computational resources and concentrating on the dynamic programming of LC trajectories, a high-precision kinematic CA (constant acceleration, CA) model is chosen to predict the vehicle’s positions in the short term. Assuming at time t_i, as shown in Figure 8, the lateral velocity of the vehicle is v_x,i, the longitudinal velocity is v_y,i, the lateral acceleration is a_x,i, and the longitudinal acceleration is a_y,i, then the position at the next moment is:

$$\left\{\begin{array}{l}{X}_{i+1}=X_i+v_{x,i}\Delta t+{\displaystyle \frac{1}{2}}{a}_{x,i}\Delta t^2\\ Y_{i+1}=Y_i+v_{y,i}\Delta t+{\displaystyle \frac{1}{2}}{a}_{y,i}\Delta t^2\end{array}\right.$$

(25)

4.4.2. Segmentation of Reference LC Trajectory

For the dynamic programming of LC trajectories, it essentially involves optimizing the reference trajectories X_ref(t) and Y_ref(x). Leveraging real-time status information of adjacent vehicles (including position, velocity, and acceleration), the reference trajectories are dynamically optimized to derive the optimal LC trajectory. By dividing the reference trajectories into segments, optimization objective functions and constraints are established, and the objective functions of each segment are optimized to obtain the optimal trajectory.

Considering the limitation of onboard computing capabilities, the value should not be excessively large to avoid increasing the computational burden. However, if N is too small, the fitting effect of the trajectory curve will be subpar. In addition, LC durations are concentrated between 3 s and 10 s. After multiple simulation tests, the final option is N = 6, indicating an average duration of approximately 0.5 s to 1.6 s per segment, fulfilling the requirements for dynamic LC trajectory planning. Therefore, for the i-th segment of the LC trajectory:

$$\left\{\begin{array}{l}{X}_i\left(t\right)=a_{0i}+a_{1i}t+a_{2i}{t}^2+a_{3i}{t}^3+a_{4i}{t}^4+a_{5i}{t}^5\\ Y_i\left(x\right)=b_{0i}+b_{1i}x+b_{2i}{x}^2+b_{3i}{x}^3+b_{4i}{x}^4+b_{5i}{x}^5\end{array}\right.$$

(26)

For each trajectory segment X_ref(t) and Y_ref(x), tl_i ≤ t ≤ tu_i, xl_i ≤ x ≤ xu_i, i = 1, 2, …, N. a₀, …, a_5i and b₀, …, b_5i are the coefficients of X_i(t) and Y_i(x), tl_i and tu_i are the lower and upper boundary of the variable t for the i-th segment X_i(t), xl_i and xu_i are the lower and upper boundary of the variable x for the i-th segment Y_i(x), and xl_i = X_i(tl_i), xu_i = X_i(tu_i).

4.4.3. Optimization Objective Function

The optimization objective of LC trajectories is to identify the point with the lowest risk field strength value and accomplish the LC in the smoothest manner with maximum efficiency, thereby avoiding prolonged disturbance to adjacent vehicles, while maintaining conformity with the reference trajectory curve as much as possible. Optimization is conducted individually for each segment of the LC trajectory. For the i-th segment trajectory, the optimization objective function is as follows:

$$\min \hspace{0.33em}{J}_i=w_{4}Di{s}_{xi}+w_{5}Di{s}_{yi}+w_6{\displaystyle \sum U_i}+w_7{f}_{ef{f}_i}+w_8{\displaystyle \sum }Jer{k}_i^2$$

(27)

where,

$$Di{s}_{xi}={{\displaystyle {\int }_{t{l}_i}^{t{u}_i}\left(X_i\left(t\right)-X_{ref}\left(t\right)\right)}}^{2}dt$$

$$Di{s}_{yi}={{\displaystyle {\int }_{x{l}_i}^{x{u}_i}\left(Y_i\left(x\right)-Y_{ref}\left(x\right)\right)}}^{2}dx$$

$$U_i=U_{road,i}\left(y\right)+{\displaystyle \sum U_{sta,i}\left(x,y\right)}+{\displaystyle \sum U_{dyn,i}\left(x,y\right)}$$

$$f_{ef{f}_i}={\displaystyle \frac{t_{LCi}}{t_{LC\mathit{max}}}}$$

$${\displaystyle \sum Jer{k}_i^2}={{\displaystyle {\int }_{t{l}_i}^{t{u}_i}\left|\stackrel{⃛}{X}\left(t\right)\right|}}^{2}dt$$

where w₄, w₅, …, w₈, are the influencing coefficients, Dis_xi is the degree of conformity in the X direction between the i-th segment trajectory curve X_i(t) and the reference trajectory curve X_ref(t), Di_xyi is the degree of conformity in the Y direction between the i-th segment trajectory curve Y_i(x) and the reference trajectory curve Y_ref(x), U_i is the risk field strength value of the i-th segment trajectory, f_effi is the efficiency function, which is the ratio LC duration of the optimized trajectory to the duration of the reference trajectory, Jerk_xi is the change rate of acceleration of the i-th segment; to maintain smoothness during LC, Jerk_xi should be as small as possible.

Therefore, for the entire optimized LC trajectory, the overall optimization function is:

$$\min \hspace{0.33em}{J}_{sum}=J_1+J_2+\cdots +J_N$$

(28)

Optimizing and solving the y–x and x–t curves involve finding quintic polynomials X_i(t) and Y_i(x) that minimize the optimization objective function J_sum under specific constraints.

4.4.4. Constraints Function

To ensure the feasibility of the LC trajectory curve after dynamic optimization, it is necessary to impose constraints on each segment of the polynomial curve to align with the vehicle’s driving requirements. These constraints encompass the starting and ending points of each polynomial segment, restrictions on the x–t curve and y–x curve, and smoothness criteria.

(1)

Constraints of Starting and Ending Points

For the optimized LC trajectory, its starting point is the starting point of the reference trajectories X_ref(t) and Y_ref(x). The initial velocity, acceleration, and other states are identical to those of the starting points of X_ref(t) and Y_ref(x). Therefore, the following constraints exist:

$$\left\{\begin{array}{l}{X}_0\left(t\right)=X_{ref\_0}\left(t\right)\\ {\dot{X}}_0\left(t\right)={\dot{X}}_{ref\_0}\left(t\right)\\ {\ddot{X}}_0\left(t\right)={\ddot{X}}_{ref\_0}\left(t\right)\\ Y_0\left(x\right)=Y_{ref\_0}\left(x\right)\\ {\dot{Y}}_0\left(t\right)=v_{y0}=0\\ {\ddot{Y}}_0\left(t\right)=a_{y0}=0\end{array}\right.$$

(29)

At the endpoint, the y-coordinate lies within the target lane, and both v_y and a_y are equal to 0. Therefore, the following constraints exist:

$$\left\{\begin{array}{l}{W}_{line}+W_{car}/2<Y_{end}\left(x\right)<2\cdot W_{line}-W_{car}/2\\ {\dot{Y}}_{end}\left(t\right)=v_{yend}=0\\ {\ddot{Y}}_{end}\left(t\right)=a_{yend}=0\end{array}\right.$$

(30)

(2)

Constraints of x–t and y–x Curve

For the x–t curve, the lateral position should lie within the predicted feasible region. Furthermore, the first, second, and third derivatives of the x–t curve represent velocity, acceleration, and jerk, respectively, and must adhere to their upper and lower limit constraints.

In addition, for the y–x curve, the edge of TV should not exceed the boundary line. Moreover, the first, second, and third derivatives of the y–x curve represent arc length, curvature, and change rate of curvature, respectively, must adhere to the corresponding constraint conditions. Therefore, the following constraints are present:

Hence, the following constraints exist:

$$\left\{\begin{array}{ll}{x}_{0\_\mathit{min}}<X_i\left(t\right)<x_{end\_\mathit{max}},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}& W_{car}/2<Y_i\left(x\right)<2\cdot W_{line}-W_{car}/2\\ v_{\mathit{min}}\le {\dot{X}}_i\left(t\right)\le v_{\mathit{max}},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}& l_{\mathit{min}}\le {\dot{Y}}_i\left(x\right)\le l_{\mathit{max}}\\ a_{\mathit{min}}\le {\ddot{X}}_i\left(t\right)\le a_{\mathit{max}},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}& {\rho }_{\mathit{min}}\le {\ddot{Y}}_i\left(x\right)\le {\rho }_{\mathit{max}}\\ Jer{k}_{\mathit{min}}\le {\stackrel{⃛}{X}}_i\left(t\right)\le Jer{k}_{\mathit{max}},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}& {\dot{\rho }}_{\mathit{min}}\le {\stackrel{⃛}{Y}}_i\left(x\right)\le {\dot{\rho }}_{\mathit{max}}\end{array}\right.$$

(31)

To ensure the safety, v_min = 0 m/s, v_max = 30 m/s, a_min = −6 m/s², a_max = 4 m/s², Jerk_xmin = −2 m/s³, Jerk_xmax = 2 m/s³, l is the arc length, ρ is the curvature of y–x curve, ensuring safety against sharp turns, and $\dot{\rho }$ is the change rate of curvature, which is crucial for maintaining vehicle stability, especially during the LC process or sharp turns. Traverse all sampling points to determine the range of values, l_min = 0.2, l_max = 6.3, ρ_min = 0.00002, ρ_max = 0.0002, ${\dot{\rho }}_{\mathit{min}}=0.0000002$, ${\dot{\rho }}_{\mathit{max}}=0.000001$.

(3)

Smoothness Constraints Condition

Additionally, the polynomial curves between segments should adhere to dynamical constraints, and nodes should possess continuity. That is, for the x–t curve and y–x curves, the motion state at the end of one segment should match the motion state at the beginning of the subsequent segment. Therefore, the following constraints must be fulfilled:

$$\left\{\begin{array}{l}{X}_i\left(t{u}_i\right)=X_{i+1}\left(t{l}_{i+1}\right),\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}{Y}_i\left(x{u}_i\right)=Y_{i+1}\left(x{l}_{i+1}\right)\\ {\dot{X}}_i\left(t{u}_i\right)={\dot{X}}_{i+1}\left(t{l}_{i+1}\right),\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}{\dot{Y}}_i\left(x{u}_i\right)={\dot{Y}}_{i+1}\left(x{l}_{i+1}\right)\\ {\ddot{X}}_i\left(t{u}_i\right)={\ddot{X}}_{i+1}\left(t{l}_{i+1}\right),\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}{\ddot{Y}}_i\left(x{u}_i\right)={\ddot{Y}}_{i+1}\left(x{l}_{i+1}\right)\\ {\stackrel{⃛}{X}}_i\left(t{u}_i\right)={\stackrel{⃛}{X}}_{i+1}\left(t{l}_{i+1}\right),\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}{\stackrel{⃛}{Y}}_i\left(x{u}_i\right)={\stackrel{⃛}{Y}}_{i+1}\left(x{l}_{i+1}\right)\end{array}\right.$$

(32)

4.4.5. Dynamic Optimization Solution

In summary, the dynamic programming problem of LC trajectory can be viewed as a typical nonlinear optimization problem, aiming to find multiple polynomial segments X_i(t) and Y_i(x) to optimize the objective function J_sum while satisfying a sequence of equality and inequality constraints to achieve the minimum value. The nonlinear optimization problem is reformulated into:

$$\left\{\begin{array}{l}\min \hspace{0.33em}{J}_{sum}\\ s.t.\\ h_m\left(\mathit{a}\right)=0,m=1,2,\cdots ,l\\ s_n\left(\mathit{a}\right)\ge 0,n=1,2,\cdots ,u\\ h_m\left(\mathit{b}\right)=0,m=1,2,\cdots ,l\\ s_n\left(\mathit{b}\right)\ge 0,n=1,2,\cdots ,u\end{array}\right.$$

(33)

Equation (33) contains inequality constraints, which can be addressed by employing the penalty function method for the aforementioned nonlinear optimization problem. By applying the given constraints, an optimal LC trajectory can be computed.

5. Simulation Scenarios

The MATLAB 2023b simulation platform is utilized to validate the proposed dynamic LC trajectory planning algorithm. Four scenarios are set up: constant velocity, TFV decelerates, TFV accelerates and variable acceleration.

5.1. Simulation Scenario 1: Constant Velocity

The initial motion states of each vehicle in simulation scenario 1 are illustrated in Figure 9 and

Table 3. Initial motion states of each vehicle in simulation scenario 1.

Vehicle	(X0, Y0)	V0 (m/s)	a0 (m/s2)
TV	(50, 1.875)	19.44	0
CPV	(88, 1.875)	13.89	0
TFV	(38, 5.625)	17.22	0
NV	(65, 9.375)	15.55	0

:

According to Section 4.2, after screening the LC trajectory cluster generated by quintic polynomial using constraints functions, an evaluation of the candidate trajectories that meet the constraints results in obtaining the reference trajectory, as shown in Figure 10.

The optimized LC trajectory and motion state parameters are shown in Figure 11 and Figure 12. Figure 11 illustrates the reference trajectory and the optimized trajectory, overlaid with the real-time distribution of the MDRF.

In scenario 1, each vehicle is moving at a constant velocity. It can be seen in Figure 11, when all vehicles are moving at a constant velocity, the optimized trajectory closely aligns with the reference trajectory. During the LC process, TV’s trajectory closely aligns with the point of minimum field strength, demonstrating the algorithm’s consideration for safety. From Figure 12, it is evident that the v_x range of the optimized LC trajectory is between 19.44 m/s and 19.54 m/s, with very slight fluctuations in acceleration a_x that fluctuate around 0, demonstrating excellent stability. v_y ranges from 0 to 1.25 m/s, initially increasing and then decreasing, while a_y ranges from −0.5 m/s² to 1 m/s². Additionally, both Jerk_x and Jerk_y remain within the defined constraints. The changes in a_x and a_y, as well as Jerk_x and Jerk_y, reflect the comfort and smoothness of the optimized trajectory. Moreover, each segment planned through programming ensures smooth transitions in path, velocity, acceleration, and jerk, ensuring smooth driving operations and passenger comfort, thus demonstrating the effectiveness and feasibility of the algorithm.

5.2. Simulation Scenario 2: TFV Decelerates

In scenario 2, apart from the TFV decelerating from 0~4 s with a_TFV = −1.5 m/s², the initial motion states of the other vehicles are identical to those in scenario 1, as shown in Figure 9.

The LC trajectory is dynamic optimized, and the optimal trajectory and motion state parameters are shown in Figure 13 and Figure 14. Since the initial states are the same as in scenario 1, the reference trajectory remains the same as in scenario 1.

Figure 13 and Figure 14 show that, when TFV decelerates at a_TFV = −1.5 m/s², the optimized LC trajectory has undergone significant adjustments compared to the reference trajectory. Specifically, at t = 2 s, based on TFV’s real-time velocity and acceleration, the trajectory of TFV was promptly optimized and modified to predict its position at the next moment. Due to TFV being positioned further back laterally due to deceleration compared to its position during constant-speed motion, the optimized LC trajectory correspondingly shifted backward. As indicated in Figure 13, the x–t curve of the optimized trajectory completed the lane change approximately 3 m earlier compared to the reference trajectory. Furthermore, Figure 14a shows that TFV’s lateral velocity v_x exhibited minimal variation, maintaining nearly constant speed with v_x ≈ 19.44 m/s and the lateral acceleration a_x ≈ 0. The longitudinal velocity v_y ranged from 0~1.43 m/s, accelerating from 0~2.6 s, followed by smooth deceleration after 2.6 s, successfully entering the target lane.

5.3. Simulation Scenario 3: TFV Accelerates

In scenario 3, apart from the TFV accelerating from 0~4 s with a_TFV = 0.8 m/s², the initial motion states of the other vehicles are identical to those in scenario 1, as shown in Figure 9. The optimized LC trajectory and motion state parameters are depicted in Figure 15 and Figure 16.

In Figure 15 and Figure 16, when TFV accelerates with a = 0.8 m/s², it can be observed that the dynamic optimized LC trajectory has been significantly adjusted compared to the reference trajectory. Relative to the reference trajectory, the optimized x–t curve is adjusted forward within a range of approximately 0~15 m and the y–x curve is adjusted within approximately 0~0.4 m. Compared to the reference trajectory, the optimized x–t curve completes the LC approximately 11 m later. The risk field distribution in Figure 15 shows that the risk associated with the optimized trajectory is significantly lower than that of the reference trajectory, indicating the effectiveness of the method in risk avoidance. According to Figure 16, TV’s lateral velocity v_x varies significantly, accelerating from 19.44 m/s to 25.6 m/s during the LC process, with an acceleration of 0 < a_x < 1.53 m/s², indicating that TV aimed to speed up to quickly move away from TFV and increase the safety distance. The longitudinal velocity v_y ranges from 0~1.1 m/s, initially accelerating from 0~2.9 s with an acceleration of 0 < a_y < 0.58 m/s², and then decelerating after 2.9 s with −0.45 m/s² < a_y < 0, signifying TV enters the target lane after 2.9 s.

5.4. Simulation Scenario 4: Variable Acceleration

In order to validate the scenario involving variable acceleration, scenario 4 is configured as depicted in Figure 17 and

Table 4. Initial motion states of each vehicle in simulation scenario 4.

Vehicle	(X0, Y0)	V0 (m/s)	ax (m/s2)	ay (m/s2)
TV	(50, 1.875)	19.44	0	-
CPV	(88, 1.875)	13.89	−1.1 (0~3 s)	-
TPV	(72, 5.625)	18.61	0	-
TFV	(38, 5.625)	17.22	−0.7 (1 s~4 s)	−0.2 (1 s~4 s)
			−1.0 (4 s~6 s)	0.32 (4 s~6 s)
NV	(65, 9.375)	15.55	0	-

.

The LC trajectory of scenario 4 is optimized, and the optimized trajectory and motion state parameters are shown in Figure 18 and Figure 19.

In scenario 4, the TFV and CPV exhibit variable acceleration patterns while considering the presence of TPV. Combining Figure 18 and Figure 19, the optimized LC trajectory has been significantly modified in comparison to the reference trajectory. Specifically, at t = 1 s, a_TFV,x = −0.7 m/s², a_TFV,y = −0.2 m/s², a_CFV = −1.1 m/s². At this point, as both TFV and CPV have initiated deceleration, TV promptly adjusts its trajectory in a timely manner based on the real-time velocity and acceleration of TFV and CPV. After predicting the positions of TFV and CPV for the next moment, TV’s trajectory is optimized accordingly. Influence by a_TFV,y, TFV begins to approach TV in the Y direction. Consequently, the position of TV in the Y direction starts to shift downward compared to the reference trajectory. The maximum downward offset of the optimized trajectory on the y–x curve is approximately 0.6 m. During the interval [4 s, 6 s], with a_TFV,x = −1.0 m/s², a_TFV,y = 0.32 m/s², TFV starts moving away from TV in the Y direction. Compared to the previous segment, the optimized trajectory in this segment exhibits a reduced downward offset in the Y direction. At this point, the downward offset of the optimized trajectory on the y–x curve is approximately 0.4 m. Moreover, the distribution of the risk field in Figure 18 indicates that the risk associated with the optimized trajectory is significantly reduced compared to the reference trajectory. As indicated in Figure 19, TV’s lateral velocity v_x remains relatively constant with v_x ≈ 19.44 m/s, a_x ≈ 0. The longitudinal velocity v_y ranges between 0 and 0.98 m/s, accelerating initially within the interval [0, 3.6 s], then decelerating after 3.6 s, indicating TV successfully enters the target lane after 3.6 s.

Furthermore, the runtime performance of the scenarios 1~4 are as follows (

Table 5. Runtime performance of the scenarios 1~4.

Scenario	Scenario 1	Scenario 2	Scenario 3	Scenario 4
Runtime Performance(s)	1.96	1.98	2.08	2.34

):

These runtimes are well within the acceptable range for real-time applications, as they allow for sufficient time to process sensor data, perform trajectory planning, and execute control commands during an LC process. Moreover, the runtime of the simulation increases with the complexity of the scenarios, which is consistent with real-world conditions. The simulations were conducted on a platform using MATLAB 2023b, and the hardware configuration used for the simulations includes a system with 8 GB of RAM and an NVIDIA GeForce GTX 1650 GPU. This setup is representative of mid-range computational capabilities, indicating that the algorithms are feasible for implementation in ICVs with similar or higher computational resources.

In conclusion, the four simulation scenarios obtained smooth and continuous LC trajectory curves. The variations in velocity, acceleration, and jerk all comply with vehicle dynamics and conditional constraints. Moreover, the TVs are capable of dynamically adjusting their trajectories in real time based on the instantaneous states of adjacent vehicles, meeting the safety requirements of the MDRF function, thus effectively enhancing driving safety. Specifically, while TV is in the current lane, based on the adjacent vehicles’ real-time velocity and acceleration, the predicted position at the next moment is obtained, and the trajectory of TV is timely adjusted and optimized. In addition, the computational complexity of the proposed algorithms is sufficiently low to meet the stringent real-time requirements of ICVs. Consequently, the LC trajectory dynamic planning algorithm proposed in this study demonstrates high adaptability and practicality, enabling safe, smooth, and comfortable LC maneuvers in various LC scenarios.

6. Conclusions

This study presents a dynamic LC trajectory planning algorithm for ICVs, leveraging a multi-vehicle MDRF model. As validated through simulations, the algorithm ensures safe and comfortable LC maneuvers in dynamic traffic environments. The MDRF model, grounded on APF theory, evaluates environmental risk by integrating the kinematic and geometric attributes of adjacent vehicles, providing a theoretical foundation for LC trajectory planning.

By decomposing trajectory planning into path planning and velocity planning, this paper employs the path-velocity decomposition approach to reduce the dimensionality of trajectory planning. Constrained criteria are then added to the LC trajectory clusters, eliminating trajectories that do not meet the requirements of vehicle dynamics and safety. Compared to traditional trajectory planning methods, this approach reduces computational power consumption and enhances algorithm efficiency.

Based on the reference trajectories X_ref(t) and Y_ref(x), the LC trajectory is segmented for dynamic planning. Utilizing the MDRF model, a multi-objective optimization function is constructed, incorporating constraints including continuity, kinematic feasibility, and smoothness. By applying the penalty function method to solve the optimization objective function, the optimal LC trajectory is determined. According to the simulation results, the dynamic LC trajectory planning algorithm based on the MDRF model proposed in this paper demonstrates better smoothness and comfort while maintaining the continuity and dynamic feasibility of the LC trajectory in various complex LC scenarios. Additionally, it exhibits remarkable safety and adaptability when confronted with intricate, dynamic LC scenarios. In conclusion, this study contributes an approach to LC trajectory planning that addresses the challenges of dynamic traffic conditions, showing the potential for practical implementation in ICV systems.

Future work will integrate human decision-making data from real-world scenarios to enhance predictive algorithms. Advanced sensor fusion using sophisticated sensors will improve perceptual capabilities and robustness. Furthermore, real traffic data will ground algorithms in practical, complex conditions, fostering a deeper understanding of vehicular interactions and environmental dynamics. Collectively, these efforts aim to refine trajectory planning algorithms, enabling effective decision-making for diverse road conditions and advancing the robustness of next-generation ICVs.