Trajectory Optimization with Dynamic Drivable Corridor-Based Collision Avoidance

Abstract

Trajectory planning for autonomous vehicles is essential for ensuring driving safety, passenger comfort, and operational efficiency. Collision avoidance constraints introduce significant computational complexity due to their inherent non-convex and nonlinear characteristics. Previous research has proposed the drivable corridor (DC) method, which transforms complex collision avoidance constraints into linear inequalities by constructing time-varying rectangular corridors within the spatiotemporal domains, thereby enhancing optimization efficiency. However, the DC construction process involves repetitive collision detection, leading to an increased computational burden. To address this limitation, this study proposes a novel approach that integrates grid-based obstacle representation with dynamic grid merging to accelerate collision detection and dynamically constructs the DC by adaptively adjusting the expansion strategies according to available spatial dimensions. The feasibility and effectiveness of the proposed method are validated through simulation-based evaluations conducted over 100 representative scenarios characterized by diverse and unstructured environmental configurations. The simulation results indicate that, with appropriately selected grid resolutions, the proposed approach achieves up to a 60% reduction in trajectory planning time compared to conventional DC-based planners while maintaining robust performance in complex environments.

1. Introduction

Autonomous driving technology plays a crucial role in improving driving safety, optimizing traffic flow, and enhancing the overall driving experience [1,2,3]. Trajectory planning, a critical component within autonomous driving systems, aims to generate safe and feasible trajectories that adhere to vehicle dynamics constraints and effectively address collision avoidance in complex environments [4,5,6]. Accurate trajectory planning enables autonomous vehicles to navigate smoothly and safely through dynamic scenarios, offering users an intelligent and comfortable driving experience. Collision avoidance, which ensures that autonomous vehicles can safely navigate around obstacles without collisions, is a core issue in trajectory planning. This directly determines the safety and reliability of autonomous driving systems [7]. This challenge is particularly pronounced in unstructured environments lacking clearly defined road boundaries or standardized traffic rules, characterized by irregular obstacle distributions and complex terrains. Recent studies have investigated diverse approaches to address these challenges, with optimization methods based on dynamic drivable corridors (DCs) receiving increasing attention.

1.1. Related Works

Collision avoidance remains a challenge in trajectory planning, and its implementation depends on the chosen planning strategy. Sampling- and search-based methods [8,9], such as State Lattice Planning [10], Rapidly exploring Random Trees and its variants [11,12], Hybrid A* [13,14], and the Dynamic Window Approach [15], typically achieve collision avoidance by discretizing the search space and conducting collision checks during node selection. These methods are advantageous due to their simplicity and computational efficiency in complex environments. However, they suffer from several limitations, including insufficient path smoothness arising from discretization, difficulties in ensuring global optimality, and sharply declining search efficiency in high-dimensional state spaces, especially when vehicle dynamic constraints are considered. Numerical optimization-based methods, in contrast, explicitly formulate collision avoidance as constrained optimization problems, typically through inequalities representing safety distances [16,17,18,19]. Such methods produce smooth, precise trajectories and naturally incorporate multiple constraints for coordinated optimization. Nonetheless, their computational complexity can be substantially higher, particularly when addressing complex, non-convex collision avoidance constraints [20].

To balance computational efficiency and trajectory quality, researchers have proposed two main improvement strategies. The first involves hierarchical planning frameworks, which employ sampling- or search-based methods to quickly generate initial paths. These preliminary paths serve as initial guesses or reference trajectories for numerical optimization, significantly reducing the computational burden [21,22]. The second strategy simplifies the formulation of collision constraints to a lower computational complexity.

Aligned with the first strategy, this work adopts a two-stage trajectory planning approach: An initial coarse trajectory is rapidly generated using the Hybrid A* algorithm, providing a feasible starting solution. Subsequently, a numerical optimization method refines this trajectory to meet precision and constraint requirements.

Many recent studies [23,24,25] have also pursued the second strategy—simplifying collision constraint formulations, as summarized in

Table 1. Comparison of collision constraint simplification methods.

Method	Advantages	Disadvantages
APF	Simple implementation and high computational efficiency	Susceptibility to local minima
Linear Constraint Approximation	Linearized constraint formulation and reduced computational complexity	Approximation errors and reduced solution reliability
DC Method	Reduction of nonlinearity in optimization and decoupling from environment complexity	High computational cost of corridor construction

. A widely employed method is the Artificial Potential Field (APF), which implicitly achieves collision avoidance by combining attractive potentials (guiding towards goals) and repulsive potentials (repelling obstacles). Despite its simplicity, APF methods are susceptible to local minima, particularly when obstacles are near goal positions, limiting global optimality. Schulman et al. [26] introduced an alternative method approximating collision avoidance constraints as linear constraints. However, this approach faces challenges in precisely quantifying approximation errors, potentially compromising reliability.

Li et al. [27,28,29] proposed the DC method to simplify complex obstacle avoidance constraints. Each drivable corridor (DC) defines a safe, constrained region within which the vehicle is expected to remain at a specific time instance. Throughout the planning horizon, a sequence of such time-indexed rectangular regions collectively forms a spatiotemporal safety envelope, ensuring that the vehicle’s trajectory stays within allowable boundaries at all times. This method transforms collision avoidance constraints—whose computational scale vary with environmental complexity—into predefined rectangular DC constraints that restrict the vehicle’s motion to specific regions during designated time intervals. By decoupling obstacle avoidance constraints from environmental complexity, the DC method reduces nonlinearity in the problem formulation, thereby facilitating subsequent numerical solver implementations. However, the DC construction process itself presents computational efficiency limitations. The DC construction strategy adopted in [27] requires iterative region expansion to progressively determine feasible areas, where each expansion necessitates obstacle collision detection to ensure corridor safety—a process that incurs significant computational overhead.

1.2. Motivations and Contributions

While constructing a feasible DC for the vehicle decouples the collision avoidance constraints from the environment, significantly reducing the complexity of these constraints and the difficulty of solving the problem, existing DC construction methods remain time-consuming and computationally inefficient due to several limitations. In practice, each DC expansion requires collision detection between the expanded rectangular region and the surrounding environmental obstacles. Because DC construction typically involves multiple expansions, the total number of collision detection operations grows, leading to higher overall computational time. Furthermore, when the environment contains numerous and highly complex obstacles, the time required for DC construction increases accordingly. This not only affects the overall efficiency of the trajectory planning process but also severely compromises the real-time performance of autonomous driving systems.

