Specification-Compliant Motion Planning of Cooperative Vehicles Using Reachable Sets

Abstract

Automated vehicles must comply explicitly with specifications, including traffic-based and handcrafted rules, in order for them to safely and effectively participate in mixed traffic. In addition to driving individually, there are many traffic situations in which cooperation between vehicles maximizes their collective benefits, including preventing collisions. To realize these benefits, we compute specification-compliant reachable sets for vehicles, i.e., sets of states which can be reached by vehicles over time that are constrained by a set of considered specifications. We summarize and combine our previous works on computing specification-compliant reachable sets and negotiating conflicting reachable sets within a group of cooperating vehicles. As a result, conflicts between specification-compliant reachable sets of vehicles are resolved, and specification-compliant trajectories can be individually planned for each vehicle within the negotiated reachable sets using arbitrary motion planners.

1 Introduction

When compared with human-driven vehicles, automated vehicles are expected to deliver enhanced road safety, passenger comfort, and traffic efficiency compared with human-driven vehicles. To safely and effectively participate in mixed traffic, in which both automated and human-driven vehicles share the road, automated vehicles must comply explicitly with specifications, including traffic regulations and handcrafted rules. Compliance with the former is essential in order to exempt manufacturers from liability claims in the event of an accident, while compliance with the latter allows motion plans to be generated that satisfy additional requirements. An example of a handcrafted rule is: Follow vehicle 1 up to step $k_1$, then completely overtake it from the left before step $k_2$. Generating a drivable trajectory that satisfies a set of specifications for an automated vehicle involves reasoning not only with continuous states (which may reflect the physical motion of the vehicle) but also discrete states (possibly due to discretization of the continuous state space or action space) of the vehicle. This poses computational challenges from a variety of aspects, including vehicle dynamics, the specifications under consideration (including collision avoidance), and dependencies between planned trajectories and constraints originating from the specifications. On the one hand, planning solely in the discrete state space may produce plans that meet specifications but violate vehicle dynamic constraints or lead to collisions. On the other hand, motion planners may generate dynamically drivable trajectories that do not comply with the specifications.

One solution to this problem is to guide the motion planning of an automated vehicle using its specification-compliant reachable set, which is defined as the set of states reachable by the vehicle over time that is constrained by a set of considered specifications. Computing the reachable sets in an over-approximative fashion will enclose all drivable trajectories of the automated vehicle [34]. The smaller the solution space is, the faster reachable sets can be computed, as demonstrated in [22]. In addition, the search space for the motion planner is greatly reduced particularly in critical situations. In contrast to conventional approaches, both effects result in quick computations even in critical situations. Low-level trajectory planning constraints can be extracted from the computed reachable sets and passed on to motion planners to generate specification-compliant trajectories.

In addition to driving individually, there are many traffic situations that demand cooperation between vehicles in order to maximize their collective benefits and to prevent collision in a potential emergency. Human drivers typically interact with each other through implicit communication and by anticipating the most likely behaviors of others. In comparison, automated vehicles can communicate and collaborate explicitly to jointly offer and suggest more sophisticated and efficient solutions in an ongoing traffic situation. One of the challenges of such cooperation lies in developing a computationally efficient scheme that does not compromise the optimality of the output solutions.

Reachable sets can be employed to tackle this challenge. The reachable sets of a group of cooperating vehicles can be computed and negotiated where conflicts in the position domain arise. This negotiation can be systematically organized such that each vehicle unambiguously receives its own negotiated reachable set, within which trajectories can be planned. This prevents exponential complexity of the collaborative motion planning.

In this chapter, we summarize and combine our previous works on computing specification-compliant reachable sets for an ego vehicle [13] as well as on negotiating conflicting reachable sets between a group of cooperating vehicles [21]. As a result, conflicts between specification-compliant reachable sets of vehicles are resolved, and each vehicle plans its own specification-compliant trajectories within its negotiated reachable set, for example, using the planners described in [22, 36].

The remainder of this article is organized as follows: Sect. 2 reviews related work on specification-compliant motion planning and cooperative motion planning. Section 3 presents the necessary preliminaries and definitions. The computation of specification-compliant reachable sets is summarized in Sect. 4 and the negotiation of reachable sets in Sect. 5. Example results are presented in Sect. 6, and we conclude in Sect. 7.

2 Related Work

In this section, review related works on specification-compliant motion planning and cooperative motion planning of vehicles.

2.1 Specification-Compliant Motion Planning

The efforts to obtain a specification-compliant trajectory can be categorized on the basis of whether compliance with specifications is examined after, during, or before motion planning.

2.1.1 Considering Compliance After Motion Planning

The most straightforward approach to obtain a specification-compliant trajectory is to examine the compliance with specifications after the trajectories have been generated. The process of checking whether an execution of a system satisfies the expected behaviors is often referred to as runtime verification or monitoring. For example, article [29] presents a monitor for formally examining the compliance of automated vehicles with traffic rules (safe distances and overtaking); a monitor for so-called responsibility-sensitive safety rules [31] is described in [10]. While monitoring can be performed efficiently, monitors typically only provide a verdict, i.e., a true or false appraisal, on whether the specifications have been satisfied. If the trajectory under examination is rejected, no alternative trajectory is returned. This often necessitates the (re)planning of multiple trajectories in order to locate a valid solution for more complex specifications.

2.1.2 Considering Compliance During Motion Planning

Works in this category often adopt a mechanism that simultaneously handles planning in both the continuous and discrete state spaces of a system, with the generated discrete plans guiding the trajectory planning process. For example, a satisfiability modulo convex programming framework for cyber-physical systems was introduced in [32] that handles both convex constraints on a continuous model and Boolean constraints on a discrete model; article [16] puts forth a multilayered synergistic framework for motion planning of robots considering linear temporal logic (LTL); timed automata are used in [37] to synthesize timed paths for indoor robots that comply with specifications expressed in metric temporal logic. In these works, discrete plans are generated in the discrete state space based on abstractions of the considered systems, and trajectories are planned in the continuous state space by motion planners, with the discrete plans taken into consideration. In most cases, the dynamic constraints of the system are not reflected in the discrete plans. Thus, the drivability of these plans is often not ensured, requiring frequent replanning of both the discrete plans and the trajectories.

2.1.3 Considering Compliance Before Motion Planning

The final category of works considers the specifications prior to trajectory planning, e.g., in high-level maneuver planners, from which trajectory planning constraints can be extracted. The work in [15] generates maneuvers that respect simple traffic rules by traversing a graph defined in a discretized state space of the ego vehicle; article [8] embraces a similar concept and produces maneuvers satisfying specifications expressed in LTL; in [33], so-called driving corridors are extracted from reachable sets of an ego vehicle that reflect different position relations to other vehicles over time. Our approach to computing specification-compliant reachable sets [13] falls into this category. It can handle propositional logic with predicates related to positions, velocities, accelerations, and certain traffic regulations introduced in [18, 19].

2.2 Cooperative Motion Planning

