Next Article in Journal
FDA-MIMO Radar Rapid Target Localization via Reconstructed Reduce Dimension Rooting
Previous Article in Journal
Internal Integrated Temperature Sensor for Lithium-Ion Batteries
Previous Article in Special Issue
Enhancing Off-Road Topography Estimation by Fusing LIDAR and Stereo Camera Data with Interpolated Ground Plane
 
 
Font Type:
Arial Georgia Verdana
Font Size:
Aa Aa Aa
Line Spacing:
Column Width:
Background:
Article

Spatio-Temporal Joint Trajectory Planning for Autonomous Vehicles Based on Improved Constrained Iterative LQR

1
National Engineering Laboratory for Electric Vehicles, Beijing Institute of Technology, Beijing 100081, China
2
State Key Laboratory of Advanced Design and Manufacturing Technology for Vehicle, Changsha 410082, China
*
Author to whom correspondence should be addressed.
Sensors 2025, 25(2), 512; https://doi.org/10.3390/s25020512
Submission received: 13 October 2024 / Revised: 5 January 2025 / Accepted: 7 January 2025 / Published: 17 January 2025

Abstract

:
With advancements in autonomous driving technology, the coupling of spatial paths and temporal speeds in complex scenarios becomes increasingly significant. Traditional sequential decoupling methods for trajectory planning are no longer sufficient, emphasizing the need for spatio-temporal joint trajectory planning. The Constrained Iterative LQR (CILQR), based on the Iterative LQR (ILQR) method, shows obvious potential but faces challenges in computational efficiency and scenario adaptability. This paper introduces three key improvements: a segmented barrier function truncation strategy with dynamic relaxation factors to enhance stability, an adaptive weight parameter adjustment method for acceleration and curvature planning, and the integration of the hybrid A* algorithm to optimize the initial reference trajectory and improve iterative efficiency. The improved CILQR method is validated through simulations and real-vehicle tests, demonstrating substantial improvements in human-like driving performance, traffic efficiency improvement, and real-time performance while maintaining comfortable driving. The experiment’s results demonstrate a significant increase in human-like driving indicators by 16.35% and a 12.65% average increase in traffic efficiency, reducing computation time by 39.29%.

1. Introduction

In recent years, the continuous development of autonomous driving technology has led to the application of autonomous driving functions in an increasing number of complex scenarios [1]. Trajectory planning is one of the critical technologies in autonomous driving [2]. By receiving upstream information, including obstacles, maps, decision results, and localization information, trajectory planning outputs a safe, smooth, and comfortable driving path point sequence, which maintains real-time performance. The trajectory directly impacts motion control output, thereby influencing the vehicle’s lateral and longitudinal dynamic movements [3]. Therefore, trajectory planning has a crucial impact on the comfort, safety, and human-like behavior of autonomous driving [4].
However, achieving autonomous driving in complex urban environments still presents several challenges. First, urban driving conditions are complex and variable, requiring trajectory planning that can effectively manage complex nonlinear constraints and respond in real time to dynamic obstacles [5]. Second, simultaneous optimization of temporal and spatial constraints is critical to meet the planned trajectory that meets safety, comfort, and human-like driving performance [6]. Additionally, it is crucial to ensure computational efficiency to enhance real-time performance.
To address challenges mentioned above, many scholars have conducted research on trajectory planning methods, including sampling-based, interpolation, and graph search techniques. While the problem of interpolation and sampling-based trajectory planning methods ensures solving efficiency, complex constraints are hardly involved in the problem-solving process [7]. In complex urban environments with multiple dynamic obstacles, sampling-based methods incur significant resource overhead [8]. The graph search method is flexible and highly generalizable; higher grid resolutions enhance the dynamic performance of autonomous vehicles [9], but they can severely impact real-time capabilities, especially in complex scenarios with multiple dynamic obstacles. Each execution cycle updates the grid map to account for dynamic obstacles, leading to significant computational overhead. The heuristic method [10] offers high adaptability and flexibility in complex environments and can achieve global optimization. However, its lack of model constraints and poor real-time performance, as seen in methods like GBSO, B-spline curves fail to satisfy vehicle model constraints under complex urban conditions [11].
Advancements in vehicle computing capabilities and machine learning now enable real-time deployment of neural network models on vehicle computing platforms to generate trajectories [12]. Neural networks have been successfully applied in path planning and provide a strong foundation for autonomous vehicle path planning [13,14]. Scholars have trained trajectory planning models using techniques such as imitation learning [15], reinforcement learning [16], deep reinforcement learning [17], and inverse reinforcement learning [18], validating them in simulations and real-world vehicles [19]. Neural networks inherently excel at addressing complex nonlinear issues, although validation results have been successful in specific scenarios; driving safety still cannot be assured, and interpretability issues persist.
Numerical optimization is the most widely used approach for trajectory planning, effectively addressing the aforementioned challenges. It formulates the planning problem as an optimization problem for constrained vehicle nonlinear dynamic systems, generating trajectories that meet constraints through dynamic mathematical optimization. This approach primarily includes nonlinear programming, convex optimization, and optimal control. Solution methods can be classified into two categories: direct and indirect approaches. Direct methods treat current trajectory points as decision variables, optimizing the trajectory by directly modifying these points. A common example is Sequential Quadratic Programming (SQP), typically implemented within a model predictive control framework that integrates various constraints and problem formulations for trajectory planning.
It is important to clarify that, in terms of constraint handling, trajectory planning can be classified into two types: spatio-temporal joint planning and spatio-temporal decoupled planning, or denoted as path velocity decomposition (PVD) and non-PVD method. The former converts a 3D problem into two 2D ones, which largely facilitates the solution process, and the latter directly plans trajectories, which has the underlying merit to improve the solution optimality, taking into account both lateral and longitudinal spatio-temporal constraints. This approach allows for a more comprehensive optimization of the vehicle’s trajectory, ensuring that it meets various constraints in both space and time [20].
Lin et al. [5] developed a trajectory planning strategy for emergency situations by integrating a model predictive controller with potential functions. This approach incorporates a sigmoid-based safety corridor into the MPC constraints to plan trajectories for emergency obstacle avoidance scenarios. Long Short-Term Memory (LSTM) networks, a type of deep neural network (DNN), are capable of extracting sequential features and managing complex dependencies. When combined with model predictive control, they improve prediction accuracy and are integrated into deep deterministic policy gradient algorithms, enhancing the robustness and efficiency of path planning, speeding up convergence, and reducing failure rates [21]. Several researchers have combined model predictive control with reinforcement learning methods. Additionally, deep reinforcement learning (DRL) components provide the capability to learn from experience and optimize control performance over time, allowing adaptation to new situations and real-time performance enhancement [22,23,24]. Zuo et al. [25] proposed a progressive model predictive control framework for trajectory planning, integrating model predictive control with artificial potential fields (APFs). This approach treats time-varying safety constraints as repulsive forces and uses an asymmetric lane potential field function to generate obstacle avoidance paths. Velocity planning is subsequently conducted along the path, and the resulting trajectories are utilized for trajectory tracking control. Svensson et al. [26] introduced an adaptive MPC-based trajectory planning method for situations where obstacles suddenly appear in front of the vehicle. This method updates the constraints in the optimization function based on real-time vehicle data, resulting in trajectories that consider time-varying tire friction constraints. However, a significant drawback of MPC-based trajectory planning is its high computational complexity. Each optimization cycle necessitates calculating the optimal trajectory over the entire prediction horizon, resulting in considerable resource waste, and it fails to ensure real-time performance for spatiotemporal trajectory planning issues.
Indirect methods generally use control inputs as decision variables, deriving the optimal trajectory through the forward propagation of the system’s dynamic equations. Differential dynamic programming (DDP) effectively addresses unconstrained optimal control problems [27] and manages upper and lower bounds on control inputs. Derived from dynamic programming theory and linear quadratic regulators (LQRs), iterative linear quadratic regulators (ILQRs) are well-suited for optimizing nonlinear systems and have demonstrated enhanced computational efficiency [28]. Chen et al. [29] introduced a CILQR by integrating comfort and safety constraints into the ILQR optimization problem. This approach enables motion planning for autonomous vehicles by improving the obstacle function to incorporate hard constraints into the optimization process [30]. Pan et al. [31] proposed a two-stage uncertainty-aware prediction method to improve the adaptability of trajectory planning across various scenarios. Building on a single-track model, Li et al. [32] improved the reliability of the kinematic model and ensured trajectory executability by incorporating higher-order terms, curvature rates, and longitudinal impacts. CILQR demonstrates advantages in tackling spatio-temporal trajectory planning problems, efficiently handling nonlinear complex constraints while maintaining high solving efficiency and rapid iteration speeds.
Nevertheless, certain aspects still require further improvement. To address these issues, we propose an improved CILQR method with three key features. (1) A new barrier function is introduced to mitigate stability challenges caused by excessive gradient variations in traditional barrier functions. (2) An adaptive weight parameter adjustment strategy is developed, enabling dynamic adjustment of the cost function weights based on vehicle states, thereby improving overall dynamic performance. (3) The hybrid A* algorithm is leveraged to create a new initial trajectory optimization approach, which generates collision-free initial trajectories more quickly, enhancing trajectory planning efficiency.
These improvements make the CILQR method more applicable to real-world autonomous driving scenarios. The proposed approach is validated through simulations and real-vehicle experiments across various scenarios, including the continuous nudge static obstacle, dynamic cut-in obstacle collision avoidance, dynamic overtaking of oncoming obstacles, and collision avoidance with dynamic and static obstacles. The results demonstrate that the enhanced CILQR effectively addresses complex driving situations that the original method struggles with. It is shown to be well-suited for autonomous driving in challenging urban environments, ensuring safety, comfort, and human-like driving behavior. The rest of this paper is organized as follows: Section 2 describes the problem, detailed features, the framework proposed in this paper, and trajectory optimization and constraints. Simulations, real-vehicle experiments, and analysis results are introduced in Section 3. The article concludes in Section 4.

