自動レーン変更と自動駐車のためのシミュレーション環境の構築 豊田工業大学スマートビークル研究センター三田誠一 2016 年 10 月 19 日 1
Titles of Contents Introduction Automated Parking Automated Lane Change Platooning Application 2
シミュレーション活用の利点 まれにしか発生しない状況の再現ができる データを再現性良く処理できる 環境 パラメータやセンサ種類を自由に設定できる シミュレーション環境から 実環境への適応が簡単にできる 開発アルゴリズムの初期段階での短時間性能検証ができる Application Examples Ontology Car Platooning Car Control Path Planning( Narrow Passage, Parking Model) Stereo Vision System Lane Change Modeling etc. 3
Application Examples Grand Truth Generated Depth Ontology Stereo Vision System Narrow Passage Path Planning Maze Path Planning Lane Change Path Planning 4
Narrow Passage and Automated Parking Path Planning Application 5
Level of Automated Parking General Parking Place Highly Structured Parking Place for Automated Driving Lower Speed Public Road Driving White Line Detection Pedestrian Detection Car Detection etc. Following Indications No Pedestrians, No Cars 6
Valet Parking 実環境 Predefined Routes Vehicle with 3D Dynamic Model 7
Simple Perception for Environment 視点変化 Driver View 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 Without pre-determined goal With pre-determined goal Parking area map Generate path from entrance to exit Follow planned path No Found parking position Yes Generate path to parking position Parking area map Update map Sensors data (laser, GPS, odometer ) Real time data Global path planner No Path No Yes Local path planner Path Yes Path follower Stop and Wait 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. N N 1 C( p ) w d( p ) w g( p ) g( p ) i 1 i 2 i i 1 i 1 i 1 N N 1 w p w ( p ) 3 i 4 i 1 i 1 i Closeness to Obstacles Gear 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
Fast Marching Method Introduced by Sethian in 1995* A numerical method for solving boundary value problems of the Eikonal equation: U F = 1 F > 0 :the front moving speed U : the travel time 1/F can also be known as objective cost Starting with an initial position for the front, the method systematically marches the front outwards one grid point at a time. *http://math.berkeley.edu/~sethian/2006/level_set.html 15
Apply FMM on 2D Grid Map 1 st step: To find: - guiding path - distance from each cell to the goal Left side data Start 2D FMM path Right side data Goal 16
Apply SVM to Find Hyperplane and Safety Field 2 nd step: To find: distance to the hyperplane 17
Apply FMM to Continuous Search Space 3 rd step: continuous search space (x, y,, g) (x, y) : 2D coordination : vehicle heading angle (g {1,-1} ): driving maneuver (forward or backward) 2D neighbors The neighbors of a node in search space Start 3D continuous search space A path generated after applying FMM 18
Experimental Result for Complicated Path Case A Unstructured map with complicated obstacles shapes Hybrid A*and Risk Potential Proposed Method 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 Hybrid A* and Risk Potential Proposed method 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 317 20
Highly Structured Parking Place 21
Automated Parking Simulation 22
Another View for Parking 23
Conventional Camera View 24
Fisheye Camera View 25
Simulink Block from Parking Experiment
Ego Car Sensor Setting Fisheye cameras Ultrasonic sensors
Ego Car Simulink Blocks Predifined Path Car dynamic model Car State Trajectory follower Fisheye Cameras Input Data from Ultra Sonic Sensors 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 Velocity Output: Steering angle, Throttle and Brake Control
Longitudinal Controller Trajectory Follower Block Lateral Controller
Car Controller Dynamic Model Input :Control Command Output :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
Titles of Contents 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 37
Why Automated Lane Change? ADAS/Semi Automated Driving/ Automated Driving Overtake Obstacles of Low Speed Moving Objects Fast Vehicle Distance Keeping Lane Departure Warning RADAR Sensor Merge or Exits to Highway Lane Change Assistant 38
Related Works 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 39
Lane Change Model- Learn from Human Driver Human Lane Change Data Lane Change Experiment Two Segments Lane Change Model host vehicle Two Segments Model: 1- Segment 1 (Behavior Segment) Make Safe Space and Time Gap Deceleration to make free space/time for lane change Lane change and accelerate to adjust speed turning the steering wheel 2- Segment 2 (Motion Segment ) Smooth and Comfort Lane Change Segment 1 Segment 2 40
Situation Modelling & Estimation Lane change situation is modelled into a state occupancy grid with different size. 5 3 0 Most Critical Cell 6 1 7 4 2 Occupied Estimation of Neighboring Vehicle Trajectory dback dfront dfront d min max{ v dback d' min max{ v ego back v v front ego,0} Time,0} Time Merge in and Exit Lane LC LC 41
Alternative Behaviors for Segment 1 (Behavior) Available Behaviors for Lane Change 1- Accelerate 2- Wait 3- Lane Change 1- Accelerate 2- Decelerate 3- Lane Change 4- Wait 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 Behavior A : Lane change with current speed Velocity Lateral acc (m/s 2 ) T A T C Behavior C Behavior A Behavior B Time T B Time Behavior B: Lane change with deceleration Behavior C: Lane change with acceleration Behavior D: Wait Behavior A Behavior C Behavior B 43
Category Classification for Lane Change 1- accelerate 2- decelerate 3- wait 1- accelerate 2- wait 1- accelerate 2- decelerate 3- wait 1- accelerate 2- wait 1- accelerate 2- decelerate 3- lane change 4- wait Category A 1- accelerate 2- decelerate 3- lane change 4- wait Lane change 1- accelerate 2- lane change 3- wait 1- accelerate 2- decelerate 3- lane change 4- wait 1- decelerate 2- wait 1- decelerate 2- wait 1- accelerate 2- decelerate 3- wait 1- accelerate 2- wait 1- accelerate 2- wait 3- lane change 1- accelerate 2- wait 3- lane change 1- accelerate 2- wait 3- lane change 1- decelerate 2- lane change 1- decelerate 2- wait 1- accelerate 2- wait 1- decelerate 2- wait 1- decelerate 2- wait Category C 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 Category B Category D Wait 44
Behavior Selection for Segment 1 Velocity Alternative 1: Accelerate and Lane Change Alternative 1: Accelerate and Lane Change dexit t min (meter) dexit t max (meter) Alternative 2: Decelerate and Lane Change Alternative 2: Decelerate and Lane Change T 1 T 2 Time For situation that has more than one behavior option Evaluation function for different action a = {acc, decc, wait} T J(a) = w jerk න ഺx 2 t + w safety (a) + w operationtime T(a) 0 Longitudinal Jerk Safety Time 45
Velocity Planning Acceleration Case : t 0 v(t 0 ) T Safety reserve v(t) r x(t 0 ) x lead (t 0 ) v lead (t 0 ) v lead (T) x(t) X Acceleration: x ሷ = f(x(t 0 ), v(t 0 ), x lead (t 0 ), v lead (t 0 ), T, r) Safety reserve: r = d min + TTR v lead (T) TTR: Time To React Error of the safety distance: Cost function: d t = x lead (t) r + t v lead t x(t) t 0 +T J = න (ω dist d t 2 + ω acc [ x(t)] ሷ 2 )dt Constraint: t 0 x T < L; x(t) ሷ < xሷ max Distance Constraint 46
Flowchart for Lateral Motion Generation-Segment 2 Lane Change is OK Making Alternative Trajectories v 1 v 2 Lane information Estimate Trajectory Position and speed of surrounding vehicle Cost Function Generate Alternative lateral trajectories Check collisions and find collision free Select Minimum cost function Send to path follower For execution Select Minimum Cost v 3 v 5 0 v h d t 1 time t 2 v 1 v 2 v 3 v 5 Minimum Cost Path t i C = න ഺy 2 t. dt + Δt i + κ t i k 2 d + න t i 1 lateral jerk heading error t i t i 1 κƴ 2 t dt smoothness 47
Automated Lane Change Flow Chart Image Sensor Laser Scanner / Radar Estimate Speed/Position of Neighboring Vehicles Estimate the Behavior of Neighboring Vehicles Lane Information Behavior A Segment 1: Do Lane Change Behavior B Segment 1: Wait Segment 2: Lane Change Generate Lateral/Longitudinal Trajectory for Neighboring Vehicles Make Grid Map Evaluation of Different Behaviors Set of Alternative Behaviors Behavior Selection Criteria's Generate Acceleration/ Deceleration/ Wait Patterns for Segment 1 Real Time Control & Execution Time buffer for re-evaluation (every timestamp: t milliseconds) Execution and Control Behavior C Segment 1: Accelerate Segment 2: Lane Change Behavior D Segment 1: Decelerate Segment 2: Lane Change Behavior A is Selected? yes no Generate Motion for Segment 2 Real time Environment Assessment 48
Automated Lane Change Simulation Design and Implement Automated lane Change Simulation and Evaluation Environment 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 PreScan Simulation Platform 8- Control Steering and Acceleration 9-3D Graphic Simulator Simulink Model Flow diagram 49
Exiting with Acceleration Behavior Observation Grid Velocity Profile Clip 50
Exiting with Deceleration Behavior t=0, start Observation Grid Segment 1: decelerate Segment 2: lane change Exit Velocity Profile Clip 51
Merging in with Acceleration Behavior Merging in with Acceleration Behavior Observation Grid t=0, start Segment 1: accelerate Segment 2: lane change Velocity Profile Merge Clip 52
Merging with Deceleration Behavior t=0, start Observation Grid Segment 1: decelerate Segment 2: lane change Merge Velocity Profile Clip 53
Evaluation and Comparison with Expert Driver Lane Change Experiments Expert Driver Data Extraction - Lateral & Longitudinal Motion - x, ሶ x, ሷ y, ሶ yሷ - Lane information - Surrounding vehicle dx, dx, ሶ y Simulation(PreScan) Algorithms Behavior Model Simulation(PreScan) Comparison and Evaluation Motion Plan Feedback Human Computer 54
Expert Driver - Data Extraction Velodyne Laser Map (18.798, -2.949) V=67[km/h] Surrounding Vehicle Driving Lane Tracker y 3.5[m] x (-5.89, -6.86) V=96[km/h] Host vehicle V=54[km/h] 2.0[m] 1.5[m] (-21.3, -3.931) V=70[km/h] 55
Experiment Results Make Environment In PreScan Expert Driver Experiment Data Behavior and Motion Generation Generate Alternative Lateral Trajectories Check Collisions and Find Collision-free Select Minimum Cost Function Simulation of Real Traffic 56
Trajectory Evaluation 57
Platooning Application 58
Self-Defensive Maneuvering 59
Simulations Simulator PreScan Control strategy Longitudinal: PID Lateral: Steering angle 60
Result Rows: longitudinal, lateral, heading angle Columns: 1 st, 2 nd, 3 rd, 4 th platoon cars and the interfering car. 61
Simulation Result 62
Thank You for Your Attention! 63