Dynamic Trajectory Planning and Tracking Based on Lane-Change Time Optimization

Abstract

With the emergence of global traffic problems, the development of safe, efficient, and reliable intelligent driving technologies has become a research hotspot. As a key component of intelligent driving technology, trajectory planning directly affects the safety, comfort, and operational efficiency of vehicles in complex traffic scenarios. Existing research typically relies on high-dimensional iterative numerical optimization or tightly coupled planning and control structures, leading to high computational complexity, insufficient real-time performance, and difficulty in ensuring trajectory smoothness. To address these issues, this paper proposes a decoupled and integrated trajectory planning and control method. Firstly, a method is proposed to construct the lateral trajectory based on a fifth-order polynomial and generate the longitudinal motion based on a quadratic acceleration model. Then, lane-change time is introduced as a single optimization variable to construct a cost function that balances comfort and efficiency, and continuous optimization is performed under longitudinal safety distance constraints. Finally, a horizontal and longitudinal hierarchical structure is constructed through model predictive control to solve the direction and speed adjustment problems and achieve high-precision tracking of the optimal trajectory. To verify the effectiveness of the proposed method, coupled simulation verification of trajectory generation and vehicle dynamic response is performed based on a joint simulation platform of MATLAB/Simulink and Carsim. The simulation results show that the proposed method can generate smooth, efficient, and controllable overtaking trajectories; significantly reduce computational complexity; and meet safety constraints, thus verifying the feasibility of the proposed method in complex lane-changing scenarios.

1. Introduction

With the increasing prominence of global transportation challenges, the development of safe, efficient, and reliable intelligent driving technologies has attracted extensive attention from researchers. As a core module of autonomous driving systems, trajectory planning directly determines vehicle safety, ride comfort, and operational efficiency, especially in complex dynamic scenarios such as emergency collision avoidance and high-speed overtaking. Generating feasible lane-changing trajectories that satisfy dynamic constraints while balancing comfort and efficiency has become a challenging and active research topic.

Analytical methods, including polynomials, splines [1], and Bezier curves [2], feature concise formulations, low computational burden, and ease of ensuring continuity of position and higher-order derivatives, enabling rapid generation of smooth trajectories. However, such approaches mainly rely on geometric constraints and offer limited consideration of vehicle dynamics, tire lateral characteristics, and acceleration limits. Spatial sampling-based search methods, represented by improved RRT [3] and AD-RRT* [4], generate numerous candidate paths in free space and integrate trajectory smoothing or dynamic feasibility verification to convert paths into executable trajectories. These methods adapt well to obstacles and large-scale environments and are robust to road roughness and scenario complexity. Nevertheless, they are constrained by random sampling quality, tree expansion strategies, and local consistency issues, making it difficult to guarantee trajectory continuity and dynamic controllability at high speeds. Optimization-based methods, such as optimal control [5], nonlinear MPC [6], and artificial potential field [7], explicitly incorporate vehicle dynamics, tire mechanics, road boundaries, collision constraints, and comfort indicators into the planning process and achieve multi-objective coordination via cost–function optimization, thus exhibiting superior performance in safety, feasibility, and dynamic consistency.

Gritschneder et al. [8] demonstrated that combining nonlinear dynamic models with sensitivity analysis or gradient solvers enables high-precision lane planning with sub-millisecond solution cycles. Werling et al. [9] highlighted that constraining curvature and curvature change rate is critical for enhancing ride comfort and vehicle stability. Zhao et al. [10] proposed a distributed observer to generate trajectory references under switching topologies but suffered from high computational complexity and strict requirements on solver efficiency and hardware. Some studies validated algorithm performance via hardware-in-the-loop (HIL) or real-vehicle tests [11], confirming dynamic consistency and safety, yet such experiments are costly and limited in adjustable variables, hindering systematic analysis of lane-change timing, constraint configurations, and working conditions. Bai et al. [12] solved the elliptical orbit transfer problem using sensitivity analysis and adaptive sequential optimization. Paparazzo, Cai et al. [13,14] improved MPC prediction accuracy in complex dynamic systems using data-driven models, while Li et al. [15] embedded dynamic constraints into analytical trajectory generation to enhance feasibility and generalization while maintaining lightweight computation.

