Introduction

With the rapid development of autonomous driving technology, trajectory planning has become an essential core function in intelligent driving systems, playing a crucial role in improving traffic efficiency and reducing traffic accidents1,2,3. This is especially true in complex road scenarios involving multiple intelligent agents, where intelligent vehicles must not only accurately plan their trajectories but also ensure safety and efficiency during operation. However, trajectory planning in multi-agent and curved-road scenarios is challenging, as vehicles need to consider geometric constraints of the road, predict the intentions of surrounding vehicles, and make optimal decisions within limited time frames4,5,6.

Traditional trajectory planning methods, often based on rule-based systems or model predictive control, perform well in simple scenarios but may fail to provide sufficient comfort and robustness in complex situations7. Advanced methods based on deep reinforcement learning (DRL), although capable of learning complex decision-making strategies, still face challenges in generalization and robustness8.

Conventional decoupled spatiotemporal decision-making and planning algorithms, which separate the temporal and spatial dimensions, can ensure real-time performance but often result in conservative or aggressive trajectory planning due to the neglect of spatiotemporal correlations9. Moreover, single-dimensional safety mechanisms are insufficient to address sudden risks in complex interactive scenarios10. Therefore, achieving joint spatiotemporal optimization and multi-layered safety protection while maintaining real-time performance has become a key issue in enhancing the reliability of intelligent driving systems.

Joint spatiotemporal trajectory planning algorithms are among the critical technologies in intelligent driving trajectory planning. These algorithms consider both temporal and spatial dimensions to achieve globally optimal trajectory planning11,12,13. However, existing joint spatiotemporal trajectory planning algorithms fail to fully account for the effects of road curvature and vehicle posture, resulting in a limited solution space and an inability to approximate the optimal solution.

To address these challenges, this paper proposes a joint spatiotemporal planning algorithm that incorporates heuristic human-like decision-making thinking. First, the proposed curvature-adaptive joint spatiotemporal trajectory planning algorithm achieves adaptive road representation, effectively decoupling the constraints of complex road curves and vehicle posture. Subsequently, the hierarchical Bézier optimization algorithm balances robustness and spatial smoothness. Finally, by integrating planning costs with real-time control, the algorithm achieves human-like decision-making capabilities.

Related works

As a core technology in intelligent driving systems, vehicle trajectory planning is perpetually confronted with the challenge of optimal solution search under multiple constraints. Its fundamental objective is to generate an optimal motion trajectory from the initial state to the target state while satisfying the triple constraints of dynamics, kinematics, and safety conditions. Building on this foundation, numerous scholars have conducted in-depth research and proposed a variety of algorithms and strategies.

Search-based planning methods rely on the discretization of environmental space to construct a solution framework. Traditional search algorithms, represented by Dijkstra and A*14,15,16, achieve path search through state grid discretization, with the advantage of strict completeness guarantees. Among them, the A* algorithm, by incorporating a heuristic function, prioritizes the expansion of nodes closer to the goal17. However, it still faces efficiency bottlenecks when dealing with high-dimensional state spaces. Notably, the MHA* algorithm significantly improves search efficiency through a multi-heuristic fusion mechanism, yet the trajectories it generates often neglect the kinodynamic constraints of vehicles, resulting in insufficient trajectory continuity. The spatiotemporal grid expansion method innovatively introduces obstacle motion prediction, achieving a breakthrough in trajectory planning under dynamic environments. However, this method still faces the dual challenges of exponentially increasing computational complexity and insufficient trajectory smoothness.

Sampling-based planning methods, represented by Rapidly-exploring Random Tree (RRT)18 and Probabilistic Roadmap (PRM)19, demonstrate unique advantages in planning within high-dimensional spaces. Their core value lies in achieving probabilistic completeness, thereby overcoming the curse of dimensionality. However, the inherent sampling uncertainty leads to significant fluctuations in path quality. In terms of technological evolution, Time-extended Batched Rapidly-exploring Random Tree (TB-RRT)20 enhances adaptability to dynamic environments through temporal dimension expansion. Nevertheless, the stochastic sampling mechanism still suffers from inherent drawbacks, such as excessive computational resource consumption and insufficient trajectory stability. Improved RRT algorithms have made breakthroughs in sampling efficiency, but the fundamental issue of trajectory feasibility verification remains unresolved.

Parametric curve methods realize trajectory geometric constraints through preset parametric curves, with typical representatives including Bézier curves and quintic polynomials21,22,23. Among them, the innovative practice of optimizing the end-point curvature of the trajectory using a fifth-order Bézier curve has engineering value, but it exposes the shortcoming of lacking a rapid determination mechanism for control points in emergency obstacle avoidance scenarios24. In comparison, the piecewise polynomial decoupled planning method significantly improves trajectory continuity, but there is still room for improvement in the computational efficiency of its parameter optimization process25.

With the advancement of intelligent algorithms, hybrid intelligent methods have demonstrated advantages in obstacle avoidance success rates. The artificial potential field method achieves efficient planning by constructing a virtual gradient field, and the design of the virtual force field has successfully eliminated trajectory oscillation26,27,28. The integration of potential field methods with optimal control theory has significantly enhanced trajectory feasibility. However, potential field modeling in complex scenarios remains an unresolved challenge. Emerging methods based on deep reinforcement learning have shown strong adaptability to environments8. Although deep predictive networks can capture behavioral uncertainty, their inference latency still exceeds the real-time control threshold, and the safety verification framework is not yet fully developed.

The spatiotemporal decoupled framework in the Frenet coordinate system, proposed by Werling et al., significantly improves computational efficiency by optimizing path and velocity in a hierarchical manner29. However, it neglects the coupling between space and time, leading to discontinuous trajectory curvature in curved scenarios. In recent years, to overcome this limitation, joint spatiotemporal optimization has gradually become a research hotspot. For example, planning methods based on spatiotemporal corridors generate safe trajectories by constructing spatiotemporal cuboid constraints30. However, these methods still suffer from insufficient real-time performance in dynamic environments.

In the field of autonomous driving trajectory planning, although significant progress has been made in previous research, two core challenges in curved road scenarios within structured roads remain to be solved:

Dynamic coupling of spatiotemporal constraints. The traditional Frenet coordinate system can effectively decouple longitudinal and lateral motion planning, but a single reference frame struggles to accurately describe the motion states of traffic participants with different curvature characteristics31,32,33.

Co-optimization of planning robustness and efficiency. Existing safety mechanisms often adopt conservative strategies that ensure collision avoidance but frequently result in non-natural driving behaviors such as sudden deceleration34. The primary passive safety strategies tend to prioritize collision avoidance at the expense of traffic efficiency, leading to conservative maneuvers that disrupt traffic flow. This safety-first approach may trigger consecutive braking by following vehicles, thereby increasing overall traffic risk and compromising robustness35,36,37.

In summary, while previous research has made significant progress in intelligent driving trajectory planning, several limitations still exist, such as limited adaptability and robustness in complex environments, challenges in safety capabilities, and the optimality of joint spatiotemporal methods. The joint spatiotemporal planning algorithm proposed in this paper, which integrates heuristic human-like decision-making thinking, comprehensively considers factors such as road curvature, vehicle posture, and planning cost, effectively addressing the aforementioned issues.

Methods

Spatiotemporal joint planning algorithms

