[PPT] - Determining the Nonexistence of Evasive Trajectories for Collision PowerPoint Presentation

SLIDE 1

Determining the Nonexistence of Evasive Trajectories for Collision Avoidance Systems

Matthias Althoff, Sebastian S¨

ntges

Technische Universit¨ at M¨ unchen

September 28, 2015

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 1 / 43

SLIDE 2

Motivation

Assessing the Risk of Traffic Situations

When to initiate an emergency maneuver? Collision “almost” unavoidable Approach: Test if there is none or only “very few” evasive trajectories

Figure: Existence of evasive trajectories

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 2 / 43

SLIDE 3

Motivation

Uncountably Many Possible Situations

How “many” future trajectories of other traffic participants are possible? How “many” evasive trajectories are there? Answer for both: Infinitely many.

Figure: Set of evasive trajectories

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 3 / 43

SLIDE 4

Motivation

Overview of Our Previous Approach

➀ occupancy prediction ➁ trajectory planning ➂ collision checking ➃ trajectory tracking

controller Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 4 / 43

SLIDE 5

Motivation

Outline

1

Model of the ego vehicle and other traffic participants

2

Set-based behavior prediction

3

Determining the nonexistence of evasive trajectories

Set representation for reachability analysis Computational tricks

4

Test results

5

Further use of reachability analysis: Verification of single trajectories

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 5 / 43

SLIDE 6

Models of the Ego Vehicle and Other Traffic Participants

Constraints for Traffic Participants

Initially the following constraints are considered: C1: positive longitudinal acceleration is stopped when a parameterized speed vmax is reached. C2: driving backwards in a lane is not allowed. C3: positive longitudinal acceleration is inversely proportional with speed above a parameterized speed vS (modeling a maximum engine power). C4: maximum absolute acceleration is limited by amax. C5: actions that cause leaving the road/lane boundary are forbidden. When a violation of a constraint of a traffic participant is sensed, it is no longer considered in future predictions for that particular traffic participant.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 6 / 43

SLIDE 7

Models of the Ego Vehicle and Other Traffic Participants

Physical Modeling (1)

Traffic participants are a point mass with

(alat)2 + (along)2 ≤ amax

(Kamm’s circle). The lateral acceleration is provided by a normalized steering input u1: alat = amaxu1. Due to the maximum absolute acceleration (C4), the longitudinal acceleration is bounded by along

c1

=

(amax)2 − (alat)2.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 7 / 43

SLIDE 8

Models of the Ego Vehicle and Other Traffic Participants

Physical Modeling (2)

Constraints C1 (max. velocity), C2 (no driving backwards), and C3 (max. engine power) are considered in another bound: along

c2

=      amax

vS v ,

vS < v < vmax ∧ u2 > 0 amax, (0 < v ≤ vS ∨ (v > vS ∧ u2 ≤ 0)) 0, v ≤ 0 ∨ (v ≥ vmax ∧ u2 > 0) The longitudinal acceleration combining both bounds is for a normalized acceleration input u2 ∈ [−1, 1] along =

along

c2

u2, along

c2

|u2| ≤ along

c1 ,

along

c1

sgn(u2), along

c2

|u2| > along

c1 .

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 8 / 43

SLIDE 9

Set-Based Behavior Prediction

Reachability Analysis

possible trajectory exact reachable set jump steady state initial set x1 x2

Informal Definition A reachable set is the set of states that can be reached by a dynamical system in finite or infinite time for a set of initial states, uncertain inputs, and uncertain parameters.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 9 / 43

SLIDE 10

Set-Based Behavior Prediction

Overapproximative Reachable Sets

verapproximative

reachable set exact reachable set invariant set unsafe set initial set x1 x2

Exact reachable set only for special classes computable → overapproximation computed for consecutive time intervals. Overapproximation might lead to spurious counterexamples. Simulation cannot prove correctness.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 10 / 43

SLIDE 11

Set-Based Behavior Prediction

Abstraction Technique for Other Traffic Participants

Overapproximative Occupancy Given are models Mi, i = 1 . . . m which are abstractions of model M0, i.e., reach(M0) ⊆ reach(Mi). The occupancy of the model M0 can be

verapproximated by

proj

reach(M0)
⊆

m

i=1

proj

reach(Mi)
.
Two models: Longitudinal dynamics along road boundaries (upper bound),

lateral dynamics towards road boundaries (left/right bound).

initial occupancy lower left bound lower right bound upper bound

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 11 / 43

SLIDE 12

Set-Based Behavior Prediction

Occupancy Along Road Boundaries