Survey articles [9, 24, 28] reviewed recent advances in cooperative driving of automated vehicles with varied focuses on architecture, maneuver planning, and motion planning use cases. Optimization-based and reservation-based approaches are common paradigms for cooperative motion planning [9, 28]. In optimization-based approaches, one or more optimization problems are formulated based on the motion planning constraints and cost functions of cooperating vehicles. The optimization problems are solved with a (centralized) optimizer, which corresponds to trajectories to be followed by the cooperating vehicles. The complexity of the optimization problem increases dramatically with the number of vehicles considered, which requires either a high computation power or a limit to the number of vehicles in a group.

Our approach to cooperative motion planning falls into the reservation-based category and employs auction algorithms for resolving conflicts in reachable sets of vehicles. Reservation-based methods assign free space to vehicles for trajectory planning. Earlier works with a focus on intersection management were introduced by Dresner [4]: Tiles are created from the intersection region, which can be requested by vehicles approaching the intersection. A centralized intersection manager proceeds to assign tiles with multiple requests to vehicles, using a first-come-first-served protocol, ensuring that no tile is occupied by more than one vehicle at any one time. Its extensions and variations are presented in [5, 6]. As the first-come-first-served policy for reservation assignment may be inefficient in situations with higher traffic density, it was replaced in [3, 27, 35] by auction-based methods. In auction-based methods, each bidder (cooperating vehicle) bids for offered packages (e.g., combinations of tiles representing road areas) in a way that reflects its interests or utilities. An auction algorithm is then executed to maximize the total revenue of the packages. Instead of tiles, some works identify possible conflicting points, regions, or moving space-time corridors and allocate them to vehicles in the event of a conflict [17, 20, 23, 38]. The corridors correspond to predefined behaviors, such as following a lane or performing a lane change; vehicles receiving such corridors must act accordingly. In [11, 25], an efficient and explicit space-time reservation protocol was devised for cooperative maneuver planning, through which a vehicle broadcasts requested space envelopes over time and drives within the envelopes once the request has been accepted by surrounding vehicles of interest.

3 Preliminaries

This section introduces the necessary preliminaries, including the general setup, coordinate systems, definitions of reachable sets, and propositional logic.

3.1 Setup and Coordinate System

In this work, the considered scenarios are described in the CommonRoad¹ [1] format, which consists of (1) a road network constructed of lanelets [2], whose left and right bounds are represented by polylines, (2) dynamic and static obstacles, and (3) traffic rule elements (such as road markings, traffic signs, and traffic lights). Figure 1 depicts an exemplary traffic scenario. We denote by $\mathcal {V}^{\texttt {c}} = \big \{V^{\texttt {c}}_1, \dots , V^{\texttt {c}}_N\big \}$ the set of cooperative vehicles $V^{\texttt {c}}_n$ with IDs $\mathcal {N} = \{1, \dots , N\}$ for which trajectories are planned. Each $V^{\texttt {c}}_n$ is associated with a planning problem with a planning horizon of up to $k_h \in \mathbb {N}_0$, which includes the initial state of $V^{\texttt {c}}_n$ and a set of goal states. A reference path $\Gamma _n$ is constructed for a planning problem with a given route planner, which is then used to establish a local curvilinear coordinate system $F^{\texttt {L}}_n$ of $V^{\texttt {c}}_n$ as described in [2]. Within $F^{\texttt {L}}_n$, $(s_n, d_n)$ describes the longitudinal coordinate $s_n$ and the lateral coordinate $d_n$. Adopting this coordinate system facilitates the formulation of maneuvers from the perspective of $V^{\texttt {c}}_n$, examples of which include lane-following and preventing driving backwards. We use $\mathcal {L}\mathcal {L}$ to denote the set of lanelets in the road network of a considered scenario. Without loss of generality, we assume obstacles present in the scenarios to be non-cooperating vehicles, denoted by $\mathcal {V}^{\texttt {o}} = \big \{V^{\texttt {o}}_1, \dots , V^{\texttt {o}}_M\big \}$ with IDs $\mathcal {M} = \{1, \dots , M\}$. In addition, we assume that the most likely predictions of trajectories of other vehicles $V^{\texttt {o}}_m$ are given as input. The conflicts between the reachable sets of vehicles in $\mathcal {V}^{\texttt {c}}$ are detected and resolved in the global Cartesian coordinate system $F^{\texttt {G}}$.

Fig. 1

A scenario containing planning problems with two cooperating ego vehicles $V^{\texttt {c}}_1$ and $V^{\texttt {c}}_2$, and four lanelets with IDs 1–4. The triangles at the beginning of each lanelet indicate the driving directions

3.2 System Dynamics

The dynamics of an ego vehicle $V^{\texttt {c}}_n$ is abstracted by a point-mass model with the center of the vehicle as the reference point. Notably, the reachable sets of the point-mass model over-approximate those of high-fidelity vehicle models; thus, this abstraction does not exclude possible behaviors of $V^{\texttt {c}}_n$. This model is represented with two double integrators in its longitudinal $s_n$ and lateral $d_n$ directions. Let $\square _n$ be a variable of $V^{\texttt {c}}_n$, with minimum and maximum values denoted by $\underline{\square }_n$ and $\overline{\square }_n$, respectively. The system dynamics of $V^{\texttt {c}}_n$ is

$$\begin{aligned} \boldsymbol{x}_{n,k+1} = f(\boldsymbol{x}_{n,k}, \boldsymbol{u}_{n,k}) = \begin{pmatrix} 1 & \Delta _t & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \Delta _t \\ 0 & 0 & 0 & 1 \end{pmatrix} \boldsymbol{x}_{n,k} + \begin{pmatrix} \frac{1}{2}\Delta _t^2 & 0\\ \Delta _t & 0 \\ 0 & \frac{1}{2}\Delta _t^2\\ 0 & \Delta _t \end{pmatrix} \boldsymbol{u}_{n,k}, \end{aligned}$$

(1)

where $k \in \mathbb {N}_0$ is a step corresponding to time $t_k = k \Delta _t$, with $\Delta _t \in \mathbb {R}_{+}$ being a predefined time increment. The variable $\boldsymbol{x}_{n,k} \in \mathcal {X}_{n,k} \subset \mathbb {R}^4$ represents the state of $V^{\texttt {c}}_n$ in the state space $\mathcal {X}_{n,k}$, and $\boldsymbol{u}_{n,k} \in \mathcal {U}_{n,k} \subset \mathbb {R}^2$ represents an input in the input space $\mathcal {U}_{n,k}$ of $V^{\texttt {c}}_n$, each at step k. The states and inputs are modeled as $\boldsymbol{x}_{n,k} = (s_{n,k}, \dot{s}_{n,k}, d_{n,k}, \dot{d}_{n,k})^\textsf{T}$ and $\boldsymbol{u}_{n,k} = (\ddot{s}_{n,k}, \ddot{d}_{n,k})^\textsf{T}$, respectively. The velocities and accelerations at a position $(s_{n,k}, d_{n,k})$ are bounded by

