A Tightly Coupled Bi-Level Coordination Framework for CAVs at Road Intersections

With the rapid development of modern industry and urbanization, more and more vehicles are entering the transportation system, which adds great pressure to the current road infrastructure. In most developed areas, there exists rather limited space to build extra flyovers and highways, to accommodate the rapidly increased number of vehicles. Meanwhile, many transportation related issues such as the safety, traffic congestion, fuel consumption, air pollution have become social problems, and received tremendous attention from various departments so far [1, 2, 3, 4, 5].

Since multiple roads merge at the road intersection, it has become one of the bottlenecks in modern transportation systems straightforwardly. According to reports in [6], most traffic congestion and accidents occur at road intersections. Traditional intersection coordination strategies include traffic lights, roundabouts, stop signs, etc. However, 90% of traffic accidents were related to the improper behaviors of human drivers, such as the dangerous or fatigue driving, negligence of traffic coordination signals or sudden appearance obstacles [7]. Therefore, it is of vital importance to design dedicated intersection management systems to improve traffic throughput, as well as ensuring the driving safety [8].

With the rapid development of vehicular networking technology in recent years, vehicle-to-vehicle (V2V) and vehicle-to-infrastructure (V2I) communications [9, 10] are no longer the bottleneck for multi-vehicle cooperation issues. Several studies [11, 12, 13] have showcased the immense potential of CAVs at signal-free intersections. In comparison to studies on traditional single-vehicle ego planners [14], this research explores more complex multi-vehicle planning problems. Coordinators at road intersections need to address two major issues. First, CAVs must be carefully coordinated to improve traffic throughput. Second, CAVs must perform ego motion planning based on environmental sensing to avoid collisions with accidental non-CAV obstacles, such as pedestrians, bicycles, and other non-connected vehicles.

Currently, investigations on the cooperative coordination of CAVs at road intersections can be categorized into two trends. The first trend focuses on intersection modeling and vehicle scheduling strategies, which are usually referred to as “centralized” or “high-level” planner. Another trend is the emergence of a bi-level structure built upon the centralized scheduling method. It is common for individual vehicle autonomous planners to get stuck in deadlocks [15] due to system errors. To address this issue, some authors have proposed combining the results of a high-level planner with the autonomous planner for obstacle avoidance. This approach effectively ensures system robustness and traffic safety. The former emphasizes overall coordination efficiency and computational speed, while the latter emphasizes system robustness.

These high-level planners typically possess the “global information” of the entire intersection and can be implemented on road-side units (RSUs) or other infrastructure. One early investigation, as described in [16], prohibited any two CAVs from entering the intersection simultaneously to avoid collisions. Besides safety, this coordinator also considered other metrics such as traffic throughput and fuel efficiency. This work utilized a linear maneuver model with fixed acceleration, simplifying the system formulation significantly.

To further enhance throughput, Kamal et al. introduced the concept of cross-collision points (CCPs) at road intersections [17]. Based on CCPs, Li et al. proposed a cooperative critical turning point-based decision-making algorithm [18], which modeled the coordinator as a partially observable Markov decision process (POMDP) problem to obtain the velocity of the critical turning points. However, the resulting velocity profiles were discontinuous and couldn’t be realized using practical vehicle kinematic models. Additionally, this strategy faced high computational complexity.

Other typical high-level coordinators can be found in [19, 20] and related references. These studies developed a strategy based on collision sets (CS), which is representative to formulating CS coordination as a mixed-integer programming (MIP) problem and solving it by introducing auxiliary variables[21]. Since a single CS solution was not efficient, efforts were made to extend it to a multi-collision sets (MCS) model to improve traffic capacity with a certain increase in computational time [22, 23]. Moreover, some researchers also considered the “stability” of the intersection when vehicles continuously enter the intersection queue [24], a topic rarely discussed before. What’s more, Zhang[25] provided a novel solution approach for the single CS strategy and incorporates local obstacle avoidance into the coordination of intersection control. However, in these investigations, vehicles adopted linear maneuver models, limiting the exploitation of the two-dimensional intersection’s spatial resources. In [26], the authors incorporated friction losses and power train into the kinematic models to achieve more realistic solutions. The authors of [27] proposed a method based on Deep Reinforcement Learning (DRL). This method fixes the lateral path of the vehicle and generates a speed profile. However, due to the fact that learning-based methods require a pre-determined model, this approach is not suitable for dynamic traffic environments (variable vehicle size and dynamics models). Additionally, in scenarios with a large number of vehicles, the efficiency of traffic flow can be compromised by vehicle grouping.

