bird view map - mathworks · 2016 年10月19日 1 ... new appear obstacle local path intended...
TRANSCRIPT
自動レーン変更と自動駐車のためのシミュレーション環境の構築
豊田工業大学スマートビークル研究センター
三田誠一
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