March 2017

Masterclass localization

AUTONOMOUS VEHICLES POWERFUL DRIVER

NAVIGATION LOCALIZATION + MAPS

WHO NEEDS LOCALIZATION?

People

Automated guided vehicles

Trains

Boats

Drones

Cars

Robots

LOCALIZATION ACCURACY

m dmcm

LOCALIZATION

Localization

Absolute

LOCALIZATION TECHNOLOGIES OVERVIEW – ABSOLUTE LOCALIZATION

x, y

y

x

x1, y1

x2, y2

x3, y3 x4, y4

LOCALIZATION TECHNOLOGIES OVERVIEW – ABSOLUTE LOCALIZATION

50.8798° N, 4.7005° E

ABSOLUTE LOCALIZATION TECHNOLOGIES

GPS

Decawave

Visual beacons or landmarks

DECAWAVE – LOCALIZATION SOLUTION FOR AREAS WITH NO GPS

Tag Anchor

poll

response

final

report

Master

POSSIBLE LOCALIZATION SYSTEM ARCHITECTURE

4x ToA 3D“multi-lateration”

Synchronization

Configuration& calibration

Tag

Application

(x1,y1,z1,Δt1) (x2,y2,z2,Δt2)

(x3,y3,z3,Δt3) (x4,y4,z4,Δt4)

Anchor 2Anchor 1

Anchor 4Anchor 3

Configuration

Criteria Measurements

Update rate Ranging 60 Hz

3D localization 15 Hz

Ranging error Max systematic Δμ 11.8 cm

Max std dev σ 2.0 cm

2D error Max systematic Δμ 21.2 cm

Max std dev σ 6.3 cm

DECAWAVE PERFORMANCE EVALUATION

HIGH ACCURACY, HIGH AVAILABILITY LOCALIZATION SYSTEMS EXIST! BUT…

HIGH ACCURACY, HIGH AVAILABILITY LOCALIZATION SYSTEMS ARE A COMBINATION OF:

High accuracy GPS

High accuracy inertial navigation systems

IMPRESSIVE PERFORMANCE:

Global localization accuracy: 0.02m ☺

Update rate: 200Hz ☺

High availability in absence of satellite signal ☺

IMPRESSIVE PRICE: 20-30KEUR

LOCALIZATION BASED ON ACTIVE LIGHT BEACONS

Light beacons are normally off

When the vehicle approaches it signals the nearest pair of beacons to turn on

Light beacons can be made to flash brighter than most of the background objects allows easy background subtraction

HOW DOES LOCALIZATION BASED ON LIGHT BEACONS WORK?

TRILATERATION LOCALIZATION USING DISTANCES BETWEEN THE BEACON AND THE VEHICLE

The distance is estimated using the apparent beacon size

The beacon real size and locations are known

TRIANGULATION LOCALIZATION USING ANGLES WITH RESPECT TO TWO VISIBLE BEACONS OF KNOWN LOCATION

FROM LIGHT BEACONS TO VISUAL LANDMARKS

The concept of light beacons can be extended to using also other visual landmarks with a known location (e.g. buildings, traffic signs)

The vehicle can position itself along the trajectory by using a combination of light beacons and visual landmarks

Fixed known coordinates:

Light beacon

Visual landmark

y

x

VISUAL LANDMARKS

LOCALIZATION

Localization

Absolute Relative

• Limited update rate• Limited accuracy

LOCALIZATION TECHNOLOGIES OVERVIEW – RELATIVE LOCALIZATION

∆x, ∆y

Initial point

∆x1, ∆y1

∆x2, ∆y2 ∆x3, ∆y3

LOCALIZATION TECHNOLOGIES OVERVIEW – RELATIVE LOCALIZATION

5.8798° ∆N, 4.7005° ∆E

RELATIVE LOCALIZATION TECHNOLOGIES

Inertial navigation

Radar

Lidar

Laser scanners

Sonar

Visual odometry

Ground facing camera

INERTIAL NAVIGATION

Acceleration Speed Distance

RegionCurrentimage

RegionPreviousimage

DFT

DFT

Phase correlatio

nIFT

Peak finding

DFT = Discrete Fourier TransformIFT = Inverse (discrete) Fourier Transform

Vehicle

Camera(s)

Active Lighting

Road Surface

Translation (𝝏𝒙, 𝝏𝒚)

GROUND FACING CAMERA - HOW DOES IT WORK ?

Translation (𝝏𝒙, 𝝏𝒚)

1 x measure