The dynamics becomes monotone when following a lane center. Definition (Monotone dynamics) For the initial state x(0) ∈ R(0) and inputs u(t) ∈ U the dynamics is monotone when the following holds for the solution χ(t, x(0), u(·)): if ∀i, j, t ≥ 0 : xi(0) ≤ ¯ xi(0), uj(t) ≤ ¯ uj(t) then ∀i, t ≥ 0 : χi(t, x(0), u(·)) ≤ χi(t, ¯ x(0), ¯ u(·)).

From this follows that e.g. the upper bound is provided by max. position,
max. velocity, and max. acceleration:

sx sy path

ccupancy set

for some time interval s

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 12 / 43

SLIDE 13

Set-Based Behavior Prediction

Occupancy Towards Road Boundaries

For lateral dynamics there exists no single combination of an initial state and an input trajectory determining the boundary. Given the vehicle-fixed angle of the acceleration vector a, possible trajectories are:

ax ay a φ sx sy

const. acceleration

(φ = 90◦)

const. acceleration

(φ = 110◦)

const. acceleration

(φ = 130◦) 2 −2 10 20 w

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 13 / 43

SLIDE 14

Set-Based Behavior Prediction

Occupancy Towards Road Boundaries: Method A

Using limit of absolute acceleration (constraint C4): Occupancies are circles with center c(t) and radius r(t): c(t) = sx(0) sy(0)

+

vx(0) vy(0)

t,

r(t) = 1 2amaxt2. From this follows the boundary of occupation: bx(t) = v0t − a2

maxt3

2v0 , by(t) =

1

4a2

maxt4 −

a2

maxt3

2v0 2 .

sx sy 4 −4 10 20 [bx(t), by(t)]T [bx(t), −by(t)]T O([tk, tk+1]) O(tk−2) r(tk+1) c(tk+1)

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 14 / 43

SLIDE 15

Set-Based Behavior Prediction

Occupancy Towards Road Boundaries: Method B and C

Method B: Assume independence of lateral and longitudinal acceleration → Solution of time ts for switching the steering angle to avoid road departure: ts =

amaxw + 1

2v 2 0 − v0

amax Method C: Combination of method A and B.

sx sy

const. acceleration

(φ = 90◦)

const. acceleration

(φ = 110◦)

const. acceleration

(φ = 130◦) method A method B method C 2 −2 10 20 w

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 15 / 43

SLIDE 16

Set-Based Behavior Prediction

Examples: Lane 1

Step 1:

×105 3.865 3.8652 3.8654 3.8656 3.8658 3.866 3.8662 3.8664 3.8666 3.8668 ×106 5.5913 5.5913 5.5913 5.5913 5.5914 5.5914 5.5914 5.5914 5.5914 5.5915

M1 M2 M3

M1: restricted absolute acceleration. M2: restricted acceleration and velocity in longitudinal direction. M3: staying within road boundaries.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 16 / 43

SLIDE 17

Set-Based Behavior Prediction

Examples: Lane 1

Step 2:

×105 3.865 3.8652 3.8654 3.8656 3.8658 3.866 3.8662 3.8664 3.8666 3.8668 ×106 5.5913 5.5913 5.5913 5.5913 5.5914 5.5914 5.5914 5.5914 5.5914 5.5915

M1 M2 M3

M1: restricted absolute acceleration. M2: restricted acceleration and velocity in longitudinal direction. M3: staying within road boundaries.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 17 / 43

SLIDE 18

Set-Based Behavior Prediction

Examples: Lane 1

Step 3:

×105 3.865 3.8652 3.8654 3.8656 3.8658 3.866 3.8662 3.8664 3.8666 3.8668 ×106 5.5913 5.5913 5.5913 5.5913 5.5914 5.5914 5.5914 5.5914 5.5914 5.5915

M1 M2 M3

M1: restricted absolute acceleration. M2: restricted acceleration and velocity in longitudinal direction. M3: staying within road boundaries.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 18 / 43

SLIDE 19

Set-Based Behavior Prediction

Examples: Lane 1

Step 4:

×105 3.865 3.8652 3.8654 3.8656 3.8658 3.866 3.8662 3.8664 3.8666 3.8668 ×106 5.5913 5.5913 5.5913 5.5913 5.5914 5.5914 5.5914 5.5914 5.5914 5.5915

M1 M2 M3

M1: restricted absolute acceleration. M2: restricted acceleration and velocity in longitudinal direction. M3: staying within road boundaries.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 19 / 43

SLIDE 20

Set-Based Behavior Prediction

Examples: Lane 1

Step 5:

×105 3.865 3.8652 3.8654 3.8656 3.8658 3.866 3.8662 3.8664 3.8666 3.8668 ×106 5.5913 5.5913 5.5913 5.5913 5.5914 5.5914 5.5914 5.5914 5.5914 5.5915

M1 M2 M3

M1: restricted absolute acceleration. M2: restricted acceleration and velocity in longitudinal direction. M3: staying within road boundaries.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 20 / 43

