Intelligent Vehicle Driving Decisions and Longitudinal–Lateral Trajectory Planning Considering Road Surface State Mutation

Abstract

In an intelligent driving system, the rationality of driving decisions and the trajectory planning scheme directly determines the safety and stability of the system. Existing research mostly relies on high-definition maps and empirical parameters to estimate road adhesion conditions, ignoring the direct impact of real-time road status changes on the dynamic feasible domain of vehicles. This paper proposes an intelligent driving decision-making and trajectory planning method that comprehensively considers the influence factors of vehicle–road interaction. Firstly, real-time estimation of road adhesion coefficients was achieved based on the recursive least squares method, and a dynamic adhesion perception mechanism was constructed to guide the decision-making module to restrict lateral maneuvering behavior under low-adhesion conditions. A multi-objective lane evaluation function was designed for adaptive lane decision-making. Secondly, a longitudinal and lateral coupled trajectory planning framework was constructed based on the traditional lattice method to achieve smooth switching between lateral trajectory planning and longitudinal speed planning. The planned path is tracked based on a model predictive control algorithm and dual PID algorithm. Finally, the proposed method was verified on a co-simulation platform. The results show that this method has good safety, adaptability, and control stability in complex environments and dynamic adhesion conditions.

1. Introduction

An autonomous driving system typically consists of four basic modules: perception, decision-making, planning, and control. Driving decisions and trajectory planning are the key bridge connecting perception and execution control [1]. Road conditions, lane infrastructure, and surrounding vehicles influence the process of vehicle driving decisions and trajectory planning [2]. Road friction, damage, or water accumulation limit the available adhesion, affecting braking and acceleration strategies. Lane width, marking, and guardrail deformation define lateral driving space, left and right lane changes, and obstacle avoidance feasibility, and the speed and intention of the surrounding vehicles form a dynamic constraint, which prompts the system to switch between lane keeping, overtaking, or longitudinal driving in real time. The decision-making and trajectory planning module must integrate this information to generate an executable path that is both safe, compliant, efficient, and smooth [3]. Most current research obtains accurate road layout and lane infrastructure information through high-definition maps and self-positioning, and acquires dynamic obstacle information through onboard sensors [4]. At the same time, it assumes that road adhesion conditions ($\mu ,$ the tire–road friction coefficient) can be provided by high-definition maps or empirical values [5], while ignoring the direct impact of real-time road changes (snow accumulation, water accumulation, wear, etc.) on the vehicle’s dynamic limits and safety indicators [6]. In fact, the road adhesion coefficient determines the maximum force that the tire can exert during acceleration, braking, and steering. If the adhesion coefficient is overestimated, it may cause the control system to request excessive power output, leading to tire slipping, skidding, and even vehicle loss of control. If the estimated value is too conservative, it will limit the vehicle’s dynamic performance and reduce traffic efficiency and maneuverability [7]. Therefore, considering real-time road changes at the front end of the planning–control link and designing a reasonable and efficient driving decision mechanism can help vehicles reasonably plan a driving path in highly dynamic scenarios such as emergency braking, obstacle avoidance, lane changing, and high-speed steering, and ensure that the motion control strategy is safely and effectively executed. This is of great significance for improving the safety, comfort, and traffic efficiency of intelligent vehicles in complex environments [8,9].

Current methods for estimating road adhesion coefficients can be classified into those based on causes, effects, and data-driven and hybrid approaches [10]. Effect-based methods typically establish the longitudinal and lateral dynamics of the vehicle and design filters or observers to measure and analyze the vehicle response caused by road changes to estimate the road adhesion coefficient [11,12]. This method does not require expensive sensor equipment and has better real-time performance and higher accuracy compared to other methods. It is widely used in vehicles [13,14]. An expectation maximization robust extended Kalman filter, which considers the influence of sensor data loss on the estimation accuracy of sideslip angle, yaw rate and vehicle speed, was proposed by Wang [15]. A method based on model learning, combined with an event-triggered cubature Kalman filter and extended Kalman neural network, was proposed by Wang to identify the road adhesion coefficient [16]. Liu proposed a method for estimating vehicle state parameters and road adhesion coefficients by integrating sliding mode theory, trigonometric function group fitting, and cubic spline interpolation within an extended Kalman filter framework [17]. A strong tracking unscented Kalman filtering method [18] with faster convergence speed and ability to dynamically estimate the road adhesion coefficient on time-varying roads was designed by Ping. RLS is one of the effect-based methods, converting the complex nonlinear adhesion coefficient estimation of the vehicle into a linear model parameter tracking problem [19]. It achieves efficient calculation and real-time updates through recursive characteristics and adapts to sudden road changes through forgetting factors. Compared to traditional offline algorithms or complex nonlinear methods such as Kalman filtering, the RLS method demonstrates strong practical value on resource-constrained on-board ECUs [20]. The least squares method was employed by Chen to perform multivariate fitting on the relationship curve between slip rate and adhesion coefficient, thereby estimating the road status [21]. A three-dimensional mapping model of slip rate, road adhesion coefficient, and road peak adhesion coefficient was established by He, and the road status was estimated based on the slope of the curve of the surface model [22].

Recently, research on intelligent vehicle driving decision-making has been evolving from rule-driven to data-driven game decision-making, with the core being to achieve multi-objective dynamic balance within a safety framework [23]. A driving operation diagram was utilized by Zhang to implicitly represent the driving styles of a self–driving vehicle and other vehicles. Through supervised learning, decision-making strategies were learned from human driving data and rule-based indicators were explicitly introduced to achieve adaptive lane-changing decisions based on driving styles [24]. Yang proposed a human–machine collaborative continuous learning framework. The driver provided demonstration data through the takeover mechanism, dynamically corrected the AV’s decision deviations, designed an anti-forgetting learning algorithm, and combined behavior cloning with Q-value optimization to generate personalized strategies [25]. Trajectory planning research has evolved multiple technical routes at “global–local” levels: traditional graph search, sampling-, optimization-, and learning-based methods [26,27]. The traditional graph search method demonstrates stability in structured environments due to its determinism and explainability, making it suitable for global path planning. However, when dealing with dynamic obstacles, unstructured roads, or uncertain behaviors of traffic participants, its adaptability is poor [28]. The sampling-based method, relying on the discretization approach, quickly forms in unknown environments, but has the bottleneck of rough trajectories and inability to meet vehicle kinematic constraints [29] directly. The optimization-based method obtains smooth trajectories and differentiable control instructions through continuous optimization. However, it is prone to falling into computational bottlenecks or local optima when the obstacle complexity or search space dimensionality is high [30]. To balance the efficiency of discrete search and the feasibility of continuous optimization, the academic community proposed the state lattice trajectory planning: first, design offline a set of motion primitives that comply with vehicle kinematics and form a “gridded” state lattice in the pose space, then use online heuristic searching to concatenate the primitives between grid points to generate a fully dynamic and feasible trajectory [31]. The lattice method benefits from its prior kinematic feasibility, discrete search convergence, real-time heuristic cost, offline optimization for comfort, and easy integration with vehicle dynamic models and constraints, and has become the mainstream solution in scenarios such as highway lane changing, urban road overtaking, and multi-vehicle formation, which combines real-time performance and trajectory quality [32]. Currently, the lattice method mainly performs a gridded search in the lateral direction. It is often used for lateral trajectory planning, ignoring the dynamic coupling effect of the longitudinal dimension in the trajectory generation process. In complex dynamic environments such as variable obstacle avoidance, emergency braking, or low-adhesion road surfaces, the longitudinal behavior is closely coupled with the lateral path, and relying solely on lateral search makes it difficult to exactly reflect overall dynamic feasibility and safety [33]. To address this issue, this paper improves the lattice method.