Translation (𝝏𝒙, 𝝏𝒚)

2 x measure

Translation (𝝏𝒙, 𝝏𝒚)

n x measure

. . . . . .

. . .

. . .

. . .

. . .

R1 R1 R2

R(1,C)

. . .

GROUND FACING CAMERA - MULTI REGIONS

LIDAR WORKING PRINCIPLE

RADAR WORKING PRINCIPLE

Calibration Egomotion

Search ImageFeatures

Features Matching

BucketingCalculate Motion

Get matchesPose

Estimation

StereoVisual Odometry

VISUAL ODOMETRY

(Synchronize) stereo images

Image Preprocessing

LOCALIZATION

Localization

Absolute Relative

• Limited update rate• Limited accuracy

• High update rate• High accuracy• Drifts in time

LOCALIZATION

Localization

Absolute Relative

• Limited update rate• Limited accuracy

• High update rate• High accuracy• Drifts in time

Sensor fusion

SENSOR FUSION IMPROVES LOCALIZATION SYSTEM PERFORMANCE

SEVERAL LOCALIZATION SYSTEMS CAN BE COMBINED THROUGH SENSOR FUSION TECHNIQUES TO LEVERAGE ON THEIR MUTUAL STRENGTHS AND OPTIMIZE THE PRICE/PERFORMANCE RATIO:

GPSDecawave

Visual beacons or landmarks

Inertial navigationRadarLidar

Laser scannersSonar

Visual odometryGround facing camera

SENSOR FUSION

48 48.5 49 49.5 50 50.5 51

17.6

17.7

17.8

17.9

18

18.1

18.2

18.3

18.4

vSBG-RTK

vppp

vcar

vGPS+IMU

SHUTTLE TRAJECTORY ESTIMATION

SHUTTLE TRAJECTORY ESTIMATION: SIMPLIFIED MODEL

Physical model describes shuttlecock trajectories.

Error on end-position in x, y and z direction of around 5 cm after 20 measurement points.

TRAJECTORY TRACKING WITH MULTIPLE KALMAN FILTERS

time

Measurement

Kalman filter estimate

Two meas.OutlierGreen KF started

No meas.

Green KF is killed

…

Orange KF started Red KF still alive Red KF is killed

New trajectory

z-coordinate

SHUTTLE TRAJECTORY ESTIMATIONMeasured pointsEstimated points

Outliers

Missing values

ACCURATE, LOW-COST LOCALIZATION SOLUTION THROUGH SENSOR FUSION

USE A COMBINATION OF LOCALIZATION SYSTEM BASED ON FIXED LIGHT BEACONS AND AN INERTIAL NAVIGATION SYSTEM

THE FIXED LOCALIZATION SYSTEM USES

active light beacon pairs placed at know positions along the vehicle trajectory

provide absolute localization at low update rate

THE INERTIAL NAVIGATION SYSTEM

relative localization between the light beacon pairs

increased update rate

y

x

Light beacon

?

Orientation

Speed

Position

Yaw EKF

Linear accelerations

BikeEKF

Vehicle PoseEKF

Kinematic and dynamic vehicle models

Angular accelerationsInertial

navigationsystem

Light beacons

POSITION ESTIMATION BASED ON SENSOR FUSION

Position of the vehicle

ADVANCED KINEMATIC MODELS IMPROVE SENSOR FUSION ACCURACY

Vehicle heading and course are not always equal the side slip angle (β) is not zero

Correct estimation of the side slip angle is essential for good localization accuracy

SIDE SLIP ANGLE CAN BE ESTIMATED USING

a linear tire model

a non-linear tire model average localization accuracy improvement of 20% using only the inertial navigation system

DYNAMIC VEHICLE MODELS CAN IMPROVE THESE RESULTS EVEN FURTHER

KINEMATIC AND DYNAMIC VEHICLE MODELS PLAY AN ESSENTIAL ROLE

TO ACHIEVE HIGH ACCURACY LOCALIZATION SENSOR FUSION NEEDS ADVANCED KINEMATIC AND DYNAMIC MODELS OF THE VEHICLE

The use of a more adequate tire model leads to an improvement in localization accuracy of 20%

EACH VEHICLE TYPE NEEDS AN APPROPRIATE MODEL TO ACHIEVE GOOD RESULTS

FLANDERS MAKE HAS THE NECESSARY EXPERTISE TO DEVELOP AND REFINE SUCH MODELS FOR EACH TYPE OF VEHICLE:

Train models

Boat models

Automated guided vehicle models

Off Highway Vehicle

42