Upload
others
View
17
Download
0
Embed Size (px)
Citation preview
1© 2016 The MathWorks, Inc.
自动驾驶算法设计和原型开发--传感器融合案例
陈 晓挺应用工程师MathWorks 中国
2
•Worst-case
scenarios
•Scenarios identified
from real world test
drive data
我如何知道我的主动安全算法足够健壮?
• OEM specific
test scenarios
• Fail Operation
test scenarios
3
Synthetic
data
主动安全算法开发流程示意
Logged
vehicle
data
ADAS
algorithm
C Code
Create new traffic scenario or refine sensor model
Drive and collect more vehicle data
Refine algorithm
Generate code
Integrate
with embedded
environment
yes
no
Expected
behavior
?
4
MATLAB环境能够帮助主动安全工程师 …
重放 和可视化 车辆记录数据帮助深入探索
合成数据 用于测试算法以减少上路时间
自动生成代码 加速算法原型开发
5
重放 和可视化 车辆记录数据帮助深入探索
Synthetic
data
C Code
Create new traffic scenario or refine sensor model
Generate code
Integrate
with embedded
environment
Logged
vehicle
data
ADAS
algorithm
Drive and collect more vehicle data
Refine algorithm
yes
no
Expected
behavior
?
6
测试车辆安装多种传感器
Delphi ESR
• 76GHz electronically scanning radar
• Dual FoVs, 90x60m, 20x174m
• CAN interface
Mobileye 560
• FoV: 38x150m
• CAN interface
Mobileye
560
Delphi
ESR
Point Grey Blackfly
• Stand “ice cube” vision camera
• 800x600, 27FPS
• GigE interface
XSENS MTI-G-700
• Stable and sensitive
• MEMS-based AHRS
• USB interface
Velodyne LiDAR HDL-32E
• Horizontal FoV: 360
• Vertical FoV: +10..-30
• Range: 80..100m
• 100 Mbps Ethernet
7
在公共道路上的各种测试场景
01_city_c2s_fcw 02_city_stopngo 03_local_streetParking
04_highway_cornering 05_highway_lanechange 06_highway_cutin
8
• Calculate Ground Speed
• Object classification
• Filtering
• Offset Compensation
Zoning
Path
Estimation
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
碰撞预警传感器融合算法
Sensor
Fusion
Kalman
Filter
MIO: Most-Important Object
Risk
Assessment
Maneuver
Analysis
Zoning
Find
MIOFCW
Kalman
Filter
Data
Pre-processing
Path
Estimation
Sensor Fusion
& Tracking
Threat
Assessment
9
• Calculate Ground Speed
• Object classification
• Filtering
• Offset Compensation
Data
Pre-processing
Zoning
Path
Estimation
Path
Estimation
碰撞预警传感器融合算法
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
Sensor
Fusion
Kalman
Filter
Sensor Fusion
& Tracking
MIO: Most-Important Object
Threat
Assessment
Risk
Assessment
Maneuver
Analysis
Zoning
Find
MIOFCW
Kalman
Filter
10
• Calculate Ground Speed
• Object classification
• Filtering
• Offset Compensation
Data
Pre-processing
Zoning
Path
Estimation
Path
Estimation
碰撞预警传感器融合算法
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
Sensor
Fusion
Kalman
Filter
Sensor Fusion
& Tracking
MIO: Most-Important Object
Threat
Assessment
Risk
Assessment
Maneuver
Analysis
Zoning
Find
MIOFCW
Kalman
Filter
11
传感器融合 Sensor Fusion
Birds-Eye View object notations
Radar object (stationary)
Radar object (moving)
Vision object
Fused object (safe zone)
Fused object (warn zone)
Fused object (FCW zone)
Fused object (most important object)
Calculate Ground Speed
Object classification
Filtering
Offset Compensation
Data
Pre-processing
Zoning
Path
Estimation
Path
Estimation
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
Sensor
Fusion
Kalman
Filter
Sensor Fusion
& Tracking
Kalman
Filter
12
传感器融合借助计算机视觉工具箱很容易实现
Vision
Object
Radar
Object
Radar
VisionR1 R2 Rm
V1
V2
Vn
Radar
costMatrix
Assignments
V1 + R2
V2 + R1
Vn + Rm
Fusion
𝑓(𝑉1) + 𝑓(𝑅2)𝑓(𝑉2) + 𝑓(𝑅1)
𝑓(𝑉𝑛) + 𝑓(𝑅𝑚)
Fused Object List
Computer Vision System Toolbox™
[assignments, unassignedVisions, unassignedRadars] = ...assignDetectionsToTracks(costMatrix, param.costOfNonAssignment);
Pairs of visions and
associated radars
13
• Calculate Ground Speed
• Object classification
• Filtering
• Offset Compensation
Data
Pre-processing
Zoning
Path
Estimation
Path
Estimation
Kalman滤波器跟踪目标
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
Sensor
Fusion
Kalman
Filter
Sensor Fusion
& Tracking
MIO: Most-Important Object
Threat
Assessment
Risk
Assessment
Maneuver
Analysis
Zoning
Find
MIOFCW
Kalman
Filter
14
What is the Kalman Filter?
It is an iterative mathematical process that uses a set of equations and
consecutive data inputs to quickly estimate the true value, position, velocity,
etc. of the object being measured, when the measured values contain
random noise.
Estimated temperature
Actual temperature
Measured temperature
15
Kalman Filter
0x̂
0P
Initial state
& covariance
1ˆ
kx
1kP
Previous state
& covariance
Pk
APk1A
TQ
(1) Predict state based on physical model and previous state
(2) Predict error covariance matrix
Time Update (“Predict”)
K k Pk
H
T(HPk
H
TR)
1(1) Compute Kalman gain
(2) Update estimate state with measurement
Measurement Update (“Correct”)
?x k ?x kKk(zk H?x k
)
(3) Update the error covariance matrix
Pk (IK kH) Pk
kkk vHxz
Measurementkx̂
kP
Output of
updated state
1 kk
Current becomes previous
R : Sensor noise covariance matrix (measurement error)
K : Kalman gain
uw
][ TE wwQ
: Control variable matrix
: Process (state) noise
][T
kkk E eeP kkk xxe ˆ
: Process (state)
covariance matrix
(estimation error)
: Process noise
covariance matrix
v
H
: Measurement noise
k minimize P
A : State matrix relates the state at the
previous, k-1 to the state at the current, k
: Output matrix relates the state to the
measurement
kkkk wBuxAx
1ˆˆ
From sensor spec or experiment
16
Kalman Filter Made Easy by MATLAB CVST
0x̂
0P
Initial state
& covariance
1ˆ
kx
1kP
Previous state
& covariance Time Update (“Predict”)
Measurement Update (“Correct”) Current Measurement
Output of
updated state
1 kk
Current becomes previous
[z_pred,x_pred,P_pred] = predict(obj)
z_pred : prediction of measurementx_pred : prediction of stateP_pred : state estimation error covariance
at the next time step
[z_corr,x_corr,P_corr] = correct(obj,z)
z_corr : correction of measurementx_corr : correction of stateP_corr : state estimation error covariance
z
x_corrP_corr
Predicted state
x_pred
kalmanFilterSysObj = vision.KalmanFilter(A,H,'ProcessNoise',Q,'MeasurementNoise',R)
Computer Vision System Toolbox™
17
FCW
• Calculate Ground Speed
• Object classification
• Filtering
• Offset Compensation
Data
Pre-processing
Zoning
Path
Estimation
Path
Estimation
碰撞预警传感器融合算法
Vision
Object
Radar
Object
Vision
LD
Vehicle
CAN
Sensor
Fusion
Kalman
Filter
Sensor Fusion
& Tracking
Risk
Assessment
Maneuver
Analysis
Zoning
Find
MIO
Threat
Assessment
Kalman
Filter
18
Synthetic
data
合成数据 用于测试算法以减少上路时间
Logged
vehicle
data
ADAS
algorithm
C Code
Create new traffic scenario or refine sensor model
Drive and collect more vehicle data
Refine algorithm
Generate code
Integrate
with embedded
environment
yes
no
Expected
behavior
?
19
测试场景合成示例 – EuroNCAP
ENCAP – CCRs ENCAP – CCRm ENCAP – CCRb
stationary
50kph
50kph
20kph
50kph
50kph
@ -2m/s2
50kph
50kph
@ -6m/s2
ENCAP – CCRb
40m
12m
Ego car
Target car
20
测试场景合成示例 – 故障操作
FO1 – cornering FO2 – overtaking FO4 – lane change
60kph
30kph130kph
90kph
stationary
50kph
Ego car
Target car
21
测试场景合成示例 – 十字路口 红绿灯
Intersection – turning w/ braking Intersection – near crash Intersection – U-turn Stop & go @ 3020kph
50kph
Ego car
Target car
22
如何获得合成的测试数据
“Buy It” Buying “Off the shelf” solutions
like PreScan and CarMaker
enable you to author traffic
scenarios and synthesize sensor
data
“Build It” Building it yourself enables
you to control the level of
fidelity/complexity appropriate
for your application
If building for multiple users, it
is important to select an
approach which is scalable,
maintainable, and testable
23
定义交通场景的关键组件 Scenario
Define Road• Road type
• NumLanes
• LaneWidth
Define Vehicles• Ego vehicle
• Target vehicles
• NumTargetsDefine Trajectories
• Path type
• Waypoints
• Velocity, acceleration
• Heading angle, yawrate
Sensor Model• Vision, Radar
• Range, FoV
• Position, Velocity error
• NumCluster
24
雷达技术规范例子
Reference) Stanislas, Leo & Peynot, Thierry, “Characterisation of the Delphi Electronically Scanning Radar for robotics applications”. In Australasian Conference
on Robotics and Automation (ACRA 2015), 2-4 December 2015, Canberra, A.C.T.
25
Architecture we selected to build a data synthesis tool
Synthesized
Data
Data Model
(Programmatic Interface)
Controls state of data
• Specifies scenario (i.e. road,
vehicles, waypoints, sensor models)
• Synthesizes data to test algorithm
• Enables automation
1
26
Architecture we selected to build a data synthesis tool
Synthesized
Data
Data Model
(Programmatic Interface)
App
(Graphical Interface)
Controls state of data
• Specifies scenario (i.e. road,
vehicles, waypoints, sensor models)
• Synthesizes data to test algorithm
• Enables automation
1
Controls state of interface
• Reacts to user input
• Updates data model
• Draws user interface and
visualizations
• Simplifies interaction with
data
2
27
Architecture we selected to build a data synthesis tool
Synthesized
Data
Data Model
(Programmatic Interface)
App
(Graphical Interface)
Unit Test
Controls state of data
• Specifies scenario (i.e. road,
vehicles, waypoints, sensor models)
• Synthesizes data to test algorithm
• Enables automation
1
Controls state of interface
• Reacts to user input
• Updates data model
• Draws user interface and
visualizations
• Simplifies interaction with
dataAutomates regression testing
• Increases likelihood of catching
issues introduced during iterations
• Facilitates collaboration
3
2
28
Architecture we selected to build a data synthesis tool
Synthesized
Data
Data Model
(Programmatic Interface)
App
(Graphical Interface)
Unit Test
Controls state of data
• Specifies scenario (i.e. road,
vehicles, waypoints, sensor models)
• Synthesizes data to test algorithm
• Enables automation
1
Controls state of interface
• Reacts to user input
• Updates data model
• Draws user interface and
visualizations
• Simplifies interaction with
dataAutomates regression testing
• Increases likelihood of catching
issues introduced during iterations
• Facilitates collaboration
3
2
29
Synthetic
data
自动生成代码 加速算法原型开发
Logged
vehicle
data
ADAS
algorithm
C Code
Create new traffic scenario or refine sensor model
Drive and collect more vehicle data
Refine algorithm
Generate code
Integrate
with embedded
environment
yes
no
Expected
behavior
?
30
用MATLAB Coder从您的算法自动生成C代码
Software in the loop &
Processor in the loop
enabled by
Embedded Coder
MATLAB
C
31
在Simulink中重用MATLAB编写的算法组件
32
模型级和C代码级覆盖率分析利用Simulink Verification and Validation
Collects coverage on MATLAB code
in “Normal” mode
Collects coverage on generated C
code in “Software in the loop” mode
33
自动化回归测试借助Simulink Test
Specify tests and Interact with results
in the Test Manager
Generate a report to share results
34
硬件快速原型利用Simulink Real-Time
Algorithm Models
Vehicle and
Environment
Models
Forward
Collision
Warning
Autonomous
Emergency
Braking
35
Synthetic
data
主动安全算法开发流程示意
Logged
vehicle
data
ADAS
algorithm
C Code
Create new traffic scenario or refine sensor model
Drive and collect more vehicle data
Refine algorithm
Generate code
Integrate
with embedded
environment
yes
no
Expected
behavior
?
36
该流程涉及到了哪些产品的关键特性?
设计Kalman滤波器 with Computer Vision System Toolbox
封装数据 with MATLAB Classes
测试MATLAB代码 with MATLAB Unit Test
自动生成C代码 from your algorithm with MATLAB Coder
验证生成的代码 using SIL with Embedded Coder
收集模型和代码覆盖率 with Simulink Verification and Validation
自动化回归测试 and reporting with Simulink Test
37
如何开始您的工作?N
um
ber
of
Us
ers
Complexity
Documentation and
Training Services can
help you learn how to use
product features
Consulting Services can help
you select and establish a
baseline architecture to meet the
needs of your application
Consulting helped us
select an architecture for
our Traffic Scenario App
38
MATLAB环境能够帮助主动安全工程师 …
重放 和可视化 车辆记录数据帮助深入探索
合成数据 用于测试算法以减少上路时间
自动生成代码 加速算法原型开发