The primary innovations of this paper are:

• In the motion decision and trajectory planning module, the influence of road conditions (adhesion coefficient, curvature) is considered to improve the safety of the autonomous driving system.
• A longitudinal and lateral coupled lattice trajectory planning method is proposed, which can improve the dynamic feasibility of the trajectory and ensure longitudinal–lateral consistency under complex working conditions.

The rest of this article is arranged as follows. In the Section 2, the overall research framework of this article is introduced. In the Section 3, vehicle–road comprehensive influencing factors are considered to aid in designing the vehicle driving decision mechanism. In the Section 4, the lattice method is improved, the longitudinal and lateral coupled trajectory planning algorithm for vehicles is studied, and the trajectory tracking module is constructed to achieve intelligent vehicle motion control. In the Section 5, Simulink–CarSim–Prescan joint simulation verification is carried out. In the Section 6, the entire article is summarized and future work is presented.

2. Overall Research Framework

This paper proposes a driving decision-making and trajectory planning framework that comprehensively considers the influencing factors of vehicles and roads, as shown in Figure 1. Firstly, in the environmental perception stage, the longitudinal adhesion coefficient of the road surface represents the maximum longitudinal traction/braking force capability that the tires can provide under the current road conditions, and is one of the basic parameters for the safety and stability control of intelligent vehicles. This paper utilizes the RLS method to estimate the longitudinal road status in real time and with high accuracy. It combines onboard sensors to gather information on surrounding obstacles, thereby providing environmental perception information that supports subsequent system safety feasibility judgments, path and strategy optimization, and other related tasks. Secondly, in the motion decision-making stage, vehicle behavior decisions are made based on road adhesion conditions and a lane weighting function reflecting the feasibility and risk level of the lane is designed, thereby constructing a driving decision-making mechanism that comprehensively considers the influence factors of vehicles and roads. In the trajectory planning and motion control stage, a longitudinal and lateral coupled trajectory planning algorithm based on lattice is developed. Longitudinal speed planning and lateral trajectory planning are carried out according to the decision results, and smooth switching between the two is achieved. Combined with model predictive control (MPC) and dual PID control algorithms, the intelligent vehicle can achieve precise trajectory tracking and stable control in complex scenarios. The algorithm is simulated and verified on a horizontal structured road, and the influence of road slope is not considered. The proposed decision-making and planning framework for autonomous driving vehicles effectively enhances the system’s adaptability to complex dynamic environments, realizes robust planning and control in low-adhesion and high-interference scenarios, significantly improves the rationality of vehicle system decision-making, trajectory feasibility, and operational safety, and lays a foundation for the practical deployment of autonomous driving systems.

3. Vehicle Driving Decision-Making Mechanism Considering the Comprehensive Impact of Vehicles and Roads

3.1. Estimation of Longitudinal Road Adhesion Coefficient Based on RLS

To obtain a real-time and accurate estimate of the longitudinal pavement adhesion coefficient and to support the subsequent development of the vehicle’s longitudinal and lateral motion controllers, this chapter constructs a two-degree-of-freedom vehicle dynamic model. A tire mechanical model combining a linear region and saturation region is used, with an emphasis on the relationship between longitudinal force and slip rate.

Establish the vehicle dynamic model:

$$\left\{\begin{array}{l}m{a}_x=m\left(\ddot{x}-\dot{y}\dot{\phi }\right)=2F_{xf}+2F_{xr}\\ m{a}_y=m\left(\ddot{y}+\dot{x}\dot{\phi }\right)=2F_{yf}+2F_{yr}\\ I_z\ddot{\phi }=2a{F}_{yf}-2b{F}_{yr}\end{array}\right.$$

(1)

The conversion relationship between the resultant force acting on the tire in the longitudinal and lateral directions and the actual lateral force and longitudinal force exerted on the tire is as follows:

$$\left\{\begin{array}{l}{F}_{xf}=F_{lf}\cos {\delta }_f-F_{cf}\sin {\delta }_f\\ F_{xr}=F_{lr}\\ F_{yf}=F_{lf}\sin {\delta }_f+F_{cf}\cos {\delta }_f\\ F_{vr}=F_{cr}\end{array}\right.$$

(2)

where $m$ is the curb weight, $a_x$ is the longitudinal acceleration, $a_y$ is the lateral acceleration, $I_z$ is the rotational inertia of the vehicle around the z-axis, ${\delta }_f$ is the rotation angle of the front wheels, $\phi$ is the roll angle, $\dot{\phi }$ is the roll angular velocity, $\beta$ is the camber angle of the center of gravity, ${\alpha }_f$ is the camber angle of the front wheels, ${\alpha }_r$ is the camber angle of the rear wheels, ${\nu }_f$ is the speed of the front wheels, $v_r$ is the speed of the rear wheels, $F_{xf},F_{xr}$ are the forces exerted by the front and rear wheels in the x-axis direction, $F_{yf},F_{yr}$ are the forces exerted by the front and rear wheels in the y-axis direction, $F_{lf},F_{lr}$ are the longitudinal forces exerted by the front and rear wheels of the vehicle, $F_{cf},F_{cr}$ are the lateral forces exerted by the front and rear wheels of the vehicle, $a$ is the distance from the center of gravity to the front axle, and $b$ is the distance from the center of gravity to the rear axle.

The lateral and longitudinal velocities of the vehicle have different expressions in the vehicle body coordinate system and the earth coordinate system. The conversion relationship between the two can be expressed as:

$$\left\{\begin{array}{l}\dot{X}=\dot{x}\sin \phi +\dot{y}\cos \phi \\ \dot{Y}=\dot{x}\cos \phi -\dot{y}\sin \phi \end{array}\right.$$

(3)

Whole-vehicle longitudinal dynamic model:

$$I_x{a}_x={\displaystyle \frac{T_t}{R}}-{\displaystyle \frac{T_b}{R}}-mgf-{\displaystyle \frac{1}{2}}{C}_{D}A\rho {\nu }_x^2$$

(4)

where $I_x=m+{\displaystyle \frac{I_{wf}+I_{wr}}{R^2}}$ represent the rotational inertia of the front and rear wheels, $g$ is the gravitational acceleration, $a_x$ is the longitudinal acceleration of the vehicle, $R$ is the rolling radius of the wheel, $f$ is the road adhesion coefficient, $C_D$ is the air resistance coefficient, $A$ is the frontal area, $\rho$ is the air density, $T_t$ is the driving torque, and $T_b$ is the braking torque, $T_b=T_{bf}+T_{br}$.

Braking or driving force of the front and rear axles of the vehicle:

$$F_x=\mu D_x\mathit{sin}\left(C_x\mathit{arctan}\left(B_x{\Phi }_x\right)\right)$$

(5)

$$B_x={\displaystyle \frac{a_3{F}_z^2+a_4{F}_z}{C_x{D}_x{e}^{a_5{F}_z}}}$$

(6)

$$C_x=1.65$$

(7)

$$E_x=a_6{F}_z^2+a_7{F}_z+a_8$$

(8)

$$D_x=a_1{F}_{\mathrm{z}}^2+a_2{F}_z$$

(9)

$${\Phi }_x=\left(1-E_x\right){\sigma }_{\mathrm{x}}+\left({\displaystyle \frac{E_x}{B_x}}\right)\mathit{arctan}\left(B_x{\sigma }_x\right)$$

(10)

where $\mu$ represents the road adhesion coefficient, $a$ is an empirical constant, and ${\sigma }_{\mathrm{x}}$ is the longitudinal slip rate of the wheel.

Slip ratio during vehicle acceleration/deceleration:

$${\sigma }_x={\displaystyle \frac{{\omega }_w{r}_w-v_x}{{\omega }_w{r}_w}}$$

(11)

$${\sigma }_x={\displaystyle \frac{{\omega }_w{r}_w-v_x}{v_x}}$$

(12)

where $r_w$ represents the radius of the wheel, while ${\omega }_w$ (wheel speed) and $v_x$ (vehicle speed) are known quantities.

From the wheel rotation dynamic equation:

$$I_w{\dot{\omega }}_w=T_w-F{f}_r-F_x{r}_w$$

(13)

where $T_w$ represents the braking or driving torque of the wheels and $f_{\mathrm{r}}$ is the coefficient of rolling resistance of the wheels.

When a vehicle is accelerating or braking, the acceleration is constrained by the road status, with the relationship being $a⩽\mu g$. Within a small slip rate range, there is approximately a direct proportional relationship between the adhesion coefficient and the slip rate:

$${\displaystyle \frac{F_x}{F_z}}=k{\sigma }_x$$

(14)

where $k$ is the slope, $F_x$ is the longitudinal force of the tire, and $F_z$ is the vertical force of the tire. In the small slip rate range, the reaction force of the ground on the driving wheel $F_t={\displaystyle \frac{T_w}{r}}$ can approximately represent the longitudinal force of the tire $F_x,$ $F_x=F_t$.

Transform it into the recursive least squares method expression:

$$y\left(t\right)={\phi }^{\mathrm{T}}\left(t\right)\theta \left(t\right)+e\left(t\right)$$

(15)

where $y\left(t\right)={\displaystyle \frac{F_x}{F_z}},\phi \left(t\right)={\sigma }_x,\theta \left(t\right)=k$.

$$\widehat{\mu }=k\times {\sigma }_m\times p$$

(16)

where ${\sigma }_{\mathrm{m}}$ represents the maximum slip rate within the linear region, and p is the proportion coefficient of the maximum road adhesion coefficient within the linear region to the peak road adhesion coefficient. The derivation process of the longitudinal road adhesion coefficient estimation algorithm based on the RLS method is as shown in Algorithm 1.

Algorithm 1. RLS method

Input: the braking or driving force $F_x$, the longitudinal acceleration $a_x$, the adhesion coefficient at the previous moment ${\widehat{\mu }}_{t-1}$, the covariance matrix at previous time $P_{t-1}$

Output: the adhesion coefficient at the current moment $\widehat{\mu }$, the covariance matrix at current time $P_t$

1. Initialization parameters: set the estimated value of the initial adhesion coefficient ${\widehat{\mu }}_0$, the covariance matrix $P_0=\alpha I$, set the forgetting factor $\lambda$ 2. Start iteration: for each time step $i,$ and perform steps 3–8 3. Data acquisition: reading the observation data of the current time step $a_x\left(i\right)$, inputting the driving force $F_x\left(i\right)$ 4. Constructing regression vectors: calculating regression variables $\Phi (i)={\displaystyle \frac{F_x\left(i\right)}{mg}}$, the linear regression form is modeled as $a_x\left(i\right)=\Phi \left(i\right)\cdot \mu +ϵ$ 5. Calculate the prediction error: $ϵ\left(i\right)=a_x\left(i\right)-\Phi \left(i\right)\cdot {\widehat{\mu }}_{t-1}$, the residual between the current observation and the previous estimate. 6. Update gain matrix: calculate recursive gain vector $K(i)={\displaystyle \frac{P_{t-1}\cdot \Phi (i{)}^T}{\lambda +\Phi \left(i\right)\cdot P_{t-1}\cdot \Phi (i{)}^T}}$ 7. Update the estimated value: update the attachment coefficient estimation according to the prediction error ${\widehat{\mu }}_t={\widehat{\mu }}_{t-1}+K\left(i\right)\cdot ϵ\left(i\right)$ 8. Update the covariance matrix: adjust the error covariance $P_t={\displaystyle \frac{1}{\lambda }}\cdot (P_{t-1}-K(i)\cdot \Phi (i)\cdot P_{t-1})$ 9. Convergence diagnose 10. Output result: estimated value of adhesion coefficient at current time $\widehat{\mu }$

3.2. Intelligent Vehicle Driving Decision Mechanism Considering the Comprehensive Impact of Vehicles and Road Surfaces

To enhance the dynamic adaptability of the autonomous driving system in complex environments, this paper proposes a driving decision-making mechanism that integrates dynamic vehicle–road information, aiming to ensure driving safety and adaptability under different adhesion conditions and traffic flow distributions.

Based on the real-time estimation method of the road adhesion coefficient proposed in Section 3.1, the current road adhesion capacity is characterized by the estimated value $\mu$. The maximum lateral acceleration that the vehicle can withstand is approximately $a_{y,max}\approx \mu g$. According to the ISO 3888-1 double-lane-change test [34], it is indicated that during the period when $\mu \le 0.5,$ the risk of vehicle skidding and rollover during lane changing is higher, resulting in lane change-related accidents accounting for more than 17% of all accidents in icy and snowy weather. Based on the above risk trend, driving behavior decision-making for low-adhesion road surfaces is designed:

$$P_L(\mu )=\left\{\begin{array}{c}(1-\mu {)}^{\eta }(l\ne l_i)\\ 0\hspace{1em}\hspace{1em}\hspace{1em}else\hfill \end{array}\right.$$

(17)

where $\eta$ is the curvature coefficient. During the simulation process, when $\eta =1,$ the penalty for the low-adhesion section is insufficient, resulting in a low lane-changing success rate and an increased risk of vehicle skidding. When $\eta >1.6,$ overly conservative behavior occurs, leading to a decrease in lane-changing efficiency. Therefore, in this framework, $\eta =1.2$ to ensure successful lane changes while also paying attention to lane-changing efficiency. $l\ne l_i$ ensures that the current lane is not penalized. This switching mechanism ensures that the vehicle can dynamically adjust its operational capability boundaries according to the actual ground conditions.

Normalized processing, attachment condition evaluation function:

$$M\left(l_i\right)=1-P_{\left(l_i\right)}$$

(18)

Under the longitudinal and lateral coupling planning mode, the system further takes into account the current road traffic scene information of the vehicle, comprehensively perceives the obstacle status and traffic flow speed distribution of each lane around the vehicle, and conducts dynamic assessment of the available lanes. The current lane of the HV is defined as $l_i,$ and a lane set $\mathcal{L}=\{l_{i-n},\dots l_i,\dots l_{i+n}\}$ is constructed within the lane expansion range $\pm n$. For each candidate lane, the comprehensive evaluation function is constructed as:

$$J\left(l_i\right)={\lambda }_1\cdot D\left(l_i\right)-{\lambda }_2\cdot V\left(l_i\right)-{\lambda }_3\cdot R\left(l_i\right)-{\lambda }_4\cdot M\left(l_i\right)-{\lambda }_5\cdot \left|\kappa \right(l_i\left)\right|$$

(19)

where $D\left(l_i\right)$ represents the forward available distance of the HV on this lane, reflecting the spatial smoothness of the lane. $V\left(l_i\right)$ represents the deviation between the target vehicle speed and the HV’s expected speed, measuring the potential following efficiency. $R\left(l_i\right)$ represents the potential risk estimation of this lane, taking into account obstacle density, the behavior of the preceding vehicle changing lanes, and interference from adjacent lanes. $M\left(l_i\right)$ represents the road adhesion condition score. The lower the adhesion coefficient, the greater the penalty. $\kappa \left(l_i\right)$ represents the curvature of the road, avoiding the choice of a fast-turning lane and improving stability and safety.

$$D\left(l_i\right)=\left[min\right(x_i-x_h)-{\Delta }_{safe}]/D_{max}\in \left[\mathrm{0,1}\right]$$

(20)

where $x_i,x_h$ represents the vertical distance between the obstacle vehicle and the self-vehicle and $D_{max}$ is the sensor’s line of sight distance, which is set at 50 m:

$$V(l_i)=1-{\displaystyle \frac{|v_h-v_f(l_i\left)\right|}{v_h}}$$

(21)

where $v_h$ represents the speed of the vehicle itself, while $v_f\left(l_i\right)$ represents the speed of the vehicle immediately in front. When the speed difference is greater than 0, the speed $V\left(l_i\right)$ is reduced.

The risk function $R\left(l_i\right)$ is used to quantify the probability that a vehicle will encounter potential conflicts, collisions, or dynamic disturbances after entering the candidate lane $l_i$. It is the core quantity for safety assessment in the decision-making module and is divided into the following key components:

$$R\left(l_j\right)=w_1\cdot R_s\left(l_j\right)+w_2\cdot R_d\left(l_j\right)+w_3\cdot R_u\left(l_j\right)$$

(22)

where $R_s$ represents the static obstacle risk term, indicating the density and distance of fixed obstacles on lane $l_i,$ $R_d$ is the dynamic risk term, reflecting the potential interference that other dynamic vehicles in the lane may cause to this vehicle, such as relative speed, relative position, and lane-changing tendency, and $R_{\mathrm{u}}$ is used to represent the uncertain term, reflecting the possible errors in the perception system [35].

In the comprehensive evaluation function, ${\lambda }_1,{\lambda }_2,{\lambda }_3,{\lambda }_4,{\mathrm{a}\mathrm{n}\mathrm{d}\lambda }_5$ are weight factors. The adaptive adjustment strategy for the weight factors is as follows:

Establish the security level of the scene:$S\in \left[\mathrm{0,1}\right]$

$$v_{safe}=\sqrt{{\displaystyle \frac{\mu g}{max\left(\right|\kappa |,\epsilon )}}},S=clip\left({\left({\displaystyle \frac{v}{v_{safe}}}\right)}^2\cdot {\displaystyle \frac{0.85}{\mu }},0,1\right)$$

(23)

where $\mu$ represents the road adhesion coefficient, $\kappa$ represents the curvature information, $\epsilon$ is a constant term, and $clip\left(\ast \right)$ indicates selecting the median value within the parentheses. Through the construction of the scene safety degree, in slippery/sudden turns/high-speed conditions, $S$ is close to 1, and the selection of the weight factor leans more towards safety, while in straight roads/dry conditions/low-speed conditions, $S$ is close to 0 and the selection of the weight factor leans more towards efficiency.

$$\lambda =S{w}_s+\left(1-S\right)w_e$$

(24)

where $w_s$ represents the security weight vector and $w_e$ represents the efficiency weight vector. The sizes of the corresponding elements of $[D,V,R,M,|\kappa \left|\right]$ in the vectors are determined through parameter adjustment. Each vector contains elements.

The system selects the lane that maximizes the benefit for each candidate lane as the current optimal driving lane:

$$l^{*}=arg \underset{l_j\in \mathcal{L}}{max} J(l_j)$$

(25)

The decision-making module outputs specific behavior instructions based on the selected optimal lane $l^{*}$ and calls the trajectory planner to generate dynamic trajectories, ensuring adaptive, robust, and efficient autonomous driving behavior in real-time changing road and traffic conditions.

4. Coupled Longitudinal and Lateral Trajectory Planning of Vehicles Based on the Lattice Method

This paper improves the traditional lattice method and designs a trajectory planning framework with both longitudinal and lateral coupling capabilities. This method can dynamically switch between joint longitudinal and lateral trajectory planning and the single longitudinal planning mode based on the behavioral instructions issued by the upper-level decision module and the current environmental perception results. It enables a smooth transition between lateral trajectory planning and longitudinal speed planning, enhancing the consistency and robustness of the decision-making and planning in complex scenarios such as slippery roads and dense obstacles. The key variables in the planning algorithm are summarized as

Table 1. Main parameters of the planning algorithm.

Sign	Implication
$\overrightarrow{x}$	position vector of the HV
${\theta }_x$	course angle
${\kappa }_x$	curvature calculation
${\nu }_x$	velocity
$a_x$	acceleration
${\overrightarrow{r}}_{\mathrm{o}\mathrm{b}\mathrm{s}}$	obstacle position vector
${cost}_s$	smoothing cost
${cost}_c$	obstacle avoidance cost
$d$	Euclidean distance

.

4.1. Coordinate Transformation

In intelligent vehicle trajectory planning and control, the Frenet coordinate system is based on the reference line and represents the vehicle position as the arc length $\left(s\right)$ along the path direction and the lateral offset $\left(l\right),$ which can effectively simplify the trajectory generation problem and make the planning more efficient and compliant with road constraints [36]. The Cartesian coordinate system $(x,y)$ reflects the actual position of the vehicle in the global environment and is necessary for perception data processing, map interaction, and control execution. The conversion between the two coordinate systems is shown in Figure 2. During the planning process, global coordinate systems are constructed to record road information, including lanes and road curvatures, and a global reference path is generated, as shown in Figure 3.

According to Figure 2, the position coordinates of the vehicle at the current moment are:

$$\overrightarrow{x}={\left(x,y\right)}^T=\overrightarrow{x}\left(s\left(t\right),l\left(s\right)\right)=\overrightarrow{r}\left(s\left(t\right)\right)+l\left(s\left(t\right)\right){\overrightarrow{n}}_r\left(s\left(t\right)\right)$$

(26)

where $\overrightarrow{x}$ represents the position vector of the HV, $s\left(t\right)$ represents the point $P$ obtained by projecting the vehicle onto the reference line, $l\left(s\right)$ is the curve distance from the starting point to point $P,$ $\overrightarrow{r}\left(s\left(t\right)\right)$ represents the position vector of the vehicle on the reference line, and ${\overrightarrow{n}}_r\left(s\left(t\right)\right)$ represents the normal unit vector at point $P$.

After conversion, the vehicle motion state is represented as $\left[\begin{array}{c}\overrightarrow{x},{\theta }_x,{\kappa }_x,{\nu }_x,a_x\end{array}\right],$ while in the Frenet system, it is expressed as $\left[\begin{array}{c}s,\dot{s},\ddot{s},l,\dot{l},\ddot{l},l^{\prime },l^{″}\end{array}\right].$ ${\theta }_x$ represents the heading angle, ${\kappa }_x$ represents the curvature in the current state, ${\nu }_x$ represents the vehicle speed, and $a_x$ represents the acceleration. $\dot{l},\ddot{l}$ represent the derivatives of the lateral displacement with respect to time, and $l^{\prime },l^{″}$ represent the derivatives of the lateral displacement with respect to the longitudinal displacement. $s$ is the distance between adjacent points.

The tangential and normal unit vectors at the projection point of the vehicle position on the reference line are expressed as:

$${\overrightarrow{\tau }}_r={\left(\mathit{cos}{\theta }_r,\mathit{sin}{\theta }_r\right)}^T$$

(27)

$${\overrightarrow{n}}_r={\left(-\mathit{sin}{\theta }_r,\mathit{cos}{\theta }_r\right)}^T$$

(28)

Course angle and curvature calculation:

$${\theta }_x=acrtan\left(dy,dx\right)$$

(29)

$${\kappa }_x={\displaystyle \frac{\mathit{sin}\left({\theta }^{\prime }\right)}{s}}$$

(30)

The conversion relationship between the two coordinate systems is summarized as follows.

• Conversion from Cartesian coordinate system to Frenet coordinate system:

$$\begin{array}{c}\left\{\begin{array}{ccc}s=& s_r& \\ \dot{s}=& \&{\displaystyle \frac{v_x\mathit{cos}\left({\theta }_x-{\theta }_r\right)}{1-k_r}}& \\ \ddot{s}=& {\displaystyle \frac{a_x\mathit{cos}\left({\theta }_x-{\theta }_r\right)-{\widehat{s}}^2\left[l^{\prime }\left(k_x\frac{1-k_{r}l}{\mathit{cos}\left({\theta }_x-{\theta }_r\right)}-k_r\right)-\left(k_r^{\prime }l+k_r{l}^{\prime }\right)\right]}{1-k_{r}l}}& \\ l=& sign\left(\left(x_x-x_r\right)\mathit{cos}\left({\theta }_r\right)-\left(y_x-y_r\right)\mathit{sin}\left({\theta }_r\right)\right)\sqrt{(x_x-x_r{)}^2+(y_x-y_r{)}^2}& \\ l^{\prime }=& (1-k_{r}l)tan({\theta }_x-{\theta }_r)& \\ l^{″}=& -\left(k_r^{\prime }l+k_r{l}^{\prime }\right)\mathit{tan}\left({\theta }_x-{\theta }_r\right)+{\displaystyle \frac{\left(1-k_{r}l\right)}{{\mathit{cos}}^2\left({\theta }_x-{\theta }_r\right)}}\left({\displaystyle \frac{1-k_{r}l}{\mathit{cos}\left({\theta }_x-{\theta }_r\right)}}{k}_x-k_r\right)& \end{array}\right.\end{array}$$

(31)

• Conversion from Frenet coordinate system to Cartesian coordinate system:

$$\begin{array}{c}\left\{\begin{array}{ccc}{x}_x=& x_r-\mathit{lsin}\left({\theta }_r\right)& \\ y_x=& y_r+\mathit{lcos}\left({\theta }_r\right)& \\ {\theta }_x=& \mathit{arctan}\left({\displaystyle \frac{l^{\prime }}{1-k_{r}l}}\right)+{\theta }_r\in \left[-\pi ,\pi \right]& \\ v_x=& \sqrt{\left[s\right(1-k_{r}l){]}^2+(s{l}^{\prime }{)}^2}& \\ a_x=& \ddot{s}{\displaystyle \frac{1-k_{r}l}{\mathit{cos}\left({\theta }_x-{\theta }_r\right)}}+{\displaystyle \frac{s^2}{\mathit{cos}\left({\theta }_x-{\theta }_r\right)}}\left[l^{\prime }\left(k_x{\displaystyle \frac{1-k_{r}l}{\mathit{cos}\left({\theta }_x-{\theta }_r\right)}}-k_r\right)-\left(k_r^{\prime }l+k_r{l}^{\prime }\right)\right]& \\ k_x=& \left(\left(l^{″}+\left(k_r^{\prime }l+k_r{l}^{\prime }\right)\mathit{tan}\left({\theta }_x-{\theta }_r\right)\right){\displaystyle \frac{{\mathit{cos}}^2\left({\theta }_x-{\theta }_r\right)}{1-k_{r}l}}+k_r^{″}\right){\displaystyle \frac{\mathit{cos}\left({\theta }_z+{\theta }_z\right)}{1-k_r{l}^{\prime }}}& \end{array}\right.\end{array}$$

(32)

4.2. Lateral Trajectory Planning

In order to take into account the feasibility of the path and the smoothness of the trajectory, this paper adopts a two-stage trajectory generation structure combining dynamic programming (DP) and quadratic programming (QP), conducting path search and trajectory optimization separately. This achieves a balance between search efficiency and trajectory quality. Firstly, from the obstacle information in the global coordinate system, obstacles existing in the forward field of view and the lateral perceptible area are selected. The obstacle information is classified and the static obstacle position, dynamic obstacle position, and velocity vector are output, which are used as input for local path planning. The position vector and longitudinal–lateral relative distance of the obstacle relative to the host vehicle are calculated:

$${\overrightarrow{r}}_{\mathrm{o}\mathrm{b}\mathrm{s}}=\left[\begin{array}{c}{x}_{\mathrm{o}\mathrm{b}\mathrm{s}}-x\\ y_{\mathrm{o}\mathrm{b}\mathrm{s}}-y\end{array}\right]$$

(33)

$$s_{\mathrm{o}\mathrm{b}\mathrm{s}}={\overrightarrow{r}}_{\mathrm{o}\mathrm{b}\mathrm{s}}\cdot {\overrightarrow{\tau }}_r$$

(34)

$$l_{\mathrm{o}\mathrm{b}\mathrm{s}}={\overrightarrow{r}}_{obs}\cdot {\overrightarrow{n}}_r$$

(35)

For the square of the distance between each obstacle point and each trajectory point:

$$d^2=(s-s_{obs}{)}^2+(l-l_{obs}{)}^2$$

(36)

Based on the starting position of the vehicle and the position of static obstacles, a dynamic programming method is used to search for a lateral trajectory path with the minimum cost within the prescribed lateral grid, and the $(s,l)$ sequence of the optimal trajectory is output. A quintic polynomial trajectory function is constructed, with the initial state $L_0=\left[l\left(s_0\right)\dot{l}\left(s_0\right)\ddot{l}\left(s_0\right)\right]$ and the target state $L_1=\left[l\left(s_1\right)\dot{l}\left(s_1\right)\ddot{l}\left(s_1\right)\right]$. For the lateral motion of the vehicle:

$$l\left(s\right)=a_0+a_{1}s+a_2{s}^2+a_3{s}^3+a_4{s}^4+a_5{s}^5$$

(37)

Derivation:

$$\dot{l}\left(s\right)=a_1+2a_{2}s+3a_3{s}^2+4a_4{s}^3+5a_5{s}^4$$

(38)

$$\ddot{l}\left(s\right)=2a_2+6a_{3}s+12a_4{s}^2+20a_5{s}^3$$

(39)

Matrix form:

$$\left[\begin{array}{c}l\left(s\right)\\ \dot{l}\left(s\right)\\ \ddot{l}\left(s\right)\end{array}\right]=\left[\begin{array}{ccc}1& s& s^2\\ 0& 1& 2s\\ 0& 0& 2\end{array}\right]\cdot \left[\begin{array}{c}{a}_0\\ a_1\\ a_2\end{array}\right]+\left[\begin{array}{ccc}{s}^3& s^4& s^5\\ 3s^2& 4s^3& 5s^4\\ 6t& 12s^2& 20s^3\end{array}\right]\cdot \left[\begin{array}{c}{a}_3\\ a_4\\ a_5\end{array}\right]$$

(40)

$$A_{l1}\left(s\right)=\left[\begin{array}{ccc}1& s& s^2\\ 0& 1& 2s\\ 0& 0& 2\end{array}\right],A_{l2}\left(s\right)=\left[\begin{array}{ccc}{s}^3& s^4& s^5\\ 3s^2& 4s^3& 5s^4\\ 6s& 12s^2& 20s^3\end{array}\right]$$

(41)

When $s=s\left(\begin{array}{c}0\end{array}\right)$:

$$\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\end{array}\right]=\left[\begin{array}{c}l\left(s_0\right)\\ l\left(s_0\right)\\ {\displaystyle \frac{\ddot{l}\left(s_0\right)}{2}}\end{array}\right]$$

(42)

When $s=s\left(\begin{array}{c}\tau \end{array}\right)$:

$$\left[\begin{array}{c}{a}_3\\ a_4\\ a_5\end{array}\right]=A_{l2}^{-1}\left(s_1\right)\left(\left[\begin{array}{c}d\left(s_1\right)\\ \dot{d}\left(s_1\right)\\ \ddot{d}\left(s_1\right)\end{array}\right]-A_{l1}\left(s_1\right)\cdot \left[\begin{array}{c}d\left(s_0\right)\\ \dot{d}\left(s_0\right)\\ {\displaystyle \frac{\ddot{d}\left(s_0\right)}{2}}\end{array}\right]\right)$$

(43)

$$A_{l2}\left(s_1\right)\left[\begin{array}{c}{a}_3\\ a_4\\ a_5\end{array}\right]=\left(\begin{array}{c}\left[\begin{array}{c}l\left(s_1\right)\\ \dot{l}\left(s_1\right)\\ \ddot{l}\left(s_1\right)\end{array}\right]-A_{l1}\left(s_1\right)\cdot \left[\begin{array}{c}l\left(s_0\right)\\ \dot{l}\left(s_0\right)\\ {\displaystyle \frac{\ddot{l}\left(s_0\right)}{2}}\end{array}\right]\end{array}\right)$$

(44)

where coefficient vector $[a_0,\dots ,a_5{]}^{\top }$ is obtained by solving the system of equations.

Separately construct the trajectory smoothing cost term and the obstacle avoidance cost term:

$${cost}_s=w_{\dot{l}}\int (\dot{l}{)}^{2}ds+w_{\ddot{l}}\int (\ddot{l}{)}^{2}ds+w_{\stackrel{⃛}{l}}\int (\stackrel{⃛}{l}{)}^{2}ds$$

(45)

$${cost}_c(d^2)=\left\{\begin{array}{ll}{w}_c,& d^2<9\\ 1000/d^2,& 9\le d^2<16\\ 0,& d^2\ge 16\end{array}\right.$$

(46)

where $w_{\dot{l}},w_{\ddot{l}},w_{\stackrel{⃛}{l}}$ are weighting coefficients, $d$ is the Euclidean distance between the vehicle trajectory point and the obstruction, $w_c$ is the set collision penalty coefficient, indicating a strong penalty, and $1000/d^2$ serves as a progressive penalty function, with a closer distance resulting in a greater cost. By constructing a trajectory smoothness cost term and an obstacle avoidance cost term, the degree of fluctuation of the trajectory is minimized, preventing the trajectory from approaching or crossing the obstacle and achieving a balance between safety and feasibility during driving.

In order to further enhance the continuity and dynamic controllability of the trajectory, this paper introduces a quadratic programming optimization module based on the DP dynamic programming path. The QP module takes the output of DP as the reference path, constructs a cubic derivative smoothing objective function, optimizes the lateral offset $l\left(s\right)$ of the vehicle and its first-order, second-order, and third-order derivative terms, and optimizes the variable vector:

$$x=[l_1,l_1^{\prime },l_1^{″},l_2,l_2^{\prime },l_2^{″},\dots ,l_n,l_n^{\prime },l_n^{″}{]}^T\in R^{3n}$$

(47)

The cost function is defined as follows:

$$\underset{x}{min} J=x^{T}Hx+f^{T}x$$

(48)

where $H\in R^{3n\times 3n}$ is a symmetric positive definite cost weight matrix, including the centerline deviation cost $w_l\cdot \sum l_i^2,$ the lateral velocity penalty term $w_{dl}\cdot \sum (l_i^{\prime }{)}^2,$ the lateral acceleration penalty term $w_{ddl}\cdot \sum (l_i^{″}{)}^2,$ and the jerk penalty term (rate of change of acceleration) $w_{dddl}\cdot \sum (l_{i+1}^{″}-l_i^{″}{)}^2.$ $f$ is a linear term.

Establishing dynamic continuity constraints:

$$\left\{\begin{array}{c}{l}_{i+1}=l_i+ds\cdot l_i^{\prime }+{\displaystyle \frac{d{s}^2}{3}}\cdot l_i^{″}+{\displaystyle \frac{d{s}^2}{6}}\cdot l_{i+1}^{″}\\ l_{i+1}^{\prime }=l_i^{\prime }+0.5\cdot ds\cdot l_i^{″}+0.5\cdot ds\cdot l_{i+1}^{″}\end{array}\right.$$

(49)

Establishing constraints on the variation range of derivatives:

$$\left\{\begin{array}{c}|l_{i+1}^{\prime }-l_i^{\prime }|\le \Delta l_{max}^{\prime }\\ |l_{i+1}^{″}-l_i^{″}|\le \Delta l_{max}^{″}\end{array}\right.$$

(50)

Establishing the lateral boundary of the obstacle, taking into account the vehicle width $w$ and the offset positions $d_1,d_2$ of the front axles:

$$l_{min}\left(s_i\right)\le l\left(s_i\right)\le l_{max}\left(s_i\right)$$

(51)

$$l(s_i+d_1)+1.5w\le l_{max},l(s_i-d_2)-1.5w\ge l_{min}$$

(52)

4.3. Longitudinal Speed Planning

To achieve the integration and smooth transition of lateral trajectory planning and longitudinal speed planning, in this section, the longitudinal speed distribution is optimized and designed, and the longitudinal speed is independently planned to generate a complete time sequence trajectory. Given the position sequence $s\left(t\right)$ under the reference line, continuous curves for time parameter $t,$ speed $\dot{s\left(t\right)},$ and acceleration $\ddot{s}\left(t\right)$ are generated to realize the integration from a spatial trajectory to a spatial–temporal trajectory.

Establish boundary conditions:

$$s(0)=0,s^{\prime }(0)={\dot{s}}_0,s^{″}(0)={\ddot{s}}_0$$

(53)

$$s\left(T\right)=s_{end}$$

(54)

Based on the integral constraints of first-order and second-order derivatives, the continuity of the vehicle’s position and velocity and acceleration are ensured at each time period:

$$s_{i+1}=s_i+{\dot{s}}_i\Delta t+{\displaystyle \frac{1}{3}}{\ddot{s}}_i\Delta t^2$$

(55)

$${\dot{s}}_{i+1}={\dot{s}}_i+{\displaystyle \frac{1}{2}}{\ddot{s}}_i\Delta t$$

(56)

Based on the end position $s_{\mathrm{e}\mathrm{n}\mathrm{d}}$ of the dynamic planning path and the recommended planning duration $T,$ an initial longitudinal velocity trajectory consisting of three dimensions—position, speed, and acceleration—is constructed. A minimum jerk trajectory optimization model with position constraints, speed limits, and acceleration constraints is adopted, and the objective function is:

$$min\left(\sum _{i=1}^n {\ddot{s}}_i^2+\lambda \sum _{i=1}^{n-1} ({\ddot{s}}_{i+1}-{\ddot{s}}_i{)}^2\right)$$

(57)

where ${\ddot{s}}_i$ represents the acceleration at the $i$ moment, the second term constrains the jerk smoothness, and $\lambda$ is the weighting coefficient.

Using $s\left(t\right)$ as the index, interpolate the lateral trajectory as the function $l\left(t\right)$. According to the Frenet transformation formula, restore it to the Cartesian trajectory and generate the final complete trajectory $\left\{x\right(t),y(t),\theta (t),\kappa (t),v(t),a(t),t\}$. Finally, concatenate the current frame’s trajectory with the remaining trajectory from the previous frame to construct a complete prediction trajectory queue, ensuring smoothness, continuity, and control tracking performance of the trajectory.

4.4. Trajectory Tracking

In this paper, the MPC algorithm is employed to achieve high-precision lateral trajectory control for vehicles. By optimizing the vehicle’s future short-term motion trajectory in real time, considering vehicle dynamic constraints and road boundary conditions, it effectively improves the path tracking accuracy and stability of the vehicle in complex conditions such as cornering and lane changing. The vehicle dynamic prediction model is as follows:

$$X(k+1)=AX\left(k\right)+BU\left(k\right)$$

(58)

where $X\left(k\right)$ represents the quantity of state, $U\left(k\right)$ represents the control input, and $A,B$ represent the system matrices obtained through the linearization of vehicle dynamics.

MPC cost function:

$$J=\sum _{i=1}^{N_p} \parallel X(k+i|k)-X_{ref}(k+i){\parallel }_Q^2+\sum _{i=0}^{N_c-1} \parallel \Delta U(k+i|k){\parallel }_R^2$$

(59)

where $N_p$ represents the length of the prediction time domain, $N_c$ represents the length of the control time domain, and $Q,R$ represent the weight matrices, which are used to balance the trajectory error and the smoothness of the control input.

In longitudinal trajectory tracking, a dual-closed-loop PID control strategy based on position loop and speed loop is designed and implemented. The outer-loop position controller generates the expected speed command based on the longitudinal position information of the target path, while the inner-loop speed controller adjusts the throttle and braking system to rapidly converge the actual vehicle speed to the expected value, ensuring the rapid response characteristic of longitudinal tracking and effectively suppressing the speed overshoot phenomenon, enabling the vehicle to smoothly follow the longitudinal changes of the target path.

Definition of error:

$$e(t)=y_{ref}(t)-y(t)$$

(60)

where $y_{ref}(t)$ represents the vehicle’s expected trajectory and $y(t)$ represents the actual vehicle trajectory.

PID control law:

$$u(t)=K_{p}e(t)+K_i{\int }_0^t e(\tau )d\tau +K_d{\displaystyle \frac{de\left(t\right)}{dt}}$$

(61)

Discrete version:

$$u(k)=K_{p}e(k)+K_i\sum _{j=0}^k e(j)\Delta t+K_d{\displaystyle \frac{e(k)-e(k-1)}{\Delta t}}$$

(62)

The collaborative application of these two control algorithms provides a reliable guarantee for the safe and efficient driving of intelligent vehicles in complex traffic environments. More details can be found in [37].

5. Simulink–CarSim–Prescan Joint Experimental Verification

To verify the validity and stability of the intelligent vehicle trajectory planning and control method proposed in this paper, experiments were conducted based on a Simulink–CarSim–Prescan joint simulation environment. The main parameters of the simulated vehicles are shown in

Table 2. Main parameters of the simulation vehicles.

Symbol	Value	Unit
$m$	2020	$\mathrm{k}\mathrm{g}$
$I_z$	4095	$\mathrm{k}\mathrm{g}·{\mathrm{m}}^2$
$L_a$	1.265	$\mathrm{m}$
$L_b$	1.682	$\mathrm{m}$
$C_f$	−175,016	$\mathrm{N}$
$C_r$	−130,634	$\mathrm{N}$
$c\_factor$	43.75	$°/\mathrm{k}\mathrm{N}$

.

Three scenarios were designed for a highway with four lanes on each carriageway: avoiding stationary obstacles on the curve, wet road conditions, and multiple moving vehicles. The width of each lane is 3.5 m. The simulated environment in global coordinates is shown in Figure 4. The X and Y axes represent the global coordinates. Additionally, a comparison was made between the results from the traditional lattice method, which does not consider changes in the road adhesion coefficient, and those from the proposed method under the same simulation environment. The simulation result videos, including the comparison of the proposed method that accounts for vehicle–road interaction factors, have been uploaded to bilibili. The links are https://www.bilibili.com/video/BV172tqz9E8b/ (accessed on 7 August 2025) and https://www.bilibili.com/video/BV1E1tqzsEJd/ (accessed on 7 August 2025).

Scene 1 consists of a straight 50 m section connected to two circular arc sections with a total length of 78.5 m, each having a curvature radius of 100 m, and no transition curves were set between each section of the road. The initial speed of the host vehicle (HV) is 0. In each of the second and third lanes, there is a static obstacle with a volume of 1 cubic meter. The vehicle trajectory is shown in Figure 5. In the initial stationary state of the HV, based on the lattice trajectory planning algorithm, it achieved safe passage around multiple obstacles on the continuous curved sections. By calculating the Euclidean distance between each trajectory point and the obstacles, the minimum distances between the vehicle trajectory and the two obstacles are 3.03 m and 2.97 m, respectively, meeting the vehicle safety envelope. By directly incorporating the curvature constraint as an optimization condition to limit the lateral acceleration and the vehicle’s optimal speed curve, a moderate increase in speed in long-radius curves and more uniform longitudinal and lateral control are achieved. The vehicle’s driving trajectory always remains between the road boundary and the lane lines, and the obstacle avoidance process is continuous, smooth, and without sudden changes. This verifies the effectiveness and feasibility of the proposed method in the mixed scenarios of curved roads and obstacles. The overall trajectory exhibits good trajectory continuity, dynamic feasibility, and obstacle safety margin, which can meet the requirements of safety and real-time performance in high-speed scenarios.

In this study, an RLS observer was built on the Simulink simulation platform, and the longitudinal road adhesion coefficient estimation model was simulated and verified jointly with CarSim. The simulation results are shown in Figure 6. Between the driving distances of 200 to 260 m, the road adhesion coefficient suddenly changed from 0.85 to 0.3. Through the RLS algorithm, the estimated curve tracked the changes in the adhesion conditions, with good real-time performance. The average estimated error was 0.0124, and the estimation accuracy was relatively high.

In Scene 2, there is a slippery road surface and moving obstacle vehicles in the first and second lanes. In Figure 7, based on the comprehensive framework for driving decision-making and trajectory planning constructed in this paper, the system can accurately identify sudden changes in road conditions. According to the simulation results in Section 3.1, when the HV reaches 200 m, the road adhesion coefficient suddenly changes from 0.85 to 0.3. At this point, the action instructions are issued by the decision-making layer, and the longitudinal and lateral coupling planning is smoothly converted to longitudinal speed planning. The HV slows down and maintains a safe distance from the preceding vehicle. After leaving the slippery road surface, the system switches back to the longitudinal and lateral coupling trajectory planning mode. Based on the environmental perception results, the decision module triggers the lane change and acceleration–overtaking actions and the HV accelerates and overtakes, effectively avoiding dynamic targets. Through the wet road–moving vehicle scenario, the intelligent decision-making and trajectory planning framework proposed in this paper has been verified to be able to coordinate multiple sources of information, reasonably switch planning modes, and effectively avoid high-risk situations such as skidding and rollover caused by insufficient adhesion under sudden road changes, thereby improving the environmental adaptability, safety, and stability of the autonomous driving system.

Scene 3 is a complex scenario involving multiple moving vehicles. The HV is faced with multiple moving vehicles (ov3 to ov6) distributed continuously across different lanes, and needs to dynamically complete multiple lane changes and overtaking maneuvers. In Figure 8, the HV is traveling in the third lane. After detecting ov4 in the same lane ahead, the decision-making module actively plans the lane change strategy based on the surrounding traffic conditions and autonomously changes lanes to the second lane to complete the overtaking. During the continued driving process, the HV again recognizes ov3 in the current lane, and the system promptly completes the second right lane change, successfully returning to the third lane to complete the overtaking operation. Throughout the process, the trajectory planning module generates paths that maintain good continuity and feasibility, with smooth trajectory curves, timely obstacle avoidance behaviors, and no sudden changes or dangerous deviations. The system effectively integrates the results of multi-vehicle situation awareness and lateral dynamic constraints, achieving coordinated avoidance of multiple dynamic targets in dense traffic flows and demonstrating the advantages of the decision-making and planning framework proposed in this paper in terms of security.

To further verify the physical feasibility and security of the generated trajectories, the curves of longitudinal velocity, lateral acceleration, and front wheel angle changing with driving distance $s$ are extracted. These three parameters are extracted from the whole simulation process, including all three scenarios, as shown in Figure 9.

In Figure 9a, the longitudinal velocity of the HV is shown. The initial speed is 0 m/s, and it accelerates smoothly to the desired speed of 16 m/s. During the obstacle avoidance process, there is a slight fluctuation. When it reaches 200 m, due to a sudden change in the road surface condition, the vehicle speed drops to 8 m/s. It maintains a safe distance and continues driving, then accelerates to overtake after leaving the slippery road surface. This verifies that the intelligent vehicle driving decisions and trajectory planning framework proposed in this paper fully considers the influence of road surface conditions, which is of great significance for improving driving safety.

The lateral acceleration curve of the HV is shown in Figure 9b. When HV is traveling in the curve in Scenario 1, the curvature radii of both sections of the curve are 100. Therefore, there are downward and upward centrifugal accelerations of 2.56 m/s² in the lateral direction. On the first section of the curve, due to the lane change to the third lane to avoid obstacles, the lateral acceleration is pulled back. On the second section of the curve, there is no lane change behavior, and the lateral acceleration fluctuates slightly. In Scenario 3, multiple obstacle vehicles are interlaced in adjacent lanes, forming a dynamic dense traffic environment. The HV needs to perform multiple consecutive lane changes and obstacle avoidance actions. The lateral acceleration curve in this section shows high-frequency fluctuations, with the maximum value being ±2.8 m/s². This mainly reflects the continuous lateral maneuvering behavior of the vehicle in complex traffic scenarios, but it has never exceeded the adhesion limit allowed range ($a_y\le \mu g$), and there is a certain safety margin without exceeding the physical limit of vehicle stability.

In Figure 9c, there is a curve showing the variation of the front wheel angle. The vehicle maintains excellent steering control performance during the driving process. There are obvious fluctuations when encountering the high-density dynamic obstacle targets in Scenario 3, but the maximum angle amplitude is controlled within a range of ±7°, not exceeding the physical limitations of lateral control. The system responds quickly and acts reasonably during the lane changing–obstacle avoidance process, verifying the controllability and stability of the proposed planning framework in complex dynamic environments.

To verify the control method used in this paper, the lateral position and speed data were extracted from the planning results, and corresponding data were also obtained from the control results. The lateral position error and speed error were calculated. The parameter comparison and error results are shown in Figure 10, Figure 11 and Figure 12.

To present the accuracy of the control algorithm more intuitively, the root mean square errors of the lateral displacement and longitudinal velocity are calculated. The maximum errors of the lateral displacement and longitudinal velocity are shown in

Table 3. Quantitative index.

	Index	RMSE (m/s)	${\mathit{e}}_{\mathit{m}\mathit{a}\mathit{x}}(\mathbf{m}/\mathbf{s})$
Parameter
$e_y$		0.125	0.516
$e_v$		0.230	0.781

.

In this table, it can be seen that in Scenario 1, due to the main vehicle avoiding obstacles in consecutive curves, the peak value of the lateral position error is 0.516 m. However, through calculation, the root mean square error is 0.125 m per second, proving that the overall lateral position error is relatively small. In Scenario 2, there is a slippery road surface, and the main vehicle needs to slow down and follow closely during driving. The peak value of the speed tracking error is 0.781 m, and the root mean square error is 0.230 m per second, still at an excellent level. This confirms that the tracking method adopted in this paper has good tracking accuracy.

6. Summary and Future Work

This paper focuses on the key issues of vehicle decision-making and trajectory planning in intelligent driving systems, and proposes a vehicle driving decision-making and trajectory planning framework that comprehensively considers the influencing factors of vehicles and roads. Various scenarios were constructed for experimental verification. The main work included a lane-level driving decision-making mechanism based on environmental perception information, fully considering key factors such as surrounding vehicle information, lane boundaries, and road adhesion conditions. The lattice longitudinal–lateral coupling trajectory planning method was improved, generating fifth-order polynomial lateral trajectories based on the decision results and planning longitudinal speed based on a cubic acceleration model, ensuring consistency of longitudinal and lateral movements while meeting obstacle avoidance and stability requirements. The MPC algorithm and dual PID algorithm were used to achieve precise trajectory tracking and stable control of intelligent vehicles in different scenarios. Finally, a complex traffic scene including continuous obstacle avoidance in curves, slippery road surfaces, and multiple moving vehicles was constructed through the joint simulation platform to verify the comprehensive capabilities of the proposed method in dynamic decision-making, environmental adaptability, and safety control.

In future research, we will consider enhancing the dynamic traffic scenario adaptability of the autonomous driving system. By introducing multi-vehicle interaction game modeling mechanisms and other methods, we will strengthen the multi-agent coordination and conflict avoidance capabilities in complex traffic scenarios. At the same time, we will introduce driver style modeling methods to conduct personalized trajectory optimization for different driving behavior patterns such as aggressive and conservative types, thereby improving the human–vehicle collaboration experience and the explainability of driving behaviors. Finally, we will consider conducting hardware-in-the-loop verification and real road vehicle tests. We will build an HIL platform including ECU, IMU, GPS, and onboard Ethernet communication to test the performance and timeliness of the algorithm in a real-time closed loop and conduct road experiments under different adhesion coefficients (slippery, icy), multi-sensor error injection, and external disturbance scenarios to systematically evaluate the stability and safety margin of the method.