To remedy the shortcomings of existing methods, this paper presents a decoupled dynamic trajectory planning and tracking control scheme for vehicle lane-changing and overtaking scenarios. The lateral trajectory is constructed using a quintic polynomial to guarantee continuous and smooth profiles of position, velocity, and acceleration, while a quadratic acceleration model is employed for longitudinal velocity planning to avoid abrupt dynamic changes and improve riding comfort. By taking lane-change time as the sole optimization variable, a composite cost function is formulated to balance driving comfort and traffic efficiency, and the optimal solution is solved through univariate continuous optimization. Meanwhile, a speed-adaptive weighting strategy and an optimization-feedback hybrid fault-tolerant switching mechanism are designed to improve the adaptability of the control system. On this basis, a hierarchical framework with decoupled lateral and longitudinal MPC controllers is established to realize high-precision trajectory tracking. The effectiveness of the presented method is validated via co-simulation on the MATLAB (R2022b)/Simulink and CarSim (2022) platform. The simulation results show that the scheme can generate smooth, dynamically feasible and efficiency-balanced overtaking trajectories while satisfying basic safety constraints, offering a feasible lightweight technical solution for intelligent vehicle lane-change trajectory planning and tracking control.

The rest of this paper is organized as follows. Section 2 introduces the trajectory planning method based on lateral–longitudinal decoupling and time optimization. Section 3 presents the hierarchical MPC tracking controller, including lateral error modeling, feedforward compensation, and longitudinal velocity control. Section 4 provides the co-simulation results and analysis of trajectory feasibility and tracking accuracy. Finally, Section 5 concludes the paper and discusses future work.

2. Trajectory Planning Based on Lane-Changing Time Optimization

In order to balance safety, comfort, and efficiency in lane-changing and overtaking scenarios on high-speed straight roads, it is necessary to plan the vehicle’s lateral and longitudinal movements reasonably. Among them, the lateral trajectory planning should ensure a smooth lane-changing process and avoid lateral impact caused by sudden steering wheel turns. Longitudinal speed planning requires ensuring continuous acceleration and smooth power output to reduce discomfort and vehicle dynamic impact. This study used a combination of horizontal fifth degree polynomial trajectories and longitudinal quadratic acceleration models to decouple horizontal and longitudinal movements, achieving high-order continuity of trajectories and smooth changes in velocity acceleration.

2.1. Trajectory Planning Based on Horizontal and Longitudinal Decoupling

The acceleration smoothing modeling method [16] has been proven to effectively improve longitudinal comfort. The fifth-degree polynomial trajectory, due to its third-order continuity, can effectively avoid lateral impact and discomfort caused by trajectory mutations during lane changing [17]. To ensure the continuity of longitudinal velocity changes and smooth power output during lane changing and overtaking; to achieve continuous changes in lateral displacement, velocity, and acceleration during lane changing; and to ensure a smooth transition from start to end of lane changing, this experiment used a quadratic function form and a fifth-degree polynomial decoupling to construct a trajectory planning model.

Assuming the lane-change time is t and the longitudinal acceleration a_x(t), its expression is given as Equation (1).

$$a_x(t)=c_{1}t+c_2{t}^2, 0\le t<T$$

(1)

Further integrate to obtain longitudinal velocity and displacement according to Equations (2) and (3).

$$v_x(t)=v_0+{\displaystyle {\int }_0^t{a}_x}(t)dt=v_0+{\displaystyle \frac{c_1}{2}}{t}^2+{\displaystyle \frac{c_2}{3}}{t}^3$$

(2)

$$s_x(t)={\displaystyle {\int }_0^t{v}_x}(t)dt=v_{0}t+{\displaystyle \frac{c_1}{6}}{t}^3+{\displaystyle \frac{c_2}{12}}{t}^4$$

(3)

where T is the end time of lane change, c₁, c₂ are undetermined coefficients. v₀ is the initial velocity.

In this study, the acceleration changed smoothly over time. By designing the acceleration to reach its maximum value a_max during the middle of the lane change and smoothly return to 0 at the end time T of the lane change, the vehicle maintained a constant speed to complete the lane change stably according to Equation (4).