2. Proposed Method

2.1. ILQR Method

Assume the vehicle model is a nonlinear discrete system described by x k + 1 = f ( x k , u k ) , where x k , u k denote the state and control sequences at time k , respectively. To tackle the trajectory planning problem, the ILQR method can be utilized for optimization based on the initial trajectory ( x k , u k ) provided from upstream. The goal is to identify a sequence of states and control inputs that minimizes the cost function.
m i n u 0 , , u N 1 x 0 , , x N J = L ( x N ) + k = 0 N 1 L x k , u k s . t .     x k + 1 = f ( x k , u k ) ,   x 0 = x s t a r t
where N denotes the number of optimization iterations, L ( x N ) is the cost function at time N , and x s t a r t refers to the initial planning point.
ILQR is an algorithm used to solve optimal control problems for nonlinear systems [33]. At each time step, it linearizes the system’s state equations using a Taylor expansion and approximates the cost function with a second-order expansion. The LQR is then applied to solve this approximated problem. By iteratively adjusting the control policy, the algorithm gradually converges to the optimal strategy. The solution to the ILQR problem primarily involves two components: the backward pass and the forward pass. The backward pass focuses on optimizing the control increments, while the forward pass updates the current trajectory based on the control increments and the vehicle model. We will first derive the computation process for the backward pass function. According to Bellman’s principle, we can define:
V N ( x N ) & = L ( x N )
V k ( x k ) & = m i n u { L ( x k , u k ) + V k + 1 ( f ( x k , u k ) ) }
where V k ( x k ) is the optimal cost-to-go start at k step, considering a small Perturbation, and define Q ( x k , u k ) = L ( x k , u k ) + V k + 1 ( f ( x k , u k ) ) , V k ( x k ) = m i n u { Q ( x k , u k ) } , by ignoring higher-order terms, getting an approximation of the Perturbation. The approximation of the Perturbation function δ Q x k , u k is convex with respect to the action increment as follows:
δ u k δ Q x k , u k = 0
We have the optimal action increment with respect to the optimal state increment:
δ u * = Q u u 1 Q u x δ x k + Q u = K δ x + d   d = Q u u 1 Q u K = Q u u 1 Q u x  
We can represent the optimal k-th step Perturbation with respect to δ x and get V first-order, second-order term, and increment:
V x = Q x + K T Q u u d + K T Q u + Q u x T d 2 V x 2 = Q x x + K T Q u u K + K T Q u x + Q u x T K Δ V = 1 2 d T Q u u d + d T Q u
At this stage, a single iteration of the backward pass is completed, and the iterative loop will persist until the cost function triggering condition is satisfied or until N = 1 . Each backward pass determines the optimal feedback gain for each time step, updating the state and control sequences x ¯ k , u ¯ k , according to the vehicle model. This process is referred to as the forward pass and is derived as follows:
x 0 = x s t a r t u k = u k + δ u k x k + 1 = f x k , u k  
where x k , u k denote the updated trajectory and control sequences, respectively. Due to approximations made in calculating the feedback gains, a line search method can further minimize the objective function. Typically, a backtracking approach is employed, referring to Equation (5), resulting in δ u k = K δ x + α d . Throughout the iterations, α is continually adjusted by multiplying it by a fixed ratio, α = ρ α , where 0 < ρ < 1 , thereby yielding a more optimal solution and avoiding overshoot.

2.2. Improved CILQR Method

