Cooperative trajectory optimization for multiple connected vehicles at an unsignalized intersection

Abstract

Due to the advancement of driverless technology, multi-vehicle cooperation and intelligent travel will become the key direction of intelligent transportation system technology. In high-level autonomous driving, the elimination of intersection signals can effectively improve the efficiency of road traffic. For unsignalized intersection scenarios, considering whether vehicles arrive simultaneously, we propose single-phase and multi-phase trajectory planning methods for multi-vehicle cooperative motion based on numerical optimization techniques. This method includes establishing a vehicle kinematic model, considering the constraints of the vehicle’s physical system and boundary conditions, and developing effective multi-vehicles collaborative motion obstacle avoidance strategies. The trajectory planning task is described in the form of an optimal control problem, and the optimal trajectory is obtained by numerical optimization with the shortest passing time to complete the intersection as the optimization objective. Through numerical simulation analysis of intersection scenarios including crossroad and roundabout, the results demonstrate that the proposed algorithm can achieve optimal trajectories for multi-vehicle cooperative motion while ensuring safe vehicle operation, which improves the efficiency of future intelligent traffic networks.

Keywords

no signal intersection multi-vehicle collaboration numerical optimization single-phase trajectory planning multi-phase trajectory planning

Introduction

The Intelligent Transportation System (ITS) is based on sophisticated infrastructure, effectively integrating advanced information technology, communication technology, sensing technology, control technology, computer technology, and other innovations into the entire transportation management system.¹ It can greatly improve the travel experience of city dwellers. Vehicles in future ITS may have L5-level autonomous driving capabilities,² it can completely replace manual driving in any environment and achieve all-weather unmanned driving, so intersections geared toward ITS can be controlled without signals. In the whole road network, intersections are crucial hubs in the road network where vehicles converge, turn, and disperse. They play a critical role in overall traffic safety.³ Figure 1 is a conceptual diagram of a future unsignalized intersection. When vehicles pass an intersection without signal lights, they can sense the surrounding driving environment through the roadside unit V2I and V2V technology. Through cloud-based supercomputing, vehicles execute cooperative motion decision-making tasks among multiple vehicles, ensuring efficient intersection passage without human intervention.⁴ This technology development will help improve road traffic safety and enhance traffic efficiency.

Figure 1.

Unsignalized intersection model.

The decision planning of multi-vehicle cluster cooperative movement is the key to realize the above-mentioned technology. The current mainstream methods of autonomous driving motion planning are divided into five categories, graph search method, geometric line method, random sampling method, reinforcement learning method, and numerical optimization method. The common algorithms are Dijkstra algorithm and A* algorithm. The improved A*⁵ obtained a shorter path length, fewer inflection points and smoother, which can avoid the collision between the vehicle contour and the obstacle. The graph search method can quickly obtain the driving path, but the planning results are relatively rough. The graph search method is usually used as a preprocessing for other methods. In literature,⁶ according to the geometric relationship between vehicles, surrounding obstacles, and target parking Spaces, geometric methods such as tangent arcs and straight lines were used to obtain parking paths. This method has the advantages of simple algorithm and high solving efficiency, but the curvature of the planned path is discontinuous. The random sampling method generates paths at random sampling points in the configuration space. Based on the RRT algorithm, literature⁷ used the target offset strategy and the pruning optimization algorithm to greatly shorten the length of the path planning and the path search time. However, the random sampling algorithm has poor ability to accurately deal with complex constraints. Machine learning algorithms learn path planning models based on a large amount of training data. The advantage of machine learning algorithm is that it has fast calculation speed and certain generalization ability, but it needs to collect a large number of training samples. Literature⁸ proposed a deep neural network (DNN)-based trajectory planning method for both parallel and perpendicular parking scenarios, and the presented strategy was able to theoretically guarantee the stability of the control scheme and the convergence of the tracking error. For the autonomous parking problem, an algorithm based on polynomial parameterization and genetic algorithm optimization was proposed in the literature.⁹ In literature,¹⁰ trajectory planning of intelligent vehicles (ICVs) in cooperative driving environment is discussed, and an adaptive trajectory planning method named AdaTP was proposed. Results demonstrated that this method can generate shorter and more reliable trajectories. Literature¹¹ introduced the localized trajectory planning problem for Automated Guided Vehicles (AGVs), and adopted reinforcement learning method based on double delay depth deterministic strategy gradient (TD3) to improve the performance and efficiency of trajectory planning. Literature^12,13 proposed a new reinforcement learning strategy to realize vehicle trajectory planning, which verified the superiority of the algorithm. The results had great significance for the practical implementation in actual scenarios.

The essence of vehicle motion is to solve the problem of dynamic system, by describing the dynamic characteristics of the vehicle and constructing the equation of the dynamic system, together with the constraints and the objective function, the optimal control problem is constituted. The research shows that the optimal control problem can describe the vehicle trajectory planning problem intuitively and accurately, which is not possessed by other methods.¹⁴ The overall idea of numerical optimization methods is to transform the vehicle motion planning problem into an optimal control problem, and then the optimal control problem is discretized into a nonlinear programming problem through the numerical optimization algorithm. Subsequently, the optimization problem is solved using gradient descent methods to obtain the planning results. This method is characterized by fast convergence speed and low sensitivity to initial value. This method is characterized by fast convergence speed and low sensitivity to initial value. The numerical optimization method can obtain the position, velocity, acceleration, heading angle, and other time-related parameters of the vehicle movement, which can be directly used for low-level tracking control of vehicles. Meanwhile, the objective function can be set to obtain the vehicle trajectory with the shortest time. Therefore, this paper adopts the numerical optimization method to solve the multi-vehicle cooperative trajectory planning problem. Literature¹⁵ introduced an optimization method for planning fast maneuvers of trailer vehicles in twisting and narrow tunnels. An optimization-based maneuver planner is proposed, which had obvious advantages over the popular sampling and search-based planners. Literature¹⁶ proposed a cooperative trajectory planning method for vehicle cut-through scenarios based on optimal control. This method used a fifth-degree polynomial to generate an initial solution to the optimal control problem. The purpose of this approach was to ensure that connected intelligent vehicles (CAVs) can merge into the main road quickly and efficiently. Literature¹⁷ described a hierarchical control algorithm for self-driving vehicle trajectory planning and tracking based on Model Predictive Control (MPC) and Gaussian Pseudo-spectral Method (GPM). The article validated the effectiveness and feasibility of the algorithm through simulations and real vehicle tests. In literature,¹⁸ the optimal trajectory planning for autonomous parking time of tractor semi-trailer based on segmental Gaussian pseudo-spectrum method is proposed. Compared with the traditional pseudo-spectrum method, this method can obtain shorter parking time and significantly increase the convergence speed.

