Robot Programming and Control

Mobile Robotics (II)

Departamento de Ingeniería
de Sistemas y Automática

Cipriano Galindo Andrades

 Lecture 1
Mobile Robotics

• Introduction
• Sensors
• Devices to get information from the real world
• Motion Model
Departamento de Ingeniería
de Sistemas y Automática

• How to mathematically model the motion of a robot

• Uncertainty
• Real world is full of uncertainty. How to manage it
• Localization
• Where am I? the endless story…
• Conclusions
 Probabilistic motion model

Motion error sources:

– Wheel slippage
– Inaccurate calibration
– Limited resolution during integration (time increments, measurement
resolution)
– Unequal floor
Departamento de Ingeniería
de Sistemas y Automática

ideal case different wheel carpet

bump diameters
 Probabilistic motion model

Example; robot path from odometry

Odometric path: the path that the
robot thinks he is following
Departamento de Ingeniería
de Sistemas y Automática

Ground Truth: Real path

followed by the robot
 Probabilistic motion model
The Gaussian Distribution in 1D
𝜎 : Standard Deviation
𝜎 2 : Variance
Departamento de Ingeniería
de Sistemas y Automática

1 standard deviation: 68% of drawn values

Probability Density Function (PDF) Cumulative Distribution Function (CDF)
 Probabilistic motion model
The Gaussian Distribution in 2D
Departamento de Ingeniería
de Sistemas y Automática

Σ: co-variance matrix (symmetric,

positive semi-definite matrix)
 Probabilistic motion model
The Gaussian Distribution in 2D
Departamento de Ingeniería
de Sistemas y Automática
 Probabilistic motion model
The Gaussian Distribution in 2D
Departamento de Ingeniería
de Sistemas y Automática

3 1
Example: Σ =
1 4
 Probabilistic motion model
The Gaussian Distribution in 2D
Departamento de Ingeniería
de Sistemas y Automática

3 1
Example: Σ =
1 4
 Probabilistic motion model
The Gaussian Distribution

Some interesting properties:

Sum of RVs following Gaussian Distributions is Gaussian (a.k.a. convolution)
N1

N2

N3
Departamento de Ingeniería
de Sistemas y Automática

Product of Gaussians is a Gaussian:

N3

N2 𝜎22 𝜇1 + 𝜎12 𝜇2
N1 𝜇3 =
𝜎12 + 𝜎22
2 2
2
𝜎 1 𝜎2
𝜎3 = 2
𝜎1 + 𝜎22
 Probabilistic motion model
The Gaussian Distribution in 1D

1.- Implementa una función que tome como parámetros la media y la

desviación de una distribución normal y el valor ‘x’ en la que se
evalúa. Desde la consola, plotea f(x) para 𝑥 ∈ [−5,5] con un
incremento de 0.1.

2.- Muestrear consiste en obtener un valor aleatorio obedeciendo

Departamento de Ingeniería

una distribución de probabilidad. Empleando la función randn, obten

de Sistemas y Automática

y dibuja muestras sobre el eje x de una distribución normal de

parámetros µ=2 y σ = 2. Comentar los resultados

3.- Dibuja el histograma para distintos valores de muestras

(100,500,1000) [comando hist]
 Probabilistic motion model
The Gaussian Distribution in 2D

1.- Considera que tenemos dos variables aleatorias distribuidas

siguiendo una normal que representan la posición en X e Y de un
robot. Para X, los parámetros son µ=2 y σ = 1 y para Y µ=3 y σ = 2.
Muestrea 1000 valores de cada una y plotealos. ¿Qué representan?

2.- Repite el ejercicio anterior utilizando la función mvnrnd. Esta

Departamento de Ingeniería

función requiere la matriz de covarianza dada por (cuando las

de Sistemas y Automática

variables son independientes):

𝜎𝑥2 0
Σ=
0 𝜎𝑦2

3.- Usa la función PlotEllipse subida en el CV para comprobar la forma

de la matriz de covarianza para diferentes valores de ‘sigma’.
 Probabilistic motion model

We need to characterize the robot motion in probabilistic terms