Path planning in the Cartesian coordinate system requires simultaneous optimization of the planar x-y coordinate parameters, which is prone to dimension coupling issues when dealing with complex road geometries. In contrast, the Frenet coordinate system, by decomposing vehicle motion into longitudinal displacement along the road centerline (S-axis) and lateral offset (L-axis), significantly reduces the dimensionality complexity of the planning problem29. This decomposition mechanism closely aligns with the cognitive pattern of human drivers regarding lane-relative positions, enabling the system to generate trajectories that are more intuitive for driving38. However, the Frenet framework has inherent limitations in multi-agent curved motion scenarios. When the road curvature radius falls below a critical value, a single reference frame can lead to misjudgments of curved steering behavior, mistaking curvature tracking for lane-changing actions39. This issue arises from the intrinsic conflict between vehicle steering dynamics and the geometric constraints of the fixed reference frame.

In light of these challenges, this paper proposes a multi-Frenet coordinate system method based on road segmentation, as illustrated in Fig. 1 This method involves concatenating curved roads to “straighten” them. It ensures smooth trajectory planning, widens the spatiotemporal corridor, and reduces the vehicle’s envelope radius. Based on this approach, we employ Bezier curves to complete the spatiotemporal trajectory planning. After obtaining the initial trajectory from the decision-making algorithm, each segment of the trajectory is translated using an inverse transformation. The spatiotemporal joint planning algorithm is responsible for transforming a series of inputs from the planning algorithm (including the ego-vehicle’s position and state, the ego-vehicle’s decision-based coarse trajectory, and the predicted future trajectories of surrounding vehicles) into the Frenet coordinate system and connecting the segments end-to-end. Simultaneously, dynamic and static obstacles are mapped into the grid map, and a three-dimensional feasible convex space in S-L-T is constructed within the grid map. Finally, based on quadratic programming, an optimal trajectory is solved using piecewise Bezier curves within the convex space.

Fig. 1
figure 1

Schematic diagram of trajectory planning for intelligent vehicles integrating curvature adaptation with spatiotemporal joint optimization.

Full size image

The planning procedure of the spatiotemporal joint planning algorithm is illustrated in Fig. 2, and its technical implementation is divided into four stages:

Fig. 2
figure 2

The flowchart of the joint spatiotemporal planning algorithm.

Full size image

Road linearization

Drawing on the commonly used road segmentation methods in previous studies on the coupling of road curvature and vehicle body posture, this paper integrates the adaptive discretization concept from dynamic path planning31,32. An adaptive curvature-based discretization method is employed to transform the vehicle’s state, the coarse-grained trajectory output from the decision-making layer, and the planned trajectories of perceived obstacles into a piecewise Frenet frame. The continuously curved road is divided into \(N\) quasi-linear segments, where the number of road segments \(N\) is determined by the following formula:

$$\begin{array}{c}\:N=\lceil\frac{1}{\Delta\:{s}_{max}}{\int}_{{s}_{0}}^{{s}_{f}}\sqrt{{\left(\frac{d\kappa\:}{ds}\right)}^{2}+{\kappa\:}^{4}ds}\rceil \\ \Delta\:{s}_{max}=\text{m}\text{i}\text{n}({\Delta\:}{s}_{0},\frac{c}{\left|\kappa\:\left(s\right)\right|+\epsilon})\end{array}$$
(1)

Here, \(\Delta {s}_{0}\) denotes the baseline maximum segment length, to balance planning accuracy and computational efficiency, its upper bound is set to the vehicle length. \(c\) is a scaling constant (default = 1), and \(\epsilon\) is a small positive constant to avoid division by zero (default \(\epsilon\) = 10-6). Consequently, in high-curvature regions, \(\Delta {s}_{max}\) decreases approximately in inverse proportion to the curvature, enabling finer segmentation when necessary.

$${{}_{s\in \left[{s}_{i},{s}_{i+1}\right]}^{\,\,\,\,\,\,\,\,max}}\left|\kappa (s)\right|\le {0.02m}^{-1}$$
(2)

A local Frenet coordinate system based on the vehicle’s center is established for each sub-segment. Through differential geometric transformations, a bidirectional mapping between the global and local coordinates is achieved, thereby forming a piecewise-straightened spatiotemporal planning corridor.

The method of joint spatiotemporal planning

A dynamic feasible region is constructed within the three-dimensional S-L-T (Station-Lateral-Time) space. The spatial occupancy of dynamic and static obstacles is modeled using a convex hull algorithm. For any obstacle, its spatiotemporal occupancy can be modeled as a convex polyhedral constraint in the S-L-T space.

Trajectory construction

The transformed road space is inverse-transformed based on the trajectory generated by the spatiotemporal joint algorithm. The generated trajectory is projected onto the global coordinate system, and then the trajectories of each segment are connected end-to-end (Path connection point). Finally, by applying \({C}^{2}\) continuity constraints at the connections using piecewise Bezier curves, a globally smooth trajectory is synthesized. Let the adjacent trajectory segment \({\Gamma }_{k}{\Gamma }_{k+1}\) be a quartic Bezier curve, with control points \({P}_{k}^{0},\dots ,{P}_{k}^{4}\text{ and }{P}_{k+1}^{0},\dots ,{P}_{k+1}^{1}\). The \({R}_{c}\) continuity must satisfy: the critical curvature radius \(\text{R}\) meets the vehicle dynamics constraints.

$${R}_{c}=\frac{{v}^{2}}{{a}_{lat}^{max}+{\delta }_{safe}}$$
(3)

Herein, \(v\) denotes the vehicle speed, \({a}_{lat}^{max}=2.5m/{s}^{2}\) represents the maximum allowable lateral acceleration, and \({\delta }_{safe}\) is the safety margin. When the road curvature radius \(R\) is smaller than \({R}_{c}\), a multi-coordinate system strategy must be employed.

Quadratic programming solution

Using the control points of the Bezier curve as decision variables, an optimization objective function is established that includes constraints on curvature continuity, jerk, acceleration, velocity, as well as penalty terms for lateral deviation and collision.

Compared with other joint spatiotemporal planning algorithms, the curvature-adaptive joint spatiotemporal planning algorithm operates within a local solution space of the given decision results, thereby avoiding the computational complexity explosion caused by decision diversity. Experiments demonstrate that this method reduces the trajectory envelopment radius compared to traditional single-Frenet-frame approaches. Furthermore, by leveraging parallel computation of subproblems, the planning time is controlled within 0.21s. This divide-and-conquer strategy effectively balances the trade-off between planning accuracy and computational efficiency, offering a novel approach for real-time trajectory planning in complex road scenarios.

The transformation between the Frenet and Cartesian coordinate systems

The Frenet coordinate system

The limitations of the Cartesian coordinate system lie in its inability to directly determine the distance traveled by a vehicle, whether surrounding vehicles have deviated from the lane centerline, or the exact location of the road. In contrast, the Frenet coordinate system is lane-based, where the S-axis represents the distance traveled along the lane, and the L-axis indicates the lateral offset from the lane centerline. Trajectory planning, as a high-dimensional optimization problem with multiple nonlinear constraints, demands significant computational resources. Given the complexity of real-world scenarios and the requirement for real-time processing, it is not always possible to quickly and simply obtain an optimal trajectory. This imposes high demands on the robustness and practicality of planning algorithms. When describing the position of objects using the Cartesian coordinate system, even if the vehicle’s location is known, it is challenging to rapidly determine the road’s position and whether the vehicle has deviated from the lane center.

In the Frenet coordinate system, however, the road centerline is typically used as the reference line, and the vehicle’s trajectory is decoupled into longitudinal and lateral components along and perpendicular to the reference line, respectively. Therefore, compared to the traditional Cartesian coordinate system with x and y axes, the Frenet coordinate system not only provides a more intuitive representation of the road position, especially for curved roads, but also simplifies the two-dimensional planning problem into the superposition of two one-dimensional problems. Additionally, the construction of the Frenet coordinate system avoids the complex transformation of the vehicle’s heading direction, which is often required in Cartesian coordinates.