The second trend focuses more on system robustness, and therefore, researchers often propose a more robust double-level framework that extends the capabilities of the low-level planner for local obstacle avoidance [25]. This approach leaves some safety redundancy and effectively prevents deadlocks. To develop a more efficient coordination strategy, integrated double-level coordination frameworks were proposed in [28, 29, 30, 31]. The high-level planner generates reference trajectories and corresponding feasible tunnels, while the low-level planner, similar to single-vehicle planners, avoids nearby non-CAV obstacles within the generated FT. A novel space-time resource searching (STRS) strategy was introduced in the high-level planner. However, this method suffers from the drawback of slow computation in the high-level planner, as it involves complex dynamic programming (DP) and quadratic programming (QP). Additionally, the efficiency of the low-level planner is comparatively low, requiring a substantial number of feasible tunnels for searching viable trajectories, due to insufficient coupling with the high-level planner. Furthermore, the authors suggest that the deep reinforcement learning-based methods proposed in [27] could also be extended to enhance the capabilities of the low-level planner [14, 32] in preventing local collisions caused by errors in localization and control systems.

In addition to the aforementioned study, one major problem faced in coordination is the presence of mixed traffic flow, where both Connected Autonomous Vehicles and Manual Vehicles (MVs) coexist. In this scenario, the difficulty of overall coordination is increased due to the uncontrolled nature of MVs. Consequently, some researchers have focused on addressing this issue. Kamal et al. [33] proposed an adaptive traffic light control scheme, where the authors divided the intersection into several non-conflicting phases based on vehicle strategies. They allocated a non-conflicting green period to each vehicle according to its arrival status at the intersection. However, this method did not fully utilize the temporal and spatial resources within the intersection. Building upon the above method, Huang et al. [34] further refined the sapce-time resources within the intersection and grouped CAVs and MVs separately, introducing two specific lane restrictions. This method improves the rationality and efficiency, providing great inspiration for handling mixed traffic flow scenarios.

Some recent studies also suggest that dividing the global vehicle coordination problem into two-stages is a promising approach, namely decoupling the vehicle passage order from the specific trajectory optimization problem. For instance, in the work of Yao [35, 13], a two-stage collaborative framework for Schedule and Trajectory Optimization is proposed. This framework first employs Mixed Integer Linear Programming (MILP) based on vehicle information to determine the optimal arrival time at intersections, and then solves the trajectory optimization problem based on the results from the scheduling phase. This method effectively reduces vehicle passage delays and improves fuel efficiency. In addition, Jiang [36] further consider the time-optimal vehicle passage order. The authors employ a heuristic Monte Carlo Tree Search (MCTS) method to accelerate the solving efficiency. It is worth noting that the authors innovatively explore the impact of all-direction turn lanes (ADTL) and specific-direction turn lanes (SDTL) on the scheduling algorithm.

However, the current research in the field urgently needs a fast and tightly coupled framework that can achieve obstacle avoidance in intersection scheduling, while ensuring high robustness and efficiency. In this case, tight coupled means that the low-level planner must heavily rely on the results of the high-level planner to ensure its effectiveness. The trajectories generated by the low-level planner should aim to avoid obstacles as closely as possible to the results of the high-level planner, reducing the probability of deviating from reference trajectories of high-level planner. Excessive deviation from the reference trajectory generated by the high-level planner can potentially lead to schedule breakdown. However, the ability of such tight coupled is not adequately demonstrated in [28]. This framework should integrate high-level and low-level planners, taking into account the limitations of existing methods, such as the assumption of identical vehicle sizes, ideal communication, and control systems. It should also address the challenges posed by obstacles and control/positioning errors. To achieve high robustness and efficiency, a coordinated scheduling framework for signal-free intersections is needed.