Building upon the DC framework established by Li et al. [27], this work introduces an efficient construction scheme to address computational bottlenecks in the DC modeling process, aiming to improve the efficiency of collision avoidance constraint modeling. The proposed method makes two key contributions: accelerating the DC construction process and reducing the collision detection time involved in DC construction:

• We propose a grid-based discretization method combined with maneuver merging techniques for efficient obstacle spatial representation, which effectively reduces the number of grids and lowers the computational load required for collision detection.
• We propose a dynamic DC expansion strategy based on spatial margins for efficient construction of drivable areas. This method avoids redundant expansion steps, ensures appropriate DC sizing for vehicle navigation, and improves computational efficiency.

1.3. Organizations

The remainder of this paper is organized as follows: Section 2 formulates the optimal control problem (OCP) for intelligent vehicle trajectory planning. Section 3 details the proposed collision avoidance constraint modeling method. Section 4 presents the simulation results along with a discussion. Finally, Section 5 summarizes the key contributions of this work and outlines potential directions for future research.

2. Problem Statement

This section formulates the trajectory planning problem for an autonomous vehicle as an OCP. In this formulation, the state variables consist of the vehicle’s position $(x,y)$, velocity $v$, heading angle $\theta$, and front-wheel steering angle $\varphi$, while the control variables include the acceleration $a$ and the angular rate of the steering angle $\omega$.

The objective of the OCP is to determine an optimal sequence of control inputs that generate a vehicle trajectory that is spatially smooth and dynamically stable while satisfying all imposed constraints—namely, kinematic feasibility, two-point boundary conditions, and collision avoidance. The formulation is detailed in the following subsections.

2.1. Vehicle Kinematics Constraints

The vehicle kinematic constraints (c.f. Figure 1) are presented as follows:

$${\displaystyle \frac{d}{dt}}\left[\begin{array}{l}x\left(t\right)\\ y\left(t\right)\\ v\left(t\right)\\ \varphi \left(t\right)\\ \theta (t)\end{array}\right]=\left[\begin{array}{l}v\left(t\right)\cdot \cos \theta (t)\\ v\left(t\right)\cdot \sin \theta (t)\\ a\left(t\right)\\ \omega \left(t\right)\\ v\left(t\right)\cdot \tan \varphi \left(t\right)/{\mathrm{L}}_{\mathrm{w}}\end{array}\right].$$

(1)

The coordinates $(x,y)$ represent the position of the vehicle’s rear axle center point P, $v$ is the velocity of P, $\theta$ is the vehicle’s heading angle, $\varphi$ is the steering angle of the front wheels, $a$ is the vehicle’s acceleration, $\omega$ is the vehicle’s angular velocity, and ${\mathrm{L}}_{\mathrm{w}}$ is the wheelbase, which is the distance between the front and rear axle centers of the vehicle. These variables are subject to the following bounds:

$$\left[\begin{array}{l}{\mathrm{a}}_{\min }\\ {\mathrm{v}}_{\min }\\ -{\mathsf{\Omega }}_{\max }\\ -{\mathsf{\Phi }}_{\max }\end{array}\right]\le \left[\begin{array}{l}a\left(t\right)\\ v\left(t\right)\\ \omega \left(t\right)\\ \varphi \left(t\right)\end{array}\right]\le \left[\begin{array}{l}{\mathrm{a}}_{\max }\\ {\mathrm{v}}_{\max }\\ {\mathsf{\Omega }}_{\max }\\ {\mathsf{\Phi }}_{\max }\end{array}\right], t\in [0, t_{\mathrm{f}}].$$

(2)

${\mathrm{a}}_{\min }$ and ${\mathrm{a}}_{\max }$ represent the lower and upper bounds of acceleration, ${\mathrm{v}}_{\min }$ and ${\mathrm{v}}_{\max }$ represent the lower and upper bounds of velocity, ${\mathsf{\Omega }}_{\max }$ represents the maximum magnitude of the front wheel angular velocity $\omega \left(t\right)$, and ${\mathsf{\Phi }}_{\max }$ represents the maximum magnitude of the front wheel steering angle $\varphi \left(t\right)$.

2.2. Two-Point Boundary Constraints

The boundary constraints refer to the states that the vehicle needs to satisfy at the starting and termination times, ensuring that the planned trajectory adheres to the required initial and final conditions:

$$\left[\begin{array}{l}x(0)\\ y(0)\\ \theta (0)\\ v(0)\\ \varphi (0)\\ a(0)\\ \omega (0)\end{array}\right]=\left[\begin{array}{l}{\mathrm{x}}_0\\ {\mathrm{y}}_0\\ {\mathsf{\theta }}_0\\ {\mathrm{v}}_0\\ {\mathsf{\Phi }}_0\\ {\mathrm{a}}_0\\ {\mathsf{\omega }}_0\end{array}\right],\left[\begin{array}{l}x(t_f)\\ y(t_f)\\ \theta (t_f)\\ v(t_f)\\ \varphi (t_f)\\ a(t_f)\\ \omega (t_f)\end{array}\right]=\left[\begin{array}{l}{\mathrm{x}}_{\mathrm{f}}\\ {\mathrm{y}}_{\mathrm{f}}\\ {\mathsf{\theta }}_{\mathrm{f}}\\ {\mathrm{v}}_{\mathrm{f}}\\ {\mathsf{\Phi }}_{\mathrm{f}}\\ {\mathrm{a}}_{\mathrm{f}}\\ {\mathsf{\omega }}_{\mathrm{f}}\end{array}\right],$$

(3)

where $[{\mathrm{x}}_0,{\mathrm{y}}_0,{\mathsf{\theta }}_0,{\mathrm{v}}_0,{\mathsf{\Phi }}_0,{\mathrm{a}}_0,{\mathsf{\omega }}_0]$ represent the kinematic states that the vehicle must satisfy at the initial time, while $[{\mathrm{x}}_{\mathrm{f}},{\mathrm{y}}_{\mathrm{f}},{\mathsf{\theta }}_{\mathrm{f}},{\mathrm{v}}_{\mathrm{f}},{\mathsf{\Phi }}_{\mathrm{f}},{\mathrm{a}}_{\mathrm{f}},{\mathsf{\omega }}_{\mathrm{f}}]$ denote the kinematic states required at the terminal time.

