Developing a Motion Planner for Highway Lane Change Maneuvers
Transcript of Developing a Motion Planner for Highway Lane Change Maneuvers
![Page 1: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/1.jpg)
0
Developing a Motion Planner for Highway Lane
Change Maneuvers
Seo-Wook Park
![Page 2: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/2.jpg)
11
Motion planner for Highway Lane Change Maneuver
βͺ Automated lane change
maneuver (LCM) system for
highway driving scenario
βͺ Generates an optimal trajectory
in Frenet space
βͺ Implement driving maneuver
behavior depending on
surrounding traffic conditions
βͺ Collision checking using
dynamic capsule-based objects
Automated Driving ToolboxTM
Navigation ToolboxTM
Model Predictive Control ToolboxTM
![Page 3: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/3.jpg)
22
Motion planner for Highway Lane Change Maneuver
Learn fundamentals
βͺ Motivation
βͺ Schematic of Motion Planner
βͺ Three MATLAB functions
β referencePathFrenet
β trajectoryGeneratorFrenet
β dynamicCapsuleList
Implement motion
planner and controller
βͺ Learn through reference example
βͺ Architecture of highway lane
change planer
βͺ Closed-loop controller
Simulate test benchβͺ Scenarios in straight and
curved roads
βͺ Scenarios imported from HERE
HD map
![Page 4: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/4.jpg)
33
Lane Change Maneuver (LCM) system β example
Initial version of LCMβͺ Feedback
β Lack of configurability and flexibility
β How to customize?
trajectoryOptimalFrenet
Reference Path Generation
Coordinate Transform
Trajectory Generation
Feasibility Checking
Cost Evaluation
Collision Checking
![Page 5: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/5.jpg)
44
Modular architecture to enable customized design
Initial version of LCM
trajectoryOptimalFrenet
Reference Path Generation
Coordinate Transform
Trajectory Generation
Feasibility Checking
Cost Evaluation
Collision Checking
βͺ Feedback
β Lack of configurability and flexibility
β How to customize?
referencePathFrenet
Reference Path Generation
Coordinate Transform
trajectoryGeneratorFrenet
Trajectory Generation
dynamicCapsuleList
Collision Checking
Feasibility Checking
Cost Evaluation
βͺ Modular architecture(Grey box means purely custom implementation)
![Page 6: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/6.jpg)
55
Modular architecture to enable customized design
Initial version of LCM Major upgrade for planner
(MATLAB version)
referencePathFrenet
Reference Path Generation
Coordinate Transform
trajectoryGeneratorFrenet
Trajectory Generation
dynamicCapsuleList
Collision Checking
Feasibility Checking
Cost Evaluation
βͺ Modular architecture(Grey box means purely custom implementation)
![Page 7: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/7.jpg)
66
Evolution of examples for Lane Change Maneuver (LCM) system
Initial version of LCM Major upgrade for planner
(MATLAB version)
Motion planner
(Simulink Model)Closed-loop system
(Simulink Model)
Modular architecture
![Page 8: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/8.jpg)
77
Incrementally learn about lane change planning and controls
referencePathFrenet
Reference Path Generation
Coordinate Transform
trajectoryGeneratorFrenet
Trajectory Generation
dynamicCapsuleList
Collision Checking
Feasibility Checking
Cost Evaluation
Motion planner
(Simulink Model)Closed-loop system
(Simulink Model)
![Page 9: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/9.jpg)
88
Frenet Space
Schematic of Motion Planner for Lane Change Maneuver
Destination
Map
referencePathFrenetWaypoints
global2frenet
egoGlobalState
Find MIOs
egoFrenetState
Terminal State
Sampler
dynamicCapsuleList checkCollisioncandidate egotrajectories
mioGlobalStates
predictedtarget
trajectories
mioFrenetStates
Optimal Trajectory
ego
MapInfo
Motion
Prediction
terminalStates
Feasibility Check
Cost Evaluation
frenet2global
trajectoryGeneratorFrenet
![Page 10: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/10.jpg)
99
Advantage of trajectory planning 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 trajectory
planning task when a car is
traveling on a curved road.
π πΏ
π
π = 1/π
0)(0,
π
[π₯ π¦ π]
Global States: π₯ π¦ π π π£ π
Frenet States: π πΏπ πΏ2π πΏ πΏπΏ πΏ2πΏβ’ π : Arc length
β’ πΏ : Perpendicular deviation from
the direction of the reference path
reference path
![Page 11: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/11.jpg)
1010
Frenet Space
Schematic of Motion Planner for Lane Change Maneuver
Destination
Map
referencePathFrenetWaypoints
global2frenet
egoGlobalState
Find MIOs
egoFrenetState
Terminal State
Sampler
dynamicCapsuleList checkCollisioncandidate egotrajectories
mioGlobalStates
predictedtarget
trajectories
mioFrenetStates
Optimal Trajectory
ego
MapInfo
Motion
Prediction
terminalStates
Feasibility Check
Cost Evaluation
frenet2global
trajectoryGeneratorFrenet
![Page 12: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/12.jpg)
1111
referencePathFrenet
βͺ Fits a smooth, piecewise, continuous curve to a set of waypoints given
as [π₯ π¦] or [π₯ π¦ π]
βͺ Convert trajectories between global and Frenet coordinate systems (frenet2global, global2frenet)
referencePathFrenet
π πΏ
Path Points: π₯ π¦ π π πΏπ π β’ π : Curvature
β’ πΏπ : Derivative of curvature
β’ π : Arc length
Global States: π₯ π¦ π π π£ π
Frenet States: π πΏπ πΏ2π πΏ πΏπΏ πΏ2πΏβ’ π : Arc length
β’ πΏ : Perpendicular deviation from
the direction of the reference path
π
π = 1/π
0)(0,
π
[π₯ π¦ π]
![Page 13: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/13.jpg)
1212
Frenet Space
Schematic of Motion Planner for Lane Change Maneuver
Destination
Map
referencePathFrenetWaypoints
global2frenet
egoGlobalState
Find MIOs
egoFrenetState
Terminal State
Sampler
dynamicCapsuleList checkCollisioncandidate egotrajectories
mioGlobalStates
predictedtarget
trajectories
mioFrenetStates
Optimal Trajectory
ego
MapInfo
Motion
Prediction
terminalStates
Feasibility Check
Cost Evaluation
frenet2global
trajectoryGeneratorFrenet
![Page 14: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/14.jpg)
1313
trajectoryGeneratorFrenet
βͺ Generates trajectory by solving 4th or 5th order polynomials that satisfy
boundary conditions in Frenet space relative to a given reference path
trajectoryGeneratorFrenet
initState = [0 0 0 0 0 0]; % [S ds ddS L dL ddL]termState = [50 0 0 3 0 0]; % [S ds ddS L dL ddL]
S = 50m
L = 3m
![Page 15: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/15.jpg)
1414
Frenet Space
Schematic of Motion Planner for Lane Change Maneuver
Destination
Map
referencePathFrenetWaypoints
global2frenet
egoGlobalState
Find MIOs
egoFrenetState
Terminal State
Sampler
dynamicCapsuleList checkCollisioncandidate egotrajectories
mioGlobalStates
predictedtarget
trajectories
mioFrenetStates
Optimal Trajectory
ego
MapInfo
Motion
Prediction
terminalStates
Feasibility Check
Cost Evaluation
frenet2global
trajectoryGeneratorFrenet
![Page 16: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/16.jpg)
1515
dynamicCapsuleList
βͺ Manages lists of capsule-based objects for collision checking
βͺ Why Capsules?
β Automatically build a buffer on front/rear of rectangular objects
β Avoid inflating the sides of rectangular objects
β Computationally efficient for collision checking
dynamicCapsuleList
0)(0,
π
[π₯ π¦ π]Vehicle
Radius
Length
Dynamic Capsule
ID:
States: [π₯ π¦ π]Geometry: Radius
Length
FixedTransform
![Page 17: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/17.jpg)
1616
dynamicCapsuleList
βͺ Check for collisions between ego and obstacles using dynamicCapsuleList
dynamicCapsuleList
ego
![Page 18: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/18.jpg)
1717
Motion planner for Highway Lane Change Maneuver
Learn fundamentals
βͺ Motivation
βͺ Schematic of Motion Planner
βͺ Three MATLAB functions
β referencePathFrenet
β trajectoryGeneratorFrenet
β dynamicCapsuleList
Implement motion
planner and controller
βͺ Learn through reference example
βͺ Architecture of highway lane
change planer
βͺ Closed-loop controller
Simulate test benchβͺ Scenarios in straight and
curved roads
βͺ Scenarios imported from HERE
HD map
![Page 19: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/19.jpg)
1818
Highway lane change planner test bench
Scene
Target
vehicles
Map data
Planner
Configuration
Parameters
Metric
assessment
VisualizationHighway Lane
Change Planner
![Page 20: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/20.jpg)
1919
Frenet Space
Schematic of Motion Planner for Lane Change Maneuver
Destination
Map
referencePathFrenetWaypoints
global2frenet
egoGlobalState
Find MIOs
egoFrenetState
Terminal State
Sampler
dynamicCapsuleList checkCollisioncandidate egotrajectories
mioGlobalStates
predictedtarget
trajectories
mioFrenetStates
Optimal Trajectory
ego
MapInfo
Motion
Prediction
terminalStates
Feasibility Check
Cost Evaluation
frenet2global
trajectoryGeneratorFrenet
![Page 21: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/21.jpg)
2020
Highway Lane Change Planner
Find MIOsMotion
PredictionreferencePathFrenetTerminal State
Sampler trajectoryGeneratorFrenet
global2frenet dynamicCapsuleList
checkCollision
![Page 22: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/22.jpg)
2121
Highway Lane Change Planner : Terminal State Sampler
Maneuver ModeFind Preferred Lane
Cruise Control
Lead Car Following
Lane Change
TerminalStates
Ego FrenetState
MIO FrenetStates
Map info
β
β
β
β
![Page 23: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/23.jpg)
2222
Preferred lane and maneuver mode
1
2
3
1
2
3
1
2
3
ego
unsafe target (πππΆπππβπππ < πππΆπ πππ)
safe target (πππΆπππβπππ β₯ πππΆπ πππ)
Terminal Frenet States
π πΏπ πΏ2π πΏ πΏπΏ πΏ2πΏ
π πΏ
Frenet States: π πΏπ πΏ2π πΏ πΏπΏ πΏ2πΏβ’ π : Arc length
β’ πΏ : Perpendicular deviation from
the direction of the reference path
reference path
Cruise Control Mode πππ ππ ππ‘ 0 πΏπππ 0 0
Lead Car Following Mode πππ πππππ 0 πΏπππ 0 0
Lane Change Modeπππ ππππ 0 πΏππππ‘ 0 0
πππ ππππ 0 πΏπππβπ‘ 0 0
πππππ
ππππ
ππππ
ππ ππ‘
πΏππππ‘
πΏπππβπ‘
Unrestricted longitudinal position
β use 4th-order polynomial
where πππΆ = Time-to-collision
![Page 24: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/24.jpg)
2323
Highway Lane Change Planner : Motion Planner
trajectoryGeneratorFrenet
Cost Evaluation
dynamicCapsuleList
checkCollision
TerminalStates
β
β
β
OptimalTrajectory
Candidateego
Trajectories
Feasibility Check
PredictedTarget
Trajectories
![Page 25: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/25.jpg)
2424
Examples for finding optimal trajectory
Infeasible trajectory
(excessive yaw rate)
unsafe
Candidate trajectories
with higher cost
safesafe
safe
unsafe
unsafe
Preferred lane = 3
Optimal trajectory
with min costPredicted target trajectories using dynamicCapsuleList
Colliding
trajectories
Optimal trajectory
![Page 26: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/26.jpg)
2525
Highway lane change : closed-loop system
Lane
Change
Planner
Lane
Change
Controller
Planner
Configuration
Parameters
Vehicle
dynamics
Metric
assessment
Visualization
Sensors and
Environment
![Page 27: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/27.jpg)
2626
Lane Change Planner
Lane Change Planner
Optimal trajectory
β Reference trajectory
(virtual lane center)
Trajectory
Generator
π₯
π¦
Reference
trajectory
Reference point
[π₯π , π¦π , ππ , π π , π£π]
Lateral
deviation
ππ
π‘π
π£πYaw angle
deviation (βπ)Path
Analyzer
![Page 28: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/28.jpg)
2727
Lane Change Planner
Optimal trajectory
β Reference trajectory
(virtual lane center)
Trajectory
Generator
Path
Analyzer
Lane Change Planner + Controller
Lane Change Controller
Acceleration
Steering
Angle
Curvature
Lateral Deviation
Relative Yaw Angle
Set velocity
Path
Following
Controller
(Adaptive
MPC)
![Page 29: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/29.jpg)
2828
Motion planner for Highway Lane Change Maneuver
Learn fundamentals
βͺ Motivation
βͺ Schematic of Motion Planner
βͺ Three MATLAB functions
β referencePathFrenet
β trajectoryGeneratorFrenet
β dynamicCapsuleList
Implement motion
planner and controller
βͺ Learn through reference example
βͺ Architecture of highway lane
change planer
βͺ Closed-loop controller
Simulate test benchβͺ Scenarios in straight and
curved roads
βͺ Scenarios imported from HD
map
![Page 30: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/30.jpg)
2929
Simulate highway lane change planner with test scenariosScenarios
in straight road
Scenariosin curved road
Scenariosimported from HD map
![Page 31: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/31.jpg)
3030
scenario_LC_14_DoubleLaneChange_Ushape
![Page 32: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/32.jpg)
3131
Collision avoidance against a disabled car
Disablecar
unsafe
unsafe
β’ Disabled car detected
β’ Trajectory for lane change
still not collide against the
unsafe target, but itβs
marginal
unsafe
β’ Unsafe target still exists
during lane changeβ’ Lane change again immediately after
reaching next lane to avoid collision
unsafe
![Page 33: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/33.jpg)
3232
Closed-loop system simulation: scenario_LC_15_StopnGo_Curved
![Page 34: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/34.jpg)
3333
Recap: Motion planner for Highway Lane Change Maneuver
Learn fundamentals
βͺ Motivation
βͺ Schematic of Motion Planner
βͺ Three MATLAB functions
β referencePathFrenet
β trajectoryGeneratorFrenet
β dynamicCapsuleList
Implement motion
planner and controller
βͺ Learn through reference example
βͺ Architecture of highway lane
change planer
βͺ Closed-loop controller
Simulate test benchβͺ Scenarios in straight and
curved roads
βͺ Scenarios imported from HERE
HD map
![Page 35: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/35.jpg)
3434
Key takeaways
βͺ Demonstrated how to design and
simulate an automated lane change
maneuver (LCM) system for highway
driving.
βͺ These reference examples can be used
as a good framework for developing a
custom LCM system.
βͺ Navigation and Automated Driving
Toolbox provide necessary components
for the LCM system.
Highway Lane Change
plannerClosed-loop system
+ MPC controller
+ vehicle dynamics
Automated Driving ToolboxTM
Navigation ToolboxTM
Model Predictive Control ToolboxTM
referencePathFrenet
trajectoryGeneratorFrenet
dynamicCapsuleList
![Page 36: Developing a Motion Planner for Highway Lane Change Maneuvers](https://reader031.fdocuments.us/reader031/viewer/2022013005/61cd2c9026d98452194d166d/html5/thumbnails/36.jpg)
35
Β© 2021 The MathWorks, Inc. MATLAB and Simulink are registered trademarks of The MathWorks, Inc. See mathworks.com/trademarks
for a list of additional trademarks. Other product or brand names may be trademarks or registered trademarks of their respective holders.
Thank you