ILQR depends on linearizing the system dynamics and applying a quadratic approximation to the cost function. This approach is effective for problems with little to no nonlinearity or constraints. However, when faced with complex nonlinearity and strict constraints, these approximations can result in greater linearization errors or inaccuracies in the quadratic approximation, leading to non-convergence of the optimization problem [34]. ILQR struggles with direct constraint handling since it lacks an inherent mechanism for this purpose. Moreover, added constraints apply only to specific time steps, and cumulative changes in trajectory during the forward pass can lead to constraint violations, complicating the management of complex control and state constraints. CILQR transforms inequality constraints into penalty terms and integrates them into the cost function, enabling the use of the ILQR algorithm framework for solving complex constraint problems, as shown in Figure 1. The improved CILQR process with three key features, which are marked in red, is presented. The CILQR problem can be expressed as follows:
m i n u 0 , , u N 1 x 0 , , x N J = L ( x N ) + k = 0 N 1 L x k , u k s . t . x k + 1 = f x k , u k , x 0 = x s t a r t g k ( x k , u k ) < 0 , g N ( x N ) < 0
where g k x k , u k denotes the inequality constraints on the state and control variables at time k , while g N ( x N ) indicates the terminal state constraint.
An ideal barrier function would be an indicator function, yielding a cost of zero when g k ( x k , u k ) < 0 and infinite otherwise. However, its non-differentiability prevents the calculation of the Hessian matrix. Consequently, it is commonly approximated using logarithmic or exponential functions, with modulation parameters ensuring it approaches positive infinity within a designated calibrated range, thereby achieving a hard constraint representation, which can be theoretically validated for convergence. For example, the typical forms of barrier functions utilizing exponential and logarithmic functions are as follows:
b k _ 1 x = q 1 e x p ( q 2 g k ( x k , u k ) )
b k _ 2 x = q 3 l o g q 4 ( q 5 g k ( x k , u k ) )
where q 1 ~ q 5 denote the parameters used to modulate the barrier function.
Regardless of the chosen barrier function, stability issues arise, leading to a trade-off between rigid obstacle avoidance constraints and numerical stability. If the barrier function’s curve changes too gently, it can result in violations of the constraints, breaching the hard constraints. Conversely, if the curve changes too abruptly, the barrier function may tend toward infinity near the constraint boundaries, leading to extreme sensitivity in the optimization problem and causing numerical solutions to become unstable or difficult to converge. While this characteristic of the barrier function is beneficial for ensuring obstacle avoidance safety, it simultaneously raises computational complexity and instability.
To resolve these challenges, a truncation strategy for the barrier function is established to avoid drastic gradient changes near the boundaries that could impact system stability. Given that the barrier function is generally monotonically increasing, upper and lower limits are enforced on its derivative. Furthermore, a piecewise strategy is employed to improve adaptability to various scenarios, and a dynamic relaxation factor is introduced to enhance stability in the optimization process.
b k _ 3 x = q m a x ,   b k _ 2 x > q u p b k _ 3 x = b k _ 2 x + ϵ k x k , u k , q l o w < b k _ 2 x q u p b k _ 3 x = q m i n , b k _ 2 x q l o w
where q m i n , q m a x denote the lower and upper bounds of the barrier function values, respectively, q l o w , q u p denote the lower and upper limits of the reciprocal of b k _ 2 x , respectively, and ϵ k x k , u k is the function of state and control sequence. To ensure second differentiability at the segmentation points, cubic spline interpolation is commonly used. This facilitates a second-order Taylor expansion, which is subsequently utilized during the backward pass for optimization.
The expected behavior of the vehicle in interaction with obstacles varies with factors such as current speed, relative speed, distance, road curvature, and obstacle type. For instance, at higher speeds, to maintain vehicle stability, the vehicle will strive to avoid generating significant lateral acceleration while allowing for greater control margins and planning a smoother curvature change in the trajectory. Consequently, it is necessary to dynamically adjust the weight parameters for lateral acceleration and curvature based on the current planned speed during the iterative process.
w a x = p a x 1 v x n e x p [ p a x 2 a x n 2 + v x n 4 κ x n 2 ] + p a x 3
w κ ˙ = p κ ˙ 1 v x n e x p [ p κ ˙ 2 a x n 2 ] + p κ ˙ 3 v n e x p [ p κ ˙ 4 x n ] + p κ ˙ 5
where p a x 1 ~ p a x 3 and p κ ˙ 1 ~ p κ ˙ 5 are calibratable parameters, and v x n , a x n , κ x n , v n , and x n denote the vehicle speed, longitudinal acceleration, curvature, velocity difference with the target vehicle, and relative distance at the n-th trajectory point, respectively. Current dynamic control margins are represented by lateral and longitudinal accelerations. Based on the kinematic assumption v y n = v x n 2 κ x n , when the acceleration vector of the planned trajectory is large, a penalty term for longitudinal acceleration requests is increased, which also grows with the current speed. Conversely, when the longitudinal acceleration is high and relative speed and distance are low, the penalty term for curvature change rate is also increased, scaling with the current speed.
The quality of the initial trajectory significantly impacts the reduction in iteration count, enhances solution efficiency, and ensures the real-time performance of trajectory planning. The initial trajectory is generated using the hybrid A* method, followed by speed planning based on simple kinematic equations and interpolation techniques. The hybrid A* algorithm combines Dijkstra’s algorithm and breadth-first search (BFS). The heuristic function h ( n ) needs to account for both the cost from the current point to the target point and the cost associated with deviations in heading angle to enable initial obstacle avoidance. Therefore, it can be defined as follows:
h ( n ) = ( x x g o a l ) 2 + ( y y g o a l ) 2 + p h ( θ θ g o a l ) 2 1 2
where p h represents the weight for heading angle errors, which is increased when lateral avoidance of obstacles is required or when approaching the target point. If the curvature of the reference trajectory exceeds a certain threshold, indicating that the vehicle must execute turns or U-turns, the cost related to heading angle errors can be ignored. Consequently, p h can be expressed as follows:
p h = p h s , if   λ < λ l p h s ( λ u λ ) λ u λ l ,   if   λ l < λ < λ u 0 ,   if   κ > κ c
where λ represents the ratio of the distance error to the obstacle and the angle error at the current node and λ l ,   λ u denote the lower and upper limits, respectively. κ c denotes the upper limit of curvature for turning scenarios.
To enhance the efficiency of Hybrid A* searches in driving scenarios and reduce invalid searches caused by excessive heading angles, heading angle constraints are applied by considering the kinematic characteristics of the vehicle and road edges. Special attention is given to obstacle positions when constraining branching directions of the heading angle.
| Δ θ | m i n   ( a b s θ R , tan 1 L R m i n )
where θ R denote the heading angle from the ego vehicle to the road edge, the second term on the right side of the equation denotes the limitation of the vehicle kinematic, and R m i n denote the minimum of the turning radius.
The improved CILQR algorithm structure has been developed by refining the barrier function, adaptive weight adjustment methods, and initial trajectory optimization. First, the current raw trajectory is optimized by using the hybrid A* method to generate a basic obstacle avoidance path, thus enhancing the efficiency of trajectory iteration. After initializing the trajectory, a warm start is applied to initialize the matrix, which reduces stability issues during the backward pass. Prior to conducting the backward pass solution, the inequality constraints must be converted into cost function terms. During each loop of the backward pass iteration, the weight coefficients will be updated to match the cost function calculations at each path point. The loop termination conditions mainly take into account several factors, such as the cost function difference between iterations, the set maximum iteration count, and the rate of change in control gains. The pseudocode for the CILQR algorithm is presented in Algorithm 1.
Algorithm 1. Improved CILQR Algorithm
1: 
Output: Trajectory vector and control sequence x , u
2: 
//enable hybrid A* method; using the PI controller, get the speed profile
3: 
InitRefPath: x 0 , u 0 , tolerance
4: 
// calculate the matrix in advance based on the initial trajectory to prevent oscillations
5: 
Warmstart: δ Q x 0 , u 0 x 0 , u 0
6: 
While | Δ J | < tolerance || stepcounter > threshold || feedforward gain ≈ 0
7: 
  do
8: 
   b k _ 3 x , b k _ 3 u g k ( x k , u k ) // convert inequality constraints into cost functions
9: 
   J J
10: 
    K , d , Δ V BACKWARDPASS( x , u )
11: 
    AdaptiveWeight: w a x , w κ ˙ // adaptive weight tuning function
12: 
    X , U , J FORWARDPASS( x , u , K , d , Δ V , J )
13: 
     α = ρ α
14: 
return  x , u , J

2.3. Problem Formulation