2.3. Collision Avoidance Constraints

Collision avoidance constraint requires that the vehicle must not collide with any obstacles in the environment at any time. In this paper, this constraint is described as that the vehicle must remain within a safe drivable area at any time $t\in [0,t_{\mathrm{f}}].$

$$p(t)\in \mathrm{SafeAreas}(t), t\in [0,t_{\mathrm{f}}],$$

(4)

where $p(t)$ denotes the vehicle’s position at time $t,$ and $\mathrm{SafeAreas}(t)$ represents the safe drivable area of the vehicle at instant $t$. Efficiently constructing $\mathrm{SafeAreas}(t)$ is a key problem to be addressed in this paper.

2.4. Cost Function

The cost function is generally used to represent the smoothness and comfort of the vehicle motion. Assuming the trajectory planning time domain is $t\in [0,t_{\mathrm{f}}]$, the cost function $J$ is constructed as follows:

$$J={\displaystyle {\int }_{\tau =0}^T\left({\mathrm{w}}_{\mathrm{a}}\cdot a^2(\tau )+{\mathrm{w}}_{\mathsf{\omega }}\cdot {\omega }^2(\tau )\right)}\cdot d\tau ,$$

(5)

where ${\mathrm{w}}_{\mathrm{a}},{\mathrm{w}}_{\mathsf{\omega }}({\mathrm{w}}_{\mathrm{a}},{\mathrm{w}}_{\mathsf{\omega }}>0)$ are user-defined weights, $a$ represents the vehicle’s acceleration, and $\omega$ represents the vehicle’s angular velocity.

2.5. Overall Problem Formulation

The complete trajectory planning problem is formulated as the following OCP:

$$\begin{array}{cc}\hfill \min & \mathrm{Cost} \mathrm{Function} \left(5\right);\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathrm{Kinematic} \mathrm{Constraints} \left(1\right),\left(2\right);\hfill \\ & \mathrm{Boundary} \mathrm{Conditions} \left(3\right);\hfill \\ & \mathrm{Collision} \mathrm{Avoidance} \mathrm{Constraints} \left(4\right).\hfill \end{array}$$

(6)

The vehicle needs to minimize the cost function while satisfying all the constraints.

3. Collision Avoidance Constraint Formulation

In the optimal control problem formulation (Section 2), collision avoidance constraints ensure the vehicle remains within a safe drivable area. This chapter focuses on the techniques for modeling these constraints within the DC region, which explicitly defines the feasible space for trajectory planning. The method for constructing the DC is detailed in the remainder of this section. The core contributions of this work focus on two aspects: the formation and dynamic integration of occupied grids for representing polygonal obstacles (Section 3.3 and Section 3.4) and the dynamic construction of the DC region (Section 3.5). These components constitute critical steps toward enhancing the efficiency of collision avoidance constraint modeling.

3.1. Preprocessing

Prior to constructing the DC, a series of preprocessing steps are required. As a common practice, the vehicle is approximated by two discs representing the front and rear parts of the vehicle body [29], as illustrated in Figure 2a. The center positions of the front and rear discs are determined by (7):

$$\begin{array}{l}{x}_{\mathrm{f}}(t)=x(t)+{\displaystyle \frac{1}{4}}(3{\mathrm{L}}_{\mathrm{W}}+3{\mathrm{L}}_{\mathrm{F}}-{\mathrm{L}}_{\mathrm{R}})\cdot \cos \theta (t),\\ y_{\mathrm{f}}(t)=y(t)+{\displaystyle \frac{1}{4}}(3{\mathrm{L}}_{\mathrm{W}}+3{\mathrm{L}}_{\mathrm{F}}-{\mathrm{L}}_{\mathrm{R}})\cdot \sin \theta (t),\\ x_{\mathrm{r}}(t)=x(t)+{\displaystyle \frac{1}{4}}({\mathrm{L}}_{\mathrm{W}}+{\mathrm{L}}_{\mathrm{F}}-{3\mathrm{L}}_{\mathrm{R}})\cdot \cos \theta (t),\\ y_{\mathrm{r}}(t)=y(t)+{\displaystyle \frac{1}{4}}({\mathrm{L}}_{\mathrm{W}}+{\mathrm{L}}_{\mathrm{F}}-{3\mathrm{L}}_{\mathrm{R}})\cdot \sin \theta (t).\end{array}$$

(7)

The radius of the discs is determined by Formula (8):

$$\mathrm{R}={\displaystyle \frac{1}{2}}\sqrt{{\left({\displaystyle \frac{{\mathrm{L}}_{\mathrm{R}}+{\mathrm{L}}_{\mathrm{W}}+{\mathrm{L}}_{\mathrm{F}}}{2}}\right)}^2+({\mathrm{L}}_{\mathrm{B}}}{)}^2.$$

(8)

The vehicle body is approximated by two discs. Thus, avoiding collisions means ensuring that neither disc intersects with any obstacles (c.f. Figure 2b). By reducing each disc to its center and expanding the obstacle radius by R in the environment (c.f. Figure 2c), the original condition transforms into ensuring that the centers of the front and rear discs, P_f and P_r (c.f. Figure 2a,b), do not collide with these expanded obstacles. To achieve this, a separate DC is constructed for each disc center at each discretized time instant.

3.2. Grid-Based Obstacle Modeling

In practical collision detection, environmental obstacles often exhibit complex shapes and diverse types, leading to high computational complexity if processed directly. To address this issue, we introduce a grid-based discretization approach. By converting complex obstacles into a series of regular axis-aligned bounding boxes (AABBs), the spatial representation of obstacles is significantly simplified.

The implementation approach consists of three key steps. First, environmental obstacles are sampled to generate a set of scattered obstacle points. Next, defining the grid map range is essential. Since distant obstacles typically do not pose a safety threat during vehicle navigation, they can be ignored to reduce collision detection targets and computational complexity. This principle is also applied in grid discretization: The considered region of the grid map is restricted to include only areas relevant to trajectory planning, ensuring that extraneous obstacle points outside its boundaries are excluded. Finally, the remaining discrete obstacle points within the grid map range are converted into occupied grid cells, forming the basis for subsequent collision checking.