Aiming at the issues on current investigations, we try to propose a low-complexity and tightly coupled bi-level coordination framework for CAVs at non-signalized road intersections. In the high-level planner, the traffic throughput, fairness and complexity are still main considerations. Unlike existing time exhausting investigations in [28], our method could significantly reduce the computing efforts. Furthermore, unlike the most existing single level coordinators, a tightly coupled low-level planner is proposed to handle non-CAV obstacles, which are not considered in the high-level planner. The main contributions can be summarized as follows.

• •

  We propose an computational efficient solution to indicate the feasible tunnels for each CAVs, which has ability to extended to various kinematics constraints and scales of CAVs. Furthermore, considering the fairness and the duration of scheduling, we give a novel benchmark of passing order priority to determine traffic sequence of vehicles.

• •

  To ensure driving safety within the “ Feasible tunnel”, we propose an effective motion plan method based on the gradient and B-spline curve. It generates the initial control points of the B-spline curve, then optimizes the control points for lower smoothness cost and avoids potential collision under the feasible tunnel constraints. Comparing to existing solutions, the tightly coupled local motion planner is more suitable to the double-level coordination framework, which could make full advantage of the result of high-level planner and higher computational efficiency.

• •

  The simulation and laboratorial experiments show that our proposed approach can effectively improve the traffic throughput. One excellent advantage is that our high-level planner has only millisecond-level latency, which means that the strategy can be deployed in real-world settings.

The rest of this paper is organized as follows: In Section II, we will explain the intersection model of our framework, vehicle kinematic model, the collision detection, construction of our framework and some state-of-art benchmark solutions for comparison. In section III, the modified coordination strategy based on space-time resource blocks is introduced. In section IV, a novel motion planner based on gradient is proposed, which is more suitable for the double-level framework. The simulation and laboratorial experiments results are demonstrated in Section V and Section VI, respectively.

We deploy a typical road intersection which consists of $R$ roads (for easy illustration, we set $R=4$), as shown in Fig. 1. The road intersection can be divided into three main sections, i.e., Waiting Area (WA), Buffer Area (BA) and Conflict Area (CA). The CA is the crucial area in our coordination framework, where potential collisions may occur. The width of lanes is $W_{L}$, the length of BA is $L_{B}$.

We use $\mathcal{R}=\{1,2,...,R\}$ and $\mathcal{I}_{r}=\{1,2,...,I_{r}\}$ to represent the sets of roads and CAVs in road $r$, respectively. At signal-free intersections, the decision making center may face high dynamic scene, e.g., the unexpected obstacles, unsatisfactory communication and control systems. It is necessary to make following assumptions:

1. 1.

   Each CAVs will send its state information (position, velocity, etc.) and intention maneuvers (i.e., go straight, turn left and turn right) to the CC via vehicle-to-infrastructure links, when entering the WA. After executing the coordination framework, the CC will broadcast the coordination information (i.e. the moment of entering intersection and feasible tunnels).

2. 2.

   After received the coordination information, the CAVs should adjust their velocity in BA to enter the intersection at the timing that broadcasted by CC. Then CAVs travel along with the reference trajectory which is predefined, if there is no unexpected obstacles.

3. 3.

   Without loss of generality, the high-level planner will allocate the space-time resource blocks to every CAVs. Considering the unexpected obstacles, there should be some reasonable redundancy involved by the feasible tunnel to make sure the low-level planner has enough space to avoid collision.

In this paper, to reduce the computational complexity, we adopt the popular kinematic bicycle model [37] to describe the movement of vehicles, as shown in Fig. 2. The kinematics state of the $i$-th vehicle in road $r$ can be described as $[x_{i,r},y_{i,r},\theta_{i,r},v_{i,r}]^{\text{T}}$, where $(x_{i,r},y_{i,r})$, $v_{i,r}$ and $\theta_{i,r}$ are the position, velocity and heading in 2D Cartesian coordinate system, respectively. During the actual driving behavior, the control input are the longitudinal acceleration and the steering angle, donated by $a_{i,r}$ and $\theta_{i,r}$, respectively. The vehicle kinematic state update equation is shown as following:

where $(\dot{\bullet}):=\frac{\partial}{\partial t}(\bullet)$, $L_{w}$ is the distance between the front and rear wheel axles. What’s more, taking into account the kinematic constraints of the vehicle, there must have:

The space-time resources was first used to describe the resources in the intersection by [28]. The resources in CA are described by three-dimensional coordinates in the $X$, $Y$ and $T$ domains. For the convenience of computation and reasonability, the space-time resources are divided into multiple blocks with the resolutions $dx$ in $X$ domain, $dy$ in $Y$ domain, $dt$ in $T$ domain. Then the resource in $(x,y,z)$ could be described as:

where $j_{x},j_{y}$ and $j_{t}$ are the indexs in $X,Y$ and $T$ domains, respectively. One space-time can not be occupied by more than one vehicle simultaneously. Thus, when attempt to allocate the space-time resource in $(j_{x},j_{y},j_{t})$, the Centralized Coordinator must check the occupancy state of the space-time resource blocks.

The allocated area for the vehicle is represented as a collection of resource blocks, which includes not only the resource blocks occupied by the vehicle, with proper redundancy. We define the resource occupied by the $i$-th vehicle of road $r$ as

where $\mathcal{A}_{i,r}$ represents the index set of resource blocks occupied by $i$-th vehicle of $r$-th road. To keep no collisions among vehicles, we need to ensure that one resource block can only be allocated to one vehicle. That means the intersection of allocated resources between the two vehicles is empty, i.e.,

with $\forall r1,r2\in\mathcal{R}$, $\forall i1\in\mathcal{I}_{r1}$ and $\forall i2\in\mathcal{I}_{r2}$.

In practice, there are noises in the localization [38] and control systems,

where $(x,y,\theta)$ and $(x_{\text{m}},y_{\text{m}},\theta_{\text{m}})$ are the actual values and measured value of localization systems, respectively. $(a,\delta)$ and $(a_{\text{f}},\delta_{\text{f}})$ are the expected values and actual value of control systems, respectively. The CAVs always deviate from the planned trajectory. These imperfections need to be treated carefully. For the safety of vehicles, we set a redundancy around the vehicle, as shown in Fig. 3. The $L_{\text{car}}$ and $W_{\text{car}}$ represent the length and width of vehicle, respectively. The $R_{\text{long}}$ and $R_{\text{lat}}$ are the longitudinal and lateral safe redundancy. The parameters of redundancy are decided by the density of obstacles and performance of control and localization system.

During the coordination process and local motion planning, the redundancy region is also considered when collision detection.

The space-time resource blocks occupied by other CAVs in time $t$ is could be considered as a occupancy grid map [39], as shown in Fig. 4. For a grid that has been occupied, it could not be occupied repeatedly, otherwise a collision will occur. In low-level planner, we assumed that the CAVs could build a Object Oriented Bounding Boxes (OOBB) topological map with the equipped sensors, then transform the OOBB map into the occupancy grid map.

In theory, redundancy is a trade-off between coordination efficiency and system robustness. Consequently, we attempt to propose a rough estimation method for redundancy size, although it should be noted that it may not be precise, through theoretical analysis. Generally, we can choose the maximum value of three times the standard deviation of the lateral and longitudinal offset errors caused by vehicle localization and control system errors as a factor to consider. In addition, unexpected obstacles within the intersection should also be taken into account. We can calculate the diameter of the circumscribed circle outside the OOBB of each obstacles as another statistical factor, which can be observed by CC. Although the obstacles may not be located on the boundary of the feasible path, which may cause the vehicles to deviate from their intended trajectories to go around the obstacles, the system is able to handle such deviations to a certain extent due to the robustness of the low-level planner. Finally, we define a rough formula for calculating the redundancy size of feasible tunnel:

where $\sigma_{\textrm{lat}}$ and $\sigma_{\textrm{long}}$ are the variances of the lateral and longitudinal tracking errors due to the subpar localization and control system, respectively, $d_{\textrm{OOBB}}$ is the diameter of the circumscribed circle outside the bounding box of the obstacle.

In both High-Level and low-level planners, we calculate the grids that occupied by CAVs and obstacles, respectively. For collision free, there can not be any overlaps of allocated grids.

The double-level coordination framework can be divided into two parts, as depicted in Fig. 5. The functions of two parts are as following.

