case study: highway lane following + lane change€¦ · case study: highway lane following + lane...
TRANSCRIPT
![Page 1: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/1.jpg)
1© 2019 The MathWorks, Inc.
Case Study: Highway Lane Following + Lane ChangeDesign and test decision making, path planning, and control modules in traffic scenarios
Mark Corless
Industry Marketing, MathWorks
Seo-Wook Park
Application Engineering, MathWorks
Marco Roggero
Application Engineering, MathWorks
![Page 2: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/2.jpg)
2
Evolution of ADAS and Autonomous Driving Car Technologies
L0 No Automation FCW, LDW
L1Driver
Assistance
ACC
AEB-Vehicle (City/Inter-Urban)
Lane Keep Assist/Lateral Support
AEB-VRU(Cyclist)
Junction Assist
AEB-VRU (Pedestrian)
L2 Partial Automation Auto Pilot: Traffic Jam Assist
L3Conditional
Automation
Auto Pilot: Highway
Auto Pilot: Parking
Auto Pilot: Road Train
L4 High Automation Self-Driving & Human-Driven Car
L5 Full Automation Self-Driving Car
2010 2014 2016 2018 2020 2025 2030
ACC
Lane Keep Assist/Lateral Support
Traffic Jam Assist (LF)
FCW
AEB-Vehicle (City/Inter-Urban)
AEB-VRU(Cyclist)
AEB-VRU (Pedestrian)
Application examples in Automated Driving Toolbox™
Auto Pilot (LF+LC)
Lane Change
ACC
Lane Keep Assist/Lateral Support
Traffic Jam Assist (LF)
![Page 3: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/3.jpg)
3
Traffic Jam Assist with ACC and Lane Following Control
ACC(Longitudinal Control)
Lane Following(Lateral Control)
Traffic Jam Assist(Longitudinal
+ Lateral Control)
Automated Driving Toolbox
![Page 4: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/4.jpg)
4
Combined lane following
and lane change control
Lane change decision
logic and lane change
trajectory planning
Auto Pilot: Lane Following plus Lane Change
Traffic Jam Assist(Longitudinal
+ Lateral Control)
Automated Driving Toolbox
Auto Lane Change(LC Decision Logic
+ Planning)
Auto Pilot(Lane Following
+ Lane Change)
Baseline example
![Page 5: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/5.jpg)
5
Single Lane Change Example
![Page 6: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/6.jpg)
6
Case Study for Lane Following plus Lane Change
Design lane following +
lane change controller
Automate
regression testing
Test robustness
with traffic agents
▪ Review baseline LF example
▪ Design sensor configuration
▪ Design additional MIO detectors
▪ Design safety zone calculation
▪ Design lane change logic
▪ Design trajectory planner
▪ Define assessment metrics
▪ Add predefined scenarios
▪ Run Simulink test
▪ Specify driver logic for traffic agents
▪ Randomize scenarios using traffic
agents
▪ Identify and assess unexpected
behavior
![Page 7: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/7.jpg)
7
Learn about developing a lane following controller
Lane Following Control with
Sensor Fusion
▪ Specify scenario and sensors
▪ Design lateral (lane keeping)
and longitudinal (lane spacing)
model predictive controllers
▪ Integrate sensor fusion
▪ Generate C/C++ code
▪ Test with software in the loop
(SIL) simulation
Model Predictive Control ToolboxTM
Automated Driving ToolboxTM
Embedded Coder®
![Page 8: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/8.jpg)
8
Review lane following test bench model architecture
Lane following
controller
Vehicle and
environment
Collision
detection
![Page 9: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/9.jpg)
9
Review lane following test bench model architecture
Vehicle and
environment
![Page 10: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/10.jpg)
10
Review vehicle and environment components
Ego vehicle
dynamicsRadar detection
sensor model
Vision detection
sensor model
Other vehicle poses
(driving scenario)
![Page 11: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/11.jpg)
11
Review lane following test bench model architecture
Lane following
controller
![Page 12: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/12.jpg)
12
Review lane following controller components
Estimate lane center
• Detect lane center
• Preview curvature
Tracking and sensor fusion
• Cluster and track detections
• Identify most important object (MIO) ahead
Model predictive controller
• Lateral: Follow lane center
• Longitudinal: Adaptive cruise control
![Page 13: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/13.jpg)
13
Partition design to enable inserting lane change functionality
Estimate lane center
• Detect lane center
• Preview curvature
![Page 14: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/14.jpg)
14
Partition design to enable inserting lane change functionality
Preview curvature
Detect lane center
![Page 15: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/15.jpg)
15
Partition design to enable inserting lane change functionality
Select signals
based on modePreview curvature
Detect lane center
![Page 16: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/16.jpg)
16
Add lane change functionality to lane following controller
1. Add more
sensors
2. Identify most important
objects in adjacent lanes
![Page 17: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/17.jpg)
17
Add lane change functionality to lane following controller
1. Add more
sensors
2. Identify most important
objects in adjacent lanes
4. Decide when to
change lanes
3. Identify safety
zones
5. Plan trajectory
![Page 18: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/18.jpg)
18
Add lane change functionality to lane following controller
1. Add more
sensors
2. Identify most important
objects in adjacent lanes
4. Decide when to
change lanes
3. Identify safety
zones
5. Plan trajectory
![Page 19: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/19.jpg)
19
System requirements for lane changeIntelligent transport systems - Lane change decision aid systems (LCDAS)
Typically implemented with
Short Range Radar
Typically implemented with
Mid Range Radar
Adjacent zones
for Blind Spot Detection
Rear zones
for closing vehicle warning
left
adjacent zone
right
adjacent zone
left
rear zone
right
rear zone
17387
![Page 20: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/20.jpg)
20
Explore sensor placement with Driving Scenario Designer
![Page 21: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/21.jpg)
21
Review sensor configuration for lane following example
Camera
FoV = 43.6 x 150m
FrontMRR
FoV = 90 x 60m
FrontLRR
FoV = 20 x 175m
• SRR: Short-Range Radar
• MRR: Mid-Range Radar
• LRR: Long-Range Radar
![Page 22: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/22.jpg)
22
Add rear looking sensors to support left lane change
Camera
FoV = 43.6 x 150m
FrontMRR
FoV = 90 x 60m
FrontLRR
FoV = 20 x 175m
LeftRearSRR
FoV = 140 x 40m
LeftRearMRR
FoV = 30 x 80m
• SRR: Short-Range Radar
• MRR: Mid-Range Radar
• LRR: Long-Range Radar
![Page 23: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/23.jpg)
23
Overall sensor configuration for lane following plus lane change
Camera
FoV = 43.6 x 150m
FrontMRR
FoV = 90 x 60m
FrontLRR
FoV = 20 x 175m
LeftRearSRR
FoV = 140 x 40m
LeftRearMRR
FoV = 30 x 80m
LeftFrontSRR
FoV = 140 x 40m
RightRearSRR
FoV = 140 x 40m
RightRearMRR
FoV = 30 x 80m
RightFrontSRR
FoV = 140 x 40m
1 front camera
8 radars
• SRR: Short-Range Radar
• MRR: Mid-Range Radar
• LRR: Long-Range Radar
![Page 24: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/24.jpg)
24
Review sensor models for traffic jam assist
1 radar
1 front camera
Visualize with
Birds Eye Scope
![Page 25: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/25.jpg)
25
Add sensor models for lane change
8 radar
8 radar
1 front camera
Visualize with
Birds Eye Scope
![Page 26: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/26.jpg)
26
Add lane change functionality to lane following controller
1. Add more
sensors
2. Identify most important
objects in adjacent lanes
4. Decide when to
change lanes
3. Identify safety
zones
5. Plan trajectory
![Page 27: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/27.jpg)
27
Identify Most Important Objects (MIO) to detect
Ego Car
EgoFront
MIO
RightFront
MIO
LeftRear
MIO
LeftFront
MIO
EgoRear
MIO
RightRear
MIO
▪ Lane following – one EgoFront MIO is enough
▪ Lane change – needs more MIOs
surrounding ego car
![Page 28: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/28.jpg)
28
Review baseline MIO detector architecture for traffic jam assist
Add screen shot of
original model and
highlight MIO block
Ego lane
One MIO
(EgoFront)
![Page 29: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/29.jpg)
29
Add MIO detectors for lane change
Add screen shot of
original model and
highlight new MIO
blocks/calculations
Adjacent
lane
6 MIOs
Ego lane
![Page 30: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/30.jpg)
30
Add lane change functionality to lane following controller
1. Add more
sensors
2. Identify most important
objects in adjacent lanes
4. Decide when to
change lanes
3. Identify safety
zones
5. Plan trajectory
![Page 31: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/31.jpg)
31
Identify safety zones to calculate
Ego Car
EgoFront
MIO
Safety Zone
for lane following
𝑣𝑓𝛿 +𝑣𝑓2
2𝑎𝑏𝑟𝑎𝑘𝑒𝑜𝑟, 𝑣𝑓𝑇𝑇𝐶𝐹𝐶𝑊
Lane following is unsafe if ego front MIO is detected
within the safety zone
![Page 32: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/32.jpg)
32
Identify safety zones to calculate
Ego Car
EgoFront
MIO
Safety Zone
for lane following
LeftRear
MIO
LeftFront
MIO
𝑣𝑙𝑓𝛿𝑓 +𝑣𝑙𝑓2
2𝑎𝑏𝑟𝑎𝑘𝑒𝑜𝑟, 𝑣𝑙𝑓𝑇𝑇𝐶𝐿𝐶
𝑣𝑙𝑟𝛿𝑟 +𝑣𝑙𝑟2
2𝑎𝑏𝑟𝑎𝑘𝑒𝑜𝑟, 𝑣𝑙𝑟𝑇𝑇𝐶𝐿𝐶
Left lane change is safeif no MIOs are detected
within the safety zone
Safe Zone
for left lane change
![Page 33: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/33.jpg)
33
Identify safety zones to calculate
Ego Car
EgoFront
MIO
LeftRear
MIO
LeftFront
MIO
EgoRear
MIO
RightFront
MIO
RightRear
MIO
Safe Zone
for lane following
Safe Zone
for right lane change
Safe Zone
for left lane change
Right lane change is unsafeif MIOs are detected
within the safety zone
![Page 34: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/34.jpg)
34
Identify safety zones to calculate
Ego Car
EgoFront
MIO
LeftRear
MIO
LeftFront
MIO
EgoRear
MIO
FCW
RightFront
MIO
RightRear
MIO
Trigger lane change
to safe side
Safe Zone
for lane following
Safe Zone
for left lane changeSafe Zone
for right lane change
![Page 35: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/35.jpg)
35
Visualize safety zones
Ego Car
EgoFront
MIO
LeftRear
MIO
LeftFront
MIO
EgoRear
MIO
FCW
RightFront
MIO
RightRear
MIO
Trigger lane change
to safe side
Safe Zone
for lane following
Safe Zone
for left lane changeSafe Zone
for right lane change
![Page 36: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/36.jpg)
36
Add lane change functionality to lane following controller
1. Add more
sensors
2. Identify most important
objects in adjacent lanes
4. Decide when to
change lanes
3. Identify safety
zones
5. Plan trajectory
![Page 37: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/37.jpg)
37
Lane change decision logic and planning
Lane Change
Decision Logic
Lane Change
Planning
MIOs
Ego &
Adjacent
lanes
LC parameters
Reference
trajectory
for lane
change
LC Trigger
Safety zone
calculations
![Page 38: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/38.jpg)
38
Design lane change decision logic
MIOs
Ego &
Adjacent
lanes
LC parameters
Reference
trajectory
for lane
change
LC Trigger
Lane Change
Decision Logic
Lane following
mode
FCW detected
Left lane change
Right lane change
Lane change
mode
![Page 39: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/39.jpg)
39
Add lane change functionality to lane following controller
1. Add more
sensors
2. Identify most important
objects in adjacent lanes
4. Decide when to
change lanes
3. Identify safety
zones
5. Plan trajectory
![Page 40: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/40.jpg)
40
Lane Change
Planning
MIOs
Ego &
Adjacent
lanes
Lane change parameters
Reference
lane
change
trajectory
Trigger
lane change
Design lane change planning
Generate
Trajectory
Generate
Reference
Point
Calculate
Deviation
![Page 41: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/41.jpg)
41
Generate trajectory
▪ Quintic polynomial
▪ Start boundary conditions
▪ 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 = ሷ𝑠𝑒𝑛𝑑
𝑥
𝑦
𝑑𝑙𝑜𝑛𝑔
𝑑𝑙𝑎𝑡
𝜃𝑣
![Page 42: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/42.jpg)
42
Example of trajectory generation for lane change
Longitudinal trajectory Lateral trajectory
![Page 43: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/43.jpg)
43
Generate reference point
▪ Find a point for the vehicle to follow
Incremental
projection
Trajectory
i
i+1
i+2
i-1
u
u = 1
Reference
Point
![Page 44: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/44.jpg)
44
Calculate deviations from reference point
𝑥
𝑦
𝑑𝑙𝑜𝑛𝑔
𝑑𝑙𝑎𝑡
[𝑥 𝑖 , 𝑦 𝑖 , 𝜃 𝑖 , 𝜅 𝑖 , 𝑣 𝑖 ]
𝜃
𝑣
Reference
trajectory
Lateral
deviation
Heading
deviation
Reference
point
![Page 45: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/45.jpg)
45
Create custom visualization for safety zones and trajectory
![Page 46: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/46.jpg)
46
Create birds eye plot with utilities from
Automated Driving Toolbox
% create birds eye plot
obj.BEP = birdsEyePlot('Parent', hax,...
'XLimits', [-60, 60],...
'YLimits', [-20, 20]);
% create lane plotter
obj.LaneBoundaryPlotter = laneBoundaryPlotter(obj.BEP,...
'DisplayName','Lane boundaries');
% create outline plotter for target actors
obj.OutlinePlotter = outlinePlotter(obj.BEP);
![Page 47: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/47.jpg)
47
Plot safety zones and trajectory with MATLAB
% create patches for safety zones
obj.ZoneFront = patch(hax,0,0,[0 0 0]);
set(obj.ZoneFront,'XData',[],'YData',[],...
'FaceColor','green','FaceAlpha',0.1);
% create line for trajectory path
obj.LCPath = line(hax, 0, 0,...
'Color','blue',...
'LineWidth',2,...
'LineStyle','-');
![Page 48: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/48.jpg)
48
Visualize safety zones and trajectory
![Page 49: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/49.jpg)
49
Case Study for Lane Following plus Lane Change
Design lane following +
lane change controller
Automate
regression testing
Test robustness
with traffic agents
▪ Review baseline LF example
▪ Design sensor configuration
▪ Design additional MIO detectors
▪ Design safety zone calculation
▪ Design lane change logic
▪ Design trajectory planner
▪ Define assessment metrics
▪ Add predefined scenarios
▪ Run Simulink test
▪ Specify driver logic for traffic agents
▪ Randomize scenarios using traffic
agents
▪ Identify and assess unexpected
behavior
![Page 50: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/50.jpg)
50
Manage testing against scenarios
System simulation
Algorithm
under test
Test Manager
Scenarios Sensors Vehicles Metrics
![Page 51: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/51.jpg)
51
Create test scenariosNo Test Name Test Description Host car Lead car Third car
1 01_SlowMoving Passing for slow moving lead car initial velocity = 20m/s
HWT = 6.5sec
(HW = 130m)
v_set = 20m/s
constant velocity =
10m/s
None
HW : Headway
HWT : Headway time
v_set : set velocity for ego car
21
Slow moving
![Page 52: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/52.jpg)
52
Create test scenariosNo Test Name Test Description Host car Lead car Third car
1 01_SlowMoving Passing for slow moving lead car initial velocity = 20m/s
HWT = 6.5sec
(HW = 130m)
v_set = 20m/s
constant velocity =
10m/s
None
HW : Headway
HWT : Headway time
v_set : set velocity for ego car
21
Slow moving
2 02_SlowMoving
WithPassingCar
Passing for slow moving Lead car
With rapidly approaching car in adjacent lane
initial velocity = 20m/s
HWT = 6.5sec
(HW = 130m)
v_set = 20m/s
constant velocity =
10m/s
Constant velocity =
33m/s
21
3
Slow moving
![Page 53: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/53.jpg)
53
Create test scenariosNo Test Name Test Description Host car Lead car Third car Spec
1 01_SlowMoving Passing for slow moving lead car initial velocity = 20m/s
HWT = 6.5sec
(HW = 130m)
v_set = 20m/s
constant velocity = 10m/s None
2 02_SlowMoving
WithPassingCar
Passing for slow moving Lead car
With rapidly approaching car in adjacent lane
initial velocity = 20m/s
HWT = 6.5sec
(HW = 130m)
v_set = 20m/s
constant velocity = 10m/s Constant
velocity =
33m/s
3 03_DisabledCar Passing for disabled lead car initial velocity = 20m/s
HWT = 12sec
(HW = 240m)
v_set = 20m/s
Stationary none
HW : Headway
HWT : Headway time
v_set : set velocity for ego car
21
21
3
21
Slow moving
Slow moving
Disabled
![Page 54: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/54.jpg)
54
Create test scenariosNo Test Name Test Description Host car Lead car Third car Spec
4 04_CutInWithBrake Passing for cut-in car with brake initial velocity = 20m/s
v_set = 20m/s
initial velocity = 18m/s
Cut-in with brake @ 6m/s2
(18m/s→10m/s)
constant
velocity =
10m/s
5 05_SingleLaneChange Single lane change with dense traffic
condition
initial velocity = 15m/s
v_set = 15m/s
Slow moving Dense traffic
6 06_DoubleLaneChange Double lane change with dense traffic
condition
initial velocity = 15m/s
v_set = 15m/s
Slow moving Dense traffic
HW : Headway
HWT : Headway time
v_set : set velocity for ego car
2
1
3
Host car
Host car
![Page 55: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/55.jpg)
55
Create test scenariosNo Test Name Test Description Host car Lead car Third car Spec
7 07_RightLaneChange Passing for slow moving lead car
to right lane
initial velocity = 20m/s
HWT = 6.5sec
(HW = 130m)
v_set = 20m/s
constant velocity = 10m/s Constant
velocity =
25m/s
(3rd & 4th cars in
left lane)
HW : Headway
HWT : Headway time
v_set : set velocity for ego car
21
4
Slow moving
3
![Page 56: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/56.jpg)
56
Add assessments
collision
safe distance against lead car
lateral deviation
![Page 57: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/57.jpg)
57
Review report generated by Test Manager test cases
![Page 58: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/58.jpg)
58
Case Study for Lane Following plus Lane Change
Design lane following +
lane change controller
Automate
regression testing
Test robustness
with traffic agents
▪ Review baseline LF example
▪ Design sensor configuration
▪ Design additional MIO detectors
▪ Design safety zone calculation
▪ Design lane change logic
▪ Design trajectory planner
▪ Define assessment metrics
▪ Add predefined scenarios
▪ Run Simulink test
▪ Specify driver logic for traffic agents
▪ Randomize scenarios using traffic
agents
▪ Identify and assess unexpected
behavior
![Page 59: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/59.jpg)
59
Simulate interaction between driver agents
▪ Graphically define driver
decision logic
▪ Integrate into cuboid driving
scenario
▪ Visualize and debug
Proof of Concept
![Page 60: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/60.jpg)
60
Scenario Reader
Ego car is controlled by the closed-loop
controller including ego vehicle dynamics
Driving scenario is
pre-defined by DSD
Scenario Reader
Block
![Page 61: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/61.jpg)
61
Traffic agent State machine
implementing driver logic
![Page 62: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/62.jpg)
62
Implement driver logic for traffic agent
Lane Keep
mode
Front MIO
detected
Lane change
mode
Maintain
set velocityFCW
detected
Collision
detected
Follow
front MIO
Le
ft L
CR
igh
t L
C
![Page 63: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/63.jpg)
63
Assign traffic agents to all vehicles except ego car
Ego car with
LF/LC controller
Traffic agent with driver
logic state machine
![Page 64: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/64.jpg)
64
Simulate with traffic agents
![Page 65: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/65.jpg)
65
Analyze results for near collision scenario
![Page 66: Case Study: Highway Lane Following + Lane Change€¦ · Case Study: Highway Lane Following + Lane Change Design and test decision making, path planning, and control modules in traffic](https://reader035.vdocuments.us/reader035/viewer/2022062219/60508db52bcac73f3122b9c6/html5/thumbnails/66.jpg)
66
Recap: Case Study for Lane Following plus Lane Change
Design lane following +
lane change controller
Automate
regression testing
Test robustness
with traffic agents
▪ Review baseline LF example
▪ Design sensor configuration
▪ Design additional MIO detectors
▪ Design safety zone calculation
▪ Design lane change logic
▪ Design trajectory planner
▪ Define assessment metrics
▪ Add predefined scenarios
▪ Run Simulink test
▪ Specify driver logic for traffic agents
▪ Randomize scenarios using traffic
agents
▪ Identify and assess unexpected
behavior