$$\begin{aligned} \underline{\dot{s}}(\Gamma _n) \le \dot{s}_{n,k} \le \overline{\dot{s}}(\Gamma _n),& \;\; \underline{\dot{d}}(\Gamma _n) \le \dot{d}_{n,k} \le \overline{\dot{d}}(\Gamma _n), \end{aligned}$$

(2a)

$$\begin{aligned} \underline{\ddot{s}}(\Gamma _n) \le \ddot{s}_{n,k} \le \overline{\ddot{s}}(\Gamma _n),& \;\; \underline{\ddot{d}}(\Gamma _n) \le \ddot{d}_{n,k} \le \overline{\ddot{d}}(\Gamma _n). \end{aligned}$$

(2b)

The bounds are chosen conservatively to consider the kinematic limitations and effects of representing the system dynamics using the point-mass model within a curvilinear coordinate system, see, for example, article [7]. We define an operator ${\text {proj}}_\Diamond (\cdot )$ for subsequent computations, which maps the input to its elements $\Diamond$. An example is: ${\text {proj}}_{(s,\dot{s})}(\tilde{\boldsymbol{x}}_{n,k}) = (s_{n,k},\dot{s}_{n,k})^\textsf{T}$ for $\tilde{\boldsymbol{x}}_{n,k} = (s_{n,k},\dot{s}_{n,k}, \ddot{s}_{n,k})^\textsf{T}$. A set $\tilde{\mathcal {X}}_{n,k}$ can be projected using the same operator:

$$\begin{aligned} {\text {proj}}_\Diamond (\tilde{\mathcal {X}}_{n,k}) = \Big \{{\text {proj}}_\Diamond (\tilde{\boldsymbol{x}}_{n,k}) \Big | \tilde{\boldsymbol{x}}_{n,k} \in \tilde{\mathcal {X}}_{n,k} \Big \}. \end{aligned}$$

3.3 Reachable Set

We denote the occupancy of $V^{\texttt {c}}_n$ by $\mathcal {Q}_n(\boldsymbol{x}_{n,k}) \subset \mathbb {R}^2$ and the occupancies of all vehicles in $\mathcal {V}^{\texttt {o}}$ as well as the regions outside the road surface by $\mathcal {O}_{n,k} \subset \mathbb {R}^2$, both within $F^{\texttt {L}}_n$. The set of forbidden states $\mathcal {X}^{\texttt {F}}_{n,k}$ of $V^{\texttt {c}}_n$ at k is defined as

$$\begin{aligned} \mathcal {X}^{\texttt {F}}_{n,k} :=\Big \{\boldsymbol{x}_{n,k} \in \mathcal {X}_{n,k} \Big | \mathcal {Q}_n(\boldsymbol{x}_{n,k}) \cap \mathcal {O}_{n,k} \ne \emptyset \Big \}. \end{aligned}$$

Let ${\mathcal {R}}^{*}_{n,0} = \mathcal {X}_{n,0}$ be the initial reachable set of $V^{\texttt {c}}_n$, with $\mathcal {X}_{n,0}$ being the initial set of states. The reachable set ${\mathcal {R}}^{*}_{n,k+1}$ of the next step is defined as the set of states reachable from the current reachable set ${\mathcal {R}}^{*}_{n,k}$ while avoiding the forbidden states:

$$\begin{aligned} {\mathcal {R}}^{*}_{n,k+1} := \Big \{ &\boldsymbol{x}_{n,k+1} \in \mathcal {X}_{n,k+1} \Big | \exists \boldsymbol{x}_{n,k} \in {\mathcal {R}}^{*}_{n,k}, \exists \boldsymbol{u}_{n,k} \in \mathcal {U}_{n,k} : \\ &\boldsymbol{x}_{n,k+1} = f(\boldsymbol{x}_{n,k}, \boldsymbol{u}_{n,k}) \wedge \boldsymbol{x}_{n,k+1} \notin \mathcal {X}^{\texttt {F}}_{n,k+1} \Big \}. \end{aligned}$$

Efficient computation of ${\mathcal {R}}^{*}_{n,k}$ is generally difficult; hence, we compute its over-approximation ${\mathcal {R}}_{n,k} \approx {\mathcal {R}}^{*}_{n,k}$, which encloses all trajectories of $V^{\texttt {c}}_n$. We adopt the union of so-called base sets $\mathcal {R}^{(i)}_{n,k}$, $i \in \mathbb {N}$ as a set representation for ${\mathcal {R}}_{n,k}$ [34]. Each base set $\mathcal {R}^{(i)}_{n,k}=\hat{\mathcal {P}}^{(i)}_{s,n,k} \times \hat{\mathcal {P}}^{(i)}_{d,n,k}$ is chosen to be a Cartesian product of two convex polytopes that enclose the reachable positions and velocities of $V^{\texttt {c}}_n$ in the $(s_{n}, \dot{s}_{n})$ and $(d_{n}, \dot{d}_{n})$ planes, respectively (see Fig. 2a, b). To simplify the notation, we also denote the collection (set of sets) of $\mathcal {R}^{(i)}_{n,k}$ by ${\mathcal {R}}_{n,k}=\left\{ \mathcal {R}^{(1)}_{n,k}, \dots , \mathcal {R}^{(i)}_{n,k}, \dots \right\}$. The projection of $\mathcal {R}^{(i)}_{n,k}$ onto the position domain yields axis-aligned rectangles $\mathcal {D}^{(i)}_{n,k}$ (see Fig. 2c), whose union is referred to as the drivable area $\mathcal {D}_{n,k}$. Similarly, we use $\mathcal {D}_{n,k}$ to denote the collection of $\mathcal {D}^{(i)}_{n,k}$.

Fig. 2

(adapted from [13])

Polytopes and drivable area of a base set $\mathcal {R}^{(i)}_{n,k}$

In this study, each base set $\mathcal {R}^{(i)}_{n,k}$ carries a set of semantic labels $\mathcal {L}^{(i)}_{n,k}$, whose collection is denoted by $\mathcal {L}_{n,k}$. The generation of $\mathcal {L}^{(i)}_{n,k}$ will be explained in Sect. 4.6.3. To store the relationships of $\mathcal {R}^{(i)}_{n,k}$ in terms of reachability and time, we create a directed and acyclic graph $G_n$, which is referred to as a reachability graph, see Fig. 3. Each node in $G_n$ corresponds to one base set with its labels. An edge connecting $\mathcal {R}^{(i)}_{n,k}$ and $\mathcal {R}^{(j)}_{n,k+1}$ indicates that $\mathcal {R}^{(j)}_{n,k+1}$ is reachable from $\mathcal {R}^{(i)}_{n,k}$ after one step.

Fig. 3

(adapted from [13])

Reachability graph $G_n$ connecting nodes of different steps. Nodes of the same color have the same labels

3.4 Propositional Logic