SLIDE 21

Set-Based Behavior Prediction

Examples: Lane 2

Step 1:

×105 3.865 3.8652 3.8654 3.8656 3.8658 3.866 3.8662 3.8664 3.8666 3.8668 ×106 5.5913 5.5913 5.5913 5.5913 5.5914 5.5914 5.5914 5.5914 5.5914 5.5915

M1 M2 M3

M1: restricted absolute acceleration. M2: restricted acceleration and velocity in longitudinal direction. M3: staying within road boundaries.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 21 / 43

SLIDE 22

Set-Based Behavior Prediction

Examples: Lane 2

Step 2:

×105 3.865 3.8652 3.8654 3.8656 3.8658 3.866 3.8662 3.8664 3.8666 3.8668 ×106 5.5913 5.5913 5.5913 5.5913 5.5914 5.5914 5.5914 5.5914 5.5914 5.5915

M1 M2 M3

M1: restricted absolute acceleration. M2: restricted acceleration and velocity in longitudinal direction. M3: staying within road boundaries.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 22 / 43

SLIDE 23

Set-Based Behavior Prediction

Examples: Lane 2

Step 3:

×105 3.865 3.8652 3.8654 3.8656 3.8658 3.866 3.8662 3.8664 3.8666 3.8668 ×106 5.5913 5.5913 5.5913 5.5913 5.5914 5.5914 5.5914 5.5914 5.5914 5.5915

M1 M2 M3

M1: restricted absolute acceleration. M2: restricted acceleration and velocity in longitudinal direction. M3: staying within road boundaries.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 23 / 43

SLIDE 24

Set-Based Behavior Prediction

Examples: Lane 2

Step 4:

×105 3.865 3.8652 3.8654 3.8656 3.8658 3.866 3.8662 3.8664 3.8666 3.8668 ×106 5.5913 5.5913 5.5913 5.5913 5.5914 5.5914 5.5914 5.5914 5.5914 5.5915

M1 M2 M3

M1: restricted absolute acceleration. M2: restricted acceleration and velocity in longitudinal direction. M3: staying within road boundaries.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 24 / 43

SLIDE 25

Set-Based Behavior Prediction

Examples: Lane 2

Step 5:

×105 3.865 3.8652 3.8654 3.8656 3.8658 3.866 3.8662 3.8664 3.8666 3.8668 ×106 5.5913 5.5913 5.5913 5.5913 5.5914 5.5914 5.5914 5.5914 5.5914 5.5915

M1 M2 M3

M1: restricted absolute acceleration. M2: restricted acceleration and velocity in longitudinal direction. M3: staying within road boundaries.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 25 / 43

SLIDE 26

Set Representation for Reachability Analysis

Selection of an Appropriate Set Representation

Reachable set of the ego vehicle We restrict the reachable set to states that can be reached by a collision-free trajectory ∀t : A (x(t)) ∩ O(t) = ∅. where A (x(t)) is the footprint of the vehicle at state x(t) and O(t) is the

bstacle region.

The representation of the set is crucial for efficient calculations: Propagation of set after one time step must suite motion model (e.g. convex polyhedron for LTI) Intersection tests with forbidden states (e.g. obstacles) Complement set operation to cut forbidden regions from reach set

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 26 / 43

SLIDE 27

Set Representation for Reachability Analysis

Overapproximation of all Reachable States

We overapproximate the reachable set of the previous vehicle model (now: only consideration of acceleration limits; for computational reasons: a∞ ≤ amax) by the union of boxes: reach(x0, t) ⊂

j

box(j)

t

Each box is four-dimensional and stores an interval of position and velocity in x, y-direction: [sx,min sx,max] × [sy,min sy,max] × [vx,min vx,max] × [vy,min vy,max]

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 27 / 43

SLIDE 28

Set Representation for Reachability Analysis

Main Algorithm

Propagate: Split (dark regions are obstacles): Repack:

for i ← 1 to steps do Ri ← ∅ for all box(i−1) in Ri−1 do box(i) ← PROPAGATE(box(i−1)) Ri ← {Ri, SPLIT(box(i))} if |Ri| > max. number boxes then Ri ← REPACK(Ri ) end if end for end for return R1, R2, . . . , Rsteps

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 28 / 43

SLIDE 29

Set Representation for Reachability Analysis

Propagation of Boxes

Overapproximation by propagation of extremal values s(L)

t+∆t = s(L) t

+ v (L)

t

∆t − 1 2a∆t2 s(H)

t+∆t = s(H) t

+ v (H)

t

∆t + 1 2a∆t2 v (L)

t+∆t = v (L) t

− a∆t v (H)

t+∆t = v (H) t

+ a∆t a is maximum acceleration/braking, ∆t is time step, (L) and (H) denote the lower and upper bound.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 29 / 43