1. 1.

   high-level planner (coordination strategy): After the CAVs entering the WA, the CC attempts to receive the their information about positions, velocity, intention maneuvers and the like by V2I (Vehicle to infrastructure) links. According to road information of the intersection, the CC generates the reference trajectories for each CAV. After calculated the traffic priority of the first CAVs in each road, the CC executes coordination strategy to get the reference trajectories and feasible tunnels of the waiting CAVs.

2. 2.

   Low-level planner (local motion planner): Based on the reference trajectory generated by the high-level planner, the low-level planner attempts to optimize the trajectory to avoid collisions within the feasible tunnel. The practice trajectory also needs to be sufficiently smooth and feasible in terms of kinematics model. What’s more, an unexceptionable controller also plays an important role in the frame, and lower control errors can reduce the probability of collisions. It is worth noting that the low-level planner is executed by CAVs based on local perception information and, therefore, does not occupy resources of the central coordinator.

In this subsection, we briefly introduce some state-of-art benchmark solutions of coordination strategy in intersection for comparison.

The simplified line $\&$ circle path [40] is one popular option. However, it is too ideal to be realized in the kinematic bicycle model due to the discontinuous second order derivative. In the three-dimensional Cartesian coordinates system XYT, the trajectory can be decoupled as Path and Speed Profile [41]. Usually, the trajectory can be expressed as

where $u$ is the arc length along with the path, $s$ is decided by velocity and acceleration of vehicle.

In order to make the path smooth and subject to the kinematics constraints, the problem can be built as a constrained optimization problem, one key constraint is the continuity of trajectories [25].

where $(x_{0},y_{0})$ and $(x_{s_{t}},y_{s_{t}})$ are the start and end position of standard line $\&$ circle path, respectively. $s_{t}$ is the total length of standard path.

In order to make the reference trajectory more consistent with traffic rules and human driving habits, the distance limitation between the actual path and the standard line $\&$ circle path is also a necessary constraint.

where $(x_{j},y_{j})$ are the equally spaced sample points on standard line $\&$ circle path. $N_{\text{s}}$ is the number of sample points, which is decided by $\Delta s$. $d_{\text{b}}$ is the distance limitation between reference and standard line $\&$ circle paths.

Therefore, the optimization problem can be expressed as

where $\bullet^{(i)}(s)$ means the $i$-th order derivative of $\bullet(s)$.

Generally, the quintic polynomial is a good way to express the trajectory, then $\mathcal{P}_{1}$ can be formulated as a typical quadratic programming problem [42], and solved by off-the-shelf solvers rapidly.

Since the length of entire standard line circle path $s_{\text{t}}$ will be changed after planning, we propose a method to generate speed profiles rapidly for the higher fuel efficiency [19] and comfort. Ideally, we want the vehicle to cross the intersection with a speed close to the reference speed $v_{\text{ref}}$ without any accelerations. After solving the reference path, we can obtain the length of the reference path $s_{n}$, thus the longitudinal displacement can be expressed as

which is the integral of velocity, also means the longitudinal travel distance along the reference path.

The high-level planner of STRS strategy can not solute in real-time, because of the high computation complexity. In order to apply the double-level framework based on space-time resource block to multi-roads intersection, we improved the model to overcome the high computation complexity and propose a new high-level planner to achieve feasible tunnel allocation for higher efficiency in traffic utilization.

First, the CC should calculate the reference trajectory for each vehicle. Specially, the potential collision exists in the Conflict Area, so we only consider the reference trajectory between the vehicle and redundancy entering the Conflict Area and leaving the Conflict Area. Then, the reference trajectory of $i$-th vehicle in road $r$ can be represented as following,

where $r_{\text{f}}$ and $r_{\text{t}}$ are the road which the vehicle come from and intend to reach, respectively. Specifically, $m$ is the maneuver (turn left, go straight, turn right) of the vehicle, that means road $r_{\text{t}}$ must be one of the feasible roads, which vehicle came from $r_{\text{f}}$ road and maneuvered as $m$ could reach. $t_{i,r}^{e}$ and $t_{r_{\text{f}},r_{\text{t}},m}$ are the moments of entering the Conflict Area and the duration of the specified reference trajectory, respectively. We can calculate the index set $\mathcal{C}_{r_{\text{f}},r_{\text{t}},m}^{t}$ of resource blocks occupied by vehicle at time $t$ state directly.