We consider specifications expressed in propositional logic [12] for $V^{\texttt {c}}_n$, denoted by $\mathcal {F}_n$, which are directly integrated during the computation of the reachable sets (see Sect. 4.6.4). Let $\varphi _n \in \mathcal {F}_n$ be a propositional logic formula, we introduce an additional syntax ${{\textbf {G}}}_I(\varphi _n)$, $I = [a,b]$, $0 \le a \le b \le k_h$, where I is an integer interval specifying steps for which $\varphi _n$ should hold. If I is not specified, we assume it to be the entire planning horizon $[0, k_h]$. For example, the following specification requires $V^{\texttt {c}}_n$ to follow $V^{\texttt {o}}_{1}$ between steps 0 and 10, and never to be on the right of $V^{\texttt {o}}_{1}$:

$$\begin{aligned} &{{\textbf {G}}}_{[0, 10]}\left( {\text {behind}}(V^{\texttt {o}}_{1}) \wedge {\text {aligned}}\_{\text {with}}(V^{\texttt {o}}_{1})\right) \wedge {{\textbf {G}}}\left( \lnot {\text {right}}\_{\text {of}}(V^{\texttt {o}}_{1})\right) . \end{aligned}$$

4 Computing Specification-Compliant Reachable Sets

To obtain specification-compliant and negotiated reachable sets for $V^{\texttt {c}}_n$, we (1) semantically label reachable sets considering relevant predicates, (2) constrain reachable sets to subsets satisfying specifications $\mathcal {F}_n$, and (3) negotiate conflicting reachable sets with other cooperating vehicles in $\mathcal {V}^{\texttt {c}}$. This section summarizes our previous work [13] covering steps 1 and 2; step 3 will be covered in the next section. A selection of considered predicates is listed in Table 1: The evaluation of a vehicle-dependent (VD) predicate is dependent on other vehicles $\mathcal {V}^{\texttt {o}}$, whereas that of a vehicle-independent (VI) predicate is not.

Table 1 Selection of considered predicates inspired by [19] (adapted from [13])

4.1 State Space Partitioning

To expedite the labeling of reachable sets, we partition the state space of $V^{\texttt {c}}_n$ based on considered position predicates. Velocity predicates are not considered in the partitioning since they require computationally demanding splitting of the state space of $V^{\texttt {c}}_n$ with (non)linear curves (see Fig. 5c, d). For efficiency, we instead directly evaluate them on individual reachable sets (see Sect. 4.6.2). Set operations such as intersection and difference are required to compute the partitions of the state space. We model the partitions for $V^{\texttt {c}}_n$ with a set of hyperrectangles $R_{n,q}$ to avoid gross approximations while keeping computational complexity at a reasonable level. This choice is not mandatory; any other set representation that captures the partitions will also suffice. $R_{n,q}$ is defined as the Cartesian product of intervals over the position and velocity domains within $F^{\texttt {L}}_n$:

$$\begin{aligned} R_{n,q}:= \left( [\underline{s}_{n,q},\overline{s}_{n,q}] \times [\underline{\dot{s}}_{n,q},\overline{\dot{s}}_{n,q}] \right) \times \left( [\underline{d}_{n,q},\overline{d}_{n,q}] \times [\underline{\dot{d}}_{n,q},\overline{\dot{d}}_{n,q}] \right) , \end{aligned}$$

(3)

where $s_{n,q}$ and $\dot{s}_{n,q}$ denote the position and velocity of the q-th hyperrectangle in the $s_n$ direction, respectively. The same applies to $d_{n,q}$ and $\dot{d}_{n,q}$ in the $d_n$ direction. A regular grid of axis-aligned cells is formed along $\Gamma _n$ and the q-th cell in the grid occupies $[\underline{s}_{n,q}, \overline{s}_{n,q}] \times [\underline{d}_{n,q}, \overline{d}_{n,q}] \subset \mathbb {R}^2$. The default values of the velocity intervals $[\underline{\dot{s}}_{n,q}, \overline{\dot{s}}_{n,q}]$ and $[\underline{\dot{d}}_{n,q}, \overline{\dot{d}}_{n,q}]$ are set according to (2a).

The set of considered position predicates as well as its power set are denoted by $\mathcal {P}^{\texttt{pos}} = \{{\sigma }_{1}, {\sigma }_{2}, \dots \}$ and $2^{\mathcal {P}^{\texttt{pos}}}$, respectively. We also denote by ${\text {part}}_{n}(k;\mathcal {Z}_{n,j})$ the set of hyperrectangles of $V^{\texttt {c}}_n$ for which the predicates in $\mathcal {Z}_{n,j}\in \mathcal {Z}_{n} \subseteq 2^{\mathcal {P}^{\texttt{pos}}}$ evaluate to true at step k. Figures 4 and 5b illustrate example partitions projected onto the $(s_n, d_n)$ and $(s_n, \dot{s}_n)$ planes, respectively.

Fig. 4

(adapted from [13])

Projection of the partitions of realizable sets of position predicates onto the position domain. Lanelet IDs are shown with numbered boxes. In this example we only consider position predicates related to $L_1$, $L_2$, and $V^{\texttt {o}}_{2}$

4.2 Position Predicates

only a few example evaluations position predicates. Vehicle-independent position predicates do not depend on other vehicles; examples are:

• ${\text {in}}\_{\text {lanelet}}(R_{n,q}; L_{\texttt {id}}) \Leftrightarrow$ ${\text {proj}}_{(s,d)}(R_{n,q}) \cap {\text {occ}}_n(L_{\texttt {id}}) \ne \varnothing$, where $L_{\texttt {id}} \in \mathcal {L}\mathcal {L}$ denotes the lanelet with ID $\texttt {id}$, and ${\text {occ}}_n(L_{\texttt {id}})$ returns its occupancy within $F^{\texttt {L}}_n$.

• ${\text {drives}}\_{\text {rightmost}}(R_{n,q}; \mathcal {X}^{\texttt {RM}}) \Leftrightarrow$ ${\text {proj}}_{(s,d)}(R_{n,q}) \cap \mathcal {X}^{\texttt {RM}} \ne \varnothing$, where $\mathcal {X}^{\texttt {RM}} \subset \mathbb {R}^2$ denotes the rightmost region of lanelets. Within this region, the distance between any point to the right bound of a lanelet does not exceed a predefined distance [19].

For the sake of brevity, we omit $R_{n,q}$ in the arguments of the predicates in the rest of this work.

Vehicle-dependent position predicates describe position relationships between an ego vehicle $V^{\texttt {c}}_n$ and non-cooperating vehicles in $\mathcal {V}^{\texttt {o}}$. Following [19], we define necessary helper functions to assist the evaluation of predicates. The functions ${\text {front}}(k; n; m)$ and ${\text {rear}}(k; n; m)$ return the maximum and minimum longitudinal coordinates of $V^{\texttt {o}}_m$ within $F^{\texttt {L}}_n$, respectively, each at step k. Along the longitudinal direction, the mutually exclusive predicates $\mathcal {P}_{n,m,s}^{\texttt{pos}} = \big \{\{{\text {in}}\_{\text {front}}\_{\text {of}}(V^{\texttt {o}}_m)\},\{{\text {behind}}(V^{\texttt {o}}_m)\}$,

$\{{\text {beside}}(V^{\texttt {o}}_m)\} \big \}$ can be evaluated as follows:

• ${\text {in}}\_{\text {front}}\_{\text {of}}(V^{\texttt {o}}_m) \Leftrightarrow$ $\underline{s}_{n,q} - l_n / 2 > {\text {front}}(k; n; m)$,

• ${\text {behind}}(V^{\texttt {o}}_m) \Leftrightarrow$ $\overline{s}_{n,q} + l_n / 2 < {\text {rear}}(k; n; m)$,

• ${\text {beside}}(V^{\texttt {o}}_m) \Leftrightarrow$ $\lnot {\text {in}}\_{\text {front}}\_{\text {of}}(V^{\texttt {o}}_m) \wedge \lnot {\text {behind}}(V^{\texttt {o}}_m) \wedge ({\text {left}}\_{\text {of}}(V^{\texttt {o}}_m) \vee {\text {right}}\_{\text {of}}(V^{\texttt {o}}_m))$.

We define the mutually exclusive set of predicates $\mathcal {P}_{n,m,d}^{\texttt{pos}} = \big \{\{{\text {left}}\_{\text {of}}(V^{\texttt {o}}_m)\},$ $\{{\text {right}}\_{\text {of}}(V^{\texttt {o}}_m)\},\{{\text {aligned}}\_{\text {with}}(V^{\texttt {o}}_m)\} \big \}$ similarly along the lateral direction.

4.3 Realizable Sets of Position Predicates

The partitions of the collection $\mathcal {Z}_{n}$ of realizable sets of position predicates of $V^{\texttt {c}}_n$ are used for splitting the reachable sets (see Sect. 4.6.2). Sets $\mathcal {Z}_{n,j}\in \mathcal {Z}_{n}$ are said to be realizable for $V^{\texttt {c}}_n$ if $\exists k \in {0, \dots , k_h}: {\text {part}}_n(k;\mathcal {Z}_{n,j}) \ne \varnothing$, with $k_h$ being the planning horizon. We refer the readers to [13, Sect. III.C] for the computation of $\mathcal {Z}_{n}$. Figure 4 shows an example of the partitions of $\mathcal {Z}_{n,j}$ projected onto the position domain for a scenario containing two lanelets and one non-cooperating vehicle. It follows from our formulation of the predicates that the aforementioned projection is collision-free with respect to other vehicles.

4.4 Velocity Predicates

We briefly present examples of the evaluation of velocity predicates required for the subsequent computation of reachable sets. Vehicle-independent velocity predicates often relate to extremum requirements on velocities. For example, rule R-G3 [19] specifies maximum velocity limits originating from different sources, which should be respected. These include velocity limits introduced by the type of lane(let), the type of vehicle, and the limited field of view of the ego vehicle.

The evaluation of vehicle-dependent velocity predicates depends on other vehicles $\mathcal {V}^{\texttt {o}}$. Examples are predicates indicating whether the ego vehicle $V^{\texttt {c}}_n$ is driving at a safe velocity with respect to a leading or a following vehicle $V^{\texttt {o}}_m$ [19, cf. Sect. IV.C]. See [19] for further examples.

4.5 General Traffic Situation Predicates

General traffic situation predicates may reveal the states of a cooperating or non-cooperating vehicle. These include whether $V^{\texttt {c}}_n$ or $V^{\texttt {o}}_m$ has conducted a lane change maneuver, whether a slow leading vehicle exists for $V^{\texttt {c}}_n$, and whether $V^{\texttt {c}}_n$ is stuck in traffic congestion. See [19] for further examples.

4.6 Computation of Reachable Sets

Algorithm 1 details one step of the computation of specification-compliant reachable sets for an ego vehicle. The reachable sets of subsequent steps are computed analogously.

Fig. 5

(adapted from [13])

Propagation, splitting, and labeling of base sets. We only show the operations in the s-direction. Labels of polytopes are shown in gray boxes. Notably, in d, the two newly split polytopes are slightly over-approximated and convexified due to the nonlinearity introduced by the velocity predicate

4.6.1 Forward Propagation

Each base set $\mathcal {R}^{(i)}_{n,k-1} \in {\mathcal {R}}_{n,k-1}$ from the previous step is forward-propagated based on the discrete-time system model (1), resulting in the propagated sets $\mathcal {R}^{{\texttt{P},(i)}}_{n,k}$ (see Fig. 5a). We perform the forward propagation as described in [34], except that additional acceleration constraints originating from the specifications can be imposed (for example, unnecessary braking rule R_G2 in [19]).

4.6.2 Splitting

The propagated sets $\mathcal {R}^{{\texttt{P},(i)}}_{n,k}$are split into new sets $\mathcal {R}^{\texttt{S},(i)}_{n,k}$ with respect to position and velocity predicates:

• 1. $\mathcal {R}^{{\texttt{P},(i)}}_{n,k}$ are split such that the new sets only intersect with a single partition (see Fig. 5b).

• 2. The split sets are further split, over-approximated, and convexified with respect to velocity predicates (see Fig. 5c, d).

4.6.3 Semantic Labeling

The semantic labels $\mathcal {L}^{(i)}_{n,k}$ of reachable sets $\mathcal {R}^{\texttt{S},(i)}_{n,k}$ are updated as follows:

• 1. $\mathcal {R}^{\texttt{S},(i)}_{n,k}$ propagated with acceleration-specific specifications include atomic propositions $\sigma \in \mathcal{A}\mathcal{P}$ corresponding to acceleration predicates in their set of labels.

• 2. $\mathcal {R}^{\texttt{S},(i)}_{n,k}$ include atomic propositions $\sigma \in \mathcal{A}\mathcal{P}$ corresponding to the position predicates associated with the partition with which it intersects, velocity predicates, and traffic situation predicates that hold in $\mathcal {R}^{\texttt{S},(i)}_{n,k}$ in their set of labels.

4.6.4 Compliance Check

In this step, we iterate through $\mathcal {R}^{\texttt{S},(i)}_{n,k}$ and examine the compliance of the labels $\mathcal {L}^{(i)}_{n,k}$ with the given specifications $\mathcal {F}_n$. We discard $\mathcal {R}^{\texttt{S},(i)}_{n,k}$ if $\exists \varphi _n \in \mathcal {F}_n: \mathcal {L}^{(i)}_{n,k}\not \models \varphi _n$. If all sets are discarded, $\mathcal {F}_n$ cannot be complied with by any trajectory of the ego vehicle (recall that our reachable sets are over-approximative). In this case, one can either recompute the reachable sets with respect to a different set of specifications or execute a previously computed fail-safe trajectory [26].

4.6.5 Creation of New Base Sets

Finally, the new base sets are created by computing the drivable areas $\mathcal {D}^{\texttt{S},(i)}_{n,k}$ of $\mathcal {R}^{\texttt{S},(i)}_{n,k}$, repartitioning $\mathcal {D}^{\texttt{S},(i)}_{n,k}$, and producing $\mathcal {R}^{(i)}_{n,k}$. We refer the reader to [34] for a detailed explanation of these steps. The reachability graph $G_n$ is updated by adding $\mathcal {R}^{(i)}_{n,k}$ along $\mathcal {L}^{(i)}_{n,k}$ as new nodes.

