Distributed Intelligent Systems – W4

An Introduction to
Localization Methods for
Mobile Robots
 Outline

• Localization for mobile robots

– Positioning systems
– Kinematic models
– Odometry (with and w/o noise)
• Localization uncertainties and
navigation
– Error sources
– Methods
M th d ffor hhandling
dli uncertainties
t i ti
– Odometry-based and feature-based
navigation methods
 Robot Localization
• Key task for:
• Path planning
?
• Mapping
• Referencing
• Coordination
di i
N 46° 31’ 13’’
E 6 ° 34’ 04’’
• Type of localization
• Absolute coordinates
• Local coordinates
• Topological information
Positioning Systems
 Classification axes
• Indoor vs. outdoor techniques
• Absolute vs.
vs relative positioning systems
• Line-of-sight vs. obstacle passing/surrounding
• U d l i physical
Underlying h i l principle
i i l andd channel
h l
• Positioning available on-board vs. off-board
• Scalability in terms of number of nodes
 Selected Indoor Positioning Systems
• Laser-based indoor GPS
• Ultrasound ((US)) + radio frequency
q y ((RF)) technology
gy
• Infrared (IR) + RF technology
• Vision-based overhead system
y
• Impulse Radio Ultra Wide Band (IR-UWB)
 Laser-Based Indoor (KPS)
( )

• Performance: a few mm in position over 5x5 m arena,

arena
25-50 Hz, a few degrees in orientation
• Position available on the robot without com (GPS-like)
( )
• Line-of-Sight (LOS) method
• Tested in 2D but extendable in 3D (2 laser base stations)
 Overhead (Multi-)Camera Systems
