bird view map - mathworks · 2016 年10月19日 1 ... new appear obstacle local path intended...

自動レーン変更と自動駐車のためのシミュレーション環境の構築

豊田工業大学スマートビークル研究センター

三田誠一

2016年10月19日

1

Introduction

Automated Parking

Automated Lane Change

Platooning Application

Titles of Contents

2

シミュレーション活用の利点

• まれにしか発生しない状況の再現ができる• データを再現性良く処理できる• 環境、パラメータやセンサ種類を自由に設定できる• シミュレーション環境から、実環境への適応が簡単にできる

開発アルゴリズムの初期段階での短時間性能検証ができる

Application Examples• Ontology• Car Platooning• Car Control• Path Planning( Narrow Passage, Parking Model)• Stereo Vision System• Lane Change Modeling etc. 3

Application Examples

Ontology Stereo Vision System

Narrow Passage Path Planning

MazePath Planning

Lane ChangePath Planning

Grand Truth Generated Depth

4

Narrow Passage and Automated ParkingPath Planning Application

5

Level of Automated Parking

Highly Structured Parking Place for

Automated DrivingGeneral Parking Place

Lower Speed Public Road Driving

• White Line Detection• Pedestrian Detection• Car Detection etc.

Following Indications

• No Pedestrians, No Cars

6

Predefined Routes

Vehicle with 3D Dynamic Model

Valet Parking

実環境

7

Driver View

Simple Perception for Environment

Top View

視点変化

8

仮想環境構築

Subjects for Automated Parking

Narrow Passage Passing Obstacle Avoidance 9

General Parking Place

10

New

appear

obstacle

Local

path

Intended parking position

is occupied

New empty

space found

New paths

generated

11

Automated Parking Scheme

Follow planned path

Parking area map

Generate path from entrance to exit

Found parking position

Generate path to parking position

Yes

Local path planner

Parking area map Global path planner

Path

Path follower

Sensors data (laser, GPS,odometer…)

No

Yes

PathUpdate map

NoYes

Stop and Wait

Real time

data

Without pre-determined goal With pre-determined goal

No

12

Automated Parking Planner’s Objectives

The safety distance of the path from the obstacles

The total travel distance of the path.

The number of times that the vehicle has to switch

the gear from forward to backward and vice versa.

The curvature of the path to satisfy the kinematic

constraints.

1

1 2 1

1 1

( ) ( ) | ( ) ( ) |N N

i i i i

i i

C p w d p w g p g p

1

3 4

1 1

| ( ) |N N

i i

i i

w p w p

Closeness to

ObstaclesGear Change

g = 1: forward

g = -1: backward

Distance between

Path Points

Curvature

13

Application of Proposed Method for Automated Parking

Proposed Parking Path Planning Method:

14

Introduced by Sethian in 1995*

A numerical method for solving boundary value problems of the Eikonalequation:

|∇U|·F = 1

F > 0 :the front moving speed

U : the travel time

1/F can also be known as objective cost

*http://math.berkeley.edu/~sethian/2006/level_set.html

Starting with an initial position

for the front, the method

systematically marches the

front outwards one grid point

at a time.

Fast Marching Method

15

Apply FMM on 2D Grid Map

2D

FMM

path

Left

side

data

Right

side

data

Start

Goal

To find:

- guiding path

- distance from each cell to the goal

1st step:

16

Apply SVM to Find Hyperplane and Safety Field

2nd step: To find: distance to the hyperplane

17

Apply FMM to Continuous Search Space

3D continuous search space

The neighbors of a node in search space

A path generated after applying FMM

continuous search space (x, y, , g)

(x, y) : 2D coordination

: vehicle heading angle

(g {1,-1} ): driving maneuver

(forward or backward) 2D neighbors

3rd step:

Start

18

Experimental Result for Complicated Path

Proposed MethodHybrid A*and Risk PotentialCase A

Unstructured map

with complicated

obstacles’ shapes

Method H-A*+RP Proposed Method

Safety Margin for Obtained Paths

(cell)4.783 5.9611

Number of Gear Change 3 1

Average Curvature 0.218145 0.17069

Average Computation Time (ms) 561 234 19

Experimental Result for Real Environment

Case B

Slam Map of

real cluttered

environment

Proposed methodHybrid A* and Risk Potential