The selection of the grid map range is as follows: Since the DC is constructed based on the initial path, the grid map range must fully cover the entire initial path. Additionally, extra space is reserved to account for potential obstacles and path variations, ensuring the smooth construction of the DC. The specific range selection is shown in the following formula:

$$\begin{array}{l}{x}_{\min }=\min (X)-\mathsf{\Delta }\mathrm{d},\\ x_{\max }=\max (X)+\mathsf{\Delta }\mathrm{d},\\ y_{\min }=\min (Y)-\mathsf{\Delta }\mathrm{d},\\ y_{\max }=\max (Y)+\mathsf{\Delta }\mathrm{d}.\end{array}$$

(9)

The set $X=\{x_1,x_2,\cdots ,x_{\mathrm{n}}\}$ represents the set of all x-coordinates on the initial path, and $Y=\{y_1,y_2,\cdots ,y_{\mathrm{n}}\}$ represents the set of all y-coordinates on the initial path. Δd denotes the reserved space, which must be greater than or equal to the maximum length of DC.

For obstacle points located outside the range of the grid map, they are directly discarded, effectively filtering out the influence of distant obstacles and improving computational efficiency.

For those within the grid map, their corresponding grid coordinates are computed based on their positions, and the associated cells are marked as occupied (see the grey-shaded grid cells in Figure 3).

Through this grid-based discretization process, collision detection targets in the environment are transformed from complex and irregularly shaped obstacles into a series of discrete, regularly shaped occupied grid cells. Compared to handling the original complex obstacles, using these regular grid cells simplifies the collision detection process and facilitates more efficient modeling and computation.

3.3. Occupied Grid Formation for Polygonal Obstacles

For polygonal obstacles, sampling along their boundaries is a convenient approach. By extracting sample points from each edge and applying the grid-based representation method introduced in the previous section, the resulting occupied grid map can be obtained (c.f. Figure 4a). However, this approach only represents the boundaries of the polygon and fails to effectively capture its interior region. When dealing with large polygonal obstacles, a target entering the interior may not be detected in time, posing potential safety risks. To address this issue, we propose an approach that ensures complete coverage of both the boundary and interior regions (c.f. Figure 4b) by sampling only the polygon edges. The implemented algorithm is presented in Algorithm 1.

The input of Algorithm 1 includes the polygonal obstacle information and the initial trajectory. The output of Algorithm 1 is the set of occupied grid cells Boxes that cover the polygonal obstacle. The core of Algorithm 1 lies in the sampling, determining the grid map boundaries, discretizing obstacle coordinates into grid cells, and obtaining the complete set of occupied grids covering the polygonal obstacles. First, the function Sample() (c.f. line 2 in Algorithm 1) is executed to perform uniform sampling along each edge of the polygon, generating a series of discrete points to ensure comprehensive coverage of the obstacle boundaries. Subsequently, the function DetermineBoundary() (c.f. line 3 in Algorithm 1) computes and establishes the grid map boundaries based on (9), ensuring that all relevant obstacle data are correctly projected onto the grid representation. Following this, for each sampled point that lies within the grid map boundaries, the algorithm applies a grid discretization process, mapping the obstacle edges to the corresponding grid cells and thereby obtaining the occupied grid cells that represent the polygonal obstacle boundaries, as illustrated in Figure 4a. Additionally, the algorithm computes and stores the indices of these occupied grid cells within the grid map to facilitate subsequent computations (c.f. lines 4–9 in Algorithm 1).

Next, the column index set of the occupancy grid is traversed, and for each column, the following operations are performed: determine the minimum and maximum row positions of the occupied grids in that column and then connect the occupied grids at the minimum and maximum row positions vertically to form a new occupied grid block (c.f. lines 10–17 in Algorithm 1).

As shown in Figure 4b, the new occupied grids generated by this method not only cover the polygon boundary but also effectively cover its interior region. Additionally, the number of occupied grids is reduced from 62 in Figure 4a to 14 in Figure 4b, significantly decreasing the number of collision detection targets.

Each polygonal obstacle in the environment is processed individually using the aforementioned method, and the resulting occupancy grids are stored in a container. These grids are then used to replace the obstacles in the environment for subsequent collision detection during the DC construction process.

Algorithm 1 Occupied Grids Formation

Input: obs, initialpath; Output: Boxes.

1. Initialize Boxes = Ø; Rows = Ø; Cols = Ø; define resolution; 2. points ← Sample (obs); 3. xmin, xmax, ymin, ymax ← DetermineBoundary (initialpath); 4. for each point ∈ points do 5. if point.x ≥ xmin and point.x ≤ xmax and point.y ≥ ymin and point.y ≤ ymax, then 6. Row, Col ← ObstacleToGridIndex (xmin, xmax, ymin, ymax, resolution); 7. Rows.add (Row); Cols.add (Col); 8. end if 9. end for 10. max_col ← max (Cols); 11. for col in {1…max_col} do 12. if col in Cols then 13. row_min, row_max ← DetermineRowRange (col); 14. Box ← GenerateColumnBox (xmin, ymin, row_min, row_max, resolution); 15. Boxes.add (Box); 16. end if 17. end for 18. return Boxes;

As shown in Figure 4c, increasing grid resolution enhances the accuracy of obstacle representation but significantly raises the number of required grid cells. Conversely, lower grid resolution reduces representation precision, causing greater spatial loss—illustrated in Figure 4b, where the grid coverage exceeds the actual obstacle region. However, this also decreases the number of grid cells needed for representation.

3.4. Dynamical Merging of Occupied Grids

The obtained occupancy grids are dynamically merged to reduce the number of collision detection objects and computational complexity (c.f. Figure 5a). The merging process follows a structured approach, which is detailed in this subsection.

First, as shown in Figure 5b, for the occupied grids in each column, they are merged according to specific conditions. The merging conditions are as follows: Within the same column, if two occupied grids overlap or are adjacent in the vertical direction, or if one occupied grid completely contains the other, they are merged into a larger occupied grid. Using this method, the number of objects involved in collision detection is reduced from the greater number of individual occupied grids before merging to a smaller number of merged grids, thereby significantly decreasing the computational complexity of collision detection and reducing the detection time. It is important to note that the merged occupancy grids remain AABBs, ensuring that the complexity of collision detection does not increase due to the merging process. The vertical merging process is shown in Algorithm 2.