The trajectory planning problem is established using a kinematic model. To account for lateral and longitudinal coupling simultaneously, the vehicle model is defined as X k + 1 = f X , U , where X = [ x , y , v x , a x , ψ , κ ] T denotes the state vector and U = [ j x , κ ˙ ] T denotes the control vector [35]. The updated states are given as follows:
x k + 1 = x k + c o s ψ k d s y k + 1 = y k + s i n ψ k d s v x k + 1 = v x k + a x k T + 1 2 j x k T 2 a x k + 1 = a x k + j x k T ψ k + 1 = ψ k + κ d s + 1 2 κ k a x k + v x k κ ˙ T 2 + 1 3 1 2 κ k j k + a x k κ ˙ T 3 + 1 8 κ ˙ j T 4 κ k + 1 = κ k + κ ˙ T
where T denotes the time step. Assuming that the longitudinal jerk j x and the curvature rate k ˙ ˙ vary linearly and remain constant during time T , we have j x = d a x / d T and k ˙ = d k / d T . By applying the small-angle assumption for the vehicle’s movement distance d s , we can obtain:
d s = v x T + 1 2 a x T 2 + 1 6 j x T 3
To simplify the model, some researchers assume constant curvature k , resulting in ψ k + 1 = ψ k + k d s . However, if the curvature rate is not considered, abrupt curvature changes can occur during forward propagation, which negatively impacts comfort and safety. We then differentiate the lateral jerk of the vehicle:
j y = d d t v x 2 κ = 2 v x κ a x + v 2 L s e c 2 δ δ ˙ = 2 v x κ a x + v x 2 κ ˙
The above equation suggests that the curvature rate can be approximated as the steering wheel rate, which justifies setting κ ˙ as a control input. To guarantee the feasibility of the planned trajectory, steering speed limits vary slightly with different velocities, and the vehicle’s minimum turning radius constraint must also be accounted for. Thus, the steering angle input can be substituted with a curvature rate constraint as the lateral control input, ensuring both controllability and comfort.
(1)
Cost function-distance to the reference line
The reference line serves as a rough solution generated from decision-making processes informed by environmental perception. Excessive deviation from this line may pose safety risks. At time step k , the objective function is formulated as follows:
c r ( k ) = w r x x r 2 y y r 2 2
where x r , y r denote the coordinates of the closest point on the reference line derived from upstream inputs and w r is a calibratable weight. x , y denote the longitudinal and lateral coordinates at time step k , To enhance clarity, subscripts k have been omitted from all state and control variables in the objective function.
(2)
Cost function-lateral motion
The lateral movement of the vehicle plays a crucial role in its comfort and safety. Consequently, the objective function is designed to consider the vehicle’s lateral speed, lateral acceleration, and lateral jerk:
c v y k = w v y v x ψ ψ r e f 2 c a y k = w a y v x 2 κ 2 c j y ( k ) = w j y 2 a x v x κ + v 2 κ ˙ 2
where ψ r e f denotes reference heading angle calculated from the reference line. Assuming the heading angle error is minimal, we approximate v ψ ψ r e f as the vehicle’s lateral speed. The parameters w v y , w a y , w j y are calibratable weights.
(3)
Cost function-longitudinal motion
To ensure the vehicle’s longitudinal comfort and safety, it is crucial to consider the objective function related to the vehicle’s longitudinal motion, focusing on longitudinal speed, acceleration, and jerk:
c v x k = w v x v x v r e f 2 c a x k = w a x a x 2 c j x ( k ) = w j x j x 2
where v r e f denotes the reference speed initially assigned using a heuristic algorithm, which takes into account the set speed, the current vehicle speed, and the speeds of surrounding dynamic obstacles. The parameters w v y , w a y , w j y are calibratable weights.
(4)
Cost function-trajectory curvature
The steering wheel rate can be approximated as the curvature rate. Rapid steering can significantly reduce comfort. Thus, we define the objective function for the curvature rate as follows:
c κ ( k ) = w κ ˙ κ ˙ x 2
where w κ ˙ denotes the calibratable weight of curvature.
The inequality constraints primarily encompass collision risk constraints for safety, specifically the distance between the bounding boxes of static and dynamic obstacles and the inflated body of the vehicle:
i = 0 N D s a f e P i P i m a t c h e d W C _ 1 P i P i m a t c h e d D C _ 1
where D s a f e considers the safety distance between the vehicle and obstacles at various speeds. The terms P i , P i m a t c h e d represent the distances between the vehicle’s current location and the nearest matched point, typically corresponding to the front and rear safety positions. W C _ 1 is a calibratable matrix.
Given the capability limits of the vehicle’s chassis actuators—such as maximum turning radius, steering wheel speed, and drive torque—it is possible to derive corresponding state constraint boundaries based on the system’s defined limits. Furthermore, by considering the vehicle’s stability boundaries, constraints on maximum lateral acceleration and yaw rate can be established; the following constraints must be satisfied:
v x < v m a x a m i n < a x < a m a x j m i n < j x < j m a x ψ P ( ψ ) < ψ d i f f _ m a x κ ˙ < κ ˙ m a x κ < κ m a x κ v x 2 < a y _ m a x 2 κ a x v x + κ ˙ v x 2 < j y _ m a x κ a + v κ ˙ < ω m a x
where P ( ψ ) denotes the trajectory heading angle at the current matched point. It is essential to address the heading angle error to enhance continuity and possibility in scenarios with small turning radii. Under the assumptions of the vehicle’s kinematic model, κ v x 2 can be approximated as lateral acceleration, while 2 κ a x v x + κ ˙ v x 2 can be similarly equated to lateral jerk. Considering ψ ¨ κ a + v κ ˙ , constraints can be applied to the vehicle’s yaw rate.

3. Experiment Validation

The efficacy of the improved CILQR method is demonstrated through the establishment of a simulation and real-vehicle testing platform. This paper details the development of an autonomous driving system based on ROS, deployed on a high-performance workstation equipped with an Intel Core i9-10900K processor (Intel, Shanghai, China), running a suitable Linux system to facilitate both simulation and real-vehicle tests.
During the simulation tests, data from real vehicles is collected to construct simulation scenarios in OSM (OpenStreetMap) format. Using actual vehicle parameters and the layered decoupling principle, a high-fidelity vehicle dynamics model is created, with key parameters presented in Table 1. A lateral and longitudinal coupled controller is designed using model predictive control for trajectory tracking. Additionally, a simulation interface module (Vehicle Interface) is created to receive various inputs, including lateral and longitudinal control commands, gear information, vehicle position data, key parameter configurations, and essential coordinate transformation matrices. This setup allows for the output of the vehicle’s current speed, position, and acceleration, facilitating closed-loop simulation.
To perform real-vehicle validation, a perception system, localization, mapping, and decision-making control systems are deployed using a specific brand of electric passenger car, as shown in Figure 2a,b, which show the autonomous driving vehicle communication topology diagram, which employs the related hardware component. The improved CILQR method is implemented on an industrial computer (IC), which communicates with the vehicle’s OBD to gather essential vehicle status information and obtains upstream inputs for trajectory planning via Ethernet (ETH). This facilitates trajectory planning, with the deployed trajectory tracking controller calculating the longitudinal and lateral control quantities. The IC transmits these control quantities to the vehicle’s chassis actuators through the On-Board Diagnostics (OBDs) interface for trajectory tracking control, thus completing real-vehicle testing. Xavier is primarily responsible for processing LiDAR data to generate point cloud information for autonomous vehicles. The INS provides precise positioning information for the vehicle.

3.1. Simulation Experiment

A Linux-based operating system with ROS is deployed, enabling the creation of a master node and the definition of interfaces and message structures for key algorithm modules to facilitate seamless inter-node communication. High-precision maps are imported, and obstacle traffic flow information is generated using simple object publish. This setup triggered data streams for the algorithm modules. Control signals are then transmitted to the vehicle dynamics model, facilitating closed-loop simulation testing, as illustrated in Figure 3. Simulation debugging provided the weight parameters listed in Table 2, revealing that p a x 2 , p κ ˙ 2 , p κ ˙ 4 have a pronounced impact on driving style. Excessively aggressive or conservative strategies notably affect both traffic efficiency and the sense of safety.

3.1.1. Continuous Nudge Static Obstacle Scenario

