Efficient Speed Planning for Autonomous Driving in Dynamic Environment with Interaction Point Model

I Introduction

Planning safe trajectories for autonomous vehicles (AVs) in complex environments is a challenging task. As demonstrated in Fig. 1, it requires the planning system to have the ability to safely react in real-time under various scenarios, such as intersection, merging, and traversing areas with a limited field of view (FoV).

A typical strategy in solving these problems is to transform traffic rules and prediction results into constraints in an optimization problem [25, 5]. Although this method can generalize to different scenarios, it does not model the prediction and interaction uncertainties well. Another strategy applies the POMDP-based methods[8, 17], which searches for the best reaction trajectory by simulating the traffic interactions in the belief space. However, these methods introduce a high computational burden, and are hard to reflect the interaction patterns using manually designed reward functions.

To better capture the interaction among traffic participants, it is essential to focus on modeling driving patterns around an “interaction point” without traffic regulations. The interaction points are observable situations where at least two road users intend to occupy the same space in the near future[6]. Some works implicitly model the planning problem on these points. For instance, car-following models[21, 18] and lane-merging approaches[10] use different values of time gap ( $Δ t$ in Fig. (a)a) to generate the proper reactive behavior for AVs. Prediction work[16] analyzes the right-of-way of collision points in the intersection. However, these methods are tailored for specific scenarios. It brings a heavy load on parameter tuning when applying multiple methods in different situations.

Fig. 1: The AV (ego) follows the path to the goal with considerations of interactions with other traffic agents (e.g., $V_{i}$ ) at different scenarios (different interaction angles and the blind zone).

When reacting to other road users at different speed and angles in various scenarios, human drivers generally show different degrees of caution that can be indicated by the time gap. Therefore, we focus on the following two questions. Question 1: When two agents intend to pass the same interaction point in various scenarios, what is the minimum time gap between them? Question 2: What determines the priority of agents to pass the interaction point in Question 1?

To answer these questions, we propose a novel Interaction Point Model (IPM), where Question 1 is solved by statistical analysis in real driving records, and Question 2

is explained using a multilayer perceptron (MLP) network. Then, based on IPM, we present a unified planning framework

²²2IPM model files and demonstration videos are available on the project website: https://github.com/ChenYingbing/IPM-Planner. to achieve safe and interactive motion planning, which guides the ego vehicle pass an interaction point only when it has a higher priority than other traffic participants.

I-a Contributions

Overall, we propose a unified interactive planning framework for AVs. The major contributions are:

A novel and general interaction solution called IPM, which uniformly formulates the interaction in terms of the protection time and priority.
A unified planning framework for AV in complex dynamic environments, including an efficient s-t graph searching to calculate the trajectory distribution under traffic regulations, and a priority determination module based on the IPM to select a safe trajectory.
Evaluations in unsignalized urban driving simulations showing our method’s computational efficiency, robustness, and effectiveness.

Ii Related Work

Ii-a Interaction Representation

Existing methods used different ways to represent traffic interactions in planning. One strategy was to represent prediction results and simulation outcomes[17, 14] in the spatio-temporal (s-t) graph. Then, the sampling methods[27, 24] and optimization methods[26] were used to extract the best-cost or lowest-risk trajectory. Another strategy applied reactive or interactive models[8] to generate action for AVs directly. Methods based on elaborated rules[21, 29]

, game theory

[28], and control theory[4] were widely used in autonomous driving. Compared to these methods, our work applies both s-t graph representation and interactive model (IPM). This combination makes it avoid deterministic representation[27, 26, 13] and limited expression of interaction[21, 29, 28, 4]. In addition, our method is highly efficient as it does not require low-efficient simulation[17, 14] and accurate collision checking[27]. It also outperforms learning-based methods[12] on interpretability, as the IPM is simpler and more intuitive.

Ii-B Interactive Motion Planning for AVs

In interactive planning, the motion of other traffic agents and the ego vehicle is modeled as interdependent. This interdependency makes it impracticable to separate the problems of prediction and planning[8]. There were several interactive planning approaches. For the discussed POMDPs[8, 17, 14], interactions were reflected by time-consuming simulations but not modeled directly. In recent years, learning-based approaches[7, 11]

have been promising to learn trajectory generation and prediction in a unified architecture. However, machine learning technologies lacked traceability when an error occurred in practice. Another was game-theoretic plannings

[20, 23], aiming to compute the Nash equilibrium solutions for all players iteratively. The major shortage of these methods was their high computational cost when the number of players increased. In addition, the game rules were elaborately designed for specific situation, making them hard to generalize to different scenarios.

Besides these, branch MPC[30] and fail-safe planners[2, 32, 33] emphasized the worst-case performance of AVs. For branch MPC[30], it was an optimization method over risk measures to minimize the worst-case expectation, which did not model the traffic interactions well. Instead of accurate risk avoidance, fail-safe plannings [32, 33]

were reactive planners that guaranteed AV’s safe reactions to all possible prediction uncertainties. It achieved this by keeping a collision-free maneuver available at all times. Following this idea, our method shows a conservative maneuver only when the interaction priority is estimated as low, which prevents the AV from too conservative behaviors

[33].

Iii Overview