Algorithm 2 takes as input a set of occupied grid cells, labeled as Boxes, which represent obstacles in the environment. Meanwhile, Algorithm 2 outputs a vertically merged version of these grid cells, referred to as VBoxes. Execute the Sort() (c.f. line 2 in Algorithm 2) to sort all occupied grids according to specific rules: They are first sorted in ascending order based on the position of the left boundary, and for grids with the same left boundary, they are sorted by the height of the bottom edge in ascending order.

As shown in lines 3–12 of Algorithm 2: Starting from the lowest occupied grid in each column, the algorithm sequentially checks upward and attempts to merge grids. If the currently occupied grid and the next grid satisfy the merging conditions (overlapping, adjacent, or containment in the vertical direction), the function MergeVerticalGrids() (c.f. line 6 in Algorithm 2) is executed to combine them into a larger occupied grid along the vertical direction. After merging, the algorithm continues to check upward from the newly formed occupied grid. If a grid that does not meet the merging conditions is encountered, the upward merging process restarts from that grid. This process is repeated until all occupied grids are processed.

Algorithm 2 Vertical Merge of Occupied Grids

Input: Boxes; Output: Vertically merged occupied grids VBoxes.

1. Initialize i = 1; VBoxes = Ø; 2. Boxes ← Sort (Boxes); 3. while i ≤ length (Boxes) do 4. j ← i + 1; 5. while j ≤ length (Boxes) ∩ Boxesi.xmin = Boxesj.xmin ∩ Boxesi.xmax = Boxesj.xmax ∩ Boxesi.ymax ≥ Boxesj.ymin do 6. Boxesi ← MergeVerticalGrids (Boxesi, Boxesj); 7. j ← j + 1; 8. end while 9. VBoxes.add (Boxesi); 10. i ← j; 11. end while 12. return VBoxes;

Building upon the vertical merging process, further horizontal merging optimization is performed as illustrated in Figure 5c. Merging conditions are defined as follows: Two occupied grids with identical boundaries in the vertical direction are merged into a larger grid if they are horizontally adjacent, overlapping, or if one grid fully contains the other. The horizontal merging process is shown in Algorithm 3.

The horizontal merging procedure follows a workflow analogous to vertical merging (Algorithm 2). Therefore, we primarily focus on the differences between Algorithm 3 and Algorithm 2: Algorithm 3 takes as input the vertically merged occupied grid cells (labeled as VBoxes) and outputs the horizontally merged occupied grid cells (labeled as LBoxes). The specific implementation of the function Sort() (c.f. line 2 in Algorithm 3) is as follows: The grids are first sorted by the height of the bottom edge in ascending order, and for grids with the same bottom edge height, they are sorted by the position of their left boundary from left to right. The merging condition (c.f. line 5 in Algorithm 3) represents the primary distinction between the two algorithms. Specifically, if the current occupied grid and the next one share identical vertical boundaries and are either adjacent or overlapping in the horizontal direction, or if one grid entirely contains the other, the function MergeHorizontalGrids() (c.f. line 6 in Algorithm 3) is executed to merge them into a larger occupied grid along the horizontal direction.

By comparing Figure 5a,c, it can be observed that after vertical and horizontal merging operations, the number of occupied grids representing obstacles is reduced from 79 to 36, a decrease of 54.4%. This significant reduction in grid count directly reduces the computational scale of subsequent collision detection.

Moreover, the merging of vertical and horizontal grid cells is designed as a computationally efficient one-time preprocessing step. This process requires only a single traversal of all occupied cells, combined with basic sorting (see line 2 in Algorithms 2 and 3) and adjacency checks (see line 5 in Algorithms 2 and 3). The merging is accomplished by directly updating the cell boundaries. Once the merging is completed, the resulting grid map remains valid throughout the entire planning process without requiring repeated operations. Therefore, this step introduces minimal computational overhead and has a negligible impact on the overall planning efficiency.

Algorithm 3 Horizontal Merging of Occupied Grids

Input: VBoxes; Output: LBoxes.

1. Initialize i = 1; LBoxes = Ø; 2. VBoxes ← Sort (VBoxes); 3. while i ≤ length (VBoxes) do 4. j ← i + 1; 5. while j ≤ length (VBoxes) ∩ VBoxesi.ymin = VBoxesj.ymin ∩ VBoxesi.ymax = VBoxesj.ymax ∩ VBoxesj.xmin ≤ VBoxesi.xmax do 6. VBoxesi ← MergeHorizontalGrids (VBoxesi, VBoxesj); 7. j ← j + 1; 8. end while 9. LBoxes.add (VBoxesi); 10. i ← j; 11. end while 12. return LBoxes;

3.5. Dynamic Construction of DC

The construction of DC is a crucial issue in collision avoidance constraint modeling. Unlike traditional stepwise expansion schemes for DC [27] (c.f. Figure 6a), this paper adopts a dynamic DC expansion approach (c.f. Figure 6b). Typically, in the early stages of DC expansion, the available space for expansion is relatively large. In this case, we can expand the DC simultaneously in all four directions in one step, as shown in Figure 6c. This method allows us to complete in one step what would normally require four expansions, meaning that instead of performing four collision checks, only one is needed. However, relying solely on this omnidirectional expansion strategy may prematurely terminate the expansion process if a collision is detected in any direction, leaving unused expandable space in other directions. This results in an undersized DC and consequently reduces the vehicle’s drivable space (c.f. Figure 6d). Therefore, when collisions are detected during simultaneous omnidirectional expansion, the current expansion is rolled back. The system then switches to a stepwise directional expansion strategy to ensure full utilization of expandable space in all directions.

The complete process of DC construction is illustrated in Figure 7. The core of the proposed method lies in dynamically adjusting the expansion strategy based on the available expandable space of the DC. By comparing the results in Figure 6a,b, it is evident that this approach significantly accelerates DC construction in the initial stages without reducing the final DC size.