• Tracking objects with one
(or more) overhead
cameras
• Absolute positions,
available outside the
robot/sensor
b t/
• Active, passive,
or no markers
• Open source software
• Major issues: Accuracy ~ 1 cm (2D)
li ht calibration
light, lib ti
Update rate ~ 20 Hz
• E.g. open-source software
SwisTrack
Sw s ac (deve
(developed
oped at # agents
g ~ 100
DISAL)
Area ~ 10 m2
Overhead Camera System
Ultrasound + Radio Technology

[From Introduction to Autonomous Mobile

Robots, Siegwart R. and Nourbakhsh I. R.]
 Ultrasound + Radio Technology
• Principle: time of arrival on 3 (2D) or 4 (3D) US
receptors, synchronization with radio signal
• Used for relative (on the robots) and absolute
positioning
i i i (fixed
(fi d beacons)
b )
• Accuracy: sub cm accuracy over several m for a 30 cm
radius platform (e.g. Michaud et al, ICRA 2008)
• Accuracy inversely proportional with size of the
module (proportional to distance between US receptors)
• Updating
U d i speed: d 1/(0.075*N_robots)
1/(0 075*N b ) Hz H ((e.g., < 1 Hz
H
with 14 or more robots) (Michaud et al, ICRA 2008)
• Better than LOS but obstacle influence sound
propagation
 Infrared + Radio Technology
gy
• Principle:
– belt of IR emitters (LED) and receivers
(photodiode)
– IR LED used as antennas; modulated
g ((carrier 10.7 MHz),
light ), RF chip
p behind
– Range: measurement of the Received
Signal Strength Intensity (RSSI)
– Bearing: signal correlation over multiple
receivers
– Measure range & bearing can be coupled
with standard RF channel (e.g. 802.11)
for heading assessment
– Can also be used for 20 kbit/s IR com [Pugh et al., IEEE Trans. on
channel Mechatronics, 2009]
– Robot ID communicated with the IR
channel (ad hoc protocol)
 Infrared + Radio Technology
gy

Performance
P f summary:
• Range: 3.5 m
• Update frequency 25 Hz with 10 neighboring robots (or
250 Hz with 2)
• Accuracyy range:
g <7% (MAX),
( ) generally
g y decrease 1/d
• Accuracy bearing: < 9º (RMS)
• LOS method
• Possible
P ibl extension i in
i 3D,
3D larger
l range (but
(b more power))
and better bearing accuracy with more photodiodes (e.g.
Bergbreiter PhD UCB 2008,
Bergbreiter, 2008 dedicated asic,
asic up to 15 m,
m
256 photodiodes, single emitter with conic lense)
Range & Bearing IR+RF System
 IR-UWB System - Principles
• Based on time-of-flight (TDOA)
• UWB tags (emitters, a few cm, low-
power) and multiple synchronized
receivers
• Emitters can be unsynchronized but then
dealingg with interferences not trivial
(e.g., Ubisense system synchronized)
• Absolute positions available on the
receiving system
• Positioning information can be fed back
to robots using a standard narrow-bad
channel
• 6 - 8 GHz central frequency
• Very large bandwidth (>0.5GHz)
→ high material penetrability
• Fine time resolution
→ high theoretical ranging accuracy
(order of cm)
 IR-UWB
IR UWB System – Shortcomings
Accuracy 15 cm (3D)
Ex. State-of-art system Update rate 34 Hz / tag
((e.g.,
g Ubisense 7000
# agents ~ 10000
Series, Compact Tag)
Area ~ 1000 m2

• Degraded accuracy performance if

– Inter-emitter interferences
– Non-Line-of-Sight (NLOS) bias → Often onlyy
– Multi-path <1 m accuracy on
smaller areas
IR-UWB
IR UWB System – Possible Fixes
– Inter-emitter interferences
→ robust algorithms at the receiver side;
emitter synchronization
– Non-Line-of-Sight (NLOS) bias
→ bias modeling, inter-robot collaborative
algorithms
– Multi-path
→ robust algorithms at the receiver side,
inter-robot collaborative algorithms
 Selected Outdoor Positioning
Techniques
• GPS
• Differential GPS (dGPS)
Global Positioning System
 Global Positioning
g System
y
• 24 satellites (including three spares) orbiting the earth every 12 hours at a height
of 20.190 km.
• Satellites synchronize
s nchroni e their transmission (location + time stamp) so that signals
are broadcasted at the same time (ground stations updating + atomic clocks on
satellites)
• Location of any GPS receiver is determined through a time of flight measurement
( accuracy!))
(ns
• Real time update of the exact location of the satellites:
- monitoring the satellites from a number of widely distributed ground stations
- a master station analyses all the measurements and transmits the actual position to
each of the satellites
• Exact measurement of the time of flight
– the receiver correlates a pseudocode with the same code coming from the satellite
– Th delay
The d l timei forf best
b correlation
l i represents theh time
i off flight.
fli h
– quartz clock on the GPS receivers are not very precise
– the range measurement with (at least) four satellites allows to identify the three values
(x, y, z) for the position and the clock correction ΔT
• Recent commercial GPS receiver devices allows position accuracies down to a
couple meters.
• 200-300 ms latency, so max 5 Hz GPS updates
 dGPS
Positionaccuracy: typically from a few to a few tens of cm
Position
Robot
R b t Localization
L li ti using
i
On-Board Sensors
 Sensors for localization
• Proprioceptive sensors:
• Epuck:
• 3D accelerometer
• Motor step counter
• Others:
• Wheel encoder
• Odometer
• Exteroceptive sensors:
• Epuck:
• IR range proximity
i i sensor
• Camera
• Others:
• Laser range finder
• Ultrasonic range finder
 Odometry: Idea and Motivation
• Positioning (and orientation) for a mobile robot is key
• Q:
Q can we track k the
h absolute
b l position
i i andd orientation
i i
(global/environmental reference frame) based on
movement information exclusively measured by on on-
board proprioceptive information?
• A: yes, using odometry! (and knowledge of initial
position and orientation)
• Needed: propioceptive movement sensors such as
– DC motors
t + encodersd (closed-loop
( l dl control)
t l)
– motor step counters (open-loop control of stepper motors but
pre-established fixed increment per pulse, as on e-puck)
– accelerometers (e-puck has a 3D one on board)
Encoder Based Odometry
Encoder-Based
for Differential-Wheel
Vehicles
 Optical Encoders
• Measure displacement (or speed) of the wheels
• Principle: mechanical light chopper consisting of photo-barriers (pair of light
emitter
i andd optical
i l receiver)
i ) + pattern on a disc
di anchored
h d to theh motor shaft
h f
• Quadrature encoder: 90º placement of 2 complete photo-barriers, 4x increase
resolution + direction of movement
• Integrate wheel movements to get an estimate of the position ->
> odometry
• Typical resolutions: 64 - 2048 increments per revolution.
• For high resolution: interpolation
 Pose (Position and Orientation)
off a Differential-Drive
Diff i lDi R Robot
b
YI ⎡ xI ⎤ ⎡ xR ⎤
YR ⎢ ⎥ ⎢ yR ⎥ = R (θ )ξ
XR
ξ =
ξ I = ⎢ yI ⎥ R ⎢ ⎥ I

⎢⎣θ ⎥⎦ ⎢⎣ θ ⎥⎦
θ
P ⎡ cos θ i θ
sin 0⎤
R(θ ) = ⎢⎢− sin θ cos θ 0⎥⎥
yR
yI ⎢⎣ 0 0 1⎥⎦
xR
From Introduction to
Autonomous Mobile
XI
xI Robots, Siegwart R. and
Nourbakhsh I. R.
 Absolute and Relative Pose of a
Differential-Drive Robot
YI
XR
ξ&R = R(θ )ξ&I
Ex θ =π/2
Ex.
θ
P ⎡ x& R ⎤ ⎡ 0 1 0⎤ ⎡ x& I ⎤ ⎡ y& I ⎤
YR ⎢ y& ⎥ = ⎢− 1 0 0⎥ ⎢ y& ⎥ = ⎢− x& ⎥
⎢ R⎥ ⎢ ⎥⎢ I ⎥ ⎢ I ⎥
⎢⎣θ&R ⎥⎦ ⎢⎣ 0 0 1⎥⎦ ⎢⎣θ&I ⎥⎦ ⎢⎣ θ&I ⎥⎦

XI
 Forward Kinematic Model
How does the robot move given the wheel speeds and geometry?
• Assumption: no wheel slip (rolling mode only)!
• In miniature robots no major
j dynamic
y effects due to low mass-
to-power ratio
⎡ x& ⎤
YI v(t) ξ&I = ⎢⎢ y& ⎥⎥ = f (l , r , θ , ϕ&1 , ϕ& 2 )
2 ⎢⎣θ& ⎥⎦

ω(t)
θ
ϕ&i = wheel i speed
1
l r (wheel radius)

XI
 Recap ME/PHY Fundamentals

P v = ωr = ϕ&r
v v = tangential speed
φ ω = rotational speed
r r = rotation radius
ω
Wheel φ = rotation
t ti anglel
Motion C
r ω C = rotation center
P = peripheral point
t-δt t P’= contact point at time t
v PP’
Rolling!
 Forward Kinematic Model
Linear speed = average YI YR XR
wheel speed 1 and 2:
rϕ&1 rϕ& 2 2 v
v= + ω θ
2 2 P 1

Rotational speed = l r
sum of rotation speeds
(wheel 1 clockwise, XI
wheel 2 counter-
clockwise): Idea: linear superposition
rϕ&1 − rϕ& 2 of individual wheel
ω= + contributions
ib i
2l 2l
 Forward Kinematic Model
1. ξ I = R (θ )ξ&R
& XR
−1
YI YR
rϕ&1 rϕ& 2 v
2. x& R = v = + 2
2 2 ω P
θ
1
3. y& R = 0 l r
& rϕ&1 − rϕ& 2
4. θ R = ω =
4 +
2l 2l XI
⎡ rϕ&1 rϕ& 2 ⎤
⎡cos θ − sin θ 0⎤ ⎢ + ⎥
2 2
ξ&I = ⎢⎢ sin θ cos θ 0⎥⎥ ⎢ 0 ⎥
⎢ rϕ& − rϕ& ⎥
⎣⎢ 0 0 1⎥⎦ ⎢ 1 + ⎥
2

⎢⎣ 2l 2l ⎥⎦
 Odometry
• Q: given our absolute pose over time, how can we calculate
the robot pose after some time t?
• A: integrate!
• Given the kinematic forward model, and assuming no slip on
both wheels
heels T T
ξ I (T ) = ξ I 0 + ∫ ξ I dt = ξ I 0 + ∫ R (θ )ξ&R dt
& −1

0 0
• Given an initial pose ξI0, after time T, the pose of the vehicle
will be ξI(T)
• ξI(T) computable with wheel speed 1, wheel speed 2, and
parameters r and l
 Real world odometry examples
• Human in the dark
• Very bad odometry sensors
• dOdometry= O(m)
• (Nuclear) Submarine
• Very good odometry sensors Courtesy of US Navy

• dOdometry= O(103 km)

• Navigation system in tunnel
uses dead reckoning based on
• Last velocity as measured by GPS
• Car’s odometer, compass

Courtesy of NavNGo
Encoder-Based
E d B d Odometry
Od t
with Uncertainities
 Odometric Error Sources
• Focus on encoder-based odometry
• Deterministic sources of error
• Limited encoder resolution
• Wheel misalignment and small differences in wheel
diameter
¾ Can be fixed by calibration
• Nondeterministic sources of error
• Variation of the contact point of the wheel
• Unequal floor contact (e.g., wheel slip, nonplanar surface)
¾ Wheels cannot be assumed to perfect roll
¾ Measured encoder values do not perfectly reflect the actual
motion
¾ Pose error is cumulative and incrementally increases
Odometry
 Odometric Error Types
• Range error: sum of the wheel movements

• Turn error: difference of wheel motion

• Drift error: difference between wheel errors

lead to heading error
Qualitatively: turn and drift errors dominant
Quantitatively: error model!
 Pose Variation during Δt
Δ sr + Δ sl
Δs =
2
Δθ
Δ x = Δ s cos( θ + )
2
Δθ
Δ y = Δ s sin( θ + )
2
Δ s r − Δ sl
Δθ = b = 2l = inter-wheel distance
b
Δsr = traveled distance right wheel
⎡x⎤ ⎡ x′⎤ Δsl = traveled distance left wheel
p = ⎢⎢ y ⎥⎥ ⎯ t⎯′= t + Δ t
⎯→ p′ = ⎢ y ′⎥
⎢ ⎥
⎢⎣θ ⎥⎦ ⎢⎣θ ′ ⎥⎦
⎡ Δ sr + Δ sl Δ s r − Δ sl ⎤
⎢ cos( θ + )⎥
⎡ x ⎤ ⎡ Δ s cos( θ + Δ θ / 2 ) ⎤ ⎡ x ⎤ ⎢ 2 2b
Δsr + Δ sl Δ sr − Δ sl ⎥
p ′ = ⎢⎢ y ⎥⎥ + ⎢⎢ Δ s sin( θ + Δ θ / 2 ) ⎥⎥ = ⎢⎢ y ⎥⎥ + ⎢ sin( θ + )⎥
⎢ 2 2b ⎥
⎢⎣θ ⎥⎦ ⎢⎣ Δθ ⎥⎦ ⎢⎣θ ⎥⎦ ⎢ Δ sr − Δ sl ⎥
⎢⎣ b ⎥⎦
 Actuator Noise → Pose Noise
Assumptions:
• Covariance
i i Σp at the
matrix h bbeginning
i i is i known
k
• Errors of the two individual wheels are independent
• Errors
E are independent
i d d t off direction
di ti off motion
ti
• Errors are proportional to the distance traveled (kr, kl model
parameters)

⎡k r Δsr 0 ⎤ ⎡σ s2r 0 ⎤
∑ Δ
= cov( Δ s r , Δ s l ) = ⎢ ⎥ = ⎢
k l Δ s l ⎦ ⎢⎣ 0
⎥
σ s2l ⎥⎦
⎣ 0
 Actuator Noise → Pose Noise
• How is the actuator noise (2D) propagated to the pose (3D)?
σ x2 ⎡ σ xx2 σ xy2 σ x2θ ⎤
⎡σ 2
0 ⎤ σ 2

? ⎢ 2 ⎥
sr sr
ΣΔ = ⎢ ⎥ σ y2 ⎢σ yx σ yy2 σ y2θ ⎥ = Σp
⎢⎣ 0 σ s2l ⎥⎦ σ s2
σ θ2 ⎢ 2 ⎥
⎣σ θx σ θ2y σ θθ2
l

⎦
• 1D to 1D example N ( μ s , σ s ) → N ( μ x , σ x )
r r

f (sr )
μx +σ x

μx
μx −σ x

μs − σ s μs μs + σ s
r r r r r
sr
• We need to linearize → Taylor Series
1 ∂f 1 ∂2 f
x ≈ f (sr ) s r = μ sr
≈ f (sr ) + ( s r − μ sr ) + ( s r − μ sr ) 2 + K
1! ∂ s r 2! ∂ s r 2
 Actuator Noise → Pose Noise
∂ f1 xy
x ≈ f1 ( s r ) s = μ ≈ f1 ( s r ) + ( s r − μ sr )
r sr
∂sr
∂f
x ≈ f1 ( s l ) s = μ ≈ f1 ( s l ) + 1 ( s l − μ sl ) μ + σ ff21 ( s rl )
l sl
∂sl xy xy

∂f μ xy
y ≈ f 2 ( s r ) s = μ ≈ f 2 ( s r ) + 2 ( s r − μ sr )
r sr
∂sr μ xy − σ xy

⎡ ∂ f1 ∂ f1 ⎤
⎢ ⎥
⎢ ∂sr ∂sl ⎥
∂f ∂f 2
F Δ rl = ⎢ 2 ⎥ μ ss − σ s μμss μ ss + σ s s rl
⎢ ∂sr ⎥
lr

∂sl
rrl rlr r rrl rlr

⎢ ∂f ∂f3 ⎥
⎢ 3 ⎥
⎣⎢ ∂ s r ∂sl ⎥⎦ Jacobian

• General error propagation law

Σ Δrl = FΔrl Σ Δ F T
Δrl
 Actuator Noise → Pose Noise
How does the state covariance Σ p evolve over time?
• Initial covariance of vehicle at t=0:
⎡ σ xx2 σ xy2 σ x2θ ⎤ ⎡0 0 0⎤
⎢ ⎥ ⎢
Σ (pt = 0 ) = ⎢σ yx2 σ yy2 σ y2θ ⎥ = ⎢0 0 0 ⎥⎥
⎢ σ θ2x σ θ2y σ θθ2 ⎥ ⎢⎣ 0 0 0 ⎥⎦
⎣ ⎦
• Additional noise at each time step Δt: Σ Δrl = FΔrl Σ Δ FΔTrl
• Covariance at t=1: Σ (pt =1) = Σ (pt =0) + Σ Δrl = Σ Δrl
• Covariance at t=2: ⎡ ∂f
⎢
∂f ∂f ⎤
⎥
1 1 1

⎢ ∂x ∂y ∂θ ⎥
(t =2) ( t =1)
Σ p = Fp Σ p F + FΔrl Σ Δ F
T
p
T
Δrl
⎢ ∂f
Fp = ⎢ 2
∂f 2 ∂f 2 ⎥
⎥
⎢ ∂x ∂y ∂θ ⎥
⎢ ∂f3 ∂f3 ∂f3 ⎥
⎢∂ ∂y ∂θ ⎥
⎣ x ⎦
 Actuator Noise → Pose Noise
Recipe
Precompute:
• Determine actuator noise Σ Δrl
• Compute mapping actuator-to-pose noise incremental FΔrll
• Compute mapping actuator-to-pose noise absolute Fp
Initialize:
• Initialize Σ (pt =0 ) = [0]
Iterate:
Σ (pt = 2 ) = Fp Σ (pt =1) FpT + FΔrl Σ Δrl FΔTrl
Accelerometer-Based
Accelerometer Based
Odometry with
Uncertainities
Accelerometer based odometry
 Error modeling
• Error happens!
• Odometryy error is cumulative.
→ grows without bound
• We need to be aware of it.
→ We need to model odometry error.
→ We need to model sensor error.
• Acceleration is random variable A
drawn from “mean-free” Gaussian
(“Normal”) distribution
distribution.
Why Gaussian ?
→ Position X is random variable with
Gaussian distribution.
Accelerometer based odometry
Accelerometer based odometry 2D
Accelerometer based odometry 2D
 Classical 2D representation

Courtesy
y of R. Siegwart
g and R. Nourbakhsh

Ellipses: typical 3σ bounds

Robot Localization based
on Propioceptive and
Exteroceptive Sensors
 Features
• Odometry based position error grows without bound.
• Use relative measurement to features (“landmarks”)
( )
to reduce position uncertainty
• Feature:
• Uniquely identifiable
• Position is known
• We can obtain relative measurements between robot and
feature (usually angle or range).
• Examples:
• Doors, walls, corners, hand rails
• Buildings, trees, lanes
Courtesy of Albert Huang
• GPS satellites
 Automatic feature extraction
• High level features:
• Doors,, ppersons

omplexity
y
• Simple visual features:
• Edges
g (Canny
( y Edge
g Detector 1983))
• Corner (Harris Corner Detector 1988)
• Scale Invariant Feature Transformation (2004)

Co
• Simple geometric features
• Lines
• Corners
• “Binary”
y feature
Courtesy of R. Siegwart and R. Nourbakhsh
Feature-based navigation
 Sensor fusion
• Given:
• Position estimate X ← N(μ=5;σ=1)
• Range estimate R ← N(μ=3.2;σ=1.2)
What is the best estimate AFTER
incorporating r ?
→ Kalman Filter
R ← N(μ=3.2;σ=1.2)
(μ=3 2;σ=1 2)
• Requires:
• Gaussian noise distribution X ← N(μ=5;σ=1) Kalman
Filter
for all measurements
X ← N(μ=5.5;σ=0.6)
• Linear motion and measurement
model
d l
cs.unc.edu/~welch/media/pdf/maybeck_ch1.pdf
• …
Feature-based navigation
 Feature-based navigation
Belief representation trough Gaussian distribution
g :
• Advantages
• Compact (only mean and variance required)
• Continuous
• Powerful tools (Kalman Filter)
• Disadvantages:
• Requires Gaussian noise assumption
• Uni-modal
• Cannot represent ignorance (“kidnapped robot problem”)
Feature-based navigation
Feature-based navigation
Feature-based navigation
 Feature-based navigation
Belief representation trough particle distribution
g :
• Advantages
• Can model arbitrary beliefs
• No assumptions on noise characteristic
• Disadvantages:
• No unique solution
• Not continuous
• Computationally expensive
• Tuning required
Feature-based navigation
Feature-based navigation
t=2 t=3 t=4
Conclusion
 Take Home Messages
• There are several localization techniques for indoor and
outdoor systems
y
• Each of the localization methods/positioning system has
advantage and drawbacks
• Odometry
Od t isi an absolute
b l t positioning
iti i methodth d using
i onlyl
propioceptive sensors but affected by a cumulative error
• Localization error can be modeled and estimated
• The error propagation methods/filtering techniques are
tools applicable to a large variety of noisy problems, not
just localization and navigation
• Feature-based navigation is a way to compensate
odometry limitations
 Additional Literature – Week 4
Books
• Siegwart R., Nourbakhsh I. R., and Scaramuzza D.,
“Introduction to Autonomous Mobile Robots”, MIT
Press, 2011 (2nd edition).
• Choset H.,
H Lynch K K. M
M., Hutchinson S.,
S Kantor G., G
Burgard W., Kavraki L., and Thrun S., “Principles of
Robot Motion”. MIT Press, 2005.
• Thrun
Th S Burgard
S., B d W.,
W andd Fox
F DD., “P
“Probabilistic
b bili ti
Robotics”, MIT Press, 2005.
• Maybeck
y P. S. “Stochastic Models,, Estimation,, and
Control”, Academic press, 1979, ch. 1. Out of print
but available at:
cs.unc.edu/~welch/media/pdf/maybeck
cs.unc.edu/ welch/media/pdf/maybeck_ch1.pdf
ch1.pdf