An overview of the proposed planning framework is presented in Fig.3. In this system, the IPM has two functions: 1) determining whether the AV has a higher priority at an interaction point and 2) extracting the speed limit if the AV intends to pass an occluded area. The framework functions as follows: Given the inputs of the path and speed limits, the spatio-temporal (s-t) graph search first outputs the valid trajectory distribution. Then, the interaction priority determination decides a safe and efficient trajectory for the AV. Finally, the trajectory is smoothed to meet the kinodynamic feasibility and is delivered to the controller for execution.

Fig. 3: Overview of the proposed planning framework with IPM and its relationship with other AV system components.

Iv The Proposed Interaction Point Model

We extract pairs of interaction data from the INTERACTION dataset [31] to analyze the protection time and priority in the interaction points. Two examples are shown in Fig. 4, where car following is a special case of the line overlap [25]. It should be noted that the proposed IPM is also applicable in one-to-many interactions.

Iv-a Assumptions

The human-robot interaction model[9] indicates that participants will exchange their intentions beyond a certain distance called the social distance. Within the social distance, the determined form of interaction is performed. Similarly, [6] points out that, before an interaction happens, traffic participants show their intentions via implicit (interactive behavior) or explicit (e.g., turn signal) communications. Inspired by these works, we make the following assumptions.

Assumption 1: The participants will be conservative when facing potential interactions until the maneuver (e.g., who goes first) is determined via communication [6].

Assumption 2: The maneuver for an interaction point can be determined only after all participants are observed. There is a location called an observation point at which the AV has no blind zone in FoV.

For Assumption 1, we believe it is rational because even in a car-following scenario [21], traffic agents act conservatively to keep a safe distance from the front vehicle. That is, the planned trajectory should guarantee the safety of the AV for the worst-case situations in the future [33]. As for Assumption 2, an example of the observation point is shown in Fig. (b)b, where the AV cannot determine its maneuver to interact with the $V_{i}$ until it reaches the observation point $p_{o}$ . How to obtain an observation point is not discussed in this work, it requires an analysis of the FoV and high-definition map [19].

Iv-B Interaction Point

As illustrated in Fig. (a)a, $p_{j}$ denotes the $j$ -th interaction point, and $v_{e}^{j}$ and $v_{i}^{j}$

are the speeds of the moment that agents pass the

p_{j}

. We further define the properties of the

p_{j}

Δ v = v_{e}^{j} - v_{i}^{j}, Δ θ = | θ_{e}^{j} - θ_{i}^{j} |, Δ t = t_{e}^{j} - t_{i}^{j},

(1)

where $Δ v$ and $Δ θ$ are the relative interaction speed and angle, and $Δ t$ is the time gap in Fig. (a)a. In the equation, similar to the definition of $v_{e}^{j}$ , $θ_{e}^{j}$ is the yaw state of the AV at the location of $p_{j}$ , and $t_{e}^{j}$ is the arrival time. The subscript letter indicates the agent, and the superscript corresponds to the index of the interaction point. Moreover, $Δ t$ is nonzero because agents cannot pass the same position simultaneously.

Iv-C Interaction Protection Time

Following Question 1, the interaction protection time is the minimum value of the time gap $Δ t$ . It has two cases: A negative $Δ t$ corresponds to overtaking, and a positive one corresponds to yielding for others. As a result, we define