The implementation of dynamic DC construction is presented in Algorithm 4: The input of Algorithm 4 is the expansion center point $(x_0,y_0)$ and the merged occupied grid; the output of Algorithm 4 is the expansion increments of the DC region in four directions. Define the set of expansion directions as direction = {π/2, 0, −π/2, π} (clockwise direction). Initialize the expansion increments for all four directions to zero. Determine the expansion step size Δs, and set the maximum expansion length parameter L_limit to avoid excessive expansion in each direction (c.f. lines 1–5 in Algorithm 4).

Execute the function SimultaneousExpansion() (c.f. line 6 in Algorithm 4): Starting from the center point$(x_0,y_0),$ expand simultaneously in four directions by step size ∆s and perform collision detection between the expanded rectangular region and the previously obtained merged occupied grid. If no collision occurs, repeat the above expansion process until the preset maximum expansion length L_limit is reached or a collision occurs.

As shown in lines 7–11 in Algorithm 4, check the expansion increment length obtained from executing SimultaneousExpansion(). If all four directions reach the maximum expansion limit, return length and terminate Algorithm 4 early.

If the length has not reached the maximum expansion limit, it indicates that at least one direction has encountered a collision, but it does not necessarily mean that all four directions have no expandable space. In such cases, we need to switch to a stepwise directional expansion strategy to ensure comprehensive expansion in all directions. Execute the function StepwiseExpansion() (c.f. line 12 in Algorithm 4): Specifically, based on the predefined set of expansion direction = {π/2, 0, −π/2, π}, the DC is expanded sequentially in each direction. If a collision occurs in a direction or the predefined maximum expansion limit is reached, the system abandons the current expansion and excludes that direction from subsequent expansions. When all four directions can no longer be expanded, the expansion process ends, and the final DC region at that position is obtained.

Algorithm 4 Dynamic DC Construction

Input: Center point $(x_0,y_0),$ merged occupied grid LBoxes; Output: Expansion increments of DC in four directions.

1. Define four expansion directions as direction = {π/2, 0, −π/2, π}; 2. Initialize an index set Y = {1, 2, 3, 4}; 3. Initialize expansion vector: length = [0, 0, 0, 0]; 4. Initialize approved region X as the mass point $(x_0,y_0),$ 5. Define the single expansion step size as Δs and maximum expansion length Llimit; 6. length ← SimultaneousExpansion ($x_0,y_0,$ Llimit, X, Δs, LBoxes, length); 7. for each j ∈ Y do 8. if lengthj > Llimit 9. return length; 10. end if 11. end for 12. length ← StepwiseExpansion $(x_0,y_0,$ Llimit, X, Y, Δs, LBoxes, length, directions); 13. return length.

Based on the incremental length for expanding the DC in four directions and the expansion center point $(x_0,y_0),$ the ranges on the x- and y-axes are calculated using (10):

$$\begin{array}{l}{x}_{\min }=x_0-{\mathrm{length}}_4,\\ x_{\max }=x_0+{\mathrm{length}}_2,\\ y_{\min }=y_0-{\mathrm{length}}_3,\\ y_{\max }=y_0+{\mathrm{length}}_1,\end{array}$$

(10)

${\mathrm{length}}_1$ represents the incremental expansion in the π/2 direction, ${\mathrm{length}}_2$ represents the incremental expansion in the 0 direction, ${\mathrm{length}}_3$ represents the incremental expansion in the −π/2 direction, and ${\mathrm{length}}_4$ represents the incremental expansion in the π direction.

Finally, the collision avoidance constraints can be expressed as the following within-DC constraints:

$$\begin{array}{l}x{f}_{\min }^k\le x_{\mathrm{f}}(t)\le x{f}_{\max }^k,\\ y{f}_{\min }^k\le y_{\mathrm{f}}(t)\le y{f}_{\max }^k,\\ x{r}_{\min }^k\le x_{\mathrm{r}}(t)\le x{r}_{\max }^k,\\ y{r}_{\min }^k\le y_{\mathrm{r}}(t)\le y{r}_{\max }^k,\\ t=(t_{\mathrm{f}}/N)\cdot k,\hspace{0.17em}k=1,\dots ,N.\end{array}$$

(11)

Here, $(x_{\mathrm{f}}(t),y_{\mathrm{f}}(t))$ is the position of the front circle center at time t, and $(x_{\mathrm{r}}(t),y_{\mathrm{r}}(t))$ is the position of the rear circle center at time $t$. $[x{f}_{\min }^k,x{f}_{\max }^k,y{f}_{\min }^k,y{f}_{\max }^k]$ represents the feasible rectangular region for the front circle center at time $t$, with the value ranges on the x- and y-axes. $[x{r}_{\min }^k,x{r}_{\max }^k,y{r}_{\min }^k,y{r}_{\max }^k]$ represents the feasible rectangular region for the rear circle center at time $t$, with the value ranges on the x- and y-axes.

4. Simulation Results and Discussion

To verify the effectiveness of the proposed approach, the formulated continuous-time OCP (6) is solved numerically. By discretizing the time domain, the formulated continuous-time OCP (6) is transformed into a nonlinear programming (NLP) problem [30]. In this process, the state variables $(x(t),y(t),\theta (t),v(t),\varphi (t))$ and control variables $(a(t),\omega (t))$ are discretized in the time domain, representing them as a set of discrete decision variables. Furthermore, the objective function (5) and constraints associated with the problem, such as the vehicle kinematic constraints (1) and (2) and collision avoidance constraints (11), also need to be discretized, converting them into algebraic equations or inequality constraints at discrete time points.

In this study, we selected the interior point method [31] as the solver for the NLP problem, leveraging its efficient and stable characteristics to rapidly obtain the optimal solution, providing accurate numerical results for trajectory planning or motion control.

All simulations were performed in MATLAB 2023a and executed on an R7-7735H CPU with 16 GB RAM that runs at 3.2 × 8 GHz. The basic simulation parameters are listed in

Table 2. Parametric settings regarding model and approach.

Parameter	Description	Setting
LF	Front hang length of vehicle.	0.96 m
LW	Wheelbase of vehicle.	2.80 m
LR	Rear hang length of vehicle.	0.929 m
LB	Width of vehicle.	1.942 m
amax	Upper bound of |$a(t)$|.	4.0 m/s2
vmax	Upper bound of |$v(t)$|.	4.0 m/s
Φmax	Upper bound of |$\varphi (t)$|.	0.85 rad
Ωmax	Upper bound of |$\omega (t)$|.	1.0 rad/s
NR+1	Number of DCs for Pr or Pf.	100
Δs	Unit step length in DC specification.	0.1 m
Llimit	Maximum step length in DC specification.	5.0 m
Nfe	Number of finite elements.	100
resolution	Spatial resolution of the grid.	0.1 m