Multi-vehicle cooperative motion planning refers to the process of solving paths or trajectories for multiple cars, given their initial motion states, destinations, and constraints.¹⁹ In centralized control, the central computing control unit makes decisions by collecting all the controlled vehicle information and environmental information, and then distributes the optimal control sequence signal to the corresponding controlled vehicle and executes it. Centralized control can effectively formulate the optimal control problem for the entire system, and the global optimal solution can be obtained through the top-level control strategy. A multi-vehicle formation trajectory planning method is proposed in literature,²⁰ and the simulation results showed that vehicles can save a lot of passing time under high traffic volume. Literature²¹ proposed a multi-vehicle cooperative autonomous parking trajectory planning based on connected fifth degree polynomials and genetic algorithm optimization, which can be significantly reduced overall stopping time. Literature²² introduced a method for multilane unsignalized intersection cooperation. This method took flexible lane directions by calculating the two-dimensional distribution of vehicles and planning conflict-free vehicle crossing of the intersection. Literature²³ proposed a hierarchical multi-vehicle cooperative motion planning method based on interactive spatio-temporal corridors (ISTCs), and simulation experiments are conducted under unsignalized intersections to verify the feasibility of the framework. For the Autonomous Intersection Management (AIM) problem, Li et al.^24–26 modeled the multi-vehicle coordination problem as a centralized optimization problem, and used batch processing or phased strategy to reduce the computational complexity. The availability of the obtained theoretical results was verified by simulation experiments, which improved the efficiency and fairness of vehicle traffic. Li and Zhang²⁷ proposed a fault-tolerant centralized collaborative optimization motion planning method for networked autonomous vehicles (CAVs). The path adjustment effect of the vehicle under different fault degrees is verified by numerical simulation, and the effectiveness of the method in some failure scenarios is demonstrated. The above research results have made a positive contribution to multi-vehicle collaborative trajectory planning. Literature²⁸ proposed a lane-free joint optimal control framework. Vehicles can freely use the two-dimensional space of the intersection to improve traffic efficiency and flexibility. At the same time, the acceleration, steering angle, fuel consumption, and expected speed tracking were simultaneously optimized in the objective function, which significantly improved passenger comfort. In this paper, a centralized frame algorithm is used to plan the path of multi-vehicle cluster motion. The innovations and research work of this paper mainly include: (1) A mathematical model of multi-vehicle cooperative motion is established, which includes the mathematical modeling of collision avoidance strategies between single vehicle and static obstacles, multi-vehicle and static obstacles, and between vehicles. (2) Taking the shortest vehicle passing time through the intersection as the optimization objective, the multi-vehicles centralized optimal trajectory planning for unsignalized intersections is transformed into a nonlinear optimal control problem, and then solved by numerical optimization method. Aiming at the complex mathematical model of the multi-vehicle cooperative motion trajectory solving problem, which cannot converge quickly. Based on the driving area of vehicles at the intersection, the process of multi-vehicle passing the intersection is divided into three phases, so as to narrow the scene range of trajectory solving at each phase, simplify constraints, and improve the convergence efficiency of the solution. At the same time, in the process of solving in phases, the continuity constraint problem of all vehicle control variables in different phases is solved, so as to ensure the one-time solution of multi-phase. Another advantage of solving in phases is that if there is a vehicle in the intersection that has not yet completely left the intersection in the next solution, it is sufficient to determine its phase in the next solution while preserving the continuity of the variables.

Vehicle motion process analysis

Individual vehicle kinematics modeling

In the early stages of intelligent transportation system, the vehicle is traveling in the working condition of urban intersection, considering the multi-vehicle safety problem, the vehicle traveling speed is low, it can be considered that all wheels are rolling state, and the effects of lateral forces on vehicles can be disregarded. Wheel slippage is not observed. Based on the Ackermann front-wheel steering principle, the vehicle kinematic model is established in a ground-fixed coordinate system, with the rear axle center point as the observed state variable.

In Figure 2, $R (x, y)$ is the rear axle center point of the vehicle; $v$ is the rear axle center point speed; $O$ is the instantaneous turning center of the vehicle; $l$ is the wheelbase of the vehicle; $f, r$ are the front and rear overhangs of the vehicle; $d$ is the width of the vehicle; $φ$ is the equivalent front wheel pendulum angle; $θ$ is the heading angle of the vehicle body, and $A, B, C, D$ are the four vertices of the vehicle body, respectively.

Figure 2.

Vehicle kinematics model.

According to the kinematic model, the differential equations describing the vehicle’s motion are as follows:

{\begin{matrix} \overset{\cdot}{x} (t) = v (t) \cdot \cos θ (t) \\ \overset{\cdot}{y} (t) = v (t) \cdot \sin θ (t) \\ \overset{\cdot}{v} (t) = a (t) \\ \overset{\cdot}{a} (t) = jerk (t) \\ \overset{\cdot}{θ} (t) = v (t) \cdot \tan φ (t) / l \\ \overset{\cdot}{φ} (t) = ω (t) \end{matrix}, t \in [t_{0}, t_{f}]

(1)

Where, $a$ and $jerk$ are vehicle acceleration and vehicle acceleration change rate, respectively, $ω$ is the front wheel swing Angle speed. $t_{0}$ and $t_{f}$ are the start time and end time of the vehicle respectively.

Constraint condition

Physical condition constraint

During vehicle driving, it is necessary to satisfy its own physical and mechanical constraints, which impose limitations on certain state variables, and control variables of the system as follows:

{\begin{matrix} | v (t) | \leq v_{max} \\ | a (t) | \leq a_{max} \\ | j (t) | \leq j_{max} \\ | φ (t) | \leq φ_{max} \\ | ω (t) | \leq ω_{max} \end{matrix}, t \in [t_{0}, t_{f}]

(2)

Where $v_{\max}$ is the maximum speed allowed in the corresponding working conditions of the vehicle; $a_{\max}$ is the maximum acceleration of the vehicle; $j_{\max}$ is the maximum acceleration change rate; $φ_{\max}$ is the maximum front wheel swing Angle; $ω_{\max}$ is the maximum front wheel swing Angle speed.

Lane boundary constraints

In this paper, the intersection and roundabout conditions are selected, when vehicles pass through an intersection, one of the first external constraints they should consider are the boundary condition constraints. Vehicles move in the boundary area of the road, as shown in Figure 3, $L$ represents the maximum length of the road in the $X$ direction, and $W$ represents the maximum length of the road in the $Y$ direction. During the movement of the vehicle from the starting point to the endpoint, the four vertices of the rectangle $A, B, C, D$ all need to lie within this area of the road, which can be expressed as:

{\begin{matrix} [Ax (t), Bx (t), Cx (t), Dx (t)] ϵ (0, L) \\ [Ay (t), By (t), Cy (t), Dy (t)] ϵ (0, W) \end{matrix}

(3)

Figure 3.

Road boundary constraint diagram: (a) intersection and (b) roundabout intersection.

Vehicle obstacle avoidance constraints

When vehicles pass through intersections, they must avoid collisions with each other and must not collide with the road boundaries during their travel. Strict constraints are necessary for the vehicles’ trajectory. Due to the vehicle’s actual profile closely resembling a rectangle, a common constraint method is to use a quadrilateral to represent the vehicle’s trajectory. Based on the vehicle’s dimensions, rear axle center point position, and geometric relationships, the horizontal and vertical coordinate values of the four contour vertices of the vehicle can be derived. Here, an expanded rectangle is used to describe the contour of the vehicle, as shown in Figure 4. $d_{safe}$ represents the safety threshold ensuring collision-free vehicle operation, then the coordinates of the four contour vertices of the enlarged vehicle are as follows:

\begin{matrix} [\begin{matrix} A_{x} (t) \\ A_{y} (t) \\ B_{x} (t) \\ B_{y} (t) \\ C_{x} (t) \\ C_{y} (t) \\ D_{x} (t) \\ D_{y} (t) \end{matrix}] = [\begin{matrix} x (t) + (l + f + d_{safe}) \cdot \cos θ (t) - (\frac{d}{2} + d_{safe}) \cdot \sin θ (t) \\ y (t) + (l + f + d_{safe}) \cdot \sin θ (t) + (\frac{d}{2} + d_{safe}) \cdot \cos θ (t) \\ x (t) + (l + f + d_{safe}) \cdot \cos θ (t) + (\frac{d}{2} + d_{safe}) \cdot \sin θ (t) \\ y (t) + (l + f + d_{safe}) \cdot \sin θ (t) - (\frac{d}{2} + d_{safe}) \cdot \cos θ (t) \\ x (t) - (r + d_{safe}) \cdot \cos θ (t) + (\frac{d}{2} + d_{safe}) \cdot \sin θ (t) \\ y (t) - (r + d_{safe}) \cdot \sin θ (t) - (\frac{d}{2} + d_{safe}) \cdot \cos θ (t) \\ x (t) - (r + d_{safe}) \cdot \cos θ (t) - (\frac{d}{2} + d_{safe}) \cdot \sin θ (t) \\ y (t) - (r + d_{safe}) \cdot \sin θ (t) + (\frac{d}{2} + d_{safe}) \cdot \cos θ (t) \end{matrix}], t \in [t_{0}, t_{f}] \end{matrix}

(4)

Figure 4.

Schematic diagram of vehicle movement obstacle avoidance at intersection.

When a rectangle is used to describe the vehicle profile, if it is necessary to meet that there is no collision between a vehicle and another vehicle, and the geometric meaning of this constraint on a two-dimensional plane is: at any moment, no point on the other vehicle falls within the rectangle outline of the own vehicle. As shown in Figure 4, the orange regions around the vehicle represent the safety threshold ensuring collision-free operation. If the vehicle does not collide with $Q_{i}$ point during its movement (where $i = A, B, C, D$ , representing the vehicle’s four vertices.), the coordinates of point $Q_{i}$ are always outside the region of rectangle $ABCD$ , and the four sides of the rectangle are noted as the straight lines $L_{AB}$ , $L_{BC}$ , $L_{CD}$ , $L_{AD}$ , whose linear equations are as follows:

{\begin{matrix} L_{AB} = k_{1} \cdot x + y_{1} \\ L_{CD} = k_{1} \cdot x + y_{2} \\ L_{AD} = k_{2} \cdot x + y_{3} \\ L_{BC} = k_{2} \cdot x + y_{4} \end{matrix}

(5)

Where $k_{1}$ is the slope of lines $L_{AB}$ and $L_{CD}$ , and $k_{2}$ is the slope of lines $L_{AD}$ and $L_{BC}$ . When the line $L_{AB}$ is parallel or perpendicular to the $X$ -axis, if the $Q_{i}$ point of another vehicle is outside the rectangle $ABCD$ , the coordinates of the $Q_{i}$ point must meet the following conditions:

{\begin{matrix} Q_{ix} (t) < D_{x} (t) | | Q_{ix} (t) > A_{x} (t) | | Q_{i y} (t) < C_{y} (t) | | \\ Q_{i y} (t) > A_{y} (t), θ = 0^{°} \\ Q_{ix} (t) < D_{x} (t) | | Q_{ix} (t) > C_{x} (t) | | Q_{i y} (t) < D_{y} (t) | | \\ Q_{i y} (t) > A_{y} (t), θ = 90^{°} \\ Q_{ix} (t) < A_{x} (t) | | Q_{ix} (t) > D_{x} (t) | | Q_{i y} (t) < D_{y} (t) | | \\ Q_{i y} (t) > C_{y} (t), θ = 180^{°} \\ Q_{ix} (t) < C_{x} (t) | | Q_{ix} (t) > A_{x} (t) | | Q_{i y} (t) < A_{y} (t) \\ | | Q_{i y} (t) > D_{y} (t), θ = 270^{°} \end{matrix}

(6)

When the line $L_{AB}$ is neither parallel nor perpendicular to the $X$ -axis, the coordinates of the point $Q_{i}$ need to satisfy the following conditions if the point $Q_{i}$ is made to be outside of $ABCD$ :

{\begin{matrix} (Q_{iy} - k_{1} \cdot Q_{ix}) < y_{2} ∥ (Q_{iy} - k_{1} \cdot Q_{ix}) > y_{1} \\ (Q_{iy} - k_{2} \cdot Q_{ix}) < y_{4} ∥ (Q_{iy} - k_{2} \cdot Q_{ix}) > y_{3} \end{matrix}

(7)

Where $i = A, B, C, D$ , represent the four vertices of the vehicle, all of which need to satisfy the above conditions.

For road boundary obstacle avoidance, the first step is to describe the boundary contours of the intersection as four rectangles. And the orange region around the road contour is the safety threshold to ensure that the boundary does not collide with the vehicle, which are rectangles $M_{1 j}$ , $M_{2 j}$ , $M_{3 j}$ , $M_{4 j}$ ( $j = A, B, C, D$ ). Here the vehicle and rectangular $M_{1 j}$ obstacle avoidance as an example. Then the coordinates of rectangle $M_{1}$ are always outside the region of rectangle ABCD. When the line $L_{AB}$ is parallel or perpendicular to the $X$ -axis, the coordinates of point $M_{1}$ must meet the following conditions:

{\begin{matrix} M_{1 x} < D_{x} (t) | | M_{1 x} > A_{x} (t) | | M_{1 y} < C_{y} (t) | | M_{1 y} > A_{y} (t), θ = 0^{°} \\ M_{1 x} < D_{x} (t) | | M_{1 x} > C_{x} (t) | | M_{1 y} < D_{y} (t) | | M_{1 y} > A_{y} (t), θ = 90^{°} \\ M_{1 x} < A_{x} (t) | | M_{1 x} > D_{x} (t) | | M_{1 y} < D_{y} (t) | | M_{1 y} > C_{y} (t), θ = 180^{°} \\ M_{1 x} < C_{x} (t) | | M_{1 x} > A_{x} (t) | | M_{1 y} < A_{y} (t) | | M_{1 y} > D_{y} (t), θ = 270^{°} \end{matrix}

(8)

When the line $L_{AB}$ is neither parallel nor perpendicular to the $X$ -axis, the coordinates of the point $M_{1}$ need to satisfy the following conditions if the point $M_{1}$ is made to be outside of $ABCD$ :

{\begin{matrix} (M_{1 y} - k_{1} \cdot M_{1 x}) < y_{2} ∥ (M_{1 y} - k_{1} \cdot M_{1 x}) > y_{1} \\ (M_{1 y} - k_{2} \cdot M_{1 x}) < y_{4} ∥ (M_{1 y} - k_{2} \cdot M_{1 x}) > y_{3} \end{matrix}

(9)

When vehicles pass through roundabout intersections, it is necessary to consider collisions with the roundabout boundary. The center of the roundabout is point $O$ , the radius of the inner roundabout is $R_{1}$ , and the rectangles $A, B, C, D$ are the four vertices of the vehicles. The orange line in Figure 5 describes the collision constraints between vehicles, while the blue line represents the collision constraints between vehicles and the traffic circle boundary. If vehicles are to avoid collisions with the inner boundary of the roundabout, then the area enclosed by the circles formed by the vehicle’s four vertices and the roundabout center must be greater than the area enclosed by the inner roundabout. This constraint can be expressed as follows:

{\begin{matrix} {(A_{x} - O_{x})}^{2} + {(A_{y} - O_{y})}^{2} \geq R_{1}^{2} \\ {(B_{x} - O_{x})}^{2} + {(B_{y} - O_{y})}^{2} \geq R_{1}^{2} \\ {(C_{x} - O_{x})}^{2} + {(C_{y} - O_{y})}^{2} \geq R_{1}^{2} \\ {(D_{x} - O_{x})}^{2} + {(D_{y} - O_{y})}^{2} \geq R_{1}^{2} \end{matrix}

(10)

Figure 5.

Schematic diagram for avoiding obstacles for vehicles traveling around the roundabout.

The vehicle collision constraint condition is the same as the obstacle avoidance of the vehicle at the intersection. Refer to equation (6). According to our traffic regulations, vehicles on traffic circle roads travel in a counterclockwise direction.

Multi-vehicle cluster cooperative motion trajectory planning

Description of multi-vehicle cluster cooperative motion problem

The optimal control problem typically contains the dynamics differential equations of the controlled system, path constraints, boundary constraints, and performance index function. The path constraints contain both equal and unequal constraints. Under the constraints satisfied by the controlled system, the optimal control parameters are obtained by solving the performance index function. The nonlinear optimal control Bolza problem can be described as:

s . t . {\begin{matrix} \overset{\cdot}{x} (t) = f (x (t), μ (t), t) \\ P (x (t), μ (t), t) \leq 0 \\ P (x (t), μ (t), t) = 0 \\ Q (x (t_{0}), μ (t_{0}), t_{0}, x (t_{f}), μ (t_{f}), t_{f}) = 0 ∥ c \end{matrix} t \in [t_{0}, t_{f}]

(11)

\min η = \emptyset (x (t_{0}), t_{0}, x (t_{f}), t_{f}) + \int_{t_{0}}^{t_{f}} g (x (t), μ (t), t) dt

(12)

In equation (11): the first row describes the differential equations of state of the system; the second row the unequal path constraints; the third row the equal path constraints; and the fourth row the boundary constraints, where $c$ is a constant. In equation (12): x(t) and μ(t) are the state variables and control variables of the system, respectively, and satisfy $x (t) \in R^{n}$ , $μ (t) \in R^{m}$ ; $t_{0}$ and $t_{f}$ are the initial and termination moments of the system, respectively; minη is the objective function of the optimal control problem. To enhance the efficiency of vehicles passing through the intersection, this paper sets the objective function of the optimal control problem as the shortest time for each group of vehicles to pass through the intersection, denoted as, $min t_{f}$ . The nonlinear optimal control problem for vehicle trajectory planning can be described as the following mathematical model:

\min t_{f}

(13)

s . t . {\begin{matrix} Equation (1) \\ Equation (2), (3) \\ Equation (6), (7), (8), (9), (10) \end{matrix} t \in [t_{0}, t_{f}]

(14)

Multi-vehicle cooperative motion trajectory solution based on numerical optimization method

The numerical optimization is a method for solving nonlinear optimal control problems. This method discretizes the continuous optimization problem at Legendre-Gauss (LG) points, Lagrange interpolation polynomials are constructed to approximate state variables and control variables, and replaces differential equation constraints with algebraic constraints. Then the optimal control problem is transformed into Nonlinear Programming (NLP) problem solving. Numerical optimization methods are characterized by their high adaptability to initial values, fast convergence speed, and high solution accuracy.

Numerical optimization method can solve single-phase and multi-phase multi-vehicles trajectory planning problems. Single-phase trajectory planning refers to the vehicle trajectory planning for a direct solution, the vehicle departs from the starting point at the same time, and then arrives at the endpoint at the same time. However, vehicles can not only start at the intersection at the same time, but also start from the intersection in the process of other vehicles passing the intersection, at this time, the trajectory solving computation increases. In this case, single-phase trajectory solution cannot be realized. Therefore, multiple-phase are used to solve the problem, and each phase has its specific constraints. At the same time, it is necessary to add corresponding endpoint constraints at the boundary between multiple phases to maintain the continuity of the values of vehicle state variables and control variables. During segmented solving, collision avoidance constraints and endpoint constraints are simplified, which improves the success rate and convergence efficiency of multi-vehicle cooperative trajectory solving. For complex scenarios like intersections, where the arrival times of vehicles at the intersection are random, multi-stage trajectory planning can be employed to solve the problem.

Time domain transformation

The vehicle start-stop motion time range [ $t_{0}, t_{f}$ ] is divided into $n$ sub-intervals, ${[t_{0}, t_{1}], [t_{1}, t_{2}], \dots, [t_{m - 1}, t_{m}], \dots, [t_{n - 1}, t_{n}]}$ , $t_{n} = t_{f}, m \in [1, n]$ . In this algorithm, the interval of time-variable discrete points is τ∈ [−1, 1], so it is necessary to transform each sub-interval moment in time domain. Any moment $t \in [t_{m - 1}, t_{m}]$ in the $m$ th sub-interval can be transformed to $τ \in [- 1, 1]$ by equation (15).

τ = \frac{2 \cdot t - (t_{m - 1} + t_{m})}{t_{m} - t_{m - 1}}

(15)

Through the time-domain transformation, the optimal control problem in the subinterval $[t_{m - 1}, t_{m}]$ can be described as follows:

\begin{matrix} min η^{(m)} = ϕ^{(m)} (x^{(m)} (τ_{0}), t_{m - 1}, x^{(m)} (τ_{f}), t_{m}) \\ + \frac{t_{m} - t_{m - 1}}{2} \int_{τ_{0}}^{τ_{f}} g (x^{(m)} (τ), μ^{(m)} (τ), τ : t_{m - 1}, t_{m}) d τ \end{matrix}

(16)

s . t . {\begin{matrix} {\overset{\cdot}{x}}^{(m)} (τ) = \frac{t_{m} - t_{m - 1}}{2} f (x^{(m)} (τ), μ^{(m)} (τ), τ : t_{m - 1}, t_{m}) \\ P (x^{(m)} (τ), μ^{(m)} (τ), τ : t_{m - 1}, t_{m}) \leq 0 \\ P (x^{(m)} (τ), μ^{(m)} (τ), τ : t_{m - 1}, t_{m}) = 0 \\ Q (x^{(m)} (τ_{0}), τ_{0}, x^{(m)} (τ_{f}), τ_{f}) = 0 ∥ c \end{matrix} τ \in [- 1, 1]

(17)

In equation (16), $x^{(m)} (τ)$ and $μ^{(m)} (τ)$ are the state variables and control variables at the moment t of the $m$ th sub-interval $[t_{m - 1}, t_{m}]$ after the time-domain transformation to τ, respectively.

Variable discretization

The above state and control variables are discretized at the Legendre-Gauss (LG) collocation points, and Lagrange polynomials at the discretization points are constructed to approximate the state variable $x$ and the control variable $μ$ of the vehicle motion process. The Legendre polynomials of order $N$ on the interval $[τ_{0}, τ_{f}]$ are:

P_{N} (τ) = \frac{1}{2^{N} \cdot N!} \times \frac{d^{N}}{d τ^{N}} \cdot [(τ^{2} - 1)^{N}]

(18)

Taking the zeros $(τ_{1}, τ_{2}, \dots, τ_{N})$ of equation (18) and the initial point $τ_{0} = - 1$ to form $N + 1$ configuration points, the state variables and control variables at each configuration point are as follows:

{\begin{matrix} x (τ_{0}), x (τ_{1}), \dots, x (τ_{N}) \\ μ (τ_{0}), μ (τ_{1}), \dots, μ (τ_{N}) \end{matrix}

(19)

The Lagrange interpolation polynomial is constructed at $N + 1$ configuration points for interpolation, and the Lagrange basis function is as follows:

{\begin{matrix} L_{i}^{(n)} (τ) = Π_{j = 0, j \neq i}^{N} \frac{τ - τ_{j}^{(n)}}{τ_{i} - τ_{j}^{(n)}} \\ {\tilde{L}}_{i}^{(n)} (τ) = Π_{j = 1, j \neq i}^{N} \frac{τ - τ_{j}^{(n)}}{τ_{i} - τ_{j}^{(n)}} \end{matrix}

(20)

From equations (18) to (20), the approximate polynomial of the state variable and control variable in the subinterval $[t_{m - 1}, t_{m}]$ can be obtained and expressed as follows:

{\begin{matrix} x^{(m)} (τ) \approx \sum_{i = 0}^{N} L_{i}^{(n)} (τ) \cdot X^{(m)} (τ_{i}) \\ μ^{(m)} (τ) \approx \sum_{i = 0}^{N} {\tilde{L}}_{i}^{(n)} (τ) \cdot U^{(m)} (τ_{i}) \end{matrix}

(21)

The derivation of equation (20) can be used to approximate the differential of the state variable by using the differential of the approximate polynomial. For example, the derivative of the state variable at the time point $τ_{k}$ can be expressed as:

{\overset{\cdot}{x}}^{(n)} (τ_{k}) \approx \sum_{i = 0}^{N} {\overset{\cdot}{L}}_{i}^{(n)} (τ_{k}) \cdot X^{(m)} (τ_{i})

(22)

Combining equations (16), (17), and (22), the kinematic differential equation constraints can be converted into algebraic equation constraints:

{\overset{\cdot}{x}}^{(n)} (τ_{k}) - \frac{t_{m} - t_{m - 1}}{2} f (X (τ_{k}), U (τ_{k}), τ_{k}) = 0

(23)

Constraints under discrete conditions

The equality and inequality constraints of the discrete system variables are as follows:

{\begin{matrix} P (X^{(m)} (τ_{k}), U^{(m)} (τ_{k}), τ_{k}) \leq 0 \\ P (X^{(m)} (τ_{k}), U^{(m)} (τ_{k}), τ_{k}) = 0 \\ Q (X^{(m)} (τ_{0}), τ_{0}, X^{(m)} (τ_{f}), τ_{f}) = 0 ∥ c \end{matrix}

(24)

In the actual motion of vehicles, it is necessary to ensure the continuity of path curvature, thus the state variables must also be continuous change, that is, the requirement of the terminal moment of each sub-interval state variables and the next sub-interval of the initial moment of the state variables of the same value, but the change of the control variables can be discontinuous, the specific constraints are as follows:

X^{(m - 1)} (τ_{f}) = X^{(m)} (τ_{0})

(25)

As can be seen from equation (19), LG match points do not contain the terminal time of each subinterval, so the terminal state can be approximated by Gauss integral according to equation (25), and the approximate value of state variable at terminal time can be obtained as follows:

X^{(m)} (τ_{f}) \approx X^{(m)} (τ_{0}) + \frac{t_{m} - t_{m - 1}}{2} \sum_{k = 0}^{N} ρ_{k} \cdot f (X (τ_{k}), U (τ_{k}), τ_{k})

(26)

Where $ρ_{k}$ is the Gaussian integral weight coefficient, which can be expressed as:

ρ_{k} = \frac{2}{(1 - τ_{k}^{2}) {({\overset{\cdot}{P}}_{N} (τ_{k}))}^{2}}, k = 1, 2 \dots, N - 1

(27)

Performance indexes under discrete conditions

Through the above discretization process, the optimal control problem for vehicle trajectory planning can be transformed into a nonlinear programming problem. The performance indexes under discrete condition is as follows:

\begin{matrix} min η^{(m)} = ϕ^{(m)} (x^{(m)} (τ_{0}), t_{m - 1}, x^{(m)} (τ_{f}), t_{m}) + \\ \frac{t_{m} - t_{m - 1}}{2} \sum_{k = 0}^{N} ρ_{k} \cdot g (X (τ_{k}), U (τ_{k}), τ_{k}) \end{matrix}

(28)

In summary, through discretization at designated points, the Bolza optimal control problem is converted into a standard NLP problem, which provides a foundation for the approximate solution of the subsequent optimal control problems.

Phased solving of unsignalized intersections vehicle trajectory

Based on the driving area of vehicles at the intersection, the process of multi-vehicles passing the intersection is divided into three phases, so as to narrow the scene range of trajectory solving at each phase, simplify constraints, and improve the convergence efficiency of solution. At the same time, in the process of solving in phases, the continuity constraint problem of all vehicle control variables in different phases is solved, so as to ensure the one-time solution of multi-phase. As shown in the figure below, the dashed box indicates what we consider as the intersection range, and the vehicle enters the region to start the centralized trajectory solving. The first phase is that the vehicles move from the initial position to the area near the intersection, and at the same time, it will adjust the vehicle position and heading angle to facilitate further entry into the intersection area, which we call the entry into the intersection phase, and the corresponding traveling area is $Z_{1}$ in the figure. In the second phase, vehicles pass through the central area of the intersection, and the obstacle avoidance constraints between vehicles and road boundary constraints are strictly met during the movement. Therefore, the vehicle needs to continuously adjust its position and attitude to satisfy the constraints. This phase is called the crossing phase, and the corresponding driving area is $Z_{2}$ in the figure. The third phase is when the vehicles leave the intersection area and complete the entire trajectory planning. This phase is called the exit intersection phase, and the corresponding driving area is $Z_{3}$ in the figure below. Due to the phased solution, the motion scene is narrowed down in each phase, the constraints are simplified, and the solving efficiency is greatly improved. Another advantage of solving in phases is that if there is a vehicle in the intersection that has not yet completely left the intersection in the next solution, it is sufficient to determine its phase in the next solution while preserving the continuity of the variables. Figure 6 shows the schematic diagram of the phase division of vehicles passing through the intersection area with left turn, right turn, and straight, $L_{x}$ is the length of the roadway in the horizontal direction, $L_{y}$ is the length of the roadway in the longitudinal direction, $L_{d}$ is the distance from the end of the intersection of the vehicle’s initial and termination positions, and $W_{x}$ is the width of the lane. Figure 7 shows the schematic diagram of the regional phase division of a vehicle passing through a roundabout. $R_{3}$ the outer island boundary of the intersection, $R_{2}$ is the boundary line of the inner island, and $R_{1}$ is the radius of the center round island.

Figure 6.

Schematic diagram of the division of the intersection area.

Figure 7.

Schematic diagram of the area division at the roundabout intersection.

The state variables of the vehicle at each phase junction should be constrained to ensure continuity.

[\begin{matrix} x_{k 2} (t_{0}) - x_{k 1} (t_{f}) \\ y_{k 2} (t_{0}) - y_{k 1} (t_{f}) \\ v_{k 2} (t_{0}) - v_{k 1} (t_{f}) \\ θ_{k 2} (t_{0}) - θ_{k 1} (t_{f}) \\ a_{k 2} (t_{0}) - a_{k 1} (t_{f}) \\ φ_{k 2} (t_{0}) - φ_{k 1} (t_{f}) \end{matrix}] = 0 & & [\begin{matrix} x_{k 3} (t_{0}) - x_{k 2} (t_{f}) \\ y_{k 3} (t_{0}) - y_{k 2} (t_{f}) \\ v_{k 3} (t_{0}) - v_{k 2} (t_{f}) \\ θ_{k 3} (t_{0}) - θ_{k 2} (t_{f}) \\ a_{k 3} (t_{0}) - a_{k 2} (t_{f}) \\ φ_{k 3} (t_{0}) - φ_{k 2} (t_{f}) \end{matrix}] = 0

(29)

Where $k$ represents the number of each vehicle. At the same time, the position of the vehicle’s contour vertex is constrained according to the driving area, and the following equations show the constraints for a vehicle turning left through the intersection.

Z_{1} {\begin{matrix} L_{d} \leq [A_{x}, B_{x}, C_{x}, D_{x}] \leq L_{x} \\ L_{y} \leq [A_{y}, B_{y}, C_{y}, D_{y}] \leq W_{x} + L_{y} \end{matrix}

(30)

\begin{matrix} Z_{2} {\begin{matrix} L_{x} - Z \leq [A_{x}, B_{x}, C_{x}, D_{x}] \leq L_{x} \\ L_{y} \leq [A_{y}, B_{y}, C_{y}, D_{y}] \leq W_{x} + L_{y} \end{matrix} & & \\ {\begin{matrix} L_{x} \leq [A_{x}, B_{x}, C_{x}, D_{x}] \leq L_{x} + 2 W_{x} \\ L_{y} \leq [A_{y}, B_{y}, C_{y}, D_{y}] \leq L_{y} + 2 W_{x} \end{matrix} \end{matrix}

(31)

\begin{matrix} Z_{3} {\begin{matrix} L_{x} \leq [A_{x}, B_{x}, C_{x}, D_{x}] \leq L_{x} + 2 W_{x} \\ L_{y} \leq [A_{y}, B_{y}, C_{y}, D_{y}] \leq L_{y} + 2 W_{x} \end{matrix} & & \\ {\begin{matrix} L_{x} + W_{x} \leq [A_{x}, B_{x}, C_{x}, D_{x}] \leq L_{x} + 2 W_{x} \\ L_{y} + 2 W_{x} \leq [A_{y}, B_{y}, C_{y}, D_{y}] \leq 2 L_{y} + 2 W_{x} - L_{d} \end{matrix} \end{matrix}

(32)

The following equations represent the constraint conditions for a vehicle to travel counterclockwise from the starting intersection and leave from the third roundabout intersection.

Z_{1} {\begin{matrix} 0 \leq [A_{x}, B_{x}, C_{x}, D_{x}] \leq W_{x} \\ - (L_{y} + R_{3} - L_{d}) \leq [A_{y}, B_{y}, C_{y}, D_{y}] \leq - R_{3} \end{matrix}

(33)

\begin{matrix} Z_{2} {\begin{matrix} 0 \leq [A_{x}, B_{x}, C_{x}, D_{x}] \leq W_{x} \\ - (Z + R_{3}) \leq [A_{y}, B_{y}, C_{y}, D_{y}] \leq - R_{3} \end{matrix} & & \\ {\begin{matrix} \sqrt{{(m_{x} - O_{x})}^{2} + {(m_{y} - O_{y})}^{2}} \leq R_{3} \\ - R_{3} \leq [A_{y}, B_{y}, C_{y}, D_{y}] \leq R_{3} \end{matrix} \end{matrix}

(34)

Z_{3} {\begin{matrix} 0 \leq [A_{x}, B_{x}, C_{x}, D_{x}] \leq W_{x} \\ R_{2} \leq [A_{y}, B_{y}, C_{y}, D_{y}] \leq L_{y} + R_{3} - L_{d} \end{matrix}

(35)

$m = (A, B, C, D)$ represents the four vertices of the vehicle.

The algorithm for solving the unsignalized vehicle trajectory planning in segments is shown in Table 1 below.

Table 1.

Description of the segmentation algorithm.

Algorithm flow
1. Parameter initialization and discretization for optimal control problem of vehicle trajectory planning
2. Without considering the collision avoidance constraints of vehicle paths, the optimal collision-free trajectory is obtained by using the numerical optimization algorithm.
3. According to the three driving areas, the above trajectory data is divided into $G_{1}$ , $G_{2}$ , $G_{3}$ , which are used as the initial values calculated by the segmentation algorithm in the regions $Z_{1}$ , $Z_{2}$ , and $Z_{3}$ , respectively.
4. The NLP problem is solved in the region $Z_{1}$ .
5. If the above solution is successful, update the initial value $G_{1}$ and return to step 4, otherwise, return the step 6;
6. Save the last optimal solution, add the time relaxation variable $t_{s}$ to the optimal objective $t_{1 f}$ in region $Z_{1}$ , update the range of the time primaries in the primaries $G_{1}$ to [ $t_{1 f}$ , $t_{1 f}$ + $t_{s}$ ], and proceed to step 7
7. The NLP problem is solved in the regions $Z_{1}$ and $Z_{2}$ .
8. If the above solution is successful, update the initial value $G_{1}$ , $G_{2}$ and return to step 7, otherwise, return the step 9;
9. Save the last optimal solution, add the time relaxation variable $t_{s}$ to the optimal objective $t_{2 f}$ in region $Z_{2}$ , update the range of the time primaries in the primaries $G_{2}$ to [ $t_{2 f}$ , $t_{2 f}$ + $t_{s}$ ], and proceed to step 10
10. The NLP problem is solved in $Z_{1}$ , $Z_{2}$ and $Z_{3}$ .
11. If the above solution is successful, update the initial value $G_{1}$ , $G_{2}$ , $G_{3}$ , return to step 10, otherwise, return the step 12;
12. Save the last optimal solution and proceed to step 13;
13. Output the optimal solution and the algorithm ends.

Simulation and verification

In the trajectory planning process, based on the vehicle’s kinematic model and constraints, the trajectory solution program based on numerical optimization method was written on the Matlab platform. The vehicle parameters and physical constraints are shown in Table 2.

Table 2.

Vehicle parameters and physical constraints.

Parameters	Parameter description	Numerical value
$l$	Wheelbase	2.560 m
$f$	Front overhang	0.902 m
$r$	Rear overhang	0.883 m
$d$	Vehicle width	1.765 m
$v_{\max}$	Maximum speed	5.0 m/s
$a_{\max}$	Maximum acceleration	2 m/s²
$j_{\max}$	Maximum accelerationchange rate	0.5 m/s³
$φ_{\max}$	Maximum front wheelequivalent swing angle	0.56 rad
$ω_{\max}$	Maximum front wheelequivalent yaw rate	0.55 rad/s

Intersection and traffic circle intersection conditions are taken as examples. The trajectory planning algorithm based on numerical optimization method is verified and simulated. The simulation types are divided into single-phase multi-vehicle cluster and two-phase multi-vehicle cluster planning simulation.

The road at the intersection is set with lengths of 125 m in both directions, and the simulated roadway has six lanes in both directions with a lane width of 3 $m$ , conforming to standard road dimensions. Figure 8(a) shows the result of single-phase multi-vehicle trajectory planning, where a total of 26 vehicles passed through the intersection. Figure 8(b) Results of multi-phase multi-vehicle trajectory planning shows that in this working condition, a total of 24 vehicles pass through the intersection. The roundabout scenario is configured with six entrances, which is in line with the daily roundabout road condition. The length of the road is set to 115 $m$ in the $X$ direction and 110 $m$ in the $Y$ direction. This roundabout scenario is divided into inner and outer rings, each consisting of six lanes. Figure 8(c) shows the result of single-stage trajectory planning, where a total of 17 vehicles passed through the roundabout intersection. Figure 8(d) displays the result of multi-stage trajectory planning, where a total of 30 vehicles passed through the roundabout intersection. From the figures, it can be observed that the algorithm successfully planned the shortest paths for vehicles starting from different points to reach different destinations. And there is no significant detouring of the vehicles so that they can pass through the intersection in the shortest time. Among them, the trajectory of individual vehicles changes greatly during the driving process because of the need to cooperate with other vehicles to avoid obstacles. Throughout the entire trajectory planning process, there were no collisions between vehicles, no crossing of the boundary line of the road. The trajectory planning results are as expected.

Figure 8.

(a) Simulation results of single-phase trajectory planning at intersection, (b) simulation results of multi-phase trajectory planning at intersection, (c) simulation results of single-phase trajectory planning at roundabout, and (d) simulation results of multi-phase trajectory planning at roundabout.

The intersection single-phase vehicle simulation is set up with four groups, and it takes a total of 140.13 $s$ to pass through the intersection. The CPU running time is 36.1 $s$ . The multi-phase vehicle simulation at the intersection is set up with five groups, which take 90.53 $s$ to pass the intersection before and after, and the CPU running time is 24.5 $s$ .

We set up a total of five batches for the multi-phase vehicle simulation at the roundabout intersection, each batch of vehicles passing through the intersection is divided into three phases, and all batches of all vehicles passed through the intersection in a total time of 106.9 $s$ , and the CPU running time is 34.87 $s$ . We set up one batch for the roundabout single-phase vehicle simulation, and all vehicles take 29.6 $s$ to pass through the intersection, and the CPU running time is 45.17 $s$ , when the roundabout single-phase simulation is also set up with five batches, the time is obviously longer than 106.9 $s$ .

https://www.bilibili.com/video/BV1de8gesEBX/.

The status parameters and control parameters of the vehicle are shown in Figure 9.

Figure 9.

State parameter information of vehicles: (a) intersection single-phase trajectory planning, (b) intersection multi-phase trajectory planning, (c) single-phase trajectory planning for roundabout, and (d) multi-phase trajectory planning for roundabout.

The above results are the parameter information of one of the vehicles in each driving condition. From Figure 9, it can be seen that the planning results of the vehicle’s state and control parameters are strictly controlled within the constraints of Table 2. The vehicle starts from the starting point at an initial speed, accelerates to approximately its maximum speed, passes through the intersection at a relatively fast pace, then begins to decelerate after crossing the intersection to reach its final speed, and the speed and acceleration of the vehicle during the whole process are relatively stable.

Through the box plots, the relevant information about each vehicle can be displayed intuitively. The box plots of vehicle speed are shown below.

It can be seen from Figure 10(a) that the median speed of vehicles in a single phase at the intersection is above 4 $m / s$ , indicating high traffic efficiency at the intersection. The red set of points is the anomaly in the box diagram, because during the process of vehicle acceleration, these values with lower speeds appear less frequently and appear outside the box as outliers. The occurrence of outliers with a speed of 0 is due to some vehicles having an initial and final speed of 0. From Figure 10(b), it can be seen that for multi-phase vehicles at the intersection, the median speed is mostly above 4 $m / s$ , with six vehicles having speeds below 4 $m / s$ . As can be seen from Figure 10(c), the median speed of more than half of the vehicles in a single phase of the roundabout intersection is above 4 $m / s$ , while the speed of other vehicles is lower. This is because the use of centralized control requires the vehicles to complete the coordinated travel in the same amount of time, and the distance of travel significantly influences vehicle speeds, the speed of vehicles with shorter travel will be lower to meet the performance indicators of optimal control problems. At the same time, the obstacle avoidance between vehicles will also cause a decrease in speed. From Figure 10(d), it can be seen that for multi-phase trajectory planning at the roundabout intersection, the median speed of 2/3 of the vehicles is above 4 $m / s$ , indicating that the intersection is highly efficient.

Rr = \frac{1}{2} \sqrt{{(f + L + r)}^{2} + d^{2}}

(28)

Figure 10.

Box plots of vehicle speeds: (a) single-phase trajectory planning at intersection, (b) multi-phase trajectory planning at intersection, (c) single-phase trajectory planning at roundabout, and (d) multi-phase trajectory planning at roundabout.

Figure 11(a) for the vehicle contour of the single circle description, equation (28) in $Rr$ for the vehicle contour circle radius, $O$ for the center of the outer circle of the rectangle ABCD, When the distance between the center points of the two vehicles dis is greater than $2 Rr$ , there is no collision between the two vehicles. Based on the vehicle parameters, $Rr$ is calculated to be 2.345 $m$ . Vehicles will not collide if the distance between the centers of two vehicles exceeds 4.7 $m$ . From Figure 11, it can be seen that this vehicle has a large redundant space on the side of a single circular profile, when the vehicles are in a parallel attitude in the $Y$ direction, as shown in Figure 11(b), the minimum distance of non-collision between the vehicles is the width of the vehicle is 1.765 $m$ . The box diagram of the minimum distance between vehicles is shown in Figure 12.

Figure 11.

Description of the minimum safe distance between vehicles: (a) Normal posture between vehicles and (b)Vehicles are in a parallel posture in the Y direction.

Figure 12.

Box plots of minimum inter-vehicle distances: (a) single-phase trajectory planning at intersection, (b) multi-phase trajectory planning at intersection, (c) single-phase trajectory planning at roundabout, and (d) multi-phase trajectory planning at roundabout.

It can be observed that for both intersection and roundabout scenarios, most of the minimum distances between vehicles at intersections and roundabout conditions are greater than 4.7 $m$ , which indicates that there is no collision between vehicles. There is also a part of the minimum distance between the vehicles is less than 4.7 $m$ , which is caused by the mutual attitude between the vehicles. When the vehicles are in the $Y$ direction in the parallel attitude between the vehicles, the minimum distance between the vehicles without collision for the width of the vehicle is 1.765 $m$ . As shown in Figure 11(b), and the minimum distance in the box plot is 2.2 $m$ . Therefore, there is no collision between the vehicles in the whole process, which satisfies the expected trajectory planning results.

Conclusion

This paper proposes a numerical optimization-based method for multi-vehicle cooperative trajectory planning at unsignalized intersections. By establishing the vehicle kinematic model, physical system constraints, boundary condition constraints, and obstacle avoidance constraints, the vehicle trajectory planning problem is converted into an optimal control problem, taking the shortest vehicle crossing time as the optimal objective function, numerical optimization is applied to solve the multi-vehicle trajectory planning optimal control problem. This method realizes the obstacle avoidance strategy of multi-vehicle cooperative motion, according to the random departure of vehicles at the intersection, the above multi-vehicle trajectory collaborative planning problem can be solved in single-phase or divided into multiple-phase.

This paper performs trajectory planning for two scenarios: intersections and roundabouts, and carries out single-phase and multi-phase multi-vehicle trajectory coordination planning for each scenario. The simulation results show that the algorithm can converge to obtain the vehicle trajectories under the both working conditions, and the obtained trajectories are relatively smooth. Based on the statistical data from box plots of vehicle speeds and minimum distances between vehicles, the median speed throughout the motion planning process ranges from 4 to 5 $m / s$ . This indicates that the trajectory enables vehicles to pass through intersections in the shortest time. Most of the minimum values of the distance between vehicles are above 4.7 $m$ , indicating that the trajectory can make the vehicle without collision in the process of driving. Therefore, the method is flexible and robust, and the simulation results show that the method is suitable for such complex conditions as unsignalized intersections.

This research can be applied to the future problem of centralized multi-vehicle cooperative trajectory planning in the unsignalized intersection of intelligent transportation system.

We have summarized some symbols and variables in the text. Please refer to Table 3 for details:

Table 3.

Parameters and physical constraints.

Parameters	Parameter description	Unit
$l$	Wheelbase	m
$f$	Front overhang	m
$r$	Rear overhang	m
$d$	Vehicle width	m
$v_{\max}$	Maximum speed	m/s
$a_{\max}$	Maximum acceleration	m/s²
$j_{\max}$	Maximum change rate of acceleration	m/s³
$φ_{\max}$	Maximum front wheel equivalent swing angle	rad
$ω_{\max}$	Maximum angular velocity of steering angle	rad/s
$x$	Longitudinal coordinates of the rear axle of the vehicle	m
$y$	Lateral coordinates of the rear axle of the vehicle	m
$Ax, Bx, Cx, Dx$	Longitudinal coordinate values of the four contour vertices of the vehicle	m
$Ay, By, Cy, Dy$	Lateral coordinate values of the four contour vertices of the vehicle	m
$d_{safe}$	The safety threshold of vehicle
$Q_{i}$	Vehicle profile vertex
$L_{AB}, L_{BC}, L_{CD}, L_{AD}$	The extended straight lines of the four sides of the vehicle outline
$k_{1}$	The slopes of the straight lines $L_{AB}$ and $L_{CD}$
$k_{2}$	The slopes of the straight lines $L_{BC}$ and $L_{AD}$
$M$	The apex of the road safety profile
$R_{1}$	Radius of center island	m
$t_{0}$	Vehicle start time	s
$t_{f}$	Vehicle termination moment	s
$L_{y}$	Maximum Lateral length of roadway	m
$L_{x}$	Maximum longitudinal length of roadway	m

Footnotes

Handling Editor: Chenhui Liang

ORCID iD

Duoyang Qiu

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was jointly funded by the National Science Foundation of China (Grant No. 42405069), the University Natural Sciences Research Project of Anhui Province (Grant No.2023AH052201, 2023AH052184 and 2024AH050276), the 2023 Talent Research Fund Project of Hefei University (No.23RC01), the 2023 Anhui Province Undergraduate Quality Engineering Construction Project (No.2023xjzlts046), and the technical development project of Hefei University (No.902/22050124149, No.902/22050124148 and No.902/22050125055).

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

References

Algherbal

Ratrout

NT.

Smart cities and intelligent transportation systems: integration of autonomous vehicles and their impact on congestion and safety. Transp Res Procedia 2025; 84: 504–511.

Aledhari

Rahouti

Qadir

, et al. Motion comfort optimization for autonomous vehicles: concepts, methods, and techniques. IEEE Internet Things J 2024; 11(1): 378–402.

Reddy

Almeida

Santos

, et al. Synchronous management of mixed traffic at signalized intersections toward sustainable road transportation. IEEE Access 2023; 11: 64928–64940.

Paul

Dhal

Mary

Relaying vehicle selection protocol for a road intersection. IEEE ACM Trans Netw 2024; 32(3): 2438–2446.

Lin

Shen

, et al. An efficient and accurate a-star algorithm for autonomous vehicle path planning. IEEE Trans Veh Technol 2023; 73(6): 9003–9008.

Zhou

Liu

Cai

GP.

A new geometry-based secondary path planning for automatic parking. Int J Adv Rob Syst 2020; 17(3): 1–17.

Huang

Jin

Path planning based on improved RRT algorithm. In: 2023 2nd international symposium on control engineering and robotics (ISCER), Hangzhou, China, 17–19 February 2023, pp.136–140. New York: IEEE.

Chai

Liu

, et al. Deep learning-based trajectory planning and control for autonomous ground vehicle parking maneuver. IEEE Trans Autom Sci Eng 2023; 20(3): 1633–1647.

Vieira

Argento

Revoredo

Trajectory planning for car-like robots through curve parametrization and genetic algorithm optimization with applications to autonomous parking. IEEE Latin Am Trans 2022; 20(2): 309–316.

10.

Gao

. AdaTP: an adaptive trajectory planning method for intelligent vehicles in cooperative driving based on reinforcement learning. In: CAA international conference on vehicular control and intelligence (CVCI), Changsha, China, 27–29 October 2023, pp.1–6. New York: IEEE.

11.

Jiang

, et al. An improved dynamic window approach based on reinforcement learning for the trajectory planning of automated guided vehicles. IEEE Access 2024; 12: 36016–36025.

12.

Ambuj Nagar

Paul

, et al. Reinforcement learning particle swarm optimization based trajectory planning of autonomous ground vehicle using 2D LiDAR point cloud. Rob Auton Syst 2024; 178: 104723.

13.

Zhang

Chen

Song

, et al. Reinforcement learning-based motion planning for automatic parking system. IEEE Access 2020; 8: 154485–154501.

14.

Acarman

Zhang

, et al. Optimization-based trajectory planning for autonomous parking with irregularly placed obstacles: a lightweight iterative framework. IEEE Trans Intell Transp Syst 2022; 23(8): 11970–11981.

15.

Acarman

, et al. Optimization-based maneuver planning for a tractor-trailer vehicle in a curvy tunnel: a weak reliance on sampling and search. IEEE Robot Autom Lett 2022; 7: 706–713.

16.

Wang

Duan

, et al. An optimal control based trajectory planning method for cooperative merging maneuvers. In: International symposium on intelligent robotics and systems (ISoIRS), Changsha, China, 26–28 May 2023, pp.28–33. New York: IEEE.

17.

Qiu

, et al. Hierarchical control of trajectory planning and trajectory tracking for autonomous parallel parking. IEEE Access 2021; 9: 94845–94861.

18.

Qiu

Cheng

Yang

, et al. Time-optimal trajectory planning for autonomous parking of tractor-semitrailer vehicle based on piecewise Gauss pseudo-spectral method. Adv Mech Eng 2023; 15(8): 168781322 3118 8829.

19.

Duan

Sun

Tian

, et al. Cooperative lane-change motion planning for connected and automated vehicle platoons in multi-lane scenarios. IEEE Trans Intell Transp Syst 2023; 24(7): 7073–7091.

20.

Nguyen

Nghiem

Nguyen

, et al. Distributed formation trajectory planning for multi-vehicle systems. In: American control conference (ACC), San Diego, USA, 31 May–2 June 2023, pp.1325–1330. New York: IEEE.

21.

Kouhi

Moradi

Multiple-vehicle cooperative autonomous parking trajectory planning using connected fifth degree polynomials and genetic algorithm optimization. IEEE Trans Intell Veh. Epub ahead of print 13 February 2024. DOI: 10.1109/TIV.2024.3365328.

22.

Cai

Chen

, et al. Multi-lane unsignalized intersection cooperation with flexible lane direction based on multi-vehicle formation control. IEEE Trans Veh Technol 2022; 71(6): 5787–5798.

23.

Zhang

Wang

, et al. A hierarchical multi-vehicle coordinated motion planning method based on interactive spatio-temporal corridors. IEEE Trans Intell Veh 2024; 9(1): 2675–2687.

24.

Zhang

Acarman

, et al. Lane-free autonomous intersection management: a batch-processing framework integrating reservation-based and planning-based methods. In: IEEE international conference on robotics and automation (ICRA), Xi’an, China, 30 May–5 June 2021, pp.7915–7921. New York: IEEE.

25.

Zhang

, et al. Near-optimal online motion planning of connected and automated vehicles at a signal-free and lane-free intersection. In: 2018 IEEE intelligent vehicles symposium (IV), Changshu, China, 26–30 June 2018, pp.1432–1437. New York: IEEE.

26.

Cao

Tang

, et al. Sharing traffic priorities via cyber-physical-social intelligence: a lane-free autonomous intersection management method in metaverse. IEEE Trans Syst Man Cybern Syst 2023; 53(4): 2025–2036.

27.

Zhang

YM.

Fault-tolerant cooperative motion planning of connected and automated vehicles at a signal-free and lane-free intersection. IFAC Pap OnLine 2018; 51(24): 60–67.

28.

Naderi

Typaldos

Papageorgiou

Optimal control of automated vehicles crossing a lane-free signal-free intersection. In: 2024 European control conference (ECC), Stockholm, Sweden, 25–28 June 2024, pp.1415–1422. New York: IEEE.