Generally, the index set of occupied resource block of each trajectory starting at time $t^{\text{e}}$ are represented as

where $\left\lfloor\bullet\right\rfloor$ means the round down of $\bullet$. $\mathcal{C}_{r_{\text{f}},r_{\text{t}},m}^{t}$ means the index set of resource blocks which are occupied vehicle with $(r_{\text{f}},r_{\text{t}},m)$ state at time $t$. Because the starting time of reference trajectory provided by Reference Trajectory Generator is $0$, the time index of reference trajectory index set must be added with a time offset $\lfloor\frac{t_{\text{e}}}{dt}\rfloor$.

When there’s only one road need to be coordinated, CAVs could only enter the intersection one by one. Thus, we should indicate the timing of entering the intersection for each CAVs.

At $k$-th round, the index set of space-time resource blocks occupied in previous $1\sim k-1$ rounds can be expressed as $\mathcal{A}_{\text{pre}}^{k}$. Particularly, $\mathcal{A}_{\text{pre}}^{1}=\emptyset$ in the first round. For potential collisions avoidance, we need to guarantee that the same resource block cannot be occupied repeatedly, i.e.,

At $k$-th round, the resource that allocated to the $i$-th CAV in $r$-th road can be represented by $\mathcal{A}_{i,r}^{k}=\mathcal{D}^{t}_{r_{\text{f}},r_{\text{t}},m}$. After $k$-th round, $\mathcal{A}_{\text{pre}}$ should be updated.

Thus, the resource allocation problem can be described as a typical linear programming problem:

where $\mathcal{A}_{\text{final}}$ represents the index set of resource blocks occupied during the whole scheduling process.

The aforementioned method can determine the optimal timing for a CAV to enter the intersection in a single-lane scenario. However, in a multi-lane intersection, there are multiple lanes to consider, which means we must select a CAV to enter the intersection first and then consider the subsequent CAVs in the next round of coordination. This implies that the problem can be solved using sliding window method, also means receding horizon control.

Due to the potential presence of multiple vehicles waiting to pass at an intersection, it is necessary to determine the sequence of vehicle movements, which signifies the need to assign a priority, known as right of way, to each vehicle. That implies the optimization objective of the problem will change. We need to consider multiple optimization objectives, rather than solely focusing on traffic efficiency. First and foremost, based on the resource blocks allocated to CAV, the passing priority term of traffic efficiency $P_{d}$ could be defined as

The general intersection coordination strategy all aim to maximize throughput, that can lead to unfair situations where the first CAV at some roads cannot be scheduled for a long time. [28] also ensures the fairness of the passage by introducing resource occupancy rate term into the performance criteria of the coordination. Nevertheless, vehicles going straight tend to increase resource occupancy rate more than turning left in most cases. In practice, the reasonableness of this item is debatable. An effective approach is to add waiting time term to the performance criteria of the coordination for fairness of passage.

where $t_{\text{waiting}}^{n}$ indicates the waiting time at $n$-th in road queue.

For queue stability [43], according to Lyapunov theorem, we should ensure the following equation holds,

where $l_{\text{r}}(t)$ is the length of $r$-th road at $t$.

Thus the performance criteria about queue stability can be defined as:

where $a_{\text{r}}^{\text{av}}$ represents the arrive rate of $r$-th road.

In every round, we could select the optimal vehicle $v$ in all road to pass base on priority of vehicle function, i.e.,

where, we neglected the inclusion of the $P_{occ}$ factor proposed in [28] due to its high coupling with $P_{d}$ factor. Specially, $v$ must be the first vehicle in all road queue in this round.

The algorithm flow of the entire high-level planner is shown in Algorithm 1.