These parameters are based on the physical specifications of a typical passenger vehicle, ensuring that the simulation settings reflect realistic vehicle dynamics.

The deployment scenarios are predominantly composed of stochastically generated, unstructured environments that capture a broad spectrum of obstacle complexity and variability. These settings are intended to simulate realistic and demanding conditions, thereby offering a rigorous and representative benchmark for assessing the proposed method’s generalizability and robustness. In these simulations, Hybrid A* is employed to generate initial trajectories due to its effectiveness in unstructured environments with nonholonomic constraints. However, it is important to note that this work focuses on accelerating the modeling of collision avoidance constraints during the trajectory optimization stage. The proposed scheme is designed to be independent of any specific initial path generation strategy and is compatible with a wide range of planners.

As shown in Figure 8, the effectiveness of the proposed DC construction scheme is validated in an unstructured scenario with randomly generated obstacles. In Figure 8, obstacles influencing the DC construction process are represented by blue grids, while polygonal obstacles not covered by blue grids are identified as irrelevant to DC construction through an intelligent filtering mechanism and excluded from the process. The figure demonstrates that both front and rear DCs remain collision-free with the expanded obstacles, satisfying safety requirements. This result preliminarily indicates that our scheme not only intelligently filters out irrelevant obstacles but also ensures safety compliance.

In Figure 9, we present the performance of the proposed planning algorithm in four typical scenarios. As the vehicle follows the optimized path, it successfully avoids all occupied cells, remaining free of collisions with the transformed grid-based obstacles. This indicates that the planned trajectory ensures both spatial safety and feasibility. The vehicle thus satisfies collision avoidance requirements and remains kinematically feasible, providing preliminary evidence for the validity of our algorithm.

Figure 10 illustrates the variation trends of the vehicle’s velocity, acceleration, front-wheel steering angle, and angular velocity in the first two typical scenarios. The results indicate that all evaluated parameters (including acceleration, angular velocity, and steering angle) remain within the permissible limits, satisfying the constraints defined by (2).

To further evaluate the performance of our approach, we introduce two comparative planners for experimental validation. The first planner (denoted as Planner 1) replaces within-DC constraints with full-size collision avoidance constraints [32], while the second (denoted as Planner 2) employs within-DC constraints but adopts a different DC construction methodology [27] from the proposed approach.

All three planners share identical configurations except for their collision avoidance strategies. We test their performance across 100 scenarios, with experimental results statistically summarized in

Table 3. Comparison of DC construction efficiency between the proposed method and Planner 2.

Method	Average CPU Time (s)	Max CPU Time (s)
Planner 2	1.9285	3.2293
This work (0.1 m resolution)	0.0893	0.1688
This work (0.2 m resolution)	0.0732	0.1454
This work (0.3 m resolution)	0.0707	0.1476
This work (0.4 m resolution)	0.0683	0.1269

and

Table 4. Comparison of solution speed and success rate across different methods.

Method	Average CPU Time (s)	99 Percentile CPU Time (s)	Solution Success Rate
Planner 1	45.6180	170.6144	-
Planner 2	2.6950	3.8358	100%
This work (0.05 m resolution)	1.1057	1.9922	100%
This work (0.1 m resolution)	1.0422	1.7455	100%
This work (0.2 m resolution)	1.0510	1.9564	100%
This work (0.3 m resolution)	0.9909	1.7857	99%
This work (0.4 m resolution)	1.0159	1.7268	100%
This work (0.5 m resolution)	1.0071	1.7383	99%
This work (0.6 m resolution)	1.0822	3.9333	98%
This work (1.0 m resolution)	1.2533	4.8272	88%

(DC expansion step size: Δs = 0.1 m). In Table 4, there are some points to note: Average CPU time represents the average duration per case across all successful scenarios; 99th percentile CPU time indicates that in all successful scenarios, and 99% of the scenarios have an execution time less than or equal to this value; a success occurs when an NLP solution process ends with a converged optimum. Conversely, a failure refers to convergence to infeasibility or no convergence within default NLP iteration/CPU time. The advantages of employing DC-based strategies for collision avoidance over non-DC approaches, as well as the sensitivity of planning performance to the ∆s parameter—the unit step length in DC specification—have been comprehensively investigated in previous work [27]. Therefore, neither a detailed comparison with Planner 1 nor a further sensitivity analysis on ∆s is presented in this paper. Rather, we focus on evaluating the proposed planner against Planner 2, as both incorporate DC, to validate the effectiveness of our method.

As shown in Table 3, our method significantly outperforms the approach used in Planner 2 for constructing the DC. Specifically, at a resolution of 0.1 m, the average time required for DC construction decreases from 1.9285 s in Planner 2 to 0.0893 s in our method, a reduction of 95.4%. As the resolution further decreases, the time required for DC construction gradually diminishes. When the resolution is reduced to 0.5 m, the reduction further improves to 96.5%. This occurs because lower resolutions correspond to larger grid sizes, enabling each grid to cover a broader area and thereby reducing the number of grids needed to represent environmental obstacles. This reduction directly decreases the computational load of collision detection during DC construction, significantly improving the efficiency of collision avoidance constraint modeling.

To comprehensively validate the feasibility and effectiveness of our proposed method, it is necessary to evaluate it within the complete numerical optimization process. Statistical results in Table 4 demonstrate that, compared to Planner 1 and Planner 2, our planner exhibits superior computational efficiency, with substantially reduced total trajectory optimization time. Specifically, under a grid resolution of 0.1 m, our scheme achieves a 61.3% reduction in average solution time compared to Planner 2, while maintaining a 100% planning success rate. This validates the significant advantages of the proposed method in computational efficiency and robustness.