Urban environments, like narrow roads and static obstacles, with limited detour space, are quite familiar. A single-lane scenario is established, with two static vehicles acting as obstacles on either side of the lane. The ego vehicle travels at a high speed, and a detour trajectory on the narrow road is required, necessitating the planning of a reasonable and continuous obstacle-avoidance trajectory for a smooth and comfortable trajectory, as shown in Figure 4a,b.
It shows that the initial trajectory optimization using the hybrid A* method enabled the improved CILQR method to plan slight heading angle adjustments as the vehicle approached obstacles. This led to minor lateral adjustments, avoiding large lateral trajectory shifts and related discomfort. Adaptive adjustment of the weight parameters significantly enhanced the longitudinal speed planning. Starting from 8 s, the speed gradually decreased in a linear fashion, producing a smoother and more continuous speed curve. This avoided sharp changes in lateral and longitudinal acceleration and jerk, ensuring comfort throughout the drive. Consequently, the average speed increased, improving passage efficiency. The time to navigate around the last obstacle improved from 21.3 s to 18.9 s, as shown in Figure 4c. Figure 4d indicates that during the obstacle avoidance process, the trajectory heading angle changes with the improved CILQR method are smoother. In the X = [150, 160] range, the lateral maneuver is smaller, with a peak heading angle of 0.056 rad. The planned curvature remains low, not exceeding 0.035 m¹, and it eliminates oscillations in the curvature due to avoidance needs. The maximum lateral and longitudinal accelerations are kept below 0.039 m/s2 and 1.2 m/s2, respectively, as shown in Figure 4e,f.
To enable comparison with mainstream methods, the EM planner, based on the literature [36], is implemented. It employs dynamic programming for path planning in the Frenet coordinate system and quadratic programming for speed planning, effectively decoupling the spatio-temporal joint planning problem. The EM planner relies on a sampling-based approach for path planning, demonstrating effective obstacle avoidance in the initial simulation phase. However, stability issues inherent to sampling-based methods lead to curvature oscillations. Combined with high vehicle speeds, this causes significant lateral acceleration, adversely affecting comfort. In contrast, the CILQR method incorporates deceleration planning, maintaining comfort even with larger curvature changes. This underscores the drawbacks of hierarchical decoupled trajectory planning in addressing unified spatio-temporal constraints in these scenarios.

3.1.2. Dynamic Cut-In with Narrow Corridor Scenario

Mixed traffic flows, especially with non-motorized vehicles like e-bikes or pedestrians, often present high flexibility and unpredictability. In scenarios with limited space for maneuvering, handling dynamic obstacles that suddenly merge in front of the ego vehicle requires maintaining safety while ensuring efficient passage. A three-lane scenario is created, as shown in Figure 5a,b. It includes two dynamic obstacles: a dynamic vehicle traveling alongside the ego vehicle and an e-bike nudge around a static obstacle. The e-bike’s maneuver and the parallel dynamic vehicle create a merging situation, resulting in a critical space for collision avoidance.
Comparing the ego vehicle’s trajectory time sequence reveals that when the e-bike cut in the ego lane (around 5 s), the improved CILQR method plans the lateral offset trajectory to lead the vehicle to make a slight lateral shift, effectively reducing collision risks. The ego vehicle overtakes the e-bike after 8 s, gradually returning to the lane centerline. The simulation time of the proposed method improved from 20 s to 17.2 s, allowing the vehicle to reach the target point 2.8 s earlier.
Figure 5c,d show that adjusting the adaptive barrier function results in a smoother and more continuous trajectory curvature. The longitudinal speed profile also exhibits significant convergence, decreasing the vehicle’s maximum deceleration from −3.60 m/s2 to −0.95 m/s2, as depicted in Figure 5f. Although a lateral acceleration of 0.51 m/s2 is observed, as shown in Figure 5e, the absolute magnitude is small, and the resulting jerk is minimal, thus having a limited impact on comfort performance.
Similar to the last scenario, the EM planner exhibits substantial curvature oscillations during path planning. This occurs because the longitudinal speed planning does not factor in the comfort costs induced by curvature variations, leading to the maintenance of high vehicle speeds and, consequently, significant lateral acceleration, which cause a critical impact on comfortable performance.

3.2. Real-Vehicle Experiment

3.2.1. Dynamic Overtaking of Oncoming Obstacles Scenario

Figure 6a shows two dynamic obstacles moving toward the ego vehicle in the same lane, with one oncoming obstacle straddling the lane and partially encroaching into the ego vehicle’s path, creating a collision risk. To avoid a collision, the ego vehicle plans a lateral shift away from the lane centerline, steering around the obstacle. After the maneuver, the vehicle returns to the lane centerline. To clearly illustrate the obstacle’s movement, only the first 10 s of its trajectory are shown.
Within the initial 8 s, the ego vehicle maintains a normal path within its lane. Significant changes in the trajectory curvature and speed occur. As shown in Figure 6b, during the avoidance maneuver between 8 s and 12 s, adjustments to the steering angle and deceleration occur almost simultaneously, effectively avoiding a collision with the oncoming obstacle. After 12 s, the ego vehicle passes the last obstacle and returns to the lane centerline. The dynamic adjustment of weight parameters allows adaptive tuning of the deceleration weight when the obstacle is near, ensuring a sense of safety during driving. Figure 6c shows the speed profile and curvature changes of the planned trajectory. It indicates that the lateral avoidance maneuver for the obstacle is initiated around the same time. From 9 s, the curvature of the planned trajectory significantly changes, reaching a peak of 9.2 × 10−3 m−1. Figure 6d shows the vehicle’s dynamic state, with maximum lateral and longitudinal accelerations of 1.05 m/s2 and 0.94 m/s2, respectively. During lateral maneuvers—whether during the initial avoidance or while returning to the lane center after passing the obstacle—the lateral jerk remains below 2.0 m/s3. These dynamic changes ensure a comfortable ride, resulting in more human-like behavior.

3.2.2. Collision Avoidance with Dynamic and Static Obstacles Scenario

Figure 7a shows a scenario involving both dynamic and static obstacles. The ego vehicle confronts an opposite dynamic obstacle in the same lane, while a static obstacle, a pedestrian, partially blocks the ego vehicle’s path. The planned trajectory should meet safety requirements while maximizing comfort and enhancing traffic efficiency. Consequently, the ego vehicle must assess the available space for maneuvering. If the space is inadequate, it must decelerate and brake until sufficient room is available to execute lateral avoidance maneuvers, thus enhancing the safety of other road users and passengers.
Figure 7b shows that the ego vehicle decelerates to reduce collision risk with the static obstacle before 7.2 s, adjusting its trajectory in the ego right direction to increase lateral space for avoiding the oncoming dynamic obstacle. This adjustment results in a lower speed profile, as shown in Figure 7c. Ego vehicle crossing with the oncoming vehicle, with the longitudinal acceleration request according to the speed profile, and simultaneous nudge around the static obstacle.
Figure 7d shows the vehicle’s dynamic states. Although the ego vehicle experiences a peak longitudinal deceleration of nearly −1.8 m/s2, the extended deceleration period leads to a gentle longitudinal jerk of −3.3 m/s3, maintaining driver comfort. During the dynamic obstacle avoidance, the limited lateral space results in moderate lateral acceleration with minimal impact on comfort. Even in the acceleration phase, the lateral acceleration remains below −0.8 m/s2, and the lateral jerk does not exceed 2.01 m/s3.

3.3. Analysis of Results

