自动驾驶算法设计和原型开发 - mathworks · c code create new traffic scenario or refine...

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


Generate code

Integrate

with embedded

environment

Logged

vehicle

data

ADAS

algorithm


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



• Filtering


Data

Pre-processing

Zoning

Path

Estimation

Path

Estimation


Vision

Object

Radar

Object

Vision

LD

Vehicle

CAN

Sensor

Fusion

Kalman

Filter

Sensor Fusion

& Tracking


Threat

Assessment

Risk

Assessment

Maneuver

Analysis

Zoning

Find

MIOFCW

Kalman

Filter

10



• Filtering


Data

Pre-processing

Zoning

Path

Estimation

Path

Estimation


Vision

Object

Radar

Object

Vision

LD

Vehicle

CAN

Sensor

Fusion

Kalman

Filter

Sensor Fusion

& Tracking


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



• Filtering


Data

Pre-processing

Zoning

Path

Estimation

Path

Estimation

Kalman滤波器跟踪目标

Vision

Object

Radar

Object

Vision

LD

Vehicle

CAN

Sensor

Fusion

Kalman

Filter

Sensor Fusion

& Tracking


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



• Filtering


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



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


Synthesized

Data

Data Model


App

(Graphical Interface)






1

Controls state of interface

• Reacts to user input

• Updates data model

• Draws user interface and

visualizations

• Simplifies interaction with

data

2

27


Synthesized

Data

Data Model


App


Unit Test






1





visualizations


dataAutomates regression testing

• Increases likelihood of catching

issues introduced during iterations

• Facilitates collaboration

3

2

28


Synthesized

Data

Data Model


App


Unit Test






1





visualizations


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



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



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环境能够帮助主动安全工程师 …




自动驾驶算法设计和原型开发 - mathworks · c code create new traffic scenario or refine...

Documents