Due to the vehicle’s kinematic constraints, usually, the vehicle cannot instantaneously accelerate to a specific velocity. Therefore, we must ensure the continuity between trajectories, which means that the generated trajectory by the planner must be continuous with the current trajectory at a specific time point in order to guarantee seamless trajectory connection and switching. After the vehicle enters the buffer area of the intersection, the vehicle’s trajectory needs to switch from its own planner to a dedicated low-level planner for the intersection. The low-level planner requires a reference path, which means that we need to generate a continuous and smooth trajectory for trajectory planning. As we aim to ensure third-order continuity of the trajectory, i.e., continuity of position, velocity, acceleration, and jerk, a fifth-degree polynomial is undoubtedly a good representation. Since the general planner operates iteratively and the previous frame trajectory ensures collision-free, sampling the points on the previous frame trajectory as the starting point for the next frame trajectory is effective. Based on the desired trajectory length, we can sample the endpoint on the reference trajectory. Similar to the Reference Trajectory Generator, we can also solve a quadratic programming problem to obtain one or multiple segments of quintic polynomials as the initial values for the low-level planner. Due to space limitations, a detailed introduction is not provided here.

Inspired by [44], we propose a gradient optimization-based trajectory generation scheme. In addition to the reference trajectory, we further consider four more factors: smoothness, kinematic feasibility, energy efficiency and collision avoidance, which can effectively avoid the time consuming trajectory re-calculation. The trajectory is represented by a $p_{\text{d}}$-th order uniform B-Spline curve, and can be optimized by the control points of the B-Spline curve $\mathbf{Q}$ by gradient of the cost function. The original control points of B-Spline curve can be calculated by reference trajectories.

In general, the maneuver smoothness can be characterized via the acceleration, jerk and snap [42], specifically, the snap can be ignored when $p_{\text{d}}\leq 3$. The kinematics feasibility cost is represented by the limitation of lateral acceleration. In terms of fuel efficiency, the optimal velocity profile is a constant $v_{\text{ref}}$ [19]. Intuitively, we try to minimize the longitudinal acceleration, to approach the expected constant velocity. Thus, the smoothing, kinematics feasibility and energy efficiency terms of cost function can be expressed succinctly as following:

where $\mathbf{h}(t)=[f(t)\;g(t)]$.

The B-Spline curve is convex, which indicates that a span B-Spline curve can be controlled by $p_{\text{d}}+1$ successive control points. Another property is that the $k$-th order derivative of a B-Spline curve is also a B-Spline curve with $p_{\text{d},k}=p_{\text{d}}-k$. In our study, $p_{\text{d}}=3$, the 3-th order derivative of a single span B-Spline curve is a constant, thus, the snap term can be ignored in the cost function [45].

For uniform B-Spline curve with $N_{\text{c}}$ control points, the number of knots is $p_{\text{d}}+N_{\text{c}}+1$, the knots vector can be represented by $\mathbf{t}_{k}=\left[t_{0},t_{1},\dots,t_{p_{\text{d}}+N_{\text{c}}}\right]^{\text{T}}$ with constant interval $\Delta t$. The velocity $\mathbf{V}_{i}$, acceleration $\mathbf{A}_{i}$ and jerk $\mathbf{J}_{i}$ can be obtained by

Thus, the cost function can be rewritten as

The most difficult part of the low-level planner is the cost function with obstacle avoidance. In [44], it is only applicable to objects with round or square outlines. In a more general sense, we focus on the research of the planner itself. Therefore, we assume that the state of dynamic obstacles can be perfectly predicted. Thus, sampling the position of dynamic obstacles at a single time point can be treated as a static obstacle [46]. In a typical going straightforward scenario, as shown in Fig. 7. we sample the position $\mathbf{P}_{b}(t_{i})$ of B-spline trajectory on the time knot, $t_{i}\in\left\{t_{p_{d}},t_{1}\dots t_{p_{d}+N_{\text{c}}+1}\right\}$, we check whether there are potential collisions. With the predictor we can get the position and state of the obstacles and the grid occupied by the obstacles can be obtained. Record the $j$-th potential collision grids nearby$\mathbf{P}_{b}(t_{i})$ as $\mathbf{p}_{ij}$, then the gradient generation vector can be calculated by

Theoretically, the closer the collision point, the greater the cost. A reasonable piecewise polynomial loss function is necessary, which is twice continuously differentiable. First, if the collision points are within a circle with the vehicle center as the center of circle and the diagonal of vehicle as the diameter, it will apply a higher penalty to the control points. Considering the error of control model and the grid size, we set an additional redundancy to keep collision-free, which apply a lower penalty. Finally, the piecewise polynomial function of collision avoidance can be represented by