Human-like behavior is a key characteristic for enhancing autonomous driving performance. Scholars have investigated metrics for human-like behavior in autonomous driving, generally considering two dimensions: vehicle dynamics and decision-making in relation to obstacles. Given that trajectory planning mainly influences the vehicle’s longitudinal and lateral motion characteristics, it is important to define human-like metrics based on the vehicle’s dynamic features.
Drawing on Makoto Yamakado’s research on the operational behaviors of professional drivers [37], it is evident that professional driving incorporates more simultaneous longitudinal and lateral actions, leading to smoother longitudinal and lateral accelerations. The G-G plot of professional drivers typically resembles a circular shape, suggesting increased coupling between longitudinal and lateral operations. Consequently, the following human-like metrics should be established:
f h l = i = 0 N q a x a x q a y a y a x 2 + a y 2 , a x < a x _ m a x , a y < a y _ m a x
where q a x , q a y denote the calibration parameter for tuning the driving style, like aggressive or moderate. The equation indicates that a smaller f h l value signifies a reduced error in longitudinal and lateral acceleration. This means that the operational point is closer to the diagonal of the G-G diagram, leading to a trajectory profile that appears more circular, which means the human-like character is more obvious. Furthermore, a quantitative analysis of comfort must consider trajectory planning metrics, such as curvature and curvature rate, as well as vehicle dynamic states, including lateral and longitudinal acceleration and yaw rate.
As shown in Table 3, analysis of simulation experiments indicates that the improved CILQR method results in significantly smoother trajectories across two simulation scenarios. The maximum curvature and rate of change in curvature decreased by 38.37% and 67.71% on average, while the maximum longitudinal deceleration dropped by 66.78%. Although lateral acceleration and yaw rate increased slightly under certain conditions, lateral comfort remained consistent. Human-like driving indicator analysis indicates improvements of 9.52% and 23.17% across two simulation conditions. Additionally, traffic efficiency increased by 11.3% and 14.0% in the respective scenarios.
Real-vehicle experiments analysis yield similar conclusions. The planned trajectory curvature and curvature change rates in both test scenarios remained low, staying below 1.0 × 10−2 m¹ and 4.7 × 10−3 m−2. This balance ensures high traffic efficiency while maintaining good lateral and longitudinal comfort performance. In interactive scenarios, human-like driving indicators remained below 0.70, providing an enhanced human-like driving experience. The improved CILQR method’s 99% iteration number N 99 t h decrease almost 45.6%, hence computation time T 99 t h achieved an average speedup of 39.29%, meeting real-time trajectory planning requirements. Notably, each trajectory contains 100 waypoints and extends no more than 200 m.

4. Conclusions

This study proposes an improved CILQR method featuring three key features to optimize spatio-temporal joint trajectory planning in complex urban scenarios. The barrier function is optimized, a dynamic weight adjustment strategy is introduced, and the initial trajectory is refined using the hybrid A* method. The trajectory planning problem is formulated using a cost function and constraints, and an autonomous driving simulation and vehicle platform are developed. A human-like indicator is defined to evaluate the effectiveness of the improved CILQR method in spatio-temporal joint planning for complex urban scenarios. The main conclusions of this study are as follows:
(1)
First, by optimizing the barrier function, the CILQR method achieves more efficient constraint handling. This enhancement allows for better trajectory adaptation while maintaining safety and comfort, leading to improved numerical stability and a 12.65% average increase in traffic efficiency in complex scenarios.
(2)
Second, the adaptive weight adjustment strategy enhances the CILQR method’s adaptability across different scenarios, improving lateral and longitudinal control coordination and increasing the human-like driving indicator by 16.35%.
(3)
Integrating the hybrid A* method for initial trajectory preprocessing improves solution efficiency, reducing computation time by 39.29%.
Future study will focus on refining evaluation methods for human-like metrics, providing guidance for further optimization. Additionally, we plan to develop human-like models with diverse driving styles using machine learning, enabling adaptive weight adjustments to customize trajectory planning for individual driver preferences.

Author Contributions

Methodology, Q.L.; Formal analysis, Q.L.; Data curation, Y.W.; Writing—original draft, Q.L.; Writing—review & editing, Q.L.; Supervision, H.H.; Funding acquisition, M.H. All authors have read and agreed to the published version of the manuscript.

Funding

This research was funded by the Science Fund of State Key Laboratory of Advanced Design and Manufacturing Technology for Vehicle (Grant No. 32215008).

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

The data that has been used in this study is confidential.

Conflicts of Interest

The authors declare no conflict of interest.