p( xt | ut , xt 1 )
pose at time t-1
pose at motion command at t-1
time t
Distribution over poses when executing the motion command ut and its
Departamento de Ingeniería
de Sistemas y Automática

pose is xt-1

ut 𝑡 = (𝑡 − 1) + Δ𝑡
ut
xt ~ p(xt | ut, xt1) Robot motion
ut
 Probabilistic motion model

Motion 1D: Starting without uncertainty:

Motion 1D: Starting with uncertainty (one step):

Departamento de Ingeniería
de Sistemas y Automática
 Probabilistic motion model

Motion 2D.

Using the same methodology

2D RV that accounts for the

possible ground truth locations
of the robot.
Departamento de Ingeniería
de Sistemas y Automática

Odometry

If the robot continues without any localization procedure, the

uncertainty in the position will increase along the time.
 Probabilistic motion model

We may consider this model in two forms:

• Analytic form, i.e. pdf of the xt distribution: used in Extended Kalman Filter
Typically: ut ~ N u , ut  xt 1 ~ N xt 1,  xt 1 
ut 
xt ~ N xt ,  xt 
xt 1 Robot motion
xt  g ( xt 1 , ut )
Departamento de Ingeniería
de Sistemas y Automática

• Sample form: used in Particle Filter

Typically: ut ~ N u , ut   x  i  1,, n

i
t 1

ut
 x  i  1,, n
i
t
Robot motion
 x  i  1,, n
i
t 1
xt  g ( xt 1 , ut )
 Probabilistic motion model

In practice, two types of motion models xt  g ( xt 1 , ut )

Velocity-based
– robot is controlled through velocities <v, w>
– applied when no wheel encoders are given

ut  [vt , wt ]T
xt ~ p( xt | ut , xt 1 )
2 variables Robot motion
Departamento de Ingeniería

xt 1
xt  g ( xt 1 , ut )
de Sistemas y Automática

Odometry-based
– robot is controlled through odometry pose increments ut  [xt , yt , t ]T
– used when robot is equipped with wheel encoders
ut  [xt , yt ,  t ]T
xt ~ p( xt | ut , xt 1 )
3 variables xt 1 Robot motion
xt  g ( xt 1 , ut )
 Odometry Motion Model

• Odometry: integrates wheel encoders information to compute robot

pose
• Given by the firmware of the robotic platform, which computes it at a
very high rate (e.g. 100 Hz) and makes it available at lower rate (e.g. 10
Hz)
• Technically it is a measurement rather than a control, but usually
treated as control to simplify the modeling
Departamento de Ingeniería
de Sistemas y Automática

ut  f (odomt , odomt 1 )
xt ~ p( xt | ut , xt 1 )
xt 1 Robot motion
xt  g ( xt 1 , ut )

• Measurements taken as control and computed from odometry at t

and t-1. Two control inputs from the odometry are normally
considered:
ut  [xt , yt ,  t ]
T

ut  [1 , d ,  2 ]T
 Odometry Motion Model (adding uncertainty)
𝑻
Sample form for the 𝒖𝒕 = 𝚫𝒙 , 𝚫𝒚 , 𝚫𝜽 case:

ut  [xt , yt ,  t ]T  x  i  1,, n

i
t

3 variables
Robot motion
xt  g ( xt 1 , ut )
 x  i  1,, n
i
t 1
“g” is the pose composition function
Departamento de Ingeniería

We consider that the 3 variables are independent, so:

de Sistemas y Automática

𝜎𝑥2 0 0
Σut = 0 𝜎𝑦2 0
0 0 𝜎𝜃2

And thus: Control signal affected by the noise

u′𝑡 = g(u𝑡 , 𝑛𝑜𝑖𝑠𝑒𝑡 ) where

𝑛𝑜𝑖𝑠𝑒𝑡 = [𝑛𝑜𝑖𝑠𝑒𝑥 , 𝑛𝑜𝑖𝑠𝑒𝑦 , 𝑛𝑜𝑖𝑠𝑒𝜃 ] ,
𝑛𝑜𝑖𝑠𝑒𝑥 ~𝑁 0, 𝜎𝑥 ,𝑛𝑜𝑖𝑠𝑒𝑦 ~𝑁 0, 𝜎𝑦 , 𝑛𝑜𝑖𝑠𝑒𝜃 ~𝑁(0, 𝜎𝜃 )
 Odometry Motion Model