where $d_{ij}=d_{\text{r}}+d_{\text{f}}-\left\|\mathbf{v}_{ij}\right\|_{2}$, $d_{\text{r}}$ and $d_{\text{f}}$ are two important parameters of piecewise polynomials. $C_{\text{c}}(i,j)$ is the collision cost value of $\mathbf{P}_{b}(t_{i})$ caused by collision gird $\mathbf{p}_{ij}$. The total collision cost of $\mathbf{P}_{b}(t_{i})$ can be evaluated independently, i.e., $C_{\text{c}}(\mathbf{P}_{b}(t_{i}))={\textstyle\sum_{i=0}^{N_{i}-1}C_{\text{c}}(i,j)}$, $N_{i}$ is the number of collision grid of $\mathbf{P}_{b}(t_{i})$. Then the collision cost of entire trajectory can be evaluated by all control points.

For the gradient of collision term, it can be evaluated by the derivative of $C_{\text{c}}$ with respect of $\mathbf{Q}_{i}$ directly.

where $\omega_{i,j}$ is the weight of $Q_{j}$ in time knot $t_{i}$.

The trajectory optimization problem considering collision avoidance can be modeled as an unconstrained optimization problem as (31), which uses some tricks to achieve approximate hard constraints.

We generate the control points of the B-Spline curve with the sampling points and states of the startpoint and endpoint of the reference trajectory. To ensure the continuity between the startpoint of the curve and the previous frame trajectory result, we cannot optimize the first three control points. However, due to the iteration of the planner, the last three control points can be optimized. Therefore, the number of control points must be greater than four. Since the objective function is higher than quadratic and quadratic terms dominate, the Hessian matrix can be used to accelerate the optimization process. In [44], L-BFGS solver [47] has a good performance in unconstrained optimization, thus, we optimize the control points with L-BFGS solver.

When obstacles appear in the intersection, bypassing these obstacles may cause the vehicle to deviate from the feasible tunnel. This means that other vehicles need to consider this vehicle as an unexpected obstacle. Large obstacles may block the entire intersection, but this is not within the scope of our research. After optimization, collision may still exist or the trajectory is not smooth enough, low-level planner can optimize again by increasing the weight of the corresponding term to make the trajectory meet the constraints, but in most cases, once is enough.

The simulation parameters are listed in Table. I. Fig. 8 shows the reference trajectory provided by the Reference Trajectory Generator, which avoids the flaw of line & circle curve paths and improves smoothness and feasibility. Theoretically, the Reference Trajectory Generator is scalable in multi-lane road intersections.

Fig. 20 shows a typical laboratory road intersection scenario. In this experiment, four mobile robots all adopt a left turn strategy. We use large safety redundancy and fast reference speed, i.e., $R_{\text{long}}=R_{\text{lat}}=0.4\text{m}$, $v_{\text{ref}}=0.4\text{m/s}$. In this experiment, due to the large safety redundancy, CAVs in the diagonal roads cannot enter the intersection simultaneously, thus, the CAVs need to enter the intersection one by one. We plotted the reference path of CAV1 with red curve to show the error of the control system. With proper security redundancy, our proposed can realize the throughput potential of the intersection without collision.

In Fig. 21, We reduce security redundancy, i.e., $R_{\text{long}}=R_{\text{lat}}=0.1\text{m}$. We plotted the reference paths of CAV1 and CAV3 with red curves, their reference trajectories don’t conflict, thus, the CAVs from the diagonal roads can enter the road intersection simultaneously.

With unexpected obstacles in the intersection or a large control error, coordination strategy requires the intervention of the low-level planner. In this laboratory experiment, we adopt relatively lower reference speed ($v_{\text{ref}}=0.2\text{m/s}$) and same redundancy with high-level planner experiment I. A static obstacle is arranged in the intersection, which is marked by red box. As a result of this obstacle, the reference trajectory of the CAV2 is exposed to potential collision. After spotting the obstacle, CAV2 generates the collision free trajectory base on the result of high-level planner by low-level planner. Fig. 22 shows the results of the experiment.