Method H-A*+RP Proposed method

Safety Margin for Obtained

Paths (cell)17.3708 19.1896

Number of gear change 4 2

Average curvature 0.218145 0.17069

Average computation time (ms) 145 31720

21

Highly Structured Parking Place

22

Automated Parking Simulation

23

Another View for Parking

24

Conventional Camera View

25

Fisheye Camera View

Simulink Block from Parking Experiment

Ego Car Sensor Setting

Fisheye cameras

Ultrasonic sensors

Ego Car Simulink Blocks

Fisheye Cameras

PredifinedPath

Car State

Trajectory follower

Input Data from Ultra Sonic Sensors

Car dynamic model

Path planning and Control

Continue ---Ultrasonic sensors

Output:Detected Obstacle Corresponding Range

and Angle from Sensor Position

Ego Car Information

Output :Ego Car State: Position in North West Direction, GPS position, Velocity, Yaw-angle

Trajectory Follower

Input :Pre-defined Path, Car State, Desired VelocityOutput: Steering angle, Throttle and Brake Control

Trajectory Follower Block

Lateral Controller

Longitudinal Controller

Car Controller Dynamic Model

Input :Control CommandOutput :Simulated Ego Car State

Inside Car Dynamic Model

Path Planning and Control

- Sensors Data Processing- Path planning- Car Control (Steering Wheel Angle, Velocity, Gear…)

Distance Constraint Model for Automated Lane Change to Merge and Exit

36

Why Automated Lane Change

Related Work

Lane Change Model

Two Segments Lane Change Modeling

Behavior Generation Model and Selection

Motion Generation Model

Simulation Results

Comparison with Human Driver

Titles of Contents

37

ADAS/Semi Automated Driving/ Automated Driving

• Overtake Obstacles of Low Speed Moving Objects

• Fast Vehicle Distance Keeping

• Lane Departure Warning

• Merge or Exits to Highway

Why Automated Lane Change ?

RADAR Sensor

Lane Change Assistant

38

• State Transition Model

– Bayesian Network [D. Kasper et al., 2012]

– Hidden Markov Model [Y. Nishiwaki et al., 2010]

• Risk Assessment [D. Althoff, et al., 2012]

– Collision Estimation Based on Trajectories

Related Works

39

Human Lane Change Data

Lane Change Model- Learn from Human Driver

Deceleration to make free

space/time for lane change

Lane change and accelerate to

adjust speed

turning the steering wheel

host vehicle

Two Segments Lane Change Model

Segment 1 Segment 2

Lane Change Experiment

Two Segments Model:

1- Segment 1 (Behavior Segment)

Make Safe Space and Time Gap

2- Segment 2 (Motion Segment )

Smooth and Comfort Lane Change

40

Situation Modelling & Estimation

0

1

2

5

6

7

3

4

Occupied

Merge in and Exit Lane

Lane change situation is modelled into a state occupancy grid with different size.

Most Critical Cell

Estimation of Neighboring Vehicle Trajectory

dback dfront

LCegoback

LCfrontego

Timevvddback

