Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving...
Transcript of Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving...
![Page 1: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/1.jpg)
1© 2020 The MathWorks, Inc.
Developing Planning and Controls
for Highway Lane Change Maneuvers
Seo-Wook Park
Application Engineering
ADAS / Automated Driving
![Page 2: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/2.jpg)
2
Highway Lane Change
▪ Automated lane change maneuver
(LCM) system for highway driving
scenario (straight & curved roads)
▪ Generates a collision-free, optimal
trajectory for lane change using
trajectoryOptimalFrenet
▪ Behavior layer to configure motion
planner
▪ State validator for collision checking
▪ Lane following controller using MPC
Navigation ToolboxTM
Model Predictive Control ToolboxTM
Automated Driving ToolboxTM
Highway Lane Change
Updated
![Page 3: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/3.jpg)
3
Highway Lane Change
Learn motion planner
Integrate motion
planner and controller
Test with real-world
scenario
▪ Optimal trajectory generation
in Frenet coordinate
▪ Planner parameters
▪ Simulate trajectory planning
with occupancy map
▪ Learn through reference example
▪ Architecture of highway lane
change
▪ Simulate closed-loop controller
with test scenarios
▪ Create scenario from HD map
▪ Update setup script and model
▪ Run simulation with new
scenario
![Page 4: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/4.jpg)
4
Lane Change PlannerSensors and Environment
Architecture of Highway Lane Change
targetActors
egoActors
RefPath
Accel/SteerAngEgoPose
MIO
Driving
Scenario
Behavior
Layer
Motion
Planner
Find
MIOLane
Detection
Lane Change
Controller
Ego
Vehicle
Dynamics
Motion
Planner
![Page 5: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/5.jpg)
5
Optimal Trajectory Generation
▪ trajectoryOptimalFrenet function generates an optimal, feasible, and
collision-free trajectory for the reference path
ReferencePath
Optimal trajectory
ego
Navigation ToolboxTM
reference path state validator
for collision checking
![Page 6: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/6.jpg)
6
State Validator for collision checking
1) Assign 2-D occupancy grid map
with binary values
2) Assign 2-D occupancy grid map
with probabilistic values (0..1)
3) Use custom state validator
![Page 7: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/7.jpg)
7
Planner parameters
▪ Terminal states
▪ No. of longitudinal
segments
▪ Feasibility parameters
1 2 3 NumSegments=3
Longitudinal terminal statesLateral terminal states
RefPath
![Page 8: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/8.jpg)
8
Optimal Trajectory Generation
Samples multiple candidate trajectories
for each pair of states (start, terminal states)
Evaluate cost for all candidate trajectories
Check feasibility for all candidate trajectories
Check collision using state validator
Choose a feasible trajectory with the least cost
→ optimal, feasible, and collision-free trajectory
Cost weights • Time, Arc length, Deviation
• Lateral/longitudinal smoothness
Feasibility parameters:• MaxCurvature, MaxAcceleration
validatorOccupancyMap
Custom state validator
Longitudinal terminal states
Lateral terminal states
RefPathOptimal Trajectory
Candidate Trajectories
![Page 9: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/9.jpg)
9
Generate trajectory in Frenet coordinate
using quintic polynomial
▪ Quintic polynomial
▪ Start boundary conditions (𝑡 = 0)
▪ End boundary conditions (𝑡 = 𝑡𝑓)
𝑠 𝑡 = 𝑎5𝑡5 + 𝑎4𝑡
4+ 𝑎3𝑡3+ 𝑎2𝑡
2+ 𝑎1𝑡 + 𝑎0ሶ𝑠 𝑡 = 5𝑎5𝑡
4 + 4𝑎4𝑡3+ 3𝑎3𝑡
2+ 2𝑎2𝑡+ 𝑎1ሷ𝑠 𝑡 = 20𝑎5𝑡
3 + 12𝑎4𝑡2+ 6𝑎3𝑡+ 2𝑎2
𝑤ℎ𝑒𝑟𝑒 𝑠 = 𝑙𝑜𝑛𝑔𝑖𝑡𝑢𝑑𝑖𝑛𝑎𝑙 𝑜𝑟 𝑙𝑎𝑡𝑒𝑟𝑎𝑙 𝑑𝑖𝑠𝑡𝑎𝑛𝑐𝑒
𝑎0 = 𝑠𝑠𝑡𝑎𝑟𝑡𝑎1 = ሶ𝑠𝑠𝑡𝑎𝑟𝑡2𝑎2 = ሷ𝑠𝑠𝑡𝑎𝑟𝑡
𝑎5𝑡𝑓5 + 𝑎4𝑡𝑓
4 + 𝑎3𝑡𝑓3 + 𝑎2𝑡𝑓
2+ 𝑎1𝑡𝑓 + 𝑎0 = 𝑠𝑒𝑛𝑑5𝑎5𝑡𝑓
4 + 4𝑎4𝑡𝑓3+ 3𝑎3𝑡𝑓
2+ 2𝑎2𝑡𝑓+ 𝑎1= ሶ𝑠𝑒𝑛𝑑20𝑎5𝑡𝑓
3 + 12𝑎4𝑡𝑓2+ 6𝑎3𝑡𝑓+ 2𝑎2 = ሷ𝑠𝑒𝑛𝑑
𝒔𝒕𝒂𝒓𝒕
𝒆𝒏𝒅𝑠
𝑙
𝑑𝑙𝑎𝑡
𝑑𝑙𝑜𝑛𝑔
𝑙𝑎𝑡𝑒𝑟𝑎𝑙 𝑑𝑖𝑠𝑡𝑎𝑛𝑐𝑒
Frenet Coordinate
jerk-optimal
trajectory
with a pair of start
and end states
![Page 10: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/10.jpg)
10
Advantage of trajectory generation in Frenet coordinate
▪ Frenet system represents an object and its
trajectory with respect to the reference path
(road center or lane center).
▪ This approach dramatically simplifies the
problem of trajectory generation when a car is
traveling on a curved road.
RefPath
RefPath
![Page 11: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/11.jpg)
11
Convert Frenet states Cartesian states
Cartesian state[𝑥, 𝑦, 𝜃, 𝜅, 𝑣, 𝑎]
Frenet states
𝑠,𝑑𝑠
𝑑𝑡,𝑑2𝑠
𝑑𝑡2, 𝑙,
𝑑𝑙
𝑑𝑠,𝑑2𝑙
𝑑𝑠2
cart2frenetfrenet2cart
• 𝑥, 𝑦 : position in meters
• 𝜃 : orientation angle in radians
• 𝜅 : curvature in m-1
• 𝑣 : velocity in m/s• 𝑎 : acceleration in m/s2
• s : arc length (longitudinal distance)
• l : normal distance (lateral distance)
from reference path
RefPath
RefPath
![Page 12: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/12.jpg)
12
Convert Frenet states Cartesian states
Current Cartesian state
[𝑥, 𝑦, 𝜃, 𝜅, 𝑣, 𝑎]
frenet2cartTrajectory generation
in Frenet states
Optimal trajectory
in Cartesian state
[𝑥, 𝑦, 𝜃, 𝜅, 𝑣, 𝑎, 𝑡]
cart2frenet
𝑠,𝑑𝑠
𝑑𝑡,𝑑2𝑠
𝑑𝑡2, 𝑙,
𝑑𝑙
𝑑𝑠,𝑑2𝑙
𝑑𝑠2
Longitudinal terminal states
Lateral terminal statesOptimal Trajectory
Candidate TrajectoriesRefPath
Start Frenet state
![Page 13: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/13.jpg)
13
Optimal Trajectory Planning in Frenet Space
- shipping example
Navigation ToolboxTM
![Page 14: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/14.jpg)
14
Optimal Trajectory Planning with binary occupancy map
: Straight road
Optimal trajectoryCandidate trajectory
RefPath
▪ TerminalStates.Longitudinal = 100
▪ NumSegments = 1
TerminalStates.Longitudinal = 100
![Page 15: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/15.jpg)
15
Optimal Trajectory Planning with binary occupancy map
: Straight road with dense traffic condition
Dense traffic
condition
TerminalStates.Longitudinal = 100
▪ TerminalStates.Longitudinal = 100
▪ NumSegments = 1
![Page 16: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/16.jpg)
16
Optimal Trajectory Planning with binary occupancy map
: Straight road with dense traffic condition ▪ TerminalStates.Longitudinal = [50 100]
▪ NumSegments = 1
Dense traffic
condition
TerminalStates.Longitudinal = [50 100]
![Page 17: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/17.jpg)
17
Optimal Trajectory Planning with binary occupancy map
: Curved road ▪ TerminalStates.Longitudinal = 100
▪ NumSegments = 3
![Page 18: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/18.jpg)
18
Highway Lane Change
Learn motion planner
Integrate motion
planner and controller
Test with real-world
scenario
▪ Optimal trajectory generation in
Frenet coordinate
▪ Planner parameters
▪ Simulate trajectory planning with
occupancy map
▪ Learn through reference example
▪ Architecture of highway lane
change
▪ Simulate closed-loop controller
with test scenarios
▪ Create scenario from HD map
▪ Update setup script and model
▪ Run simulation with new scenario
![Page 19: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/19.jpg)
19
Test Bench Model for Highway Lane Change
Navigation ToolboxTM
Automated Driving ToolboxTM
Sensor and
Environment
Lane Change
Planner
Lane Change
Controller
Vehicle
Dynamics
Metrics
Assessment
Visualization
![Page 20: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/20.jpg)
20
Lane Change PlannerSensors and Environment
Architecture of Highway Lane Change
targetActors
egoActors
RefPath
Accel/SteerAngEgoPose
MIO
Driving
Scenario
Behavior
Layer
Motion
Planner
Find
MIOLane
Detection
Lane Change
Controller
Ego
Vehicle
Dynamics
![Page 21: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/21.jpg)
21
Lane Change Planner
Lane Change Planner
Behavior
Layer
Motion
Planner
targetActors
Find
MIOLaneDetections
RefPath
egoActors
MIO
![Page 22: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/22.jpg)
22
Lane Change Planner
Motion Planner
Lane Change Planner
Behavior
Layer
Trajectory
Generator
MIO
targetActors
Find
MIOLaneDetections
RefPath
egoActors
Path
Analyzer
![Page 23: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/23.jpg)
23
planner parameters
• Terminal States
• Weights
Lane Change Planner
Behavior
Layer
Trajectory
Generator
MIOvalidator
Optimal trajectory
targetActors
Find
MIOLaneDetections
RefPath
egoActors
![Page 24: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/24.jpg)
24
Find Most Important Objects (MIO)
Ego Car
RightFront
MIO
LeftRear
MIO
LeftFront
MIO
EgoRear
MIO
RightRear
MIO
EgoFront
MIO
MIO
targetActors
Find
MIOLaneDetections
▪ Find MIOs in ego and neighbor lanes
▪ MIOs are provided for the custom state validator
for collision checking.
Max. 6 MIOs
![Page 25: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/25.jpg)
25
Custom State Validator for Collision checking𝑋
𝑣
𝑣𝑡𝑘
𝑌
Find MIOs
MIO motion prediction
based on constant
speed model
candidate
trajectory
MIO
𝑡1 = ∆𝑡
𝑡2 = 2∆𝑡
𝑡𝑘 = 𝑘∆𝑡
𝑡0 = 0
![Page 26: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/26.jpg)
26
Behavior Layer
RefPath
planner parameters
• Terminal States
• Weights
Behavior
Layer
RefPath
egoActors
Longitudinal terminal states
Lateral terminal states
NumLatStates = 2
(No Left Lane)
NumLatStates = 2
(No Right Lane)
NumLatStates = 3
![Page 27: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/27.jpg)
27
Motion Planner Trajectory Generator
RefPath
MIO
Planner Parameters
• Terminal States
• Weights
Trajectory
GeneratorOptimal trajectory
![Page 28: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/28.jpg)
28
Motion Planner Periodic Re-planning
RefPath
MIO
Planner Parameters
• Terminal States
• Weights
Re-plan1s
Trajectory
GeneratorOptimal trajectory
![Page 29: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/29.jpg)
29
Motion Planner
Motion Planner Path Analyzer
RefPath
MIO
Optimal trajectory
→ Reference trajectory
(virtual lane center)
Planner Parameters
• Terminal States
• Weights
Re-plan
Trajectory
Generator
𝑥
𝑦
Reference
trajectory
Reference point
[𝑥𝑘 , 𝑦𝑘 , 𝜃𝑘 , 𝜅𝑘 , 𝑣𝑘]
Lateral
deviation
𝜃𝑘
𝑡𝑘
𝑣𝑘Yaw angle
deviation (∆𝜃)
Path
Analyzer
1s
![Page 30: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/30.jpg)
30
Motion Planner
Motion Planner and Lane Change Controller
RefPath
MIO
Optimal trajectory
→ Reference trajectory
(virtual lane center)
Planner Parameters
• Terminal States
• Weights
Re-plan
Lane Change Controller
Acceleration
Steering
Angle
Curvature
Lateral Deviation
Relative Yaw Angle
Set velocity
Path
Analyzer
Path
Following
Controller
(Adaptive
MPC)
Trajectory
Generator
1s
![Page 31: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/31.jpg)
31
Simulate Highway Lane Change with Test Scenarios
Test Name scenario_LC_01_SlowMoving
Test
Description
Passing slow moving lead car
(1) Host Car initial velocity = 20m/s
HWT = 6.5sec
HW = 130m
v_set = 20m/s
(2) Lead Car constant velocity = 10m/s
Other Cars None
21
Slow moving
HWT : Headway time
HW : Headway
v_set : set velocity for ego car
![Page 32: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/32.jpg)
32
Simulate Highway Lane Change with Test Scenarios
Test Name scenario_LC_07_RightLaneChange
Test
Description
Passing slow moving lead car to right lane
(1) Host Car initial velocity = 20m/s
HWT = 6sec
HW = 120m
v_set = 20m/s
(2) Lead Car constant velocity = 10m/s
Other Cars Constant velocity = 25m/s (3rd car in left lane)
Constant velocity = 24m/s (4th car in left lane)
HWT : Headway time
HW : Headway
v_set : set velocity for ego car
21
4
Slow moving
3
![Page 33: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/33.jpg)
33
Simulate Highway Lane Change with Test Scenarios
Test Name scenario_LC_10_SingleLaneChange_Curved
Test
Description
Passing slow moving lead car to left lane in curved road
(1) Host Car initial velocity = 15m/s
v_set = 15m/s
(2) Lead Car Slow moving
Other Cars Dense traffic
HWT : Headway time
HW : Headway
v_set : set velocity for ego car
Host car
Host car
![Page 34: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/34.jpg)
34
Highway Lane Change
Learn motion planner
Integrate motion
planner and controller
Test with real-world
scenario
▪ Optimal trajectory generation in
Frenet coordinate
▪ Planner parameters
▪ Simulate trajectory planning with
occupancy map
▪ Learn through reference example
▪ Architecture of highway lane
change
▪ Simulate closed-loop controller
with test scenarios
▪ Create scenario from HD map
▪ Update setup script and model
▪ Run simulation with new scenario
![Page 35: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/35.jpg)
35
Create highway road network from HD map
M23
Hwy96
![Page 36: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/36.jpg)
36
Import HERE HD Live Map into Driving Scenario
Automated Driving ToolboxTM
![Page 37: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/37.jpg)
37
Add actors and export driving scenario with MATLAB Function
![Page 38: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/38.jpg)
38
Setup driving scenario and workspace variables
Add new driving
scenario
![Page 39: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/39.jpg)
39
Update Lane Change Planner Simple Behavior Layer
Longitudinal
Sampler
Velocity
Sampler
Update longitudinal & velocity sampler
to read input parameter
![Page 40: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/40.jpg)
40
scenario_LC_11_Hwy96_M23_WithMergingCar
ego car
Merging car
passing car
ego car
Merging car
passing car
![Page 41: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/41.jpg)
41
scenario_LC_12_Hwy96_M23_WithCutInCar
ego car
Cut-in car
passing car
ego car
Cut-in car
passing car
High curvature
Road segment
![Page 42: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/42.jpg)
42
Recap: Highway Lane Change
Learn motion planner
Integrate motion
planner and controller
Test with real-world
scenario
▪ Optimal trajectory generation in
Frenet coordinate
▪ Planner parameters
▪ Simulate trajectory planning with
occupancy map
▪ Learn through reference example
▪ Architecture of highway lane
change
▪ Simulate closed-loop controller
with test scenarios
▪ Create scenario from HD map
▪ Update setup script and model
▪ Run simulation with new
scenario
![Page 43: Developing Planning and Controls for Highway Lane Change ... · (LCM) system for highway driving scenario (straight & curved roads) Generates a collision-free, optimal trajectory](https://reader035.fdocuments.us/reader035/viewer/2022081614/5fd14e91baa86b49922a61fd/html5/thumbnails/43.jpg)
43
Presenter contact info and poll questions
▪ Poll question : I found this
technique the most interesting
a. Motion planner for lane change
b. MPC path following controller
c. Driving scenario creation
d. Import HD map into driving scenario
▪ If you would like to an individual
follow-up, please let us know in
the WebEx poll area.
Please contact me at [email protected] with questions