5 Negotiation of Reachable Sets

This section summarizes our previous work on the negotiation of conflicting reachable sets ${\mathcal {R}}_{n,k}$ among a group of cooperating vehicles [21]. We use the notation $[\square _n]^{N}_{1} = [\square _1, \dots , \square _N]$ to denote a list of elements $\square _n$ of vehicles $V^{\texttt {c}}_n$. Algorithm 2 details the steps for resolving conflicts between cooperating vehicles at each step k:

• 1. Compute specification-compliant reachable sets for each cooperating vehicle.

• 2. Identify conflicting cells based on reachable sets of cooperating vehicles (see Sect. 5.1).

• 3. Determine the optimal allocation of packages of cells among cooperating vehicles (see Sect. 5.2).

• 4. Compute negotiated reachable sets for each cooperating vehicle (see Sect. 5.2).

Step 1 is computed as described in Sect. 4; we will now elaborate on steps 2–4.

5.1 Problem Statement

We denote by $\mathcal {C} = \{C_0, C_1, \dots , C_{\tilde{i}}, \dots \}$ a grid with cells $C_{\tilde{i}}$ of rectangular shape, created by tessellation of the position domain within the global Cartesian coordinate system $F^{\texttt {G}}$. Each cell is an individual asset representing an area of the road surface and can be combined into unions of assets, which we refer to as packages $\mathcal {C}_{\tilde{j}}$. We specify the mapping ${\text {cell}}_n: 2^{\mathcal {X}_{n,k}} \rightarrow 2^{\mathcal {C}}$ that returns the cells $C_{\tilde{i}} \in \mathcal {C}$ occupied by vehicle $V^{\texttt {c}}_n$ due to its set of states $\mathcal {X}_{n,k}$ at step k and its shape. The cooperating vehicles in $\mathcal {V}^{\texttt {c}}$ act as bidders and propose bids to packages $\mathcal {C}_{\tilde{j}}$ for which $\mathcal {C}_{\tilde{j}}\cap {\text {cell}}_n(\mathcal {R}_{n,k}) \ne \varnothing$ holds. Let us introduce $2^{\mathcal {N}}_{>=2}$ to denote all subsets of the power set of $\mathcal {N}$ with a cardinality greater than one, $\mathcal {C}_k^{\texttt {C}}\subseteq \mathcal {C}$ denotes the set of conflicting cells requested by at least two vehicles at step k:

$$\begin{aligned} \mathcal {C}_k^{\texttt {C}}:= \bigcup _{\mathcal {I} \in 2^{\mathcal {N}}_{>=2}} \bigcap _{n \in \mathcal {I}} {\text {cell}}_n(\mathcal {R}_{n,k}). \end{aligned}$$

(4)

We restrict the packages to those containing at least one conflicting cell, denoted by $\mathcal {C}_k^{\texttt {P}}\subseteq 2^{\mathcal {C}_k^{\texttt {C}}}$ (see Fig. 6). We assume that every cooperating vehicle $V^{\texttt {c}}_n$ bids its true value, with $\overline{b}_{k}(\mathcal {C}_{\tilde{j}})$ being the maximum bid of the package $\mathcal {C}_{\tilde{j}}$ proposed by $\mathcal {V}^{\texttt {c}}$. The overall revenue is maximized, while no single cell is assigned to multiple bidders:

$$\begin{aligned} \max _{\delta _k(\mathcal {C}_{\tilde{j}})} \sum _{\mathcal {C}_{\tilde{j}}} \delta _k(\mathcal {C}_{\tilde{j}}) \, \overline{b}_{k}(\mathcal {C}_{\tilde{j}}), \end{aligned}$$

(5)

where $\delta _k(\mathcal {C}_{\tilde{j}}) = 1$ if package $\mathcal {C}_{\tilde{j}}$ is assigned to the bidder with the highest bid at step k. Problem (5) is known as the winner determination problem, and its solution is NP-hard [30]. Furthermore, accepting every package $\mathcal {C}_{\tilde{j}}$ demands that each bidder $V^{\texttt {c}}_n$ bids for $2^{|\mathcal {C}_k^{\texttt {C}}|} -1$ packages at step k, which becomes more computationally demanding as $|\mathcal {C}_k^{\texttt {C}}|$ grows. Using a hierarchical tree structure for the packages allows us to attain computational tractability and ensures that the optimal allocation of packages will be found in the time $O(|\mathcal {C}_k^{\texttt {C}}|^2)$ [30].

Fig. 6

(adapted from [21])

Visualization of the road grid $\mathcal {C}$, the set of conflicting cells $\mathcal {C}_k^{\texttt {C}}$, the set of packages $\mathcal {C}_k^{\texttt {P}}$, and the individual packages $\mathcal {C}_{\tilde{j}}$

5.2 Conflict Resolution

We employ an auction-based mechanism to resolve conflicts with occupied road cells between cooperating vehicles. At every step k, the conflicts are resolved as follows:

• 1. Determine packages $\mathcal {C}_{\tilde{j}}$ based on $\mathcal {C}_k^{\texttt {C}}$ and their position within the hierarchical tree (see Sect. 5.2.1).

• 2. Evaluate individual bids of packages $\mathcal {C}_{\tilde{j}}$ and determine the maximum bid $\overline{b}_{k}(\mathcal {C}_{\tilde{j}})$ (see Sect. 5.2.2).

• 3. Determine the optimal allocation $\mathcal {W}^*$ of packages to cooperating vehicles (see Sect. 5.2.3).

Fig. 7

(adapted from [21])

Example grouping of conflicting cells

5.2.1 Hierarchical Tree of Packages

All conflicting cells $\mathcal {C}_k^{\texttt {C}}$ at k are included in the root node of a hierarchical tree T. At each level of the tree, the cells in a parent node are decomposed into disjoint sets of cells, each of which is a package associated with a child node (see Fig. 7). To decompose the cells into more granular packages, we consider the following levels:

• 1. Connected components: Connected regions on the road surface prevents ego vehicles having disjointed drivable areas, which would complicate subsequent motion planning. We aggregate connected cells into packages.

• 2. Road network: Vehicles have to obey the traffic rules imposed by the road network; therefore, we encourage the creation of packages based on lanelets. A cell is assigned to the lanelet with which it has the largest intersecting area.

• 3. Longitudinal position coverage: The packages of the parent nodes are decomposed in the longitudinal direction such that the longitudinal coverage of each new package does not exceed a predefined threshold.

• 4. Lateral position coverage: The packages of the parent nodes are decomposed in the lateral direction such that the lateral coverage of each new package does not exceed a predefined threshold.

• 5. Singletons: The packages comprise only a single cell.

5.2.2 Bids on Packages

We adopt a common utility function for cooperating vehicles to avoid a situation in which a vehicle could continuously outbid others due to differences in the scales and weights used to calculate the bids on packages. We use the following sets as the basis for computing the utility of $V^{\texttt {c}}_n$ for $\mathcal {C}_{\tilde{j}}$ to determine $b_{n,k}(\mathcal {C}_{\tilde{j}})$:

• 1. the conflict-free reachable set: $\mathcal {R}^{\texttt{CF}}_{n,k}:= \left\{ x_{n,k} \in \mathcal {R}_{n,k}\big | {\text {cell}}_n(\{x_{n,k}\}) \cap \mathcal {C}_k^{\texttt {C}}= \varnothing \right\}$.

• 2. the conflicting reachable set depending on package $\mathcal {C}_{\tilde{j}}$ that would be lost if $\mathcal {C}_{\tilde{j}}$ was not assigned to $V^{\texttt {c}}_n$: $\mathcal {R}^{\texttt{CP}}_{n,k}(\mathcal {C}_{\tilde{j}}) := \left\{ x_{n,k} \in \mathcal {R}_{n,k}\big | {\text {cell}}_n(\{x_{n,k}\}) \cap \mathcal {C}_{\tilde{j}}\ne \varnothing \right\}$.

• 3. the assigned reachable set that $V^{\texttt {c}}_n$ possesses given that $\mathcal {C}_{\tilde{j}}$ is assigned to $V^{\texttt {c}}_n$: $\mathcal {R}^{\texttt{AS}}_{n,k}(\mathcal {C}_{\tilde{j}}) := \mathcal {R}^{\texttt{CF}}_{n,k}\cup \mathcal {R}^{\texttt{CP}}_{n,k}(\mathcal {C}_{\tilde{j}})$.

For computational reasons, the sets $\mathcal {R}^{\texttt{CF}}_{n,k}$,$\mathcal {R}^{\texttt{CP}}_{n,k}(\mathcal {C}_{\tilde{j}})$, and $\mathcal {R}^{\texttt{AS}}_{n,k}(\mathcal {C}_{\tilde{j}})$ are approximated by the union of base sets (see Sect. 3.3) and are denoted by $\cup _i\mathcal {R}^{\texttt{CF},(i)}_{n,k}$, $\cup _i\mathcal {R}^{\texttt{CP},(i)}_{n,k}$, and $\cup _i\mathcal {R}^{\texttt{AS},(i)}_{n,k}$, respectively. To take the objectives of the vehicles into account while preventing the complete loss of the reachable set of a vehicle (so that a trajectory can still be found), the utilities of vehicles are computed differently for regular mode and survival mode:

$$\begin{aligned} b_{n,k}(\mathcal {C}_{\tilde{j}}) := {\left\{ \begin{array}{ll} U^{\texttt{R}}_{n,k}(\mathcal {C}_{\tilde{j}}), & {\text {area}}(\mathcal {R}^{\texttt{CF}}_{n,k}) > \underline{A}, \text {(regular mode)} \\ U^{\texttt{S}}_{n,k}(\mathcal {C}_{\tilde{j}}), & \text {otherwise}, \text {(survival mode)} \end{array}\right. } \end{aligned}$$

where ${\text {area}}(\cdot )$ returns the size of the drivable area of the input (see Sect. 3.3) and A is a threshold. We now proceed with explaining the regular mode and survival mode.

(1) Regular Mode: The utility of $\mathcal {R}^{\texttt{AS}}_{n,k}$ (or $\mathcal {R}^{\texttt{CF}}_{n,k}$, as the case may be) is defined as the sum of the utilities of $\mathcal {R}^{\texttt{AS},(i)}_{n,k}$ (or $\mathcal {R}^{\texttt{CF},(i)}_{n,k}$), weighted by their areas. The function $U^{\texttt{R}}_{n,k}(\mathcal {C}_{\tilde{j}})$ reflects the utility of $\mathcal {C}_{\tilde{j}}$ for $V^{\texttt {c}}_n$ by computing the ratio of the utility of $\mathcal {R}^{\texttt{AS}}_{n,k}$ to that of $\mathcal {R}^{\texttt{CF}}_{n,k}$:

$$\begin{aligned} U^{\texttt{R}}_{n,k}(\mathcal {C}_{\tilde{j}}) = \frac{\sum _{i} \left( u^{\texttt{pos}}(\mathcal {R}^{\texttt{AS},(i)}_{n,k}) + u^{\texttt{ref}}(\mathcal {R}^{\texttt{AS},(i)}_{n,k})\right) \times {\text {area}}\big (\mathcal {R}^{\texttt{AS},(i)}_{n,k}\big )}{\sum _{i} \left( u^{\texttt{pos}}(\mathcal {R}^{\texttt{CF},(i)}_{n,k}) + u^{\texttt{ref}}(\mathcal {R}^{\texttt{CF},(i)}_{n,k})\right) \times {\text {area}}\big (\mathcal {R}^{\texttt{CF},(i)}_{n,k}\big )}, \end{aligned}$$

with partial utility functions $u^{\texttt{pos}}$ and $u^{\texttt{ref}}$. To encourage advances in traffic flow, we reward progression in the longitudinal direction with

$$\begin{aligned} u^{\texttt{pos}}(\square _{n,k}) = y\left( \frac{\max ({\text {proj}}_{(s)}(\square _{n,k})) - \max ({\text {proj}}_{(s)}(\mathcal {R}^{\texttt{N}}_{n, k-1}))}{\frac{1}{2} \, \overline{\ddot{s}}_{n,k} \, \Delta _t^2 + \overline{\dot{s}}_{n,k} \, \Delta _t} \right) , \end{aligned}$$

where $\overline{\ddot{s}}_{n,k}$ and $\overline{\dot{s}}_{n,k}$ are determined according to (2a), and y is a generalized logistic function that maps the utility to (0, 1) ; in addition to [21], we also consider the deviation of $V^{\texttt {c}}_n$ from its reference path:

$$\begin{aligned} u^{\texttt{ref}}(\square _{n,k}) = e^{-w\,d'}, d' = \min (\left\{ |d''| \, \big | d'' \in {\text {proj}}_{(d)}(\square _{n,k})\right\} ), \end{aligned}$$

where $w \in \mathbb {R}_{+}$ is a tunable weight that dictates how fast $u^{\texttt{ref}}(\square _{n,k})$ approaches 0 as the deviation increases.

(2) Survival Mode: Two countermeasures are introduced to prevent reachable sets of $V^{\texttt {c}}_n$ from vanishing: (1) if any $V^{\texttt {c}}_n$ is in survival mode, no other vehicle in regular mode can bid on the package $\mathcal {C}_{\tilde{j}}$; (2) the utility function is switched to

$$\begin{aligned} U^{\texttt{S}}_{n,k}(\mathcal {C}_{\tilde{j}}) = \frac{{\text {area}}\big (\mathcal {R}^{\texttt{CP}}_{n,k}(\mathcal {C}_{\tilde{j}})\big )}{ {\text {area}}\big (\mathcal {R}_{n,k}\big )}, \end{aligned}$$

which reflects how close the reachable set of $V^{\texttt {c}}_n$ is to vanishing given that $\mathcal {C}_{\tilde{j}}$ is not assigned to $V^{\texttt {c}}_n$.

5.2.3 Optimal Allocation of Packages

The algorithm for finding the optimal allocation $\mathcal {W}^*$ of packages $\mathcal {C}_{\tilde{j}}$ is based on [30]. In each iteration, we retrieve the deepest node $N^{\texttt{deep}}$ in the hierarchical tree T (see Sect. 5.2.1), its parent node $N^{\texttt{parent}}$, and the set of child nodes $\mathcal {N}^{\texttt{child}} = \left\{ \dots ,N^{\texttt{child}}_{\tilde{q}},\dots \right\}$ of $N^{\texttt{parent}}$. Next, we compare the summed maximum bids (revenue) of all child nodes ${\text {rev}}(\mathcal {N}^{\texttt{child}}) := \sum _{\tilde{q}} \overline{b}_{k}(N^{\texttt{child}}_{\tilde{q}})$ with the maximum bid of the parent node $\overline{b}_{k}(N^{\texttt{parent}})$:

• If $\overline{b}_{k}(N^{\texttt{parent}}) > {\text {rev}}(\mathcal {N}^{\texttt{child}})$, $\mathcal {N}^{\texttt{child}}$ is excluded from $\mathcal {W}^*$.

• If $\overline{b}_{k}(N^{\texttt{parent}}) \le {\text {rev}}(\mathcal {N}^{\texttt{child}})$, $N^{\texttt{parent}}$ is excluded from $\mathcal {W}^*$.

Following this comparison, $\mathcal {N}^{\texttt{child}}$ is removed from the tree. The process is repeated until $N^{\texttt{parent}}$ becomes the root node. After obtaining $\mathcal {W}^{*}$, each ego vehicle $V^{\texttt {c}}_n$ proceeds to determine its negotiated reachable sets:

$$\begin{aligned} \mathcal {R}^{\texttt{N}}_{n,k}:= \left\{ x_{n,k} \in \mathcal {R}_{n,k}\big | {\text {cell}}_n(\{x_{n,k}\}) \cap \mathcal {C}_{n,k}^{\texttt{UA}} = \varnothing \right\} , \end{aligned}$$

where $\mathcal {C}_{n,k}^{\texttt{UA}} \subseteq \mathcal {C}_k^{\texttt {C}}$ denotes the set of unassigned cells of $V^{\texttt {c}}_n$ based on $\mathcal {W}^{*}$.

6 Evaluation

This section provides example results for specification-compliant reachable sets for a single ego vehicle and its extension to cooperative vehicles. The implementation is based on the CommonRoad-Reach toolbox [14] for computing the reachable sets of vehicles.

6.1 Scenario I: Precise Overtaking

This scenario depicts a situation in which the vehicle $V^{\texttt {c}}_1$ should overtake a leading vehicle $V^{\texttt {o}}_{1}$ in the presence of another vehicle $V^{\texttt {o}}_{2}$. Let the following specification be issued by a high-level maneuver planner of $V^{\texttt {c}}_1$:

$$\begin{aligned} {{\textbf {G}}}_{[0, 15]}&\left( {\text {behind}}(V^{\texttt {o}}_{1}) \wedge {\text {aligned}}\_{\text {with}}(V^{\texttt {o}}_{1})\right) \; \wedge \\ {{\textbf {G}}}_{[16, 38]}&\big ({\text {in}}\_{\text {lanelet}}(L_2) \vee {\text {in}}\_{\text {lanelet}}(L_4)\big ) \; \wedge \\ {{\textbf {G}}}_{[39, 45]}&\left( {\text {in}}\_{\text {front}}\_{\text {of}}(V^{\texttt {o}}_{1}) \wedge {\text {behind}}(V^{\texttt {o}}_{2}) \wedge {\text {in}}\_{\text {lanelet}}(L_3)\right) . \end{aligned}$$

The specification-compliant reachable sets are computed as described in Sect. 4. The non-empty result implies that it is possible to find a trajectory that meets the specifications. Figure 8 visualizes the drivable areas of $V^{\texttt {c}}_1$ along with a trajectory planned within the reachable sets using the motion planner described in [22]. For a more detailed evaluation of computing specification-compliant reachable sets for a single ego vehicle, we refer the reader to [13].

Fig. 8

(adapted from [13])

Overtaking scenario. a Drivable area at different steps. b A trajectory planned within the reachable set

6.2 Scenario II: Highway

In this scenario, we negotiate the reachable sets of four cooperating vehicles driving on a highway. Figure 9 shows the computation results at different steps. As can be seen, our method can allocate road areas to cooperating vehicles even in such complex traffic situations with many non-cooperating traffic participants. For a more detailed evaluation of negotiating reachable sets among a group of cooperating vehicles, we refer the reader to [21].

Fig. 9

Highway scenario. Subfigures b–d show the drivable areas of the negotiated reachable sets of vehicles at different steps

6.3 Scenario III: Roundabout

This scenario illustrates a situation in which two vehicles $V^{\texttt {c}}_1$ and $V^{\texttt {c}}_2$ should cooperate to go around a roundabout. We show the computation results under different settings: (1) no specification is considered; (2) $V^{\texttt {c}}_1$ yields to $V^{\texttt {c}}_2$; (3) $V^{\texttt {c}}_2$ yields to $V^{\texttt {c}}_1$. The latter two settings are relevant when a yield traffic sign is present at the junction and specifies which vehicle has to yield to other vehicles entering with a higher passing priority. The specification can be expressed as follows:

$$\begin{aligned} {{\textbf {G}}}({\text {exists}}\_{\text {yield}}\_{\text {sign}} \wedge {\text {exists}}\_{\text {other}}\_{\text {entering}}\_{\text {vehicle}} \Rightarrow {\text {brake}}\_{\text {to}}\_{\text {stop}}), \end{aligned}$$

which can be regarded as a simplified version of the intersection rules described in [18] but without temporal logic connectives. Figure 10 illustrates the computation results under these settings. In Fig. 10b, $V^{\texttt {c}}_2$ can either accelerate and enter the roundabout ahead of $V^{\texttt {c}}_1$ or decelerate to enable $V^{\texttt {c}}_1$ to enter first. In Fig. 10c, d, the yielding vehicles have to brake in order to stop and yield to the other entering vehicle.

Fig. 10

Roundabout scenario. $V^{\texttt {c}}_2$ intends to reach the first exit, and $V^{\texttt {c}}_1$ intends to reach the second exit. Subfigures b–d show the drivable areas of the negotiated reachable sets of vehicles at step $k=30$

7 Conclusions

In this chapter, we summarized our previous works on computing specification-compliant reachable sets for an ego vehicle and negotiating conflicting reachable sets between a group of cooperating vehicles. The specification-compliant and negotiated reachable set is used to guide subsequent motion planners to find specification-compliant trajectories. As a result, the cooperative vehicles can consider traffic rules and handcrafted rules expressed in propositional logic that involve position, velocity, acceleration, and general traffic situation predicates. A limitation of the method is that it does not yet handle specifications formulated in temporal logic, which reflects temporal requirements on vehicles, both in the computation and negotiation of the reachable sets. This will be a subject of future research.