Timevvddfront

}0,max{min'

}0,max{min

41

Alternative Behaviors for Segment 1 (Behavior)

1- Accelerate

2- Wait

3- Lane Change

1- Accelerate

2- Decelerate

3- Lane Change

4- Wait

Available Behaviors for Lane Change

1- accelerate

2- decelerate

3- lane change

4- wait

1- accelerate

2- lane change

3- wait

1- accelerate

2- wait

3- lane change

1- accelerate

2- wait

3- lane change

1- accelerate

2- decelerate

3- lane change

4- wait

1- decelerate

2- lane change

1- decelerate

2- lane change

3- wait

1- decelerate

2- lane change

3- wait

1-decelerate

2- wait

3- lane change

1-decelerate

2- wait

3- lane change

42

Alternative Behavior for Lane Change

Time

Velocity

Behavior A

Behavior B

Behavior C

Time

Lateral acc (m/s2)

Behavior A Behavior C Behavior B

𝑇𝐴𝑇𝐶

𝑇𝐵

Behavior A : Lane change with current speed

Behavior B: Lane change with deceleration

Behavior C: Lane change with acceleration

Behavior D: Wait

43

1- accelerate

2- decelerate

3- wait

1- accelerate

2- wait

1- accelerate

2- decelerate

3- wait

1- accelerate

2- wait

1- decelerate

2- wait

1- decelerate

2- wait

1- accelerate

2- decelerate

3- wait

1- accelerate

2- wait

1- decelerate

2- wait

1- accelerate

2- wait

1- decelerate

2- wait

1- decelerate

2- wait

Category A

Category B

Category C

Category D

Lane change

Wait

Category Classification for Lane Change

1- accelerate2- decelerate3- lane change4- wait

1- accelerate2- lane change 3- wait

1- accelerate2- wait3- lane change

1- accelerate2- decelerate3- lane change4- wait

1- decelerate2- lane change

1- accelerate2- wait3- lane change

1- accelerate2- decelerate3- lane change4- wait

1- accelerate2- wait3- lane change

1- decelerate2- lane change3- wait

1- decelerate2- lane change3- wait

1-decelerate2- wait3- lane change

1-decelerate

2- wait3- lane change

44

Behavior Selection for Segment 1

𝑑𝐸𝑥𝑖𝑡𝑡𝑚𝑖𝑛 (meter)

𝑑𝐸𝑥𝑖𝑡𝑡𝑚𝑎𝑥 (meter)

Alternative 1: Accelerate and Lane Change

Alternative 2: Decelerate

and Lane Change Time

Velo

city

Alternative 1: Accelerate and Lane Change

Alternative 2: Decelerate

and Lane Change

𝑇1 𝑇2

𝐽(𝑎) = 𝑤𝑗𝑒𝑟𝑘න0

𝑇

ഺ𝑥2 𝑡 + 𝑤𝑠𝑎𝑓𝑒𝑡𝑦(a) + 𝑤𝑜𝑝𝑒𝑟𝑎𝑡𝑖𝑜𝑛𝑇𝑖𝑚𝑒𝑇(𝑎)

For situation that has more than one behavior option

Evaluation function for different action 𝑎 = {𝑎𝑐𝑐, 𝑑𝑒𝑐𝑐, 𝑤𝑎𝑖𝑡}

TimeLongitudinal Jerk Safety

45

Safety reserve

Acceleration:

Cost function:

𝑟 = 𝑑𝑚𝑖𝑛 + 𝑇𝑇𝑅 ∗ 𝑣𝑙𝑒𝑎𝑑(𝑇)

Error of the safety distance:

ሷ𝑥 = 𝑓(𝑥(𝑡0), 𝑣(𝑡0), 𝑥𝑙𝑒𝑎𝑑 (𝑡0), 𝑣𝑙𝑒𝑎𝑑(𝑡0), 𝑇, 𝑟)

𝑡0

𝐽 = න

𝑡0

𝑡0+𝑇

(𝜔𝑑𝑖𝑠𝑡 ∆𝑑 𝑡 2 + 𝜔𝑎𝑐𝑐[ ሷ𝑥(𝑡)]2)𝑑𝑡

∆𝑑 𝑡 = 𝑥𝑙𝑒𝑎𝑑 (𝑡) − 𝑟 + 𝑡 ∗ 𝑣𝑙𝑒𝑎𝑑 𝑡 − 𝑥(𝑡)

Safety reserve:

𝑣𝑙𝑒𝑎𝑑(𝑡0)

𝑥(𝑡0) X

𝑣(𝑡0)

𝑣𝑙𝑒𝑎𝑑(𝑇)

𝑥𝑙𝑒𝑎𝑑 (𝑡0)

𝑟

𝑥(𝑇)

𝑣(𝑇)

Acceleration Case :

Velocity Planning

Constraint: 𝑥 𝑇 < 𝐿;

T

ሷ𝑥(t) < ሷ𝑥𝑚𝑎𝑥

Distance Constraint

TTR: Time To React

46

Flowchart for Lateral Motion Generation-Segment 2

Lane Change is

OK

Generate

Alternative lateral

trajectories

Check collisions

and find collision

free

Select Minimum

cost function

Lane information

Position and speed

of surrounding

vehicle

Estimate Trajectory

Send to path

follower

For execution

𝑣1 𝑣2

𝑣3

𝑣5

𝑣ℎd

𝑡1 𝑡2time0

Making Alternative Trajectories

smoothness

𝐶 = න𝑡𝑖−1

𝑡𝑖

ഺ𝑦2 𝑡 . 𝑑𝑡 + Δ𝑡𝑖 + 𝜅 𝑡𝑖 − 𝑘𝑑2 +න

𝑡𝑖−1

𝑡𝑖

ƴ𝜅2 𝑡 𝑑𝑡

Select Minimum Cost

𝑣1 𝑣2

𝑣3

𝑣5

Cost Function

Minimum Cost Path

lateral jerk heading error

47

Automated Lane Change Flow Chart

Estimate Speed/Position of

Neighboring Vehicles

Estimate the Behavior of

Neighboring Vehicles

Generate Lateral/Longitudinal

Trajectory for Neighboring Vehicles Lane Information

Make

Grid Map

Behavior A

Segment 1: Do Lane Change

Behavior B

Segment 1: Wait

Segment 2: Lane Change

Behavior C

Segment 1: Accelerate

Segment 2: Lane Change

Behavior D

Segment 1: Decelerate

Segment 2: Lane Change

Image Sensor Laser Scanner / Radar

Set of Alternative

Behaviors

Evaluation of

Different

Behaviors

Behavior Selection

Criteria's

Generate

Acceleration/

Deceleration/

Wait Patterns

for Segment 1

Generate

Motion for

Segment 2

Behavior A

is Selected ?yes

no

Time buffer for

re-evaluation

(every timestamp:

∆𝒕 milliseconds)

Execution

and Control

Real time

Environment

Assessment

Real Time Control

& Execution

48

Automated Lane Change Simulation

Simulink Model – Flow diagram

Designed Modules

1- Adaptive Grid Generation

2- Estimate Surrounding Vehicles Trajectory

3- Check the Collision Avoidance

4- Automated Behavior Generation

5- Automated Lateral/Longitudinal Motion

Generation

6- Lane Detection Module

7- Automated Lane Change Scenario Generation

8- Control Steering and Acceleration

9- 3D Graphic Simulator

Design and Implement Automated lane

Change Simulation and Evaluation

Environment

PreScan Simulation Platform

49

Clip

Exiting with Acceleration Behavior

Observation Grid

Velocity Profile

50

Clip

Exiting with Deceleration Behavior

Observation Grid

Velocity Profile

t=0 , start

Segment 1:

decelerate

Segment 2:

lane change

Exit

51

Clip

Merging in with Acceleration BehaviorObservation Grid

Velocity Profile

t=0 , start

Segment 1:

accelerate

Segment 2:

lane change

Merge

Merging in with Acceleration Behavior

52

Clip

Merging with Deceleration Behavior

Observation Grid

Velocity Profile

t=0 , start

Segment 1:

decelerate

Segment 2:

lane change

Merge

53

Evaluation and Comparison with Expert Driver

Behavior Model

Motion Plan

Algorithms

Simulation(PreScan)

Comparison and Evaluation

Human ⇆ Computer

Lane Change Experiments

Expert Driver

- Lateral & Longitudinal Motion- ሶ𝒙, ሷ𝒙, ሶ𝒚, ሷ𝒚- Lane information- Surrounding vehicle 𝐝𝐱, 𝐝 ሶ𝒙, 𝒚

Data Extraction

Simulation(PreScan)

Feedback

54

x

y

(18.798, -2.949)V=67[km/h]

(-21.3, -3.931)

V=70[km/h]

3.5[m]

2.0[m] 1.5[m]

(-5.89, -6.86)V=96[km/h]

Velodyne Laser Map

Surrounding Vehicle

V=54[km/h]Host vehicle

Driving LaneTracker

Expert Driver - Data Extraction

55

Make Environment In PreScan

Behavior and Motion Generation

Generate Alternative Lateral Trajectories

Check Collisions and Find Collision-free

Select Minimum Cost Function

Expert DriverExperiment Data

Experiment Results

Simulation of Real Traffic 56

Trajectory Evaluation

57

Platooning Application

58

Self-Defensive Maneuvering

59

• Simulator

– PreScan

• Control strategy

– Longitudinal: PID

– Lateral: Steering angle

Simulations

60

.

• Rows: longitudinal, lateral, heading angle

• Columns: 1st, 2nd, 3rd, 4th platoon cars and the interfering car

Result

61

62

Simulation Result

Thank You for Your Attention !

63