References

  1. Tang, Y.; He, H.; Wang, Y.; Mao, Z.; Wang, H. Multi-Modality 3D Object Detection in Autonomous Driving: A Review. Neurocomputing 2023, 553, 126587. [Google Scholar] [CrossRef]
  2. Xia, T.; Chen, H. A Survey of Autonomous Vehicle Behaviors: Trajectory Planning Algorithms, Sensed Collision Risks, and User Expectations. Sensors 2024, 24, 4808. [Google Scholar] [CrossRef] [PubMed]
  3. Teng, S.; Hu, X.; Deng, P.; Li, B.; Li, Y.; Ai, Y.; Yang, D.; Li, L.; Zhe, X.; Zhu, F.; et al. Motion Planning for Autonomous Driving: The State of the Art and Future Perspectives. IEEE Trans. Intell. Veh. 2023, 8, 3692–3711. [Google Scholar] [CrossRef]
  4. Li, Q.; Ou, B.; Liang, Y.; Wang, Y.; Yang, X.; Li, L. TCN-SA: A Social Attention Network Based on Temporal Convolutional Network for Vehicle Trajectory Prediction. J. Adv. Transp. 2023, 2023, 1286977. [Google Scholar] [CrossRef]
  5. Lin, P.; Tsukada, M. Model Predictive Path-Planning Controller with Potential Function for Emergency Collision Avoidance on Highway Driving. IEEE Robot. Autom. Lett. 2022, 7, 4662–4669. [Google Scholar] [CrossRef]
  6. Parekh, D.; Poddar, N.; Rajpurkar, A.; Chahal, M.; Kumar, N.; Joshi, G.P.; Cho, W. A Review on Autonomous Vehicles: Progress, Methods and Challenges. Electronics 2022, 11, 2162. [Google Scholar] [CrossRef]
  7. Gammell, J.D.; Barfoot, T.D.; Srinivasa, S.S. Informed Sampling for Asymptotically Optimal Path Planning. IEEE Trans. Robot. 2018, 34, 966–984. [Google Scholar] [CrossRef]
  8. Strub, M.P.; Gammell, J.D. Adaptively Informed Trees (AIT*) and Effort Informed Trees (EIT*): Asymmetric Bidirectional Sampling-Based Path Planning. Int. J. Robot. Res. 2022, 41, 390–417. [Google Scholar] [CrossRef]
  9. Maw, A.A.; Tyan, M.; Lee, J.W. iADA*: Improved Anytime Path Planning and Replanning Algorithm for Autonomous Vehicle. J. Intell. Robot. Syst. 2020, 100, 1005–1013. [Google Scholar] [CrossRef]
  10. Abdulsaheb, J.A.; Kadhim, D.J. Classical and Heuristic Approaches for Mobile Robot Path Planning: A Survey. Robotics 2023, 12, 93. [Google Scholar] [CrossRef]
  11. Zhou, Q.; Gao, S.; Qu, B.; Gao, X.; Zhong, Y. Crossover Recombination-Based Global-Best Brain Storm Optimization Algorithm for UAV Path Planning. Proc. Rom. Acad. Ser. A-Math. Phys. Tech. Sci. Inf. Sci. 2022, 23, 207–216. [Google Scholar]
  12. Aradi, S. Survey of Deep Reinforcement Learning for Motion Planning of Autonomous Vehicles. IEEE Trans. Intell. Transp. Syst. 2022, 23, 740–759. [Google Scholar] [CrossRef]
  13. Zhong, Y.; Shirinzadeh, B.; Yuan, X. Optimal Robot Path Planning with Cellular Neural Network. Int. J. Intell. Mechatron. Robot. 2011, 1, 20–39. [Google Scholar] [CrossRef]
  14. Hills, J.; Zhong, Y. Cellular Neural Network-Based Thermal Modelling for Real-Time Robotic Path Planning. Int. J. Agil. Syst. Manag. 2014, 7, 261. [Google Scholar] [CrossRef]
  15. Sun, L.; Peng, C.; Zhan, W.; Tomizuka, M. A Fast Integrated Planning and Control Framework for Autonomous Driving via Imitation Learning. arXiv 2017, arXiv:1707.02515. [Google Scholar]
  16. Li, J.; Sun, L.; Chen, J.; Tomizuka, M.; Zhan, W. A Safe Hierarchical Planning Framework for Complex Driving Scenarios Based on Reinforcement Learning. arXiv 2021, arXiv:2101.06778. [Google Scholar]
  17. Perot, E.; Jaritz, M.; Toromanoff, M.; Charette, R.D. End-to-End Driving in a Realistic Racing Game with Deep Reinforcement Learning. In Proceedings of the Computer Vision & Pattern Recognition Workshops, Honolulu, HI, USA, 21–26 July 2017. [Google Scholar]
  18. Wu, Z.; Sun, L.; Zhan, W.; Yang, C.; Tomizuka, M. Efficient Sampling-Based Maximum Entropy Inverse Reinforcement Learning with Application to Autonomous Driving. arXiv 2020, arXiv:2006.13704. [Google Scholar] [CrossRef]
  19. Wang, Y.; Tan, H.; Wu, Y.; Peng, J. Hybrid Electric Vehicle Energy Management with Computer Vision and Deep Reinforcement Learning. IEEE Trans. Ind. Inform. 2021, 17, 3857–3868. [Google Scholar] [CrossRef]
  20. Han, Z.; Wu, Y.; Li, T.; Zhang, L.; Pei, L.; Xu, L.; Li, C.; Ma, C.; Xu, C.; Shen, S.; et al. An Efficient Spatial-Temporal Trajectory Planner for Autonomous Vehicles in Unstructured Environments. IEEE Trans. Intell. Transp. Syst. 2024, 25, 1797–1814. [Google Scholar] [CrossRef]
  21. Ramezani, M.; Habibi, H.; Sanchez-Lopez, J.L.; Voos, H. UAV Path Planning Employing MPC-Reinforcement Learning Method Considering Collision Avoidance. In Proceedings of the 2023 International Conference on Unmanned Aircraft Systems (ICUAS), Warsaw, Poland, 6 June 2023; IEEE: Piscataway, NJ, USA; pp. 507–514. [Google Scholar]
  22. Pfrommer, S.; Gautam, T.; Zhou, A.; Sojoudi, S. Safe Reinforcement Learning with Chance-Constrained Model Predictive Control. In Proceedings of the Learning for Dynamics and Control Conference, Stanford, CA, USA, 23–24 June 2022; pp. 291–303. [Google Scholar]
  23. Wang, Y.; Wu, Y.; Tang, Y.; Li, Q.; He, H. Cooperative Energy Management and Eco-Driving of Plug-in Hybrid Electric Vehicle via Multi-Agent Reinforcement Learning. Appl. Energy 2023, 332, 120563. [Google Scholar] [CrossRef]
  24. Wang, Y.; He, H.; Wu, Y.; Wang, P.; Wang, H.; Lian, R.; Wu, J.; Li, Q.; Meng, X.; Tang, Y.; et al. LearningEMS: A Unified Framework and Open-Source Benchmark for Learning-Based Energy Management of Electric Vehicles. Engineering 2024, S2095809924007136. [Google Scholar] [CrossRef]
  25. Zuo, Z.; Yang, X.; Li, Z.; Wang, Y.; Han, Q.; Wang, L.; Luo, X. MPC-Based Cooperative Control Strategy of Path Planning and Trajectory Tracking for Intelligent Vehicles. IEEE Trans. Intell. Veh. 2021, 6, 513–522. [Google Scholar] [CrossRef]
  26. Svensson, L.; Bujarbaruah, M.; Kapania, N.; Törngren, M. Adaptive Trajectory Planning and Optimization at Limits of Handling. In Proceedings of the 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China, 3–8 November 2019. [Google Scholar]
  27. Su, J.; Gao, R.; Xu, Z.; Zhang, W. VS-DDP: Variable Splitting Based Differential Dynamic Programming Algorithm for Autonomous Trajectory Planning. In Proceedings of the 2023 42nd Chinese Control Conference (CCC), Tianjin, China, 24–26 July 2023; pp. 4382–4387. [Google Scholar]
  28. Ma, J.; Cheng, Z.; Zhang, X.; Lin, Z.; Lewis, F.L.; Lee, T.H. Local Learning Enabled Iterative Linear Quadratic Regulator for Constrained Trajectory Planning. IEEE Trans. Neural Netw. Learn. Syst. 2023, 34, 5354–5365. [Google Scholar] [CrossRef]
  29. Chen, J.; Zhan, W.; Tomizuka, M. Constrained Iterative LQR for On-Road Autonomous Driving Motion Planning. In Proceedings of the 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC), Yokohama, Japan, 16–19 October 2017; IEEE: Piscataway, NJ, USA, 2017; pp. 1–7. [Google Scholar]
  30. Chen, J.; Zhan, W.; Tomizuka, M. Autonomous Driving Motion Planning with Constrained Iterative LQR. IEEE Trans. Intell. Veh. 2019, 4, 244–254. [Google Scholar] [CrossRef]
  31. Pan, Y.; Lin, Q.; Shah, H.; Dolan, J.M. Safe Planning for Self-Driving Via Adaptive Constrained ILQR. In Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Las Vegas, NV, USA, 24 October 2020; IEEE: Piscataway, NJ, USA, 2020; pp. 2377–2383. [Google Scholar]
  32. Li, H.; Zhang, J.; Zhu, S.; Tang, D.; Xu, D. Integrating Higher-Order Dynamics and Roadway-Compliance into Constrained ILQR-Based Trajectory Planning for Autonomous Vehicles. arXiv 2023, arXiv:2309.14566. [Google Scholar]
  33. Guo, J.; Xie, Z.; Liu, M.; Hu, J.; Dai, Z.; Guo, J. Data-Driven Enhancements for MPC-Based Path Tracking Controller in Autonomous Vehicles. Sensors 2024, 24, 7657. [Google Scholar] [CrossRef]
  34. Ye, X.; Lian, J.; Zhao, G.; Zhang, D. A Novel Closed-Loop Structure for Drag-Free Control Systems with ESKF and LQR. Sensors 2023, 23, 6766. [Google Scholar] [CrossRef]
  35. Wang, Y.; Lian, R.; He, H.; Betz, J.; Wei, H. Auto-Tuning Dynamics Parameters of Intelligent Electric Vehicles via Bayesian Optimization. IEEE Trans. Transp. Electrif. 2024, 10, 6915–6927. [Google Scholar] [CrossRef]
  36. Fan, H.; Zhu, F.; Liu, C.; Zhang, L.; Zhuang, L.; Li, D.; Zhu, W.; Hu, J.; Li, H.; Kong, Q. Baidu Apollo Em Motion Planner. arXiv 2018, arXiv:1807.08048. [Google Scholar]
  37. Yamakado, M.; Abe, M. An Experimentally Confirmed Driver Longitudinal Acceleration Control Model Combined with Vehicle Lateral Motion. Veh. Syst. Dyn. 2008, 46, 129–149. [Google Scholar] [CrossRef]