Sample form for the 𝒖𝒕 = 𝜽𝟏 , 𝒅, 𝜽𝟐 𝑻 case:
From the odometry readings [ xˆt , yˆ t , ˆt ] [ xˆt 1 , yˆ t 1 , ˆt 1 ]
we compute ut  [1 , d ,  2 ]
T

ut  [1 , d ,  2 ]T  x  i  1,, n
i
t

 x  i  1,, n
i
t 1
Robot motion
xt  g ( xt 1 , ut )
Departamento de Ingeniería
de Sistemas y Automática

1  atan2( yˆ t  yˆ t 1, xˆt  xˆt 1 )  ˆt 1

d ( xˆt  xˆt 1 ) 2  ( yˆ t  yˆ t 1 ) 2 t
2
 2  ˆt  ˆt 1  1
 xˆt 
 xˆt 1   yˆ 
 yˆ  d  t
 t 1  ˆt 
ˆt 1   t 1 1
Courtesy of Thrun et. al.
 Odometry Motion Model (adding uncertainty)
Sample form for the 𝒖𝒕 = 𝜽𝟏 , 𝒅, 𝜽𝟐 𝑻 case:

ut  [1 , d ,  2 ]T  x  i  1,, n
i
t

 x  i  1,, n
i
t 1
Robot motion
xt  g ( xt 1 , ut )

Here “g” is NOT pose composition function; see next

Departamento de Ingeniería

Considers 4 parameters (𝛼1 , 𝛼2 , 𝛼3 , 𝛼4 )for computing the noise (it is

de Sistemas y Automática

more realistic) . Thus now:

𝜃1 ′ = 𝜃1 + 𝑛𝑜𝑖𝑠𝑒𝜃1 where 𝑛𝑜𝑖𝑠𝑒𝑥 ~𝑁 0, 𝛼1 𝜃12 + 𝛼2 𝑑 2

𝑑′ = 𝑑 + 𝑛𝑜𝑖𝑠𝑒𝑑 where 𝑛𝑜𝑖𝑠𝑒𝑑 ~𝑁(0, 𝛼3 𝑑 2 + 𝛼4 (𝜃12 + 𝜃22 ))
𝜃2 ′ = 𝜃2 + 𝑛𝑜𝑖𝑠𝑒𝜃2 where 𝑛𝑜𝑖𝑠𝑒𝜃2 ~𝑁(0, 𝛼1 𝜃22 + 𝛼2 𝑑 2 )
Control signal affected by the noise
 Odometry Motion Model (adding uncertainty)
The algorithm:
//Input: x_t computed using odometry model and pose
composition
xt 1   xt 1 , yt 1 ,  t 1 , xt   xt , yt ,  t 
//Compute t1,d,t2 and its noise. sample(b) draws sample from
N(0,b)
1 '  1  sample(1 12   2 d 2 )
d '  d  sample( 3 d 2   4 (12   22 )
 2 '   2  sample(1  22   2 d 2 )
Departamento de Ingeniería
de Sistemas y Automática

//Compute the final, noisy pose

xt '  xt 1  d ' cos( t 1  1 ' )
yt '  yt 1  d ' sin( t 1  1 ' )
 t '   t 1  1 ' 2 '

Courtesy of Thrun et. al.

 Odometry Motion Model (adding uncertainty)

Running the above algorithm with different set of parameters 𝛼

Departamento de Ingeniería
de Sistemas y Automática

Start

Concatenating a sequence on motions Courtesy of Thrun et. al.

 Probabilistic Motion Model

Conclusions

• Robot motion is not precise at all. Odometry is not a

reliable source to locate the robot.
• Here we have modelled the uncertainties in the
motion of a differential robot.
Departamento de Ingeniería

• Noise can be modelled as Gaussian, and as far as the

de Sistemas y Automática

robot moves, the higher is the variance, i.e. the

uncertainty about the position of the robot.
• Probabilistically, the robot pose can be modeled by
Gaussian distributions or by a set of particles.