Transformation between Frenet and Cartesian coordinates

The information obtained from the perception module, including road boundaries and obstacles, is represented in the global Cartesian coordinate system. The joint spatiotemporal planning algorithm solves for the optimal trajectory in the Frenet coordinate system. However, the final motion trajectory must be executable and trackable by the control module. Therefore, it is necessary to project the perception information from the global Cartesian coordinate system onto the Frenet coordinate system. Simultaneously, the planned trajectory obtained in the Frenet coordinate system must be transformed back to the global Cartesian coordinate system for execution.

Fig. 3 illustrates the correspondence between the motion trajectory of an intelligent driving vehicle in the Frenet coordinate system and the global coordinate system. Adopting the core idea of the Frenet-Serret framework and based on the geometric transformation between the Frenet coordinate system and the Cartesian coordinates, the following derivation can be obtained29. Assuming the position of the intelligent driving vehicle in the global coordinate system is represented as \((x,y)\), the vehicle’s position coordinates at the current moment can be expressed as:

$$\mathop{x}\limits^{\rightharpoonup} = (x,\;y) = \mathop{x}\limits^{\rightharpoonup} (s,\;l)$$
(4)
Fig. 3
figure 3

The illustration of the transformation between the Frenet coordinate system and the global coordinate system.

Full size image

In the equation, \(\mathop{x}\limits^{\rightharpoonup}\) denotes the position vector of the intelligent driving vehicle in the global coordinate system. \(s\) represents the arc length distance from the starting point to the corresponding point on the reference line when the vehicle’s position is projected onto it, indicating the longitudinal travel distance of the vehicle along the reference line. \(l\) is the lateral offset distance between the vehicle’s position and the point corresponding to the longitudinal distance.

The state variables of interest for the vehicle in the Cartesian coordinate system are \([\mathop{x}\limits^{\rightharpoonup} ,\;v,\;a,\;\theta_{x} ,\;k_{x} ]\) where \(v\) represents the vehicle’s velocity in the Cartesian coordinate system, \(a\) denotes the vehicle’s acceleration in the global coordinate system, \({\theta }_{x}\) is the vehicle’s yaw angle in the Cartesian coordinate system, and \({k}_{x}\) represents the curvature. When these state variables are transformed into the Frenet coordinate system, they correspond to \([s,\dot{s},\ddot{s},l,{l}{\prime},{l}^{{\prime}{\prime}}]\).

As shown in Fig. 3, the current position of the vehicle is projected onto the reference line. The reference point at the vehicle center is denoted as \(\mathop{r}\limits^{\rightharpoonup} = (x_{r} ,\;y_{r} )\). The \(s\) value of this reference point corresponds to the longitudinal distance of the vehicle in the Frenet coordinate system. \(\mathop{r}\limits^{\rightharpoonup}\) represents the position vector of the reference point in the Cartesian coordinate system. The geometric relationship yields:

$$\begin{gathered} \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\mathop{x}\limits^{\rightharpoonup} = \mathop{r}\limits^{\rightharpoonup} + l\mathop{N}\limits^{\rightharpoonup} _{r} \hfill \\ \;\;\;\;\;\;\;\;\;\;\;\;\;l = (\mathop{x}\limits^{\rightharpoonup} - \mathop{r}\limits^{\rightharpoonup} )^{T} \mathop{N}\limits^{\rightharpoonup} _{r} \hfill \\ \;\;\;\;\; = \left\| {\mathop{x}\limits^{\rightharpoonup} - \mathop{r}\limits^{\rightharpoonup} } \right\|_{2} \cos \left( {\theta_{x - r} - \left( {\theta_{r} + \frac{\pi }{2}} \right)} \right)\;\; \hfill \\ = \left\| {\mathop{x}\limits^{\rightharpoonup} - \mathop{r}\limits^{\rightharpoonup} } \right\|_{2} \left( {\sin (\theta_{x - r} )\cos (\theta_{r} ) - \cos (\theta_{x - r} )\sin (\theta_{r} )} \right) \hfill \\ \;\; \hfill \\ \end{gathered}$$
(5)

In the equation, \({\theta }_{x-r}\) is the direction angle of vector \(\mathop{x}\limits^{\rightharpoonup} - \mathop{r}\limits^{\rightharpoonup} \;\theta_{r} + \frac{\pi }{2}\); is the direction angle of the normal unit vector \(\mathop{N}\limits^{\rightharpoonup}\) of the reference trajectory.

Assuming \(\mathop{x}\limits^{\rightharpoonup} = (x_{x} - y_{x} )\;\;{\text{and}}\;\;\mathop{r}\limits^{\rightharpoonup} = (x_{x} - y_{x} )\) in the Frenet coordinate system, the vector \(\left\| {\mathop{x}\limits^{\rightharpoonup} - \mathop{r}\limits^{\rightharpoonup} } \right\|_{2} = \sqrt {(x_{x} - x_{r} )^{2} + (y_{x} - y_{r} )^{2} }\) from each coordinate point to its corresponding point on the reference line is either in the same direction or in the opposite direction to the normal vector \(\mathop{N}\limits^{\rightharpoonup} _{r}\) of that reference point. Therefore:

$$\left\{\begin{array}{ll}\text{sin}\left({\theta }_{x-r}\right)cos\left({\theta }_{r}\right)-cos\left({\theta }_{x-r}\right)sin\left({\theta }_{r}\right)=1 { \theta }_{r}-{\theta }_{x-r}=\frac{\pi }{2}\\ \text{sin}\left({\theta }_{x-r}\right)cos\left({\theta }_{r}\right)-cos\left({\theta }_{x-r}\right)sin\left({\theta }_{r}\right)=-1 { \theta }_{x-r}-{\theta }_{r}=\frac{\pi }{2}\end{array}\right.$$
(6)
$$\frac{\text{sin}\left({\theta }_{x-r}\right)}{\text{cos}\left({\theta }_{x-r}\right)}=\frac{{y}_{x}-{y}_{r}}{{x}_{x}-{x}_{r}}$$
(7)

Based on the positive or negative relationship of equations (6) and (7), the positive or negative value of the lateral distance \(l\) in equation (8) can be determined, which in turn allows us to determine on which side of the reference line the intelligent driving vehicle is at the current moment. Therefore, the formula for calculating the lateral distance \(l\) is:

$$l=\text{sign}\left(\left({y}_{x}-{y}_{r}\right)\text{cos}\left({\theta }_{r}\right)-\left({x}_{x}-{x}_{r}\right)\text{sin}\left({\theta }_{r}\right)\right)\sqrt{{\left({x}_{x}-{x}_{r}\right)}^{2}+{\left({y}_{x}-{y}_{r}\right)}^{2}}$$
(8)

After determining the longitudinal and lateral distances \(s\) and \(l\) in the Frenet coordinate system, the longitudinal velocity \(\dot{s}\) and longitudinal acceleration \(\ddot{s}\) in the Frenet coordinate system can be calculated from the vehicle’s speed, yaw angle, and curvature in the global coordinate system. Additionally, the first and second derivatives of the lateral distance with respect to the longitudinal distance, \(l^{\prime}\;{\text{and}}\;l^{\prime\prime}\) can be computed.

By differentiating the coordinates \((x_{x} - y_{x} )^{T} \mathop{N}\limits^{\rightharpoonup} _{r}\) with respect to time, we can obtain:

$$i = (\dot{\mathop{x}\limits^{\rightharpoonup} } = \dot{\mathop{r}\limits^{\rightharpoonup} })^{T} \mathop{N}\limits^{\rightharpoonup} _{r} + (\mathop{x}\limits^{\rightharpoonup} - \mathop{r}\limits^{\rightharpoonup} )^{T} \mathop{N}\limits^{\rightharpoonup} _{r}$$
(9)

From the geometric relationship of vectors \(\mathop{x}\limits^{\rightharpoonup} \;{\text{and}}\;\mathop{r}\limits^{\rightharpoonup}\) in Fig. 3, we can derive:

$$\left\{ \begin{gathered} \dot{\mathop{x}\limits^{\rightharpoonup} } = \frac{{{\text{d}}\mathop{x}\limits^{\rightharpoonup} }}{{{\text{d}}t}} = \frac{{{\text{d}}\left\| {\mathop{x}\limits^{\rightharpoonup} } \right\|}}{{{\text{d}}t}}\mathop{T}\limits^{\rightharpoonup} _{x} = v_{x} \mathop{T}\limits^{\rightharpoonup} _{x} \hfill \\ \;\;\;\;\;\;\;\;\;\;\;\dot{\mathop{r}\limits^{\rightharpoonup} } = \dot{s}l\mathop{T}\limits^{\rightharpoonup} _{r} \hfill \\ \end{gathered} \right.$$
(10)

In the Frenet coordinate system, the Frenet formulas are required. These formulas are used to describe the motion of a particle along a continuously differentiable curve in the Euclidean space R. They represent the interrelationships among the tangent, normal, and binormal vectors of the curve14,15,16. The specific formulas are as follows:

$$\left\{ \begin{gathered} \;\;\;\;\;\;\frac{{{\text{d}}\mathop{T}\limits^{\rightharpoonup} }}{{{\text{d}}s}} = K\overline{N} \hfill \\ \frac{{{\text{d}}\mathop{T}\limits^{\rightharpoonup} }}{{{\text{d}}s}} = - K\overline{T} = \tau \overline{B} \hfill \\ \;\;\;\;\;\;\frac{{{\text{d}}\mathop{B}\limits^{\rightharpoonup} }}{{{\text{d}}s}} = - \tau \overline{N} \hfill \\ \end{gathered} \right.$$
(11)

In the formulas, \(\overline{T }\) represents the unit tangent vector of the curve at a given point, \(\overline{N }\) is the corresponding unit normal vector, and \(\overline{B }\) is the binormal vector. \(k\) denotes the curvature, and \(\tau\) represents the torsion. In the context of local trajectory planning for autonomous driving, the road surface can be assumed to be a plane, and thus the torsion \(\uptau\) is generally zero.

Substituting Equations (10) and (11) into Equation (9) yields:

$$i = \left[ {v_{x} {\mathop{T}\limits^{\rightharpoonup}}_{x} - \dot{s}\mathop{T}\limits^{\rightharpoonup} } \right]^{1}\,{\mathop{N}\limits^{\rightharpoonup}} _{r}-l\,{\mathop{N}\limits^{\rightharpoonup}}_{r}^{T} ks\,{\mathop{N}\limits^{\rightharpoonup}}_{r}$$
(12)

Under normal circumstances, the actual trajectory of an autonomous vehicle will always move along the reference line and will not move in the opposite direction of the reference line. Therefore, the vehicle’s movement satisfies the following two constraints during its travel:

$$\begin{array}{c} - \frac{\pi }{2} \le \theta_{x} - \theta_{r} \le \frac{\pi }{2} \\ 1 < \frac{1}{k} \\ \end{array}$$
(13)

The rate of change of the lateral and longitudinal distances of an autonomous vehicle in the Frenet coordinate system can be derived from the lateral and longitudinal velocities. Combining the constraints in Equation (13), we obtain:

$${l}{\prime}=\frac{\sqrt{{\dot{s}}^{2}(1-kl)+{\dot{l}}^{2}}\text{sin}({\theta }_{x}-{\theta }_{r})}{\dot{s}}=(1-kl)\text{tan}({\theta }_{x}-{\theta }_{r})$$
(14)

By integrating the formulas for the longitudinal and lateral velocities as well as the rate of change of the lateral distance, we can derive the complete transformation relationship between the Frenet coordinate system and the Cartesian coordinate system:

$$\begin{array}{c}s={s}_{r}\\ \dot{s}=\frac{{v}_{x}\text{cos}({\theta }_{x}-{\theta }_{r})}{1-{k}_{r}l}\\ \ddot{s}=\frac{{a}_{x}\text{cos}({\theta }_{x}-{\theta }_{r})-{\dot{s}}^{2}[{l}{\prime}({k}_{x}\frac{1-{k}_{r}l}{\text{cos}({\theta }_{x}-{\theta }_{r})}-{k}_{r})-({k}_{r}{\prime}l-{k}_{r}{l}{\prime})]}{1-{k}_{r}l}\\ l=sign({(y}_{x}-{y}_{r})\text{cos}({\theta }_{r})-({x}_{x}-{x}_{r})sin({\theta }_{r}))\sqrt{{({x}_{x}-{x}_{r})}^{2}+{{(y}_{x}-{y}_{r})}^{2}} \\ {l}{\prime}=(1-kl)\text{tan}({\theta }_{x}-{\theta }_{r})\\ {l}^{{\prime}{\prime}}=-({k}_{r}{\prime}l+{k}_{r}{l}{\prime})\text{tan}({\theta }_{x}-{\theta }_{r})+\frac{(1-{k}_{r}l)}{{\text{cos}}^{2}({\theta }_{x}-{\theta }_{r})}(\frac{(1-{k}_{r}l)}{\text{cos}({\theta }_{x}-{\theta }_{r})}{k}_{x}-{k}_{r})\end{array}$$
(15)

The transformation formula from the Frenet coordinate system to the Cartesian coordinate system is:

$$\begin{array}{c}{x}_{x}={x}_{r}-lsin({\theta }_{r})\\ {y}_{x}={y}_{r}+l\text{cos}({\theta }_{r})\\ {\theta }_{x}=arctan(\frac{{l}^{\prime}}{1-{k}_{r}l})+{\theta }_{r}[-\pi ,\pi ]\\ {v}_{x}=\sqrt{\dot{s}(1-{k}_{r}l)+{(\dot{s}{l}^{\prime})}^{2}}\\ {a}_{x}=\ddot{s}\frac{1-{k}_{r}l}{\text{cos}({\theta }_{x}-{\theta }_{r})}+\frac{{\dot{s}}^{2}}{\text{cos}({\theta }_{x}-{\theta }_{r})}[{d}^{\prime}({k}_{x}\frac{1-{k}_{r}l}{\text{cos}({\theta }_{x}-{\theta }_{r})}-{k}_{r})-({k}_{r}^{\prime}l+{k}_{r}{l}^{\prime})]\\ {k}_{x}=(({l}^{{\prime}{\prime}}+({k}_{r}^{\prime}l+{k}_{r}{l}^{{\prime}{\prime}})\text{tan}({\theta }_{x}-{\theta }_{r}))\frac{{\text{cos}}^{2}({\theta }_{x}-{\theta }_{r})}{1-{k}_{r}l}+{k}_{r})\frac{\text{cos}({\theta }_{x}-{\theta }_{r})}{1-{k}_{r}l}\end{array}$$
(16)

The curvature \({k}_{x}\) is calculated in the Frenet coordinate system by taking three consecutive coordinate points on the path and using differential approximation and Taylor expansion to calculate \({l}{\prime}\), \({l}^{{\prime}{\prime}}\), and \(\dot{s}\). These values are then substituted into the following formula to solve for \({k}_{x}\):

$${k}_{x}=\frac{{l}^{\prime}}{{(1+{{l}{\prime}}^{2})}^\frac{2}{3}}$$
(17)

Similarly, the velocity \({v}_{x}\) and acceleration \({a}_{x}\) can be calculated by directly using the position and velocity change rates of adjacent path points to approximate them.

Construction of a spatiotemporal joint free feasible convex space

Construction of a three-dimensional Spatiotemporal (S-L-T) joint map

The core idea of constructing a spatiotemporal joint map is to extend the static and dynamic information of vehicles, obstacles, and other elements along the time axis, thereby recording the dynamic changes of environmental information over a period of time, as shown in Fig. 4. The three coordinate axes of the spatiotemporal joint map are the S-axis, L-axis, and T-axis. The S-axis and L-axis are coordinate axes in the Frenet coordinate system, while the T-axis represents the time axis. To construct the spatiotemporal joint map, it is necessary to project the environmental information in the Cartesian coordinate system onto the Frenet coordinate system.

Fig. 4
figure 4

Schematic diagram of spatiotemporal joint.

Full size image

As shown in Equation (18), the map \(M\) within the time interval \(T\) (starting from \(t\)) consists of three parts. At each time layer \(t\), it includes the predicted positions of the surrounding vehicles relative to the ego vehicle, denoted as \(\sum_{n=1}^{{N}_{D}}{P}_{t,n}^{D}\) , the positions of static obstacles, denoted as \(\sum_{n=1}^{{N}_{s}}{P}_{t,n}^{s}\) , and the road geometry information, denoted as \(\sum_{n=1}^{{N}_{L}}{R}_{l,n}\).

$$\left\{\begin{array}{c}M=\{\sum\nolimits_{t={t}_{0}}^{T}\sum\limits_{n=1}^{{N}_{D}}{P}_{t,n}^{D}\}\bigcup \{{\sum }_{t={t}_{0}}^{T}\sum\limits_{n=1}^{{N}_{s}}{P}_{t,n}^{s}\}\bigcup \{{\sum }_{t={t}_{0}}^{T}\sum\limits_{n=1}^{{N}_{L}}{P}_{t,n}^{L}\}\\ {P}_{t,n}^{D}=[{S}_{t,n},{l}_{t,n}]\\ {P}_{t,n}^{s}=[{S}_{t,n},{l}_{t,n}]\\ {P}_{t,n}^{L}=[{L}_{{w}_{t,n}},{L}_{{id}_{t,n}},{n}_{{s}_{t,n}},{n}_{{l}_{t,n}}]\end{array}\right.$$
(18)

In the equation, \({N}_{D}\), \({N}_{s}\), and \({N}_{L}\) represent the number of surrounding vehicles, static obstacles, and lanes, respectively. \([{S}_{t,n},{l}_{t,n}]\) denotes the coordinates in the Frenet coordinate system for \(s\) and \(l\). \([{L}_{{w}_{t,n}},{L}_{{id}_{t,n}}\)] represent the width and the number of the lane, respectively. \({[n}_{{s}_{t,n}},{n}_{{l}_{t,n}}]\) denotes the Frenet coordinates of the lane centerline.

Construction of Three-Dimensional Convex Space Based on S-L-T Using a Grid Map

The decision-making algorithm module generated and screened out a feasible coarse-grained trajectory with the minimum cost. Based on the coarse trajectory generated by the decision-making process and the information of static and dynamic obstacles obtained by the perception module, the planning algorithm module performed obstacle filling in the grid map and constructed convex spaces based on the state points on the coarse trajectory obtained from the decision-making process. The planning algorithm module constructed a three-dimensional convex space in the S-L-T domain. The static and dynamic obstacles in the spatiotemporal integrated map are shown in Fig. 5(a-c).

Fig. 5
figure 5

The planning algorithm module constructed a spatiotemporal integrated convex space.

Full size image

Projection of trajectory into the global coordinate

The decision-making algorithm module generated and screened out a feasible coarse-grained trajectory with the minimum cost. Based on the coarse trajectory generated by the decision-making process and the information of static and dynamic obstacles obtained.

Generation of the coarse trajectory projection of trajectory onto the Cartesian coordinate system

The construction of the three-dimensional convex space in the S-L-T domain mainly involves the following processes: First, the planning algorithm transforms the acquired obstacle information into the Frenet coordinate system using a coordinate transformation method. Subsequently, the transformed obstacle information and the coarse trajectory information obtained from the decision-making process are filled into a grid map of a specified size constructed by the planning algorithm. After completing these tasks, the planning algorithm performs a convex space search based on the coarse-grained trajectory from the decision-making process.

During the search, the algorithm traverses all state points on the coarse trajectory and searches for convex spaces in three directions along the axes. The search continues until a free collision-free space is identified as a convex space, stopping when the boundary of an obstacle is encountered. Fig. 5 illustrates a typical overtaking scenario with lane change, where Fig. 5(a) shows the ego vehicle as a green square, the target lane vehicle as a yellow vehicle, and the preceding vehicle in the current lane as a blue vehicle. The S-T and L-T diagrams for constructing the convex space are shown in Fig. 5(b) and (c), respectively. The red points represent the vehicle state points on the decision-making coarse trajectory, and the constructed convex spaces are depicted as a series of rectangles in Fig. 5(b) and (c). The dashed lines indicate the optimized trajectory.

Projection of trajectory onto the Cartesian coordinate system

To project the generated trajectory back into the real-world traffic coordinate system, the trajectories generated in each Frenet coordinate frame are first subjected to inverse operations based on the original coordinate transformations. Specifically, the trajectories in each Frenet frame are initially translated by an offset of -d. Subsequently, the translated trajectories are transformed from the Frenet coordinate system to the Cartesian coordinate system based on their respective rotation points. Finally, the segments of the transformed coordinates are concatenated end-to-end to form the reference trajectory.

Subsection

Currently, most trajectory generation schemes employ quintic polynomials to fit the lateral and longitudinal trajectories. However, quintic polynomials are not suitable for optimization in spatiotemporal integrated planning algorithms. The primary reason is that the expressive capability of a polynomial segment is limited, making it less appropriate for problems with complex configuration spaces and dynamic constraints. Additionally, in previous studies on polynomial trajectories, constraints were enforced and checked only on a finite set of sampling points. This approach may fail to detect collisions between sampling points, thereby not guaranteeing smoothness and optimal solutions. In contrast, Bézier curves, with their convex hull property and convexity-preserving characteristics, can generate fine-grained trajectories and effectively address the aforementioned issues.

Bézier curve

For an \(m\)-th order Bézier curve \(f(t)\)24, its mathematical expression is:

$$f(t)={P}_{0}{b}_{m}^{0}(t)+{P}_{1}{b}_{m}^{1}(t)+\dots +{P}_{m}{b}_{m}^{m}(t)=\sum_{i=0}^{m}{P}_{i}.{b}_{m}^{i}(t)$$
(19)

In the equation, \({P}_{i}\) represents the control points of the curve, and \({b}_{m}^{i}(t)=\left(\genfrac{}{}{0pt}{}{m}{i}\right){t}^{i}{(1-t)}^{m-i}\) is the Bézier polynomial.

The set of control points is denoted as \(P\):

$$P=[{P}_{0},{P}_{1},\dots ,{P}_{m}]$$
(20)

The convex hull property of the Bézier curve is applicable to trajectory optimization problems defined in a convex space. By constraining the curve within the convex space, it can be guaranteed that the generated Bézier curve is collision-free. The convex hull property of the Bézier curve implies that the derivative \(\frac{\text{d}f(t)}{\text{d}t}\) of the Bézier curve \(f(t)\) remains a Bézier curve. By applying the convex hull property to the derived Bézier curve \(\frac{\text{d}f(t)}{\text{d}t}\), the higher-order quantities (velocity, acceleration) of the Bézier curve \(f(t)\) can be constrained within the specified limits.

Trajectory optimization based on piecewise bézier curves

To connect the constructed convex spaces, piecewise Bézier curves are employed. To ensure the smoothness and feasibility of the trajectory, an optimization objective function, \(cost\) (total cost of the path), is formulated for the piecewise Bézier curve optimization problem. This cost function includes jerk cost, velocity difference cost, time cost, lateral offset cost, and collision cost. Since Bézier curves are defined within a fixed interval, and considering that the acceleration variation of the vehicle during travel should not be excessive to ensure passenger comfort, the cost value of the trajectory is used as the optimization cost function. Specifically, the cost function for the \(j\)-th segment of the Bézier curve, \({cost}_{j}\), can be expressed as:

$${cost}_{j}={}_{p }{}^{min} \sum_{i=1}^{N}\left({\omega }_{1}\int k\_jerk\text{d}t+{\omega }_{2}k\_t + {\omega }_{3}\int k\_v\text{d}t+{\omega }_{4}\int k\_offset\text{d}t+{\omega }_{5}k\_collision\right)$$
(21)

Here, \(p\) represents the control point matrix. \(k\_jerk\) denotes the cost of path smoothness, \(k\_t\) represents the cost of computation time, \(k\_v\) denotes the cost of velocity discrepancy, \(k\_offset\) represents the cost of lateral deviation, and \(k\_collision\) denotes the cost of collision.

Jerk cost

Denoted by formula \({\omega }_{1}\int k\_jerkdt\), is the integral of the squared jerk (rate of change of acceleration) over the trajectory segment. Jerk reflects the rate of change of acceleration with respect to time. Excessive jerk can cause a jarring sensation during vehicle travel, compromising ride comfort. Therefore, minimizing the jerk cost can further improve the smoothness of the trajectory.

Time cost

Denoted by formula \({\omega }_{2}k\_t\), It aims to quantify the temporal cost required for trajectory execution. Minimizing this cost can effectively reduce the travel time along the path and enhance the efficiency of the planning process.

Speed deviation cost

Denoted by formula \({\omega }_{3}\int k\_vdt\) . The speed deviation cost is utilized to constrain the deviation of vehicle speed from the desired speed, thereby suppressing abrupt acceleration or deceleration. By minimizing the speed deviation cost, the smoothness of the driving process can be ensured, reducing passenger discomfort and avoiding increased energy consumption or handling stability issues caused by severe speed fluctuations.

Lateral offset cost

The lateral offset cost, represented by formula \({\omega }_{4}\int k\_offsetdt\), measures the deviation of the vehicle from the target path in the lateral direction. It calculates the squared lateral offset to reflect the lateral distance between the vehicle and the target path. This cost helps ensure that the vehicle stays as close as possible to the ideal trajectory, thereby enhancing the accuracy of the planned path.

Collision cost

The collision cost, denoted by formula \({\omega }_{5}k\_collision\), embodies the safety constraints in trajectory planning. It is a function based on the probability of collision risk, evaluating the potential collision hazards between the trajectory segment and surrounding obstacles. By assigning higher weights to segments with higher collision risks, it effectively avoids collisions and ensures the safety of the vehicle during travel.

The weight coefficients \(({\omega }_{1}{,\omega }_{2},{\omega }_{3}\),\({\omega }_{4}\),\({\omega }_{5}\)) for each cost term are adaptively adjusted through a covariance matrix. This adaptive adjustment mechanism enables the algorithm to dynamically balance the weights of different cost terms according to various driving scenarios and conditions, thereby achieving optimal comprehensive performance in terms of smoothness, efficiency, and safety.

Experiment and analysis

Dataset

The data in this study are based on a parametric sampling strategy for intelligent driving trajectory planning. A multi-dimensional constraint model and a multi-objective optimization function are established to generate safe and efficient data. This method employs a hierarchical planning architecture: in the longitudinal dimension, the speed profile is constructed based on the S-T (space-time) model, with sampling at a resolution of 0.2 s within the time domain [4.5 s, 5.5 s] to generate S-T curves that satisfy the terminal velocity constraints (5.0 – 8.33 m/s). In the lateral dimension, the lateral offset trajectory is generated based on the L-T (lateral-time) model, with discrete sampling at a step length of 1.0 m within the road width of ±8 m, ensuring that the final acceleration is zero. A verification mechanism is established, including dynamic constraints (velocity ≤ 13.89 m/s, acceleration ≤ 8.0 m/s2, curvature ≤ [curvature limit]), collision detection, and S-L to X-Y coordinate transformation, to filter out physically feasible candidate trajectories. On this basis, a multi-objective cost function is designed to comprehensively evaluate the smoothness of the trajectory (jerk), efficiency (time cost), safety (lateral offset), and velocity tracking accuracy. The candidate trajectories are optimized and ranked through weight allocation.

The experimental environment was established on CentOS, with hardware configurations including a 12th Generation Intel Core i7-12650H processor (base frequency 2.30 GHz), 32 GB DDR5 RAM, and an NVIDIA GeForce RTX 4060 GPU. The algorithm was implemented in Python 3.8, relying on NumPy for matrix operations and Matplotlib for visualization. The experimental design includes the following key elements:

Road model

A circular bidirectional dual-lane environment composed of four curves was constructed. Each lane has a width of 8 m, with a longitudinal length of 100 m and a lateral width of 80 m. The lane boundaries are solid lines, while the center lane line is a dashed line, with lateral boundary constraints of ±8 m.

Obstacle configuration

One obstacle was placed in each curve to increase the difficulty of the bend and simulate scenarios where vehicles intrude into the opposite lane.

Planner parameters

The time sampling interval was set to 0.2 s, with a prediction time domain of [4.5 s, 5.5 s]. The lateral sampling step size was 0.1 m, and the velocity sampling step size was 5 km/h.

Vehicle parameters

A simplified bicycle model was used, with a wheelbase of 2.5 m, vehicle width of 1.8 m, and maximum steering angle of 0.6 rad. Detailed parameters are shown in Table 1.

Table 1 Basic parameters of the vehicle.
Full size table

Trajectory planning experiment in static scenarios

Validation and comparison of models

The integrated space-time model effectively combines the longitudinal and lateral motions of the vehicle. It not only integrates the current state of the vehicle and the surrounding environment but also considers the potential intentions and possible behaviors of other vehicles. Additionally, the model employs curvature-adaptive discretization to straighten the road, effectively addressing planning deviations caused by road curvature and vehicle posture. To validate the effectiveness of the proposed model, a circular simulation scenario with four curved roads was designed, as shown in Fig. 6 and 7. The green lines represent the vehicle’s driving trajectory, the yellow lines indicate the planned trajectory set at the current moment, and the red lines denote the real-time optimal trajectory. Fig. 6 illustrates the path planned using the traditional Bézier curve method, while Fig. 7 shows the trajectory planned using the space-time integrated algorithm.

Fig. 6
figure 6

Trajectory planning based on Bézier curves.

Full size image
Fig. 7
figure 7

Trajectory planning based on spatiotemporal integration algorithm.

Full size image

Comparing the experimental results in Fig. 6(a-f) and 7(a-f), it is evident that under the same constraints, the optimal trajectory planned by the traditional Bézier curve successfully bypasses obstacles but maintains a larger clearance from them. In contrast, the trajectory planned by the spatiotemporal coupling algorithm, which employs the temporal-spatial transformation in the Frenet coordinate system, avoids errors caused by vehicle posture and trajectory smoothness. As a result, it provides more precise obstacle avoidance and a smoother operational path. Comparing Fig. 6(a) and 7(a), it can be observed that during the bypass of the first obstacle, the steering angle of the spatiotemporal coupling planning is smaller than that of the traditional Bézier curve planning algorithm, resulting in a smoother driving experience. When comparing the data of other bends and obstacles, it is apparent that the spatiotemporal coupling algorithm offers a smoother trajectory that is more in line with human driving habits.

Cruise cost analysis

To comprehensively evaluate the superiority of the algorithm, an objective function, cost, is established for single-path optimization. The total cost of the path comprises jerk cost, speed deviation cost, time cost, lateral offset cost, and collision cost. Additionally, cruising cost weights and vehicle operation observation parameters are designed, as shown in Tables 2 and 3.

Table 2 Basic parameters of the cost function.
Full size table
Table 3 Kinematic parameters of vehicles.
Full size table

An intelligent driving trajectory planning method based on a designed detour route and employing a parametric sampling strategy was developed. It achieved secure and efficient data generation through the establishment of a multi-dimensional constraint model and a multi-objective optimization function. The method enabled vehicle simulation and generated a detour cost curve, as shown in Fig. 8.

Fig. 8
figure 8

Comparison of constraint costs.

Full size image

Experimental results indicate that the algorithm based on spatiotemporal integration has an average cost of 2.229, whereas the trajectory planning method based on traditional Bézier curves has an average cost of 33.671. It is evident that, despite both methods utilizing Bézier curves, the spatiotemporal integration algorithm successfully planned optimal detour paths without collisions in both single detour and actual driving scenarios. In contrast, although the traditional Bézier curve-based trajectory planning method also successfully navigated around obstacles, it generated collision-prone routes during four specific time intervals in single detour planning. The algorithm considering spatiotemporal integration demonstrated superior performance in terms of jerk cost, velocity deviation cost, time cost, lateral offset cost, and collision cost. It exhibited enhanced robustness in planning, smoothness of the trajectory, and better alignment with human driving habits.

Performance analysis of obstacle-avoidance trajectory planning model

To verify the trajectory planning performance of the spatiotemporal joint model, the Bezier curve planning model proposed in reference13 was adopted as the comparison model in this study. Under the condition that the basic driving parameters of the intelligent vehicle were identical during the testing process, comparisons were made between the proposed model and the comparison model in terms of longitudinal jerk, longitudinal acceleration, longitudinal velocity, lateral jerk, lateral acceleration, and lateral velocity.

In the longitudinal dimension, compared with Fig. 9(a) and (b), the spatiotemporal joint algorithm exhibits superior performance in terms of both longitudinal jerk and acceleration compared with the reference model, resulting in smoother vehicle motion. As shown in Fig. 9(c) and (f), the speed transition of the spatiotemporal joint algorithm is also more continuous. In the lateral dimension, as depicted in Fig. 9(d) and (e), the lateral acceleration and jerk of the spatiotemporal joint algorithm are slightly higher than those of the reference model, but the differences are minimal. Overall, the spatiotemporal joint algorithm demonstrates higher average speed, acceleration, and jerk in the longitudinal direction compared with the reference model, indicating enhanced ride comfort during operation. In the lateral motion, the average speed, acceleration, and jerk are all higher than those of the reference model, suggesting more efficient lane-changing maneuvers.

Fig. 9
figure 9

Comparison of longitudinal and lateral performance.

Full size image

Comparative analysis of spatiotemporal joint model algorithms

The spatiotemporal joint algorithm was compared with quintic polynomial and cubic B-spline algorithms. Based on the comparative experimental data, the spatiotemporal joint algorithm demonstrated significant advantages in several key performance indicators, as detailed in Table 4.

Table 4 Algorithm comparison analysis.
Full size table

From the comparative data, it can be observed that in terms of trajectory planning density, the spatiotemporal joint algorithm achieved an optimal planning frequency of 135 times, surpassing the quintic polynomial (133 times), B-spline curve (134 times), and Bezier curve (130 times). This indicates its superior global optimization capability. In terms of safety performance, the spatiotemporal joint algorithm successfully achieved zero planning collisions (0 times), compared to the quintic polynomial (11 times), B-spline curve (13 times), and Bezier curve (8 times), demonstrating its robustness in dynamic obstacle avoidance. In longitudinal motion control, the spatiotemporal joint algorithm maintained the lowest average longitudinal acceleration (0.0484 m/s2) and jerk (0.000766 m/s3), indicating its ability to generate smoother longitudinal motion trajectories. Although its average longitudinal velocity (6.03502 m/s) is slightly lower than that of the comparative algorithms, it achieves enhanced motion comfort through optimized acceleration control. In lateral dynamics, the spatiotemporal joint algorithm exhibited an average lateral velocity of -0.05945 m/s, which is comparable to the values of the comparative algorithms (-0.0537 to -0.0588 m/s), verifying its comparable lateral motion stability.

Overall, the experimental results demonstrate that the spatiotemporal joint algorithm achieves a better balance in planning efficiency, safety, and motion smoothness. It is particularly suitable for scenarios with high requirements for safety and multi-objective optimization, such as intelligent driving and complex dynamic environments. Despite its relatively lower computational efficiency, the zero planning collisions and minimal comprehensive cost in the planning phase make it a more optimal choice.

Ablation experiments of spatiotemporal joint algorithm

The spatiotemporal joint algorithm employs a curvature-adaptive discretization method to linearize the road. The ego-vehicle state, the coarse-grained trajectory output from the decision-making layer, and the predicted trajectories of perceived obstacles are all uniformly transformed into a piecewise Frenet frame. By dividing the continuously curved road into N quasi-linear segments, the influence of vehicle posture and road curvature on planning is reduced. Details are provided in Table 5.

Table 5 Ablation studies on the spatiotemporal joint algorithm.
Full size table

Based on the data from the comparative experiments, the joint spatiotemporal algorithm demonstrates superior comprehensive performance under the collaborative optimization of road curvature and vehicle attitude. In terms of motion control, the longitudinal velocity of the joint spatiotemporal planning algorithm (6.0350 m/s) is reduced by 4.00% compared with the non-integrated algorithm (6.2864 m/s), while the longitudinal acceleration (0.0484 m/s2) and jerk (0.00077 m/s3) are decreased by 3.01% and 9.41%, respectively, thereby effectively enhancing the smoothness of motion. The lateral dynamic indicators remain stable, with the lateral acceleration (0.0053 m/s2) and jerk (0.0048 m/s3) being reduced by 7.3%–25.9% compared with the non-joint spatiotemporal algorithm, indicating a significant improvement in the precision of lateral motion control. Regarding planning efficiency, the joint spatiotemporal algorithm achieves a 100% planning success rate across all scenarios with a planning time of 0.2096s, representing a 6.15% increase in success rate compared with the conventional algorithm. Meanwhile, the path cost is significantly reduced from 33.67 to 2.23. The experiments demonstrate that the collaborative optimization strategy integrating road geometric features and vehicle dynamic constraints can significantly enhance the safety and comfort of trajectory planning while ensuring real-time performance.

Trajectory planning experiments in dynamic scenarios

Trajectory planning in dynamic scenarios

To further enhance the validation of the reliability and applicability of our method and to more realistically simulate real-world driving scenarios, experiments involving dynamic obstacle scenarios (lane changing, overtaking, and multi-vehicle environments) are conducted in the following sections.

Dynamic scenario - Lane changing

We simulate the lane-changing behavior of vehicles, including both overtaking by lane changing and lane changing to avoid obstacles. In the overtaking scenario, the vehicle needs to change lanes at an appropriate time and accelerate to pass the preceding vehicle, thereby verifying the path planning capability and safety of our proposed method during lane changes. In the obstacle-avoidance scenario, the vehicle needs to change lanes promptly upon detecting an obstacle ahead to avoid a collision. While ensuring successful obstacle avoidance, the smoothness and comfort of the vehicle’s trajectory are also maintained. The specific simulation results are shown in Fig. 10(a) and (b), which respectively illustrate the lane-changing scenarios in the inner lane, middle lane, and outer lane.

Fig. 10
figure 10

Dynamic scenario - lane changing

Full size image
Dynamic scenario – Overtaking

We simulate the overtaking behavior of vehicles, including both single overtaking and consecutive overtaking scenarios. Fig. 11 illustrates the entire overtaking process, where Fig. 11(a) and (b) respectively show the planning processes of two consecutive overtaking maneuvers, and Fig. 11(c) shows the completion of the overtaking maneuver. In the single overtaking scenario, the vehicle needs to change lanes at an appropriate time and accelerate to pass the preceding vehicle. In the consecutive overtaking scenario, the vehicle needs to successively overtake multiple slow-moving vehicles ahead. These scenarios are designed to verify the path planning capability of our proposed method during overtaking maneuvers.

Fig. 11
figure 11

Dynamic Scenario – Overtaking.

Full size image
Dynamic scenario - Multi-vehicle environment

We simulate the scenario where the vehicle encounters multiple dynamic obstacles during its journey. The specific simulation results are shown in Fig. 12, where Fig. 12(a) represents entering a multi-vehicle environment, Fig. 12(b) represents being in the multi-vehicle environment, and Fig. 12(c) represents exiting the multi-vehicle environment. In these scenarios, the motion trajectories and velocities of the dynamic obstacles are randomly generated to simulate the uncertainties in real traffic conditions. These experiments are designed to verify the rapid response capability and path re-planning ability of our proposed method in the presence of dynamic obstacles.

Fig. 12
figure 12

Dynamic Scenario - Multi-Vehicle Environment.

Full size image

This study designed three typical dynamic scenarios: lane-changing scenarios, overtaking scenarios on a straight road, and overtaking scenarios on a curved road. By simulating complex traffic events such as intrusions from the oncoming lane and sudden braking of the preceding vehicle, the results of the experiments, as shown in Figures 10-12, demonstrate that the spatiotemporal joint algorithm performs well in dynamic obstacle avoidance, path smoothness, and planning success rate. In the lane-changing scenario, the target lane is intruded upon by an oncoming vehicle, and the ego vehicle needs to complete the lane change while ensuring safety. The spatiotemporal joint algorithm dynamically constructs a three-dimensional convex space in the S-L-T coordinate system, allowing for rapid trajectory adjustment in response to intrusions and achieving smooth lane changes (Fig. 10(b)). In the overtaking scenario on a straight road, the preceding vehicle is traveling at a low speed, and there is oncoming traffic in the adjacent lane. The spatiotemporal joint algorithm extends the prediction time domain to anticipate the trajectory of the oncoming vehicle and generates a collision-avoidance overtaking path (Fig. 11(b)). In the overtaking scenario on a curved road, which poses dual challenges of road curvature and dynamic obstacles, the spatiotemporal joint algorithm employs road segmentation and adaptive transformation of multiple coordinate systems to generate an optimized trajectory that conforms to the geometric characteristics of the curve (Fig. 12(b)).

Comparison of model planning performance in dynamic environments

To further validate the trajectory planning performance of the spatiotemporal joint model, this study compared it with several mainstream and representative trajectory planning algorithms, such as the Adaptive Bezier Algorithm40 and the Model Predictive Trajectory Control (MPTC) Algorithm41. The comparison was conducted from multiple aspects, including obstacle avoidance performance and passenger comfort (jerk and lateral acceleration fluctuations). By expanding the dimensions of result analysis and conducting a comprehensive evaluation through quantitative metric comparisons and visual presentations, the results are shown in Fig. 13.

Fig. 13
figure 13

Comparison of model planning performance in dynamic environments.

Full size image

To more clearly illustrate the performance of the algorithms, the basic driving parameters of the intelligent vehicle for both the proposed model and the comparison models were kept identical during the testing process. The longitudinal jerk, longitudinal acceleration, longitudinal velocity, lateral jerk, lateral acceleration, and lateral velocity were separately recorded. The detailed comparisons are shown in Table 6.

Table 6 Comparison of model planning performance in dynamic environments.
Full size table

Overall, the statistical results clearly demonstrate that in the dynamic scenarios, the proposed spatiotemporal integrated trajectory planning algorithm shows significant improvements in six out of the nine actual indicators evaluated for path planning safety, while the remaining three indicators experience a certain degree of decline. Compared with the MPTC method, the comprehensive cost is reduced by 41.02%, and the planning space is increased by 4.42%. Indicators such as longitudinal smoothness have also been enhanced. Although the lateral acceleration, lateral jerk, and computation time have increased to some extent, the absolute values of these increases are relatively small and have a minimal impact on the actual experience of drivers and passengers. Considering the overall cost comprehensively, the spatiotemporal integrated trajectory planning algorithm exhibits a distinct advantage.

Conclusions

Aiming at the limitations of traditional trajectory planning methods in complex road scenarios, this paper proposes a curvature-adaptive and spatiotemporal integrated trajectory planning algorithm for intelligent vehicles. The algorithm discretizes curved roads into quasi-straight segments using a road segmentation algorithm, constructs a hierarchical planning framework based on the joint optimization of multiple Frenet coordinate systems, and integrates Bezier curve optimization with a multi-objective cost function evaluation. This effectively addresses the key challenge of co-optimizing the dynamic coupling of spatiotemporal constraints and planning robustness.

Experiments show that, in terms of motion control, the spatiotemporal integrated planning algorithm reduces the longitudinal velocity by 4.00% compared to the non-integrated algorithm, with the longitudinal acceleration and jerk decreasing by 3.01% and 9.41%, respectively, thereby significantly improving motion smoothness. Lateral dynamic indicators remain stable, and the precision of lateral motion control is notably enhanced. In terms of planning efficiency, the spatiotemporal integrated algorithm achieves a 100% planning success rate across all scenarios with a planning time of 0.2096 ms. Compared to traditional algorithms, the success rate is increased by 6.15%, and the path cost is significantly reduced from 33.6714 to 2.2288, which is more in line with human driving habits. This algorithm provides a new technological approach for trajectory planning in structured roads for intelligent driving systems. Its divide-and-conquer strategy balances the trade-off between planning accuracy and real-time performance, offering high engineering application value.

Discussion

This framework provides a novel technological approach for trajectory planning in complex road scenarios for intelligent driving systems. Its divide-and-conquer strategy effectively balances the trade-off between planning accuracy and real-time performance, offering significant engineering application value. Future research will focus on further optimizing the computational efficiency of the algorithm to accommodate more complex dynamic environments and scenarios with higher real-time requirements. Additionally, efforts will simultaneously be devoted to enhancing the algorithm’s capability to handle dynamic obstacles and its adaptability to complex road conditions such as icy and snowy surfaces, thereby improving its applicability and reliability under intricate traffic scenarios and providing stronger support for the advancement of intelligent driving technologies.