SLIDE 30

Set Representation for Reachability Analysis

Collision Check of Boxes and Split Boxes

Split rectangle along both axes if it intersects with obstacle Similar to quadtrees Repeat splitting until split rectangles do not intersect any obstacle or the diagonal is less than inner circle of vehicle For occupancy grids collision check can be done in O(1) using a summed area table

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 30 / 43

SLIDE 31

Set Representation for Reachability Analysis

Restrict Velocities in Split Boxes

ti+1 ti+2 ti+3

Find reachable velocities within a position subregion Using Pontryagin’s maximum principle to find candidate functions to minimize/maximize final velocity Bang-bang input Find start and goal state with extremal final velocities

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 31 / 43

SLIDE 32

Set Representation for Reachability Analysis

Repack Reachable Set to Non-Overlapping Boxes

Union of axis-aligned rectangles can be done in O(m log m + p log(2m2/p)) p: number of edges in the contour m: number of rectangles Sweep line algorithm using a segment tree to do fast interval union Modified algorithm to split into non-overlapping boxes in the same sweep Velocity set for each new box is obtained from minimum and maximum velocity of all intersecting original boxes

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 32 / 43

SLIDE 33

Set Representation for Reachability Analysis

Example Scenario

Scene with ego vehicle and two other vehicles Other vehicles are assumed to stay on lane, but may accelerate or brake arbitrarily within some bound

host vehicle vehicle A vehicle B road boundary

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 33 / 43

SLIDE 34

Set Representation for Reachability Analysis

Example Scenario

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 34 / 43

SLIDE 35

Set Representation for Reachability Analysis

Example Scenario

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 35 / 43

SLIDE 36

Set Representation for Reachability Analysis

Example Scenario

t1t2 t3 t1 t2 t3 t3t2t1

(a) initial scenario at t1, t2 and t3 (b) t1: reachO(s0(t1), 3 s) (c) t2: reachO(s0(t2), 3 s) (d) t3: reachO(s0(t3), 3 s)

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 36 / 43

SLIDE 37

Further Use: Verification of Single Trajectories

Trajectory Verification: Situation

bstacle

reference trajectory

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 37 / 43

SLIDE 38

Further Use: Verification of Single Trajectories

Trajectory Verification: Standard Approach

bstacle

reference trajectory vehicle occupation

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 38 / 43

SLIDE 39

Further Use: Verification of Single Trajectories

Trajectory Verification: Considering Uncertainties

bstacle

reference trajectory reachable set of the center

Robust Safety Problem Is the planned maneuver of the autonomous vehicle still safe under uncertain initial states, uncertain measurements, and disturbances? Objective: Guarantee safety when bounds on uncertainties are known.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 39 / 43

SLIDE 40

Further Use: Verification of Single Trajectories

Trajectory Verification: Formal Verification Reveals Problems

reachable set of the center vehicle occupation possible collision

Robust Safety Problem Is the planned maneuver of the autonomous vehicle still safe under uncertain initial states, uncertain measurements, and disturbances? Objective: Guarantee safety when bounds on uncertainties are known.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 40 / 43

SLIDE 41

Further Use: Verification of Single Trajectories

Online Verification Of Automated Driving

lane change maneuver B lane change maneuver A Test site Test vehicle

−20 20 40 60 80 100 120 −5 5

reference trajectory

ther vehicle

ego vehicle ego vehicle (braking part) initial occupancy final occupancy

bstacle

x-position [m] y-position [m]

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 41 / 43

SLIDE 42

Further Use: Verification of Single Trajectories

Test Drive Results

sx sy

Ψ

β lr lf δ x y v

sx, sy [m] x- and y-position Ψ [rad]

rientation

β [rad] slip angle at center of mass δ [rad] front wheel angle v [m/s] velocity

2.5 3 −0.5 0.5

Ψ [rad] ˙ Ψ [rad/s] lc B lc A

−0.2 0.2 2.4 2.6 2.8 3

δ [rad] Ψ [rad]

−0.2 0.2 −0.5 0.5

δ [rad] ˙ Ψ [rad/s] lc A lc B

computation time: ≈ 1.8 times faster than maneuver time (Intel i7, 1.6GHz)

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 42 / 43

SLIDE 43

Further Use: Verification of Single Trajectories

Conclusions and Future Work

Conclusions All possible behaviors can be computed. The approach considers uncertain measurements of other traffic participants and the ego vehicle. Assuming that the mathematical model captures all real behaviors, we can prove that no evasive trajectory exists. The computation is constantly improving and already less than a second. Future Work Comparison of results with real world measurements. Implementation in C++ for deployment in a vehicle.

Matthias Althoff Nonexistence of Evasive Trajectories September 28, 2015 43 / 43