However, as the resolution decreases, the planning success rate of the planner shows a declining trend. As can be seen from Table 4, when the resolution is reduced to 1 m, the success rate drops from 100% at resolutions of 0.05 to 0.2 m to 88%, indicating a significant decrease in planning success rate. Therefore, in the selection of grid resolution, it should neither be too large nor too small, and a balance between efficiency and success rate needs to be struck: Lower grid resolutions (larger grid sizes) reduce the number of occupied grids representing obstacles, thereby decreasing collision detection computational load and improving efficiency. However, a lower resolution may lead to insufficient precision in obstacle representation, resulting in a loss of passable space (theoretically, the maximum loss can be up to twice the grid resolution), especially in already narrow passages, which may further reduce the drivable area and increase the risk of planning failure. Conversely, a higher grid resolution can depict obstacles more finely, enhancing the accuracy of path planning, but the number of grids used to represent obstacles in the environment increases, leading to higher computational complexity and time consumption for collision detection. Thus, practical applications require an appropriate balance in grid resolution to ensure complementary improvements in planning accuracy and computational efficiency.

As shown in Figure 11, the passable space between two obstacles is relatively narrow. When the grid size exceeds a certain threshold, this region fails to construct a valid DC. Since the initial solution passes through this region and the optimized trajectory maintains a homotopy [33] relationship with the initial solution, the optimized trajectory must also traverse this area. However, due to the low grid resolution, the obstacle representation lacks precision, resulting in the loss of passable space. Consequently, a valid DC cannot be constructed in this region, ultimately leading to solution failure.

Table 4 demonstrates that as the grid size increases from 0.05 m to 0.3 m, the average computation time shows a consistent decreasing trend. This observation indicates that, within this parameter range, reducing the resolution can effectively improve the solver’s computational efficiency. However, when the grid size exceeds 0.5 m, not only does the success rate of the solver decrease, but the computation time also increases. The primary reason for this phenomenon is the significant increase in the optimizer’s computation time, as illustrated in Figure 12: When the grid resolution is too low (i.e., grid sizes are large), although the number of grids representing environmental obstacles is reduced, the passable space is significantly compressed. This leads to a substantial deviation between the constructed DC and the initial solution. Such deviation directly increases the complexity of the search space during the optimization phase and may introduce additional nonlinearities or unfavorable local optima, thereby increasing the difficulty of solving the problem. Before obtaining the converged optimal trajectory (depicted by the blue curves in Figure 12), the optimizer must undergo multiple iterations, during which several intermediate trajectories (shown as black curves) are generated. This iterative process significantly increases the overall computation time.

To further evaluate the performance of the proposed method, we designed an experimental scenario (c.f. Figure 13). Initially, a small number of obstacles were placed in the environment, and the optimization times of both Planner 2 and the proposed method were recorded. Subsequently, the number of obstacles was gradually increased from 1 to 8, and the optimization times of the two planners were measured. The results are summarized in Figure 14. As shown, with the increase in the number of obstacles, the optimization time of Planner 2 exhibits a continuously rising trend, whereas the optimization time of the proposed method remains within a relatively stable range without significant growth, demonstrating its superior scalability in complex environments.

In summary, our approach significantly reduces the computational time compared to Planner 2 due to the following key reasons: First, in our approach, both the DC and the occupied grids representing obstacles are represented as AABBs. This representation allows the collision detection process to be performed via simple range comparisons, avoiding complex geometric operations and point-by-point checks. Specifically, AABB collision detection only requires comparing the boundary values of the rectangles $({\mathrm{x}}_{\min },{\mathrm{x}}_{\max },{\mathrm{y}}_{\min },{\mathrm{y}}_{\max }),$ with a linear time complexity, significantly improving detection efficiency. Second, a reasonable grid resolution is selected and maintained within an appropriate range. On one hand, the resolution is fine enough to accurately represent obstacles in the environment; on the other hand, the resolution is not too low, thus avoiding significant compression of the passable space. This design ensures that the constructed safety driving corridor closely aligns with the real passable space, providing the optimizer with a reliable solution space. Third, we adopt a dynamic DC expansion scheme, which differs from the traditional step-by-step expansion used in Planner 2. In the initial expansion stages, where the available space is larger, we expand simultaneously in all four directions, significantly reducing the number of expansion steps and collision detections, thereby improving efficiency. Furthermore, to avoid issues related to the reduction of passable space, when a collision is detected, the expansion method switches to a more cautious one-direction-at-a-time approach, ensuring the integrity of the DC. This dynamic adjustment strategy significantly accelerates the construction speed of the DC.

The simulation environment in this study is set in unstructured scenarios, where a simplified kinematic bicycle model is sufficient to meet the fundamental requirements of path planning. The core idea of the dynamic corridor (DC) method lies in constructing a collision-free driving corridor through geometric constraints that isolate the vehicle’s trajectory from surrounding obstacles. A key feature of this approach is its generalized framework, which maintains functional independence from specific vehicle dynamics models. Theoretical analysis indicates that the method remains valid not only for simplified kinematic models but also for more complex dynamic models involving nonlinear tire forces, provided that appropriate safety margins and conservative treatments are introduced. The primary contribution of this work lies in accelerating the DC construction process through more efficient modeling techniques, thereby enabling rapid adaptation to diverse scenarios and dynamic models. Future research may explore the extension of this framework to more sophisticated vehicle dynamics, focusing on refined constraint formulations and enhanced computational efficiency.

5. Conclusions

We have developed a rapid trajectory planning approach that first employs grid discretization to simplify obstacle spatial representation, followed by lateral and longitudinal grid merging techniques to reduce the number of grids required for obstacle characterization. Subsequently, it dynamically constructs the drivable corridor by implementing four-direction synchronous expansion in open spaces while switching to single-direction iterative expansion when the expandable area becomes constrained. Simulation results demonstrate that, compared to the classical DC planner, our approach reduces collision avoidance constraint construction time by over 95% at reasonable grid resolutions, while the overall solving time of the optimizer decreases by more than 60%, all while maintaining the same planning success rate. Furthermore, the solving time remains stable as the number of obstacles increases, demonstrating the robustness of the proposed method.

Although selecting an appropriate resolution can balance accuracy and efficiency, the current process relies on empirical tuning and lacks a systematic theoretical foundation. Future work could focus on developing an adaptive grid resolution adjustment mechanism based on scene characteristics, enabling the system to dynamically adjust resolution in response to varying environmental complexities. Moreover, the proposed method is not directly applicable to scenarios involving dynamic obstacles. Extending the approach to accommodate dynamic environments represents a promising direction for future work.