{\begin{matrix} Δ t_{p}^{-} & = max Δ t, \forall Δ t \in T \cap R^{-}, Δ t_{p}^{+} & = min Δ t, \forall Δ t \in T \cap R^{+}, \end{matrix}

(2)

where $Δ t_{p}^{-}$ denotes the maximum time gap when overtaking, $Δ t_{p}^{+}$ is the minimum time gap when giving way, and $T$ is the set of possible values of the time gap in the data set. Then, $Δ t_{p}^{(\cdot)}$ is modeled as functions of $Δ θ$ and $Δ v$ respectively.

$Δ t_{p}^{-} (Δ v)$	$= C_{1} {[\begin{matrix} Δ v^{4} & Δ v^{3} & Δ v^{2} & Δ v & 1 \end{matrix}]}^{T},$	(3)
$Δ t_{p}^{-} (Δ θ)$	$= C_{2} {[\begin{matrix} Δ θ^{2} & Δ θ & 1 \end{matrix}]}^{T},$
$Δ t_{p}^{+} (Δ v)$	$= C_{3} {[\begin{matrix} Δ v^{4} & Δ v^{3} & Δ v^{2} & Δ v & 1 \end{matrix}]}^{T},$
$Δ t_{p}^{+} (Δ θ)$	$= C_{4} {[\begin{matrix} Δ θ^{2} & Δ θ & 1 \end{matrix}]}^{T},$

where $Δ t_{p}^{(\cdot)} (Δ v)$ and $Δ t_{p}^{(\cdot)} (Δ θ)$ are represented as fitting curves, and $C_{(\cdot)}$ is the fitting coefficient. Given the values of $Δ v$ and $Δ θ$ together, the boundaries become

{\begin{matrix} Δ t_{p}^{-} & = min (Δ t_{p}^{-} (Δ v), Δ t_{p}^{-} (Δ θ)), Δ t_{p}^{+} & = max (Δ t_{p}^{+} (Δ v), Δ t_{p}^{+} (Δ θ)) . \end{matrix}

(4)

More illustrations will be given in Sect. VII-B.

Iv-D Speed Constraint at Observation Point

Recalling Assumption 1 and 2, before all occluded vehicles are observed, the agent needs to prepare for the worst-case in the future, that is, to give way to any emergent vehicles. As the example shown in Fig. (b)b, before the AV reaches the observation point $p_{o}$ , the remaining time $T$ to reach $p_{j}$ should be greater than $Δ t_{p}^{+}$ . This means the speed upper bound

¯ ¯ ¯ v = \frac{d_{e}^{j}}{T} \leq \frac{d_{e}^{j}}{Δ t_{p}^{+}}, d_{e}^{j} \geq Dist (p_{o}, p_{j}),

(5)

where $d_{e}^{j}$ is the distance between the AV and the $p_{j}$ , $Dist (\cdot)$ returns the Euler distance, and the upper bound meets its minimum $¯ ¯ ¯ v = Dist (p_{o}, p_{j}) / Δ t_{p}^{+}$ . In this equation, $Δ t_{p}^{+}$ can be quickly estimated by setting $Δ v$ to zero, which means there is no need to predict agents’ speed in the occluded area.

Iv-E Interaction Priority

The definition of the interaction priority is from Question 2. It is treated as a conditional distribution $P_{j} (Δ t < 0 | f)$ , where $f$

is the feature vector. Before representing the input features, we introduce two metrics: the minimum arrival time

min (t_{i}^{j})

and the maximum arrival time

max (t_{i}^{j})

. These two metrics are calculated by the constant acceleration model

min (t_{i}^{j})

= \frac{\sqrt{{v_{0}}_{i}^{2} + 2 max (a_{i}) d_{i}^{j}} - {v_{0}}_{i}}{max (a_{i})},

(6)

	$max (t_{i}^{j})$	$= \frac{{v_{0}}_{i} - \sqrt{{v_{0}}_{i}^{2} + 2 \cdot min (a_{i}) d_{i}^{j}}}{\| min (a_{i}) \|},$		(7)
	$s.t. a_{i}$	$\geq - {v_{0}}_{i}^{2} / (2 \cdot d_{i}^{j}),$		(7)

where ${v_{0}}_{i}$ denotes the initial speed of agent $V_{i}$ , $a_{i}$ is the possible average acceleration before reaching the interaction point, and $max (a_{i})$ and $min (a_{i})$ are the maximum and minimum values of acceleration. When the AV and $V_{i}$ intend to pass $p_{j}$ , two features named the overtaking ability $M^{-}$ and giving way ability $M^{+}$ are defined as

	$M_{j}^{-}$	$= min (t_{e}^{j}) - min (t_{i}^{j}) + \| Δ t_{p}^{-} \|,$		(8)
	$M_{j}^{+}$	$= max (t_{e}^{j}) - max (t_{i}^{j}) - \| Δ t_{p}^{+} \|,$		(8)

where the expression of $M^{-}$ indicates that the overtaking needs to suffer the protection time $Δ t_{p}^{-}$ , and $M^{+}$ is the time difference when the two agents slow down simultaneously. In this equation, when $M^{-}$ is less than zero, the AV can safely occupy the interaction point in advance because it can arrive much earlier than $V_{i}$ . When $M^{-}$ is positive, if the AV wants to overtake, the communications mentioned in Assumption 1 and the traffic regulations must be considered.

Our goal is to learn a binary classifier

P_{j} (Δ t < 0 | M^{-}, M^{+})

. Since agents in the experiments are not as intelligent as in the dataset, the classifier is modified as

{Pj(Δt<0)=0.0,M−≥tm,Pj(Δt<0)=\textscMLP(M−,M+),M−<tm,

(9)

where MLP is the trained 3-layer MLP model, $t_{m} = min (C_{r}^{1} \cdot t_{e}^{j} + C_{r}^{2}, C_{r}^{3})$ is the time threshold, and $C_{r}^{(\cdot)}$ are constants ( $C_{r}^{1}$ is positive). When the AV is far from the interaction point ( $t_{e}^{j}$ is large), it is assumed that the interaction follows patterns learned from the dataset ( $M^{-}$ is less than $t_{m}$ ). In contrast, when $t_{e}^{j}$ is small, it turns into a more conservative model in which $M^{-}$ plays a key role.

V Forward Simulation

1:Notation: open list

O

, layer indexs

l

and

f

, path points

{s_{1}, s_{2}, . . ., s_{k}}

, node list

V

, the maximum layer

k

, constants

t_{h}

and

c_{v}

2:Initialize:

O \leftarrow {q^{0}}

V \leftarrow \emptyset

f \leftarrow 0

3:for

niter∈{0,…,max\_iter}

qi≡(sl,v,t)i←\textscPopBestNode(O)

f \leftarrow max (f, l)

V_{l} \leftarrow V_{l} \cup q^{i}

6: if not OutOfBounds(

q^{i}, l, k, t_{h}, c_{v}

) then

7: for

a \in {a_{1}, \dots, a_{n}}

q^{c} \leftarrow

ForwardExpansion(

q^{i}, a, s_{l}, s_{l + 1}

)

J =

CalculateCost(

q^{i}, q^{c}

)

10: if LocalTruncation(

q^{c}, J, R_{v}, R_{t}

) then

11:

O \leftarrow O \cup q^{c}

12: end if

13: end for

14: end if

▹

Check if not out of bounds

15:end for

16:

q∗←\textscBestNode(Vf)

17:return

V

, PathTo(

q^{*}

)

Algorithm 1 S-t graph search

The minimum arrival time in the IPM uses a constant acceleration model, which is not competent when the speed limit changes. Hence, a spatio-temporal (s-t) graph search method is applied to sample speed profiles along the path. This module does not generate a collision-free speed profile but obtains the future trajectory distribution under variational speed limits. As shown in Fig. 5, we present the graph node as $q^{i} = (s_{l}, v, t)^{i}$ , where $i$ is the node index, $s_{(\cdot)}$ is the accumulated distance, the subscript $l$ is called the layer index, and $v$ and $t$ are the speed and timestamp values.

V-a Path Point Selection

The first step is to select a sequence of path points ${s_{1}, s_{2}, . . ., s_{k}}$ to build the search space, where $k$ is the maximum layer. This process considers the curvatures and locations of the interaction points, which obeys: (i) The selected point should reflect the speed limit change between bends and straights. (ii) The path point should involve the location of the interaction point and observation point. (iii) The number of layers should stay a suitable size. Too many layers will increase the computation amount, while a too-small number will reduce the search space.

V-B Speed Constraints

During forward simulation, traffic elements, e.g., traffic lights, stop signs, and speed limits, can be transformed into spatio-temporal constraints in the search space [26]. This work involves two kinds of velocity constraints: one comes from the curvature, and the other from the observation point in (5). We denote $a_{lat}^{max}$ as the maximum lateral acceleration and $κ (s)$ as the curvature at $s$ . The speed limit of the curvature [13] is

{¯ ¯ ¯ v}_{κ} \leq v_{κ}^{max} = \sqrt{a_{lat}^{max} / | κ (s) |} .

(10)

V-C Forward Expansion

We present the whole process in Algorithm 1 and a demonstration in Fig. (a)a. During the expansion, the best node popped from the open list (Algo. 1, Line 4) is forwarded by

v^{c} = \sqrt{(v^{i})^{2} + 2 a \cdot Δ s}, t^{c} = t^{i} + \frac{v^{c} - v^{i}}{a},

(11)

where $Δ s = s_{l + 1} - s_{l}$ , and $q^{c} \equiv (s_{l + 1}, v, t)^{c}$ is the child node. $v^{c} \in (v -, ¯ ¯ ¯ v)_{l + 1}$ , $(v -, ¯ ¯ ¯ v)_{l + 1}$ are the speed bounds at layer $l + 1$ . $a$ is the acceleration from a set of discretized control inputs $U = {a - - \leq a_{1}, \dots, a_{n} \leq ¯ ¯ ¯ a}$ . During the expansion, any child node which exceeds the layer bound $k$ or time limit $t_{h}$ , or has a speed that is lower than $c_{v}$ will not be expanded (Algo. 1, Line 6). The expanded node has cost

J = J_{p} + w_{a} J_{a} + w_{v} J_{v},

(12)

where $J_{p}$ denotes the cost of the parent node, $J_{a} = u^{2} Δ t$ is the control cost, $J_{v} = | v - ¯ ¯ ¯ v |$ is the deviation from the speed limit, and $w (\cdot)$ is the weights.

V-D Local Truncation

As illustrated in Fig. (a)a, for nodes in a discretized $(v, t)$ grid with resolutions $R_{v}$ and $R_{t}$ , only the node with the lowest cost is allowed to undergo expansion (Algo. 1, Line 10). This operation significantly improves the search efficiency, and it has little impact on the search result since the abandoned nodes have similar values to the lowest-cost one.

Vi Interaction Priority Determination

Once the graph search is finished, trajectories to graph nodes $V$ and the lowest-cost speed profile $q_{1 : f} = {q^{1}, q^{2}, \dots, q^{f}}$ are obtained. For node $q^{i}$ in $q_{1 : f}$ , we use notation $i$ to represent both the node and layer index for convenience. The next step is to determine which trajectory to perform, and the IPM is applied for priority checking among the sampled trajectories.

Vi-a Speed Profile Selection

The whole process is outlined in Algorithm 2. $V_{0 : i}$ denotes the graph nodes from layer $0$ to $i$ , and the IPM is used to check the priority of $q_{1 : f}$ in turn until it reaches the maximum layer $f$ or quits when the AV’s priority is lower than that of other participants (Algo. 2, Line 4):

\exists P_{i} (Δ t < 0 | q^{i}, V_{x}) < C_{p}, \forall V_{x} \in A_{i},

(13)

where $A_{i}$ is the set of agents who intend to pass the interaction point at layer $i$ , $P_{i}$

denotes the probability in (

9), and

C_{p}

is a constant threshold. In the implementation, protection times

Δ t_{p}^{-}

and

Δ t_{p}^{+}

are calculated by setting

Δ v

to zero since it is hard to estimate

Δ v

accurately. Moreover,

min (t_{e}^{i})

M^{-}

uses the

t

value in

q^{i}

, and

M^{+}

is calculated via the constant acceleration model. Following the illustration in Fig. (b)b, once

q_{f}

has a lower priority, nodes at previous layers (e.g.,

s_{f - 1}

) will be checked to see if they meet the following condition

(s, v, t)_{l}

\in S (t_{l}), l < f,

(14)

where $S (\cdot)$ is the invariable safe set [33] that at least one action exists for the AV to remain safe for an infinite time horizon. Our work involves two kinds of invariable safe sets. One is to promise the AV can stop before the intersection (Time-to-Brake [15] greater than 0), and another is to obey the responsibility safety criterion [21] when following a car. Then, a one-step forward expansion will be conducted at $s_{f - 1}$ , and the generated child should obey its parent’s constraint (14). For example, as the green edges show in Fig. (b)b, child nodes at the new layer $s_{f^{*}}$ are sampled if they allow the AV to stop before a safe distance to the intersection.

1:Notation: agent list

A

2:Initialize:

V,q1:f←\textscStGraphSearch()

3:for i in

{1, \dots, f}

4: if

\textscPriorityIsLow(qi,Ai)

then

q1:f∗←\textscCheckAndExpand(V0:i−1)

6: break

7: end if

8:end for

9:if

q1:f∗==\textscNone

then

10: return

\textscSmooth(q1:f)

11:end if

12:return

\textscPriorityDetermination(V,q1:f∗)

Algorithm 2 Priority Determination

Finally, the best speed profile $q_{1 : f^{*}}$ is picked up, and the above operations are executed iteratively (Algo. 2, Line 12) until all nodes of the speed profile have a higher interaction priority. Fig. (b)b shows three speed profiles in orange, purple and grey, respectively. The orange one is the best speed profile derived from the forward simulation while having a low priority at $p_{2}$ . As a result, the speed profile in purple is selected instead. Then, the grey speed profile will be applied if the purple speed profile has a low priority at $p_{1}$ .

Vi-B Speed Profile Smoothing

The chosen speed profile $q_{1 : f^{*}}$ can be smoothed further to meet the kinodynamic feasibility using a quintic piecewise Bézier curve in the spatio-temporal semantic corridor (SSC [26]). Based on the ${s_{1}, s_{2}, . . ., s_{k}}$ and speed limits in the search space, our method use $q_{1 : f^{*}}$ as the initial seeds to build SSC, while dynamic obstacles are not mapped to the SSC because the initial seeds have guaranteed the AV’s safety.

Vii Experimental Results

We train and test the IPM in the INTERACTION dataset [31] because it contains motions of various traffic participants in various driving scenarios without traffic lights. The proposed planner is validated in the CARLA [3].

Vii-a Implementation Details

We use Pytorch

[1] to train the MLP network in IPM, and the whole planning framework is implemented with C++ backend. All experiments are conducted on a computer with an AMD-5600X CPU (3.7 GHz). From the INTERACTION dataset [31], we first extract the data that have coincident trajectories between agents (as plotted in Fig. 4) and divide them into different pairs of interactions. Some examples are available on our project website, where agents are simplified as circles. At the same interaction point, agent

m

overtaking

n

is identical to the case where agent

n

gives way to

m

. As these two cases are essentially the same scenario viewed from different perspectives, we only add this type of data to our dataset once to avoid data duplication.

In the simulation, the AV’s acceleration limits are $(- 3.0, 1.5)$ ${m/s}^{2}$ , and its field of view is limited, where other traffic agents will not be observed if their distance to the AV is greater than $120 \cdot max (1.0 - 2 \cdot | arctan (Δ y, Δ x) | / π, 0.2)$ m. In the IPM, the MLP network and parameters $C_{(\cdot)}$ in (3

) are available on our project website. For each intersection with a limited field of view, we apply the heuristic function

Dist (p_{o}, p_{j}) = 0.15 \times L

to roughly estimate the speed limit in (5), where

L

is the length of the lane in the intersection. During forward simulation, the maximal distance between two sampled path points is

10 m

. In Algorithm 1, the expansion conditions

t_{h} = 10 s

c_{v} = 0.1

m/s, grid resolutions

R_{v} = 0.2

m/s, and

R_{t} = 0.2

s. In the classifier (9),

C_{r}^{1} = 0.5

C_{r}^{2} = 1.5

C_{r}^{3} = 5.0

, and

C_{p} = 0.95

Time	$<$ 1ms	1-10ms	10-20ms	20-30ms	30-40ms
Percentage	10.97%	42.53%	39.31%	7.16%	$<$ 0.03%

TABLE I: Time Consumption of Planning Framework

Vii-B Interaction Protection Time

(a) Giving way to other vehicles. (1) Due to the speed limit at $p_{o}$ , the AV slows down from 5.1m/s, and it has a lower priority than $V_{537}$ at both $p_{1}$ and $p_{2}$ . (2) When $V_{537}$ turns right, the AV decides to occupy $p_{1}$ . (3) Finally, the AV follows $V_{537}$ while maintaining a certain speed.


	Town02 (Seed 1)			Town02 (Seed 2)
Methods	Completion (m) $↑$	Collision Num. $↓$	Avg. Speed^a (m/s) $↑$	Completion (m) $↑$	Collision Num. $↓$	Avg. Speed (m/s) $↑$
SSC[26]	1232.00 $\pm$ 187.71	0.0 $\pm$ 0.00	4.10 $\pm$ 0.61	1444.94 $\pm$ 081.07	0.2 $\pm$ 0.45	4.8 $\pm$ 0.25
MCTS[14]	1295.73 $\pm$ 054.82	0.0 $\pm$ 0.00	4.43 $\pm$ 0.31	1401.86 $\pm$ 104.27	0.0 $\pm$ 0.00	4.7 $\pm$ 0.36
MMFN-Expert[22]	1340.84 $\pm$ 412.39	1.0 $\pm$ 0.71	4.86 $\pm$ 0.53	1620.00 $\pm$ 059.71	0.8 $\pm$ 0.84	5.4 $\pm$ 0.18
PD- $M^{-}$	1193.86 $\pm$ 107.04	0.0 $\pm$ 0.00	3.98 $\pm$ 0.38	1303.22 $\pm$ 035.57	0.0 $\pm$ 0.00	4.3 $\pm$ 0.11
PD-CVel	1303.08 $\pm$ 161.48	0.3 $\pm$ 0.48	4.41 $\pm$ 0.52	1398.94 $\pm$ 081.53	0.2 $\pm$ 0.45	4.7 $\pm$ 0.30
PD-IPM	1358.11 $\pm$ 060.73	0.0 $\pm$ 0.00	4.71 $\pm$ 0.48	1457.96 $\pm$ 042.54	0.0 $\pm$ 0.00	4.9 $\pm$ 0.13

is the average velocity before the AV collides with other vehicles and cannot continue with the navigation task.

TABLE II: Quantitative Experimental Results

We analyze the extracted pairs of interactions (mentioned in Sect. VII-A) and plot $P (Δ t | Δ θ)$ and $P (Δ t | Δ v)$ in Fig. (a)a. Inside a box, the green line represents the mean value, and the orange line denotes the median. The color of the box represents the sample size, where orange means the size is greater than 10000, yellow (than 1000), blue (than 100), light blue (than 10), and black is [1,10). In addition, the solid lines are fitting curves of $Δ t_{p}^{-} (\cdot)$ and $Δ t_{p}^{+} (\cdot)$ in (3), which are based on the Gaussian $3 σ$ intervals.

Vii-C Interaction Priority

As shown in Fig. (b)b, we label samples with negative $Δ t$ as overtaking (red points) and positive as giving way (cyan points) in the dataset. The background color denotes $\textscMLP(Δt<0|M−,M+)$ in (9), and there are four areas. For B and C, the priority is clear that can be quickly classified. In A and D, $M^{+}$ plays a key role. Most agents in the dataset can give way to their competitors when the giving way ability is higher. We further classify $\textscMLP(Δt<0|M−,M+)≥0.5$ as overtaking to evaluate the prediction accuracy. The trained MLP network achieves 99.79% accuracy in the training set with a data size of 5283 and 99.81% in the validation set (10071 data size). In the validation experiment, we test all time slots before reaching the interaction point, which makes the validation set’s size larger than the training set’s.

Vii-D Advance Deceleration for Emergency

Besides the response time factor, the IPM explains why an advance deceleration is necessary for an emergency from another perspective. As shown in Fig. (c)c, the purple line represents the braking speed limit, which is the maximal speed when the braking distance is less than $d_{e}^{j}$ , and the dashed lines are the speed limits defined by (5). This figure suggests that when $Δ t_{p}^{+}$ is $2.0$ s and the AV runs at $8.0$ m/s, the braking is hard to prevent a collision if a vehicle suddenly appears within $16$ m. That is, it reflects the speed limits of the grey area.

Vii-E Qualitative Results

To verify that the proposed planning framework is competent in highly interactive scenarios, we test our method in Town02 and Town04 of CARLA. In the experiments, traffic agents in the simulation are set to ignore the traffic signals and signs, and two simulation results are discussed in Fig. 7. In addition, more qualitative results are available in the supplementary video on the project website.

Vii-F Quantitative Results

1) Real-time Performance: A 30-minute navigation test with dynamical agents is conducted in CARLA to verify the computational efficiency of our proposed planning framework. The test results are shown in Table I, where the computation time includes all planning modules in Fig. 3, and the average calculation time is $10.24$ ms. We observe that in 92.8% of cases, the runtime of our proposed method is less than $20$ ms, which shows the real-time performance.

2) Performance Comparisons: We select Town02 as the test scenario because the map is relatively small and more suitable for comparing the interaction performance. In addition, the position of interaction points in Town02 is fixed so that we do not need to predict other agents’ paths in advance. Like previous experiments, all junctions and intersections on the map are unprotected by the traffic rules. The following algorithms are compared with the same initial location and different random seeds. SSC [26]: constant velocity (CVel) model is applied to generate the SSC. MCTS [14]: Monte Carlo tree search algorithm, where other agents are predicted to slow down to stop or speed up to a certain speed when tracking the path, and their goal probabilities are uniform. MMFN-Expert [22]: an elaborated expert agent. PD-M $^{-}$ : this method is based on the proposed planning framework, but the priority determination module decides to overtake when $M^{-}$ is less than zero. PD-CVel: like PD- $M^{-}$ , it overtakes when the $Δ t$ estimated by CVel is less than zero. PD-IPM: the proposed planning framework using (9) to determine the priority. The speed constraint (5) is enforced in all methods since most benchmarks [26, 14] did not model the occluded factors.

Each algorithm is tested with a series of goals in five minutes. The tests are repeated five times, and the performances are recorded in Table II

. In the table, the numbers are the mean values and standard deviations for three metrics: completion distance, number of collisions, and average speed. The experimental results demonstrate that our method is safe and efficient. In the seed 1 setting, PD-IPM achieves no collision situations with the longest completion length. In the seed 2 setting, although the MMFN-Expert has the longest length and highest average speed, it is unsafe and has a high collision number in red. By contrast, our PD-IPM gets a zero-collision rate and outperforms other approaches

[26, 14]. Besides this, the performance of PD-

M^{-}

is conservative but safe, which shows our planning framework’s reliability. The comparison between PD-CVel and PD-IPM reflects that the IPM can make the planning system more effective.

Viii Conclusions

This paper presented a novel unified planning framework based on the proposed interaction point model (IPM), which enabled a uniform description of the interaction under various driving scenarios. First, the IPM was trained and tested in the real traffic data, showing the general interaction rule in different scenarios. Next, the planning framework was introduced, and the efficient graph search and interaction priority determination were elaborated. Experiments showed that our framework enabled the system to interact safely with other agents, and the IPM made it more efficient. Future work will include introducing more complicated scenarios into the planning framework, with interactions in open space, with pedestrians, or between other agents under more traffic regulations modeled.

References

[1] A. P. et al. (2019)
Pytorch: an imperative style, high-performance deep learning library
. NeurIPS 32. Cited by: §VII-A.
[2] A. C. et al. (2021) Lookout: diverse multi-future prediction and planning for self-driving. In
Proc. IEEE Conf. Comput. Vis. Pattern Recognit. (CVPR)
, pp. 16107–16116. Cited by: §II-B.
[3] A. D. et al. (2017) CARLA: an open urban driving simulator. pp. 1–16. Cited by: §VII.
[4] A. L. et al. (2021) Prediction-based reachability for collision avoidance in autonomous driving. In IEEE Int. Conf. Rob. Autom. (ICRA), pp. 7908–7914. Cited by: §II-A.
[5] F. G. et al. (2018) Online safe trajectory generation for quadrotors using fast marching method and bernstein basis polynomial. In IEEE Int. Conf. Rob. Autom. (ICRA), pp. 344–351. Cited by: §I.
[6] G. M. et al. (2020) Defining interactions: a conceptual framework for understanding interactive behaviour in human and automated road traffic. Theoretical Issues in Ergonomics Science 21 (6), pp. 728–752. Cited by: §I, §IV-A, §IV-A.
[7] H. S. et al. (2020) Pip: planning-informed trajectory prediction for autonomous driving. In Eur. Conf. Comp. Vis. (ECCV), pp. 598–614. Cited by: §II-B.
[8] H. et al. (2018) Automated driving in uncertain environments: planning with interaction and uncertain maneuver prediction. ieee_j_iv 3 (1), pp. 5–17. Cited by: §I, §II-A, §II-B.
[9] H. H. et al. (2006) Investigating spatial relationships in human-robot interaction. In IEEE/RJS Int. Conf. Intell. Rob. Syst. (IROS), pp. 5052–5059. Cited by: §IV-A.
[10] J. Y. et al. (2019) Target vehicle motion prediction-based motion planning framework for autonomous driving in uncontrolled intersections. ieee_j_its 22 (1), pp. 168–177. Cited by: §I.
[11] J. R. et al. (2022) TAE: a semi-supervised controllable behavior-aware trajectory generator and predictor. arXiv preprint arXiv:2203.01261. Cited by: §II-B.
[12] J. C. et al. (2022) MPNP: multi-policy neural planner for urban driving. In IEEE/RJS Int. Conf. Intell. Rob. Syst. (IROS), Cited by: §II-A.
[13] J. C. et al. (2022) Real-time trajectory planning for autonomous driving with Gaussian process and incremental refinement. pp. 8999–9005. External Links: Document Cited by: §II-A, §V-B.
[14] J. P. H. et al. (2021) Interpretable goal recognition in the presence of occluded factors for autonomous vehicles. In IEEE/RJS Int. Conf. Intell. Rob. Syst. (IROS), pp. 7044–7051. Cited by: §II-A, §II-B, §VII-F, §VII-F, TABLE II.
[15] J. H. et al. (2006) A multilevel collision mitigation approach—its situation assessment, decision making, and performance tradeoffs. ieee_j_its 7 (4), pp. 528–540. Cited by: §VI-A.
[16] L. S. et al. (2022) Domain knowledge driven pseudo labels for interpretable goal-conditioned interactive trajectory prediction. arXiv preprint arXiv:2203.15112. Cited by: §I.
[17] L. Z. et al. (2020) Efficient uncertainty-aware decision-making for automated driving using guided branching. In 2020 IEEE Int. Conf. Robot. Automat., pp. 3291–3297. Cited by: §I, §II-A, §II-B.
[18] M. et al. (2019) Response time and time headway of an adaptive cruise control. an empirical characterization and potential impacts on road capacity. ieee_j_its 21 (4), pp. 1677–1686. Cited by: §I.
[19] M. L. et al. (2017) Collision risk assessment for possible collision vehicle in occluded area based on precise map. In IEEE Int. conf. Intell. Transp. Sys. (ITSC), pp. 1–6. Cited by: §IV-A.
[20] M. W. et al. (2021) Game-theoretic planning for self-driving cars in multivehicle competitive scenarios. ieee_j_ro 37 (4), pp. 1313–1325. Cited by: §II-B.
[21] P. K. et al. (2019) Autonomous vehicles meet the physical world: rss, variability, uncertainty, and proving safety. In Int. Conf. Comp. Safety, Reliability, Security, pp. 245–253. Cited by: §I, §II-A, §IV-A, §VI-A.
[22] Q. Z. et al. (2022) MMFN: multi-modal fusion net for end-to-end autonomous driving. Cited by: §VII-F, TABLE II.
[23] R. C. et al. (2022) Gameplan: game-theoretic multi-agent planning with human drivers at intersections, roundabouts, and merging. ieee_j_ral 7 (2), pp. 2676–2683. Cited by: §II-B.
[24] T. Z. et al. (2020) Trajectory planning based on spatio-temporal map with collision avoidance guaranteed by safety strip. ieee_j_its. Cited by: §II-A.
[25] W. Z. et al. (2017) Spatially-partitioned environmental representation and planning architecture for on-road autonomous driving. In IEEE Intell. Veh. Symp. (IV), pp. 632–639. Cited by: §I, §IV.
[26] W. D. et al. (2019) Safe trajectory generation for complex urban environments using spatio-temporal semantic corridor. ieee_j_ral 4 (3), pp. 2997–3004. Cited by: §II-A, §V-B, §VI-B, §VII-F, §VII-F, TABLE II.
[27] W. L. et al. (2019) Hybrid trajectory planning for autonomous driving in on-road dynamic scenarios. ieee_j_its 22 (1), pp. 341–355. Cited by: §II-A.
[28] Y. R. et al. (2021) Helping automated vehicles with left-turn maneuvers: a game theory-based decision framework for conflicting maneuvers at intersections. ieee_j_its. Cited by: §II-A.
[29] Y. Z. et al. (2016) Combined variable speed limit and lane change control for highway traffic. ieee_j_its 18 (7), pp. 1812–1823. Cited by: §II-A.
[30] Y. C. et al. (2022) Interactive multi-modal motion planning with branch model predictive control. ieee_j_ral 7 (2), pp. 5365–5372. Cited by: §II-B.
[31] Z. W. et al. (2019) Interaction dataset: an international, adversarial and cooperative motion dataset in interactive driving scenarios with semantic maps. arXiv preprint arXiv:1910.03088. Cited by: §IV, §VII-A, §VII.
[32] C. Pek and M. Althoff (2018) Computationally efficient fail-safe trajectory planning for self-driving vehicles using convex optimization. In ITSC, pp. 1447–1454. Cited by: §II-B.
[33] C. Pek and M. Althoff (2020) Fail-safe motion planning for online verification of autonomous vehicles using convex optimization. ieee_j_ro 37 (3), pp. 798–814. Cited by: §II-B, §IV-A, §VI-A.

Efficient Speed Planning for Autonomous Driving in Dynamic Environment with Interaction Point Model

A Customizable Dynamic Scenario Modeling and Data Generation Platform for Autonomous Driving

Brain-Inspired Modelling and Decision-making for Human-Like Autonomous Driving in Mixed Traffic Environment

A Learning-Based Tune-Free Control Framework for Large Scale Autonomous Driving System Deployment

Resolving Conflict in Decision-Making for Autonomous Driving

Efficient Game-Theoretic Planning with Prediction Heuristic for Socially-Compliant Autonomous Driving

DOROTHIE: Spoken Dialogue for Handling Unexpected Situations in Interactive Autonomous Driving Agents

Verifiable Goal Recognition for Autonomous Driving with Occlusions

Code Repositories

IPM-Planner

I Introduction

I-a Contributions

Ii Related Work

Ii-a Interaction Representation

Ii-B Interactive Motion Planning for AVs

Iii Overview

Iv The Proposed Interaction Point Model

Iv-a Assumptions

Iv-B Interaction Point

Iv-C Interaction Protection Time

Iv-D Speed Constraint at Observation Point

Iv-E Interaction Priority

V Forward Simulation

V-a Path Point Selection

V-B Speed Constraints

V-C Forward Expansion

V-D Local Truncation

Vi Interaction Priority Determination

Vi-a Speed Profile Selection

Vi-B Speed Profile Smoothing

Vii Experimental Results

Vii-a Implementation Details

Vii-B Interaction Protection Time

Vii-C Interaction Priority

Vii-D Advance Deceleration for Emergency

Vii-E Qualitative Results

Vii-F Quantitative Results

Viii Conclusions

References

Efficient Speed Planning for Autonomous Driving in Dynamic Environment with Interaction Point Model

Related Research

A Customizable Dynamic Scenario Modeling and Data Generation Platform for Autonomous Driving

Brain-Inspired Modelling and Decision-making for Human-Like Autonomous Driving in Mixed Traffic Environment

A Learning-Based Tune-Free Control Framework for Large Scale Autonomous Driving System Deployment

Resolving Conflict in Decision-Making for Autonomous Driving

Efficient Game-Theoretic Planning with Prediction Heuristic for Socially-Compliant Autonomous Driving

DOROTHIE: Spoken Dialogue for Handling Unexpected Situations in Interactive Autonomous Driving Agents

Verifiable Goal Recognition for Autonomous Driving with Occlusions

Code Repositories

IPM-Planner

I Introduction

I-a Contributions

Ii Related Work

Ii-a Interaction Representation

Ii-B Interactive Motion Planning for AVs

Iii Overview

Iv The Proposed Interaction Point Model

Iv-a Assumptions

Iv-B Interaction Point

Iv-C Interaction Protection Time

Iv-D Speed Constraint at Observation Point

Iv-E Interaction Priority

V Forward Simulation

V-a Path Point Selection

V-B Speed Constraints

V-C Forward Expansion

V-D Local Truncation

Vi Interaction Priority Determination

Vi-a Speed Profile Selection

Vi-B Speed Profile Smoothing

Vii Experimental Results

Vii-a Implementation Details

Vii-B Interaction Protection Time

Vii-C Interaction Priority

Vii-D Advance Deceleration for Emergency

Vii-E Qualitative Results

Vii-F Quantitative Results

Viii Conclusions

References