$$\left\{\begin{array}{l}{c}_1{\displaystyle \frac{T}{2}}+c_2{\displaystyle \frac{T^2}{4}}=a_{\max }\hfill \\ c_{1}T+c_2{T}^2=0\hfill \end{array}\right.$$

(4)

Hence, we derive the following:

$$c_1={\displaystyle \frac{4a_{\max }}{-T}}, c_2={\displaystyle \frac{4a_{\max }}{T^2}}$$

(5)

To ensure that there is no deviation between the vehicle and the lane centerline during the start and end stages of the lane-change action and that the lateral velocity and lateral acceleration are both zero, the following boundary conditions were set according to Equation (6).

$$\left\{\begin{array}{l}y(t_1)=W, \dot{y}(t_1)=0, \ddot{y}(t_1)=0\hfill \\ y(0)=0, \dot{y}(0)=0, \ddot{y}(0)=0\hfill \end{array}\right.$$

(6)

where W is the lateral displacement required for lane changing.

The lateral displacement of the vehicle is denoted as y(t), and by introducing the normalized time variable τ = t/T, the polynomial can be expressed as Formula (7).

$$y(t)=W\left(b_3{\tau }^3-b_4{\tau }^4+b_5{\tau }^5\right)$$

(7)

By taking the derivative of the polynomial over time, the expressions for lateral velocity and lateral acceleration can be obtained according to Equations (8) and (9).

$$\dot{y}(t)={\displaystyle \frac{W}{T}}\left(c_2{\tau }^2-c_3{\tau }^3+c_4{\tau }^4\right)$$

(8)

$$\ddot{y}(t)={\displaystyle \frac{W}{T^2}}\left(d_1\tau -d_2{\tau }^2+d_3{\tau }^3\right)$$

(9)

where algebraic coefficients b₃, b₄, and b₅ that satisfy the boundary conditions can be obtained by solving the system of equations. The coefficients c_i, d_i (i = 1, 2, 3, 4) are calculated to ensure that the velocity and acceleration during the start and end stages are zero.

Regarding the adaptive adjustment of acceleration and lateral acceleration amplitude, at the same time, the longitudinal acceleration exhibited a characteristic of smooth growth followed by gradual attenuation during the process, which can effectively avoid the problem of sudden power changes. The peak value did not exceed the set limit, further ensuring ride comfort.

2.2. Continuous Optimization Solution for Lane-Changing Time

2.2.1. Optimization Time Modeling for Lane Changing

The univariate time optimization method can significantly improve the real-time performance of the solution while ensuring trajectory smoothness and feasibility. To achieve efficient lane changing under safety and smoothness constraints, this paper constructed a constrained nonlinear optimization model with T as the sole optimization variable and established the optimization objective function (Equation (10)).

$$\begin{array}{c}\underset{T}{\min }J(t_1)=w_1{\displaystyle \frac{\underset{t}{\max }|a_y(t)|}{a_{y,\mathrm{ref}}}}+w_2{\displaystyle \frac{T}{T_{\mathrm{ref}}}}\\ \mathrm{s}.\mathrm{t}. c(T)\le 0, 3.0 \mathrm{s}\le T\le 10.0 \mathrm{s}\end{array}$$

(10)

where weights w₁ and w₂ are used to balance comfort and efficiency.

The lateral acceleration is given by the second derivative of the lateral trajectory: a_y(t) = Y(t_k|T). To achieve normalization, reference accelerations a_y,ref and reference time T_ref can be used as normalization scales.

2.2.2. Constraint Design

Longitudinal safety constraints require ensuring a safe distance between the self-driving vehicle and the target vehicle within the stable lane-changing interval [t₁ + Δt_stable, t₁ + Δt_stable + Λ].

$${\min }_{k\in [N_s,N_e]}\left[s(t_k)-s_{\mathrm{tar}}(t_k)\right]\ge d_{\mathrm{safe}}$$

(11)

where the longitudinal position of the target vehicle is s_tar(t) = s_tar,0 + v_tart, sampling index N_s = ⌊(t₁ + Δt_stable)/Δt⌋, N_e = ⌊(t₁ + Δt_stable + Λ)/Δt⌋, usually t₁ is taken as the starting time for lane changing; Δt_stable is the short-term stable window; and Λ is the extension time to verify safety margin. Equal distance discretization is used for the time interval [0, T]. During each target and constraint evaluation, only the sampling points within the current interval were calculated, and the longitudinal safety distance constraint was verified by sampling within the stable lane-changing interval.

At each sampling point, the lateral and longitudinal velocities (Equation (12)) and accelerations (Equation (13)) are approximately calculated using the center difference method.

$$\dot{X}(t_k)\approx {\displaystyle \frac{X(t_{k+1})-X(t_{k-1})}{2\Delta t}}$$

(12)

$$\ddot{X}(t_k)\approx {\displaystyle \frac{X(t_{k+1})-2X(t_k)+X(t_{k-1})}{\Delta t^2}}$$

(13)

Similarly, Y(t_k) and Y(t_k) can be obtained.

Setting threshold truncation when calculating expressions involving velocity ratios or normalization quantities can effectively prevent numerical explosion of constraint function gradients in low-speed regions.

The nonlinear constraint function is given as Equation (14).

$$c(T)=\left[{\displaystyle \frac{{\max }_k\left|a_y(t_k;T)\right|-a_{y,\max }}{d_{\mathrm{safe}}-{\min }_{i\in \mathcal{I}}\left(s(t_i;T)-S_{\mathrm{tar}}(t_i)\right)}}\right]$$

(14)

where ($ℒ$) represents the set of sampling indices within the stable interval after lane changing. The first term is lateral acceleration constraint, and the second term is longitudinal safety distance constraint. To avoid the difficulties caused by non-smooth points to the solver, this paper adopted a smooth approximation method and replaced the maximum value operation with a soft max function [18].

The function is given as Equation (15).

$$sofe-{\max }_{\alpha }(x_i)={\displaystyle \frac{1}{\alpha }}\ln {\sum }_i{e}^{\alpha xi}$$

(15)

where α denotes a smoothing coefficient such that the true maximum is approached as α tends to infinity. In practice, α is set to 50 to balance computational stability and numerical accuracy.

3. Tracking Control Based on Hierarchical Control

For the reference trajectory obtained by the above-mentioned method of introducing a single time variable, this study adopted the model predictive control (MPC) method. By performing rolling optimization in the prediction time domain, it can simultaneously handle multi-objective problems such as lateral position and heading errors, longitudinal velocity errors, and acceleration smoothness while explicitly considering acceleration constraints and longitudinal safety distance constraints. In terms of lateral control, the controller predicted and optimized the lateral displacement and heading of the vehicle based on the model, and it introduced a feedforward compensator to improve response speed and trajectory smoothness. In terms of longitudinal control, the augmented integral model was used to predict and optimize longitudinal velocity and acceleration, achieving continuous acceleration and deceleration to maintain ride comfort. In addition, the controller dynamically adjusted the weights during the optimization process to enable precise tracking at low speeds and smooth response during fast lane changes, thereby achieving lateral and longitudinal decoupling and collaborative control (Figure 1).

3.1. Lateral Controller Design

3.1.1. Lateral Error Calculation Model

Lateral error is a key state variable in trajectory tracking, representing the geometric deviation from the reference path. To balance computational efficiency and real-time performance, a geometric projection-based error model was adopted, which avoided curvature integration, ensured numerical stability, and offered superior real-time performance compared to Frenet coordinates [19,20,21]. Assuming the current position of the vehicle is P = (X, Y), the reference trajectory point is P_r = (X_r, Y_r), and its corresponding heading angle is ϕ_r. The lateral error of the vehicle e_d is defined as Equation (16).

$$e_d=n_r^T\left(P-P_r\right)$$

(16)

where the expression of normal vector n_r is given as Equation (17).

$$n_r=\left[\begin{array}{c}-\sin {\varphi }_r\\ \cos {\varphi }_r\end{array}\right]$$

(17)

The heading error adopting a periodic differential angle is given as Equation (18).

$$e_{\varphi }=\mathrm{atan}2\left(\sin \left(\varphi -{\varphi }_r\right), \cos \left(\varphi -{\varphi }_r\right)\right)$$

(18)

To obtain smoother error derivatives, exponential filtering is introduced to the discrete differential signal in the simulation, as shown in Formulas (19)–(21):

$$\stackrel{.}{e_d}(k)=\alpha {\displaystyle \frac{e_d(k)-e_d(k-1)}{T_s}}+(1-\alpha )\stackrel{.}{e_d}(k-1)$$

(19)

$$\stackrel{.}{e_{\varphi }}(k)=\alpha {\displaystyle \frac{e_{\varphi }(k)-e_{\varphi }(k-1)}{T_s}}+(1-\alpha )\stackrel{.}{e_{\varphi }}(k-1)$$

(20)

$$\alpha =0.3+0.4\min \left(1,{\displaystyle \frac{V_x}{15}}\right)$$

(21)

This strategy improved smoothness at low speeds to suppress error oscillations and enhances response sensitivity at high speeds, thus balancing steady-state accuracy and dynamic performance.

3.1.2. Model Predictive Controller

Bulleted lists looked like: Model predictive control (MPC) is an effective means of achieving multi constraint and multi-objective coordinated control by solving future control sequences through rolling optimization. Previous studies have shown that compared with PID and LQR methods, MPC can show better dynamic performance and stability in autonomous driving trajectory tracking [22].

This article adopted a linearized lateral dynamics form based on the monorail model.

$$\begin{array}{cc}x=\left[\begin{array}{c}{e}_d\\ {\dot{e}}_d\\ e_{\varphi }\\ {\dot{e}}_{\varphi }\end{array}\right],& u=\delta \end{array}$$

(22)

where e_d is the lateral error, e_d is the lateral velocity error, e_ϕ is the heading angle error, e_ϕ is the heading angular velocity error, and δ is the front wheel steering angle.

Based on the assumption of small angle and steady state, the linearized monorail model can be expressed as Equation (23).

$$x_{k+1}=A_c{x}_k+B_{c}u$$

(23)

The continuous system matrix is given as Equations (24) and (25).

$$A_c=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -{\displaystyle \frac{2\left(C_{af}+C_{ar}\right)}{m{V}_x}}& m& -{\displaystyle \frac{2\left(L_2{C}_{ar}-L_1{C}_{af}\right)}{m{V}_x}}\\ 0& 0& 0& 1\\ 0& -{\displaystyle \frac{2\left(L_2{C}_{ar}-L_1{C}_{af}\right)}{I_z{V}_x}}& {\displaystyle \frac{2\left(L_2{C}_{ar}-L_1{C}_{af}\right)}{I_z}}& -{\displaystyle \frac{2\left(L_1^2{C}_{af}+L_2^2{C}_{ar}\right)}{I_z{V}_x}}\end{array}\right]$$

(24)

$$B_c=\left[\begin{array}{c}0\\ {\displaystyle \frac{2C_{af}}{m}}\\ 0\\ {\displaystyle \frac{2L_1{C}_{af}}{I_z}}\end{array}\right]$$

(25)

where m is the vehicle mass, I_z is the yaw moment of inertia, L₁ and L₂ are the front and rear wheelbase, C_af and C_ar are the front and rear wheel lateral stiffness, and V_x is the longitudinal velocity.

After discretization, the system state prediction model can be expressed as Equation (26).

$$x_{k+1}=e^{A_c\Delta t}{x}_k+{\displaystyle {\int }_0^{\Delta t}{e}^{A_{c}t}}{B}_{c}t\Delta {\delta }_k$$

(26)

Based on the predictive model, the optimization objective is given as Equation (27).

$$J={\displaystyle {\displaystyle \sum _{i=1}^{N_p}}{x}_{k+i|k}^T}Q{x}_{k+i|k}+{\displaystyle {\displaystyle \sum _{j=0}^{N_c-1}}\Delta }{\delta }_{k+j}^{T}R\Delta {\delta }_{k+j}$$

(27)

where Q is the state error weight matrix, R is the control variable weight matrix, N_p is the predicted time domain length, and N_c is the control time domain length. To improve the adaptability of the controller at different speeds, a speed correction factor (Equation (28)) was introduced.

$$\begin{array}{cc}{Q}^{\prime }=Q\alpha (V_x),& R^{\prime }={\displaystyle \frac{R}{\alpha (V_x)}}\end{array}$$

(28)

This was calculated to achieve dynamic adjustment of weighting coefficients with vehicle speed [23]. This adaptive weighting strategy can improve control accuracy under low-speed conditions and enhance smoothness under high-speed conditions.

The control input was physically constrained and embedded in a linear form in a quadratic programming problem through a built-in optimization solver [24]. The corresponding expression is given as Equation (29).

$$\begin{array}{c}{\min }_{\Delta U}{\displaystyle \frac{1}{2}}\Delta U^{T}H\Delta U+f^T\Delta U\\ \mathrm{s}.\mathrm{t}. A_{\mathrm{ineq}}\Delta U\le b_{\mathrm{ineq}}\end{array}$$

(29)

where H and f are the Hessian and gradient vectors of the cost function, respectively, constructed by Q and R matrices.

When the optimizer failed to converge normally under specific operating conditions and lost its dynamic adjustment capability, the controller automatically switched to PID mode to realize optimization-feedback hybrid control, thereby improving the continuity and stability of the system with the “optimization-feedback hybrid” fault-tolerant mechanism.

3.1.3. Feedforward Compensator Design

The steady-state yaw mechanics model can effectively reduce the dynamic lag of vehicles and improve the smoothness of path tracking [25]. In order to improve the foresight of the response, this paper introduced curvature feedforward compensation based on steady-state yaw mechanic’s model. When the vehicle was turning steadily, the lateral forces of the front and rear wheels satisfied a balance relationship according to Equation (30).

$$C_{af}\left(\delta -\beta -{\displaystyle \frac{L_1\dot{r}}{V_x}}\right)+C_{ar}\left(-\beta +{\displaystyle \frac{L_2\dot{r}}{V_x}}\right)=m{V}_x\left(r+\dot{\beta }\right)$$

(30)

Based on the assumptions of small angle and steady state, the required front wheel steering angle for steady state can be derived according to Equation (31).

$${\delta }_{\mathrm{ff}}={\displaystyle \frac{m{V}_x^2-L_1{C}_{af}+L_2{C}_{ar}}{C_{af}L}}k$$

(31)

where κ is the curvature of the road. In order to enhance the adaptability of feedforward control at different vehicle speeds, this paper introduced speed adaptive gain according to Equation (32).

$$K_{\mathrm{ff}}(V_x)=\left\{\begin{array}{ll}0.5,\hfill & V_x<3\hfill \\ 1.0,\hfill & 3\le V_x\le 20\hfill \\ 1.2,\hfill & V_x>20\hfill \end{array}\right.$$

(32)

The final corner output is given as Equation (33).

$${\delta }_{\mathrm{out}}=K_{\mathrm{ff}}(V_x){\delta }_{\mathrm{ff}}$$

(33)

This design can suppress excessive steering at low speeds and apply angle compensation in advance at high speeds, thereby reducing lateral response lag. The feedforward gain K_ff(V_x) was optimized through multiple simulation tests. This gain was set in the form of a piecewise function to accommodate different longitudinal velocities.

3.2. Longitudinal Controller Design

Previous studies have shown that using a model predictive control framework for longitudinal velocity control can effectively balance acceleration smoothness and constraint handling capabilities while ensuring real-time performance [26,27]. This article designs a longitudinal controller under a unified MPC framework to achieve minimum velocity error and continuous acceleration control.

3.2.1. State Modeling

The longitudinal dynamics of a vehicle can be approximated as an integral model under simplified conditions according to Equation (34).

$$V_{k+1}=V_k+T a_k$$

(34)

To describe the continuity of acceleration changes, an augmented state was introduced.

$$x_k={\left[V_k,a_{k-1}\right]}^T$$

(35)

We defined a_k = a_k−1 + Δa_k, then the discrete system can be expressed in linear integral form according to Equation (36).

$$x_{k+1}=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]x_k+\left[\begin{array}{c}T\\ 1\end{array}\right]\Delta a_k$$

(36)

where T is the sampling period. Based on the predictive model, the speed error was defined as Equation (37).

$$e_{v,k+i|k}=V_{k+i|k}-V_{\mathrm{ref},k+i}$$

(37)

Optimization objective is given as Equation (38).

$$J={\displaystyle {\displaystyle \sum _{i=1}^{N_p}}{\left(e_{v,k+i|k}\right)}^2}{Q}_v+{\displaystyle {\displaystyle \sum _{j=0}^{N_c-1}}{\left(\Delta a_{k+j}\right)}^2}{R}_a$$

(38)

where Q_v and R_a are the weighted matrices of speed error and control smoothness, respectively. The first term is used to penalize speed error, and the second term is used to suppress sudden acceleration changes.

To ensure ride comfort and power feasibility, acceleration constraints were applied to limit the longitudinal acceleration amplitude and acceleration change rate, respectively. The quadratic programming (QP) principle obtained by constraint solving was the same as Formula (34). This MPC design can explicitly handle longitudinal physical constraints, achieving dual objective coordination of speed tracking and comfort.

3.2.2. Lower-Level Actuator Switching Logic

Based on the energy balance and sliding priority strategy in reference [28], this paper improved the longitudinal MPC output characteristics. The expected acceleration a_req output by MPC was converted into corresponding driving or braking torque through the actuator layer model, making the transition between sliding and driving smoother. The sliding priority strategy was designed to coordinate the longitudinal and lateral control allocation. It ensured the trajectory tracking stability under the MPC optimization framework, and its effectiveness was verified by co-simulation results.

According to the longitudinal dynamic balance relationship of the vehicle, it can be concluded according to Equation (39).

$$F_{\mathrm{res}}=mgf+0.5\rho C_d{A}_f{V}_x^2+mg\sin \theta$$

(39)

Among them, m is the curb weight of the car, g is the acceleration due to gravity, f is the rolling resistance coefficient, ρ is the air density, C_d is the air resistance coefficient, A_f is the windward area of the car, V_x is the longitudinal speed of the car, and θ is the road slope angle. The natural sliding acceleration is defined as a_t = −F_res/m. The system determined whether the vehicle should be in driving, braking, or coasting mode based on the balance between expected acceleration and current resistance. The execution logic is given as Equation (40).

$$\left\{\begin{array}{ll}{a}_{\mathrm{req}}>a_t+\Delta a\hfill & \mathrm{Driving} \mathrm{Mode}\hfill \\ a_{\mathrm{req}}<a_t-\Delta a\hfill & \mathrm{Braking} \mathrm{Mode}\hfill \\ |a_{\mathrm{req}}-a_t|\le \Delta a\hfill & \mathrm{Coasting} \mathrm{Mode}\hfill \end{array}\right.$$

(40)

This logic implemented a sliding priority strategy through energy balance, allowing for the controller to actively release the driving force output under low load conditions, achieving conflict free switching between driving and sliding.

4. Simulation Verification

To validate the proposed dynamic trajectory generation method optimized for lane-changing time as well as the effectiveness of the MPC trajectory tracking control strategy, this paper conducted simulation studies from two aspects: the analysis of trajectory feasibility with lane-changing time optimization and the vehicle dynamics tracking control.

4.1. Feasibility Verification of Trajectory Planning

The simulation parameters are shown in

Table 1. Simulation parameters.

Parameter Name	Symbol	Data	Unit
Initial longitudinal velocity	$v_0$	16	m/s
Obstacle vehicle longitudinal speed	$v_t$	12	m/s
Lane width	$W$	3.75	m
Maximum lateral acceleration limit	$max\left|a_y\right|$	3.0	m/s2
Maximum longitudinal acceleration	$a_{max}$	2.0	m/s2
Initial relative longitudinal distance	$s_0$	30	m
Weight coefficient	$\begin{array}{cc}{w}_1,& w_2\end{array}$	0.6, 0.4	-
Total simulation time	T	12	s
Safety distance constraint	$d_{safe}$	3	m

. The lateral motion was modeled by a fifth-order polynomial to ensure smooth position, velocity, and acceleration profiles, enabling continuous curvature and natural heading variation during lane changes. The longitudinal motion adopted a segmented quadratic acceleration model to ensure smooth transitions and avoid abrupt acceleration, thereby enhancing vehicle stability. The corresponding simulation results are shown in Figure 2.

From the analysis of Figure 2, it can be concluded that under the optimal lane-changing time, the lateral acceleration exhibits a typical “S”-shaped variation, with a peak value far below the comfort limit, and no local overshoot or high-frequency oscillation occurs. The heading angle first rises and then falls back, corresponding to the “lane-change return” stage. The curvature curve exhibits a symmetrical bimodal structure, corresponding to two turning actions of entering and exiting the lane change, with small peak values and a gentle rate of change. The above results indicate that the proposed method can ensure continuous differentiability, smooth curvature, and stable posture at the trajectory level and has good ride comfort and dynamic feasibility.

From Figure 3, it can be seen that the longitudinal speed reaches its maximum value at the end of the lane change, an increase of about 42% compared to the initial value, indicating that the proposed model balances overtaking efficiency and acceleration requirements. The speed curve is smooth without any jumps; the peak acceleration does not exceed the limit and gradually decays to zero, finally entering the steady-state uniform speed stage. The overall longitudinal dynamic constraints are satisfied, indicating that this method can complete the necessary acceleration and overtaking actions during lane changing.

On this basis, lane-change time T is further introduced as a key decision variable. By performing a scan analysis on longitudinal speed, lateral acceleration, safe distance, and the comprehensive cost function, the impact of T on comfort, safety, and time efficiency is quantitatively assessed. The simulation results are shown in Figure 4.

Shorter lane-change times increase lateral jerk and reduce safety margins, while longer times impair efficiency. The dual-weight cost function (Figure 4a) yields an optimal time that balances comfort and safety. Compared to aggressive or gentle maneuvers, this optimum provides the smoothest lateral acceleration (Figure 4c), improved safety margins (Figure 4d), and a smooth velocity profile (Figure 4b), achieving stable lateral–longitudinal coupling and an effective compromise between safety and efficiency.

In summary, the proposed method not only meets the geometric and dynamic feasibility requirements at the trajectory level but also has attainability in the time dimension. By introducing time parameters, it enables the lane-changing strategy to achieve adjustable, analyzable, and interpretable performance trade-offs, providing an engineering-feasible parametric design framework for intelligent driving lane-changing decisions.

4.2. Trajectory Tracking Simulation Analysis

To verify the trajectory tracking performance of the designed MPC controller in complex lane-changing overtaking scenarios, this study conducted experiments based on the CarSim Simulink joint simulation platform (Figure 5). In the simulation, the C-Class Hatchback vehicle provided by CarSim 2022 was selected, with its initial position set to (0, 0) and an initial heading angle of 0 rad.

In autonomous driving trajectory tracking, the controller must not only ensure that the vehicle runs along the planned trajectory, but also achieve safe, smooth, and comfortable control under multiple constraints. Taking lane changing and overtaking as an example (Figure 6), the system needs to coordinate lateral and longitudinal control; comprehensively consider vehicle dynamics, acceleration constraints, and ride comfort; and combine this with predictive information about the road and surrounding vehicles to maintain a safe distance and ensure stability and safety throughout the entire process. Unlike traditional PID or LQR controllers, MPC can predict the future dynamics of the system and optimize the control sequence online while explicitly satisfying state and control variable constraints. Therefore, it has strong adaptability under complex dynamic conditions such as lane changes and overtaking.

Figure 7 compares the proposed method, PID control, and the ideal trajectory. The proposed method closely tracks the reference with fast response, high accuracy, and minimal steady-state error, while PID exhibits noticeable lag, larger deviations, and persistent oscillations. Moreover, the proposed method achieves smaller overshoot, faster convergence, and improved stability, demonstrating superior dynamic and tracking performance over PID. Figure 8 presents the longitudinal speed profiles of the proposed method, the ideal reference, and PID control. The proposed method achieves smooth acceleration from 16 m/s to 23 m/s with close tracking and minimal fluctuations, while PID exhibits larger errors and noticeable oscillations. Additionally, the proposed method ensures faster convergence and superior steady-state stability, demonstrating improved tracking accuracy and smoothness over PID.

Figure 9 illustrates the evolution of lateral, yaw angle, and curvature errors during lane changing. Initial transient fluctuations arise from trajectory geometry variations and vehicle dynamics, while lateral and yaw errors remain small, indicating effective suppression of deviation. All errors rapidly converge to near zero with minimal oscillation, demonstrating that the MPC controller achieves smooth, robust, and high-precision trajectory tracking under high-speed conditions.

The comprehensive simulation results show that the designed MPC controller can achieve high-precision trajectory tracking under complex lane-changing overtaking conditions, with smooth vehicle motion, stable posture, and controllable longitudinal and transverse deviations. The ride comfort and safety are guaranteed, fully verifying the feasibility and reliability of the control strategy.

5. Discussion

5.1. Interpretation of Results

The simulation results demonstrate that the proposed method successfully balances safety, comfort, and efficiency. Lateral acceleration follows a smooth profile with a peak value below the comfort limit thanks to the quintic polynomial that enforces zero acceleration at both endpoints. The symmetric curvature curve corresponds to the entry and exit steering actions during lane changing.

Longitudinally, the vehicle accelerates smoothly without jerk-induced discomfort, as the quadratic acceleration model ensures gradual growth and decay of acceleration. The time-segment scan analysis reveals a clear trade-off: shorter lane-change time increases lateral jerk and reduces safety margins, while longer lane-change time impairs efficiency. The dual-weight cost function identifies an optimal time that balances these competing objectives.

The MPC-based controller tracks the planned trajectory with high accuracy, with lateral, yaw, and curvature errors converging rapidly to near zero. Compared to PID, the proposed controller exhibits faster response, smaller overshoot, and better stability, benefiting from predictive action and feedforward compensation.

5.2. Comparison with Existing Methods

Compared to pure analytical methods, the proposed method retains computational simplicity while explicitly incorporating dynamic constraints. Compared to sampling-based methods, it guarantees higher-order continuity essential for ride comfort at highway speeds. Compared to full optimization methods, the univariate approach significantly reduces computational cost, making it more suitable for real-time applications. Relative to the Frenet-frame approach, the present method introduces speed-adaptive weighting and a fault-tolerance mechanism for additional robustness.

5.3. Limitations and Open Questions

This study has several inherent limitations. First, validation is limited to simulation-based lane-changing and overtaking scenarios on straight roads, without incorporating sensor noise, actuator dynamic characteristics, perception uncertainty, complex curved road conditions, or multi-vehicle interactive driving scenarios. Second, the current work adopts linear tire simplification and an ideal perception assumption, while hardware-in-the-loop verification and real-vehicle experimental tests have not been implemented. Furthermore, this paper primarily concentrates on constructing a lightweight trajectory planning and lateral–longitudinal decoupled MPC tracking framework; extensive comparative evaluations with state-of-the-art approaches, including Frenet-based planning and nonlinear MPC methods, are not performed at this stage. In addition, in-depth research on tire friction ellipse constraints, TTC safety constraints, computational efficiency benchmarking, and quantitative robustness verification is not fully explored herein. Future research will address the above limitations by conducting embedded hardware testing, analyzing sensitivity to perception errors, developing learning-based adaptive weight adjustment strategies, designing uncertainty-aware MPC algorithms, extending the framework to multi-vehicle interactive scenarios, and completing further validation via hardware-in-the-loop platforms and real-road vehicle experiments.

6. Conclusions

This paper proposed a lateral–longitudinal decoupled dynamic trajectory planning method for lane-changing scenarios, using lane-change time as the sole optimization variable to balance comfort and efficiency, combined with an MPC-based tracking controller. The co-simulation results verified that the proposed method generates smooth, dynamically feasible trajectories while satisfying safety constraints. Future work will extend the method to multi-vehicle interactive scenarios and validate it on hardware-in-the-loop platforms.