Figure 1. Improved CILQR process.
Figure 1. Improved CILQR process.
Sensors 25 00512 g001
Figure 2. Autonomous driving platform. (a) Vehicle test platform. (b) Communication topology diagram.
Figure 2. Autonomous driving platform. (a) Vehicle test platform. (b) Communication topology diagram.
Sensors 25 00512 g002
Figure 3. Autonomous driving platform.
Figure 3. Autonomous driving platform.
Sensors 25 00512 g003
Figure 4. Continuous nudge static obstacle scenario, trajectory, and vehicle dynamic comparison profiles. (a) Time sequence diagram of improved CILQR trajectory planning. (b) Time sequence diagram of CILQR trajectory planning. (c) Speed profile. (d) Curvature of Trajectory. (e) Lateral acceleration. (f) Longitudinal acceleration.
Figure 4. Continuous nudge static obstacle scenario, trajectory, and vehicle dynamic comparison profiles. (a) Time sequence diagram of improved CILQR trajectory planning. (b) Time sequence diagram of CILQR trajectory planning. (c) Speed profile. (d) Curvature of Trajectory. (e) Lateral acceleration. (f) Longitudinal acceleration.
Sensors 25 00512 g004
Figure 5. Dynamic cut-in obstacle collision avoidance scenario, trajectory, and vehicle dynamic comparison profiles. (a) Time-sequence diagram of improved CILQR trajectory planning. (b) Time sequence diagram of CILQR and EM trajectory planning. (c) Speed profile. (d) Curvature of Trajectory. (e) Lateral acceleration. (f) Longitudinal acceleration.
Figure 5. Dynamic cut-in obstacle collision avoidance scenario, trajectory, and vehicle dynamic comparison profiles. (a) Time-sequence diagram of improved CILQR trajectory planning. (b) Time sequence diagram of CILQR and EM trajectory planning. (c) Speed profile. (d) Curvature of Trajectory. (e) Lateral acceleration. (f) Longitudinal acceleration.
Sensors 25 00512 g005
Figure 6. Dynamic overtaking of oncoming obstacles scenario, trajectory, and vehicle dynamic comparison profiles. (a) Time-sequence diagram of improved CILQR trajectory planning. (b) Vehicle steering wheel angle request and longitudinal acceleration request. (c) Speed profile and curvature of trajectory. (d) Longitudinal and lateral acceleration of trajectory.
Figure 6. Dynamic overtaking of oncoming obstacles scenario, trajectory, and vehicle dynamic comparison profiles. (a) Time-sequence diagram of improved CILQR trajectory planning. (b) Vehicle steering wheel angle request and longitudinal acceleration request. (c) Speed profile and curvature of trajectory. (d) Longitudinal and lateral acceleration of trajectory.
Sensors 25 00512 g006
Figure 7. Collision avoidance with dynamic and static obstacles scenario, trajectory, and vehicle dynamic comparison profiles. (a) Time-sequence diagram of improved CILQR trajectory planning. (b) Vehicle steering wheel angle request and longitudinal acceleration request. (c) Longitudinal and lateral acceleration of trajectory. (d) Longitudinal and lateral acceleration of trajectory.
Figure 7. Collision avoidance with dynamic and static obstacles scenario, trajectory, and vehicle dynamic comparison profiles. (a) Time-sequence diagram of improved CILQR trajectory planning. (b) Vehicle steering wheel angle request and longitudinal acceleration request. (c) Longitudinal and lateral acceleration of trajectory. (d) Longitudinal and lateral acceleration of trajectory.
Sensors 25 00512 g007
Table 1. Vehicle key parameters.
Table 1. Vehicle key parameters.
ParameterDescriptionValueUnit
m Vehicle mass2063.4 k g
l f Front axle distance1351.8 m m
l r Rear axle distance1548.2 m m
C f Equivalent front axle cornering stiffness100,640N/rad
C r Equivalent rear axle cornering stiffness100,640N/rad
R t i r e Equivalent tire rolling radius366mm
I z Yaw moment of inertia about the Z-axis3347.8 k g · m 2
V a Vehicle length4766mm
V b Vehicle width2267mm
V c Vehicle height1684mm
G x Longitudinal distance from the center of mass to the rear axle1549mm
G y Lateral position of the center of mass5mm
G z Vertical position of the center of mass596mm
f e d g e Distance from the front bumper to the rear axle center3829mm
r e d g e Distance from the rear bumper to the rear axle center937mm
S r a t i o Steering ratio between front wheels and steering wheel16.7/
Table 2. Calibratable weights and matrices.
Table 2. Calibratable weights and matrices.
Adaptive Weighting ParametersValueCost Function WeightingValue
p a x 1 1.2 w r 2.5
p a x 2 0.1 w v y 12
p a x 3 20 w a y 18
p κ ˙ 1 120 w j y 5
p κ ˙ 2 0.12 w v x 25
p κ ˙ 3 600 w j x 3
p κ ˙ 4 0.05
p κ ˙ 5 300
Table 3. The comfort and human-like indicators for various methods in both real vehicle and simulation scenarios, along with the 99th% computation time.
Table 3. The comfort and human-like indicators for various methods in both real vehicle and simulation scenarios, along with the 99th% computation time.
ScenariosComfortable IndicatorHuman-Like IndicatorComputation Cost
κ   [ m 1 ] κ ˙   [ m 2 ] a x   [ m / s 2 ] a y   [ m / s 2 ] ω   [ d e g / s ] f h l T 99 t h   [ m s ] N 99 t h
Simulation 1CILQR6.2 × 10−24.8 × 10−2−4.55 −0.81 −0.14 0.63 48.3455
Improved CILQR−3.8 × 10−22.3 × 10−2−1.77 0.28 0.09 0.57 24.1025
Simulation 2CILQR−1.5 × 10−2−1.2 × 10−2−3.34 0.47 −0.06 0.82 44.7149
Improved CILQR9.4 × 10−3−2.0 × 10−3−0.92 0.67 0.08 0.63 29.3429
Vehicle Test 18.5 × 10−3−3.3 × 10−3−1.31 1.27 0.22 0.51 30.1528
Vehicle Test 21.0 × 10−24.7 × 10−3−1.91 0.80 0.05 0.68 33.8831
Both simulation and real vehicle tests used the same software system and hardware platform.
Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.

Share and Cite

MDPI and ACS Style

Li, Q.; He, H.; Hu, M.; Wang, Y. Spatio-Temporal Joint Trajectory Planning for Autonomous Vehicles Based on Improved Constrained Iterative LQR. Sensors 2025, 25, 512. https://doi.org/10.3390/s25020512

AMA Style

Li Q, He H, Hu M, Wang Y. Spatio-Temporal Joint Trajectory Planning for Autonomous Vehicles Based on Improved Constrained Iterative LQR. Sensors. 2025; 25(2):512. https://doi.org/10.3390/s25020512

Chicago/Turabian Style

Li, Qin, Hongwen He, Manjiang Hu, and Yong Wang. 2025. "Spatio-Temporal Joint Trajectory Planning for Autonomous Vehicles Based on Improved Constrained Iterative LQR" Sensors 25, no. 2: 512. https://doi.org/10.3390/s25020512

APA Style

Li, Q., He, H., Hu, M., & Wang, Y. (2025). Spatio-Temporal Joint Trajectory Planning for Autonomous Vehicles Based on Improved Constrained Iterative LQR. Sensors, 25(2), 512. https://doi.org/10.3390/s25020512

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Metrics

Back to TopTop