Lecture1-Mobile Robotics (III)

Robot Programming and Control
Dpto. de Ingeniería de Sistemas y Automática

Departamento de Ingeniería
de Sistemas y Automática
Dr. D. Cipriano Galindo Andrades

Lecture 1
Mobile Robotics
(TOC)
• Introduction
• Sensors
• Devices to get information from the real world
• Motion Model
• 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
Localization Problem
• If I would have:
• A map of the environment
• A number of sensor measurements
• I could:
• Estimate my pose
• In a variety of situations:
• Position tracking.I approximately know where I am.
• Global localization. No idea where I am.
• Kidnapped robot problem. I am completely wrong
about the position I think I am.
“Using sensory information to locate the robot in its environment is the most
fundamental problem to providing a mobile robot with autonomous
capabilities.” [Cox ’91]
• Maps used for localization

Largely depends on the type of used sensor. Focusing on 2D laser
scanners:
• Metric/Geometric maps
Based on geometric data coming from the sensors, mainly stored in the
form of gridmaps (occupancy or probabilistics)

Occupancy. A cell is occupied

Probabilistic. Each cell stores a
or not
probability about its occupancy.
• Metric/Geometric maps
The simplest metric map is a set of landmarks located at known
positions:
𝑙1 = {𝑥1 , 𝑦1 } 𝑙2 = {𝑥2 , 𝑦2 }
𝑑1 𝑑2
Locating the set of
landmarks helps in
𝑑3 estimating the robot
pose.
𝑙3 = {𝑥3 , 𝑦3 }

Different types of landmarks: visual patterns, reflectors, etc.

• Maps used for (coarse) localization and other purposes
• Topological maps
Based on graphs where nodes represents locations and arcs the
connectivity between them.
• Hybrid maps
Combination of geometric and topological maps. Commonly used in
robotics.
• Hybrid maps
The graph permits the computation of paths and coarse localization,

while the stored geometric maps permit precise localization.
Least Squares Positioning
Problem:
Given a vector of measurements 𝑧 related to the robot pose 𝑥 by
the linear observation model:
𝑧 = 𝐻𝑥
We want to find the “best” pose 𝑥ො
Direct solution:
𝑥 = 𝐻 −1 𝑧 Only if 𝐻 is invertible… 𝐻
is square with det(𝐻) ≠ 0
General solution:
find the 𝑥 closest to the ideal, i.e. minimizing the error 𝑒 = 𝐻𝑥 − 𝑧
𝑥ො = arg min 𝑒 𝑇 𝑒 = arg min 𝐻𝑥 − 𝑧 2 = arg min[ 𝐻𝑥 − 𝑧 𝑇 (𝐻𝑥 − 𝑧)]

𝑥 𝑥 𝑥
Solution:
𝑥ො = arg min[ 𝐻𝑥 − 𝑧 𝑇 (𝐻𝑥 − 𝑧)]
𝑥
2
𝐻𝑥 − 𝑧 = 𝑥 𝑇 𝐻𝑇 𝐻𝑥 − 𝑥 𝑇 𝐻𝑇 𝑧 − 𝑧𝐻𝑥 + 𝑧 𝑇 𝑧
We want to minimize this expression, so we derive and set to zero:
2
𝛿 𝐻𝑥 − 𝑧
= 2𝐻𝑇 𝐻𝑥 − 2𝐻𝑇 𝑧 = 0
𝛿𝑥
𝑥 = 𝐻𝑇 𝐻 −1 𝐻 𝑇 𝑧
Pseudo-inverse of H
Example Robot moving in 1D:
Robot moves on a line with odometric errors. It can measure the
distance to a number of landmarks (3 in this case).
landmarks
Red: odometry
Green: estimation
Blue: ground truth
clear all, close all
nLandmarks = 3;
Map(1)=10;Map(2)=20;Map(3)=30;
% Initialization
sigma_sensor = 0.05^2; %variance of the range measurement noise per meter
z = zeros(nLandmarks,1); %All the observations equals zero
z_aux = zeros(nLandmarks,1); %All the observations equals zero
% Robot parameters
SigmaX = 0.05; %1D -> The robot only moves along X axis
inc_x=1; %The robot always move 10 cm. at each step, but affected by noise
xTrue = 0; %True unkown position, includes motion errors
xEst = 0; %Position estimated by the LSE method
xOdom = 0; % Position given by odometry where the robot thinks he is located
after applying motion
% commands without noise.
%initial graphics
figure(1); hold on; grid off;% axis equal;
plot(Map(1,:),0,'sm','LineWidth',2);hold on;
loop=1;
num_loops=25;
while loop<num_loops
%%%% Moving the robot a fixed amount
u=inc_x; % simple motion command, just for unifying nomenclature
noise=sqrt(SigmaX)*randn(1,1);
unoise=u+noise; % Here the composition of poses is just a sum
xTrue=xTrue+unoise; %Idem for this composition; xOdom=tcomp(xOdom,unoise);
xOdom=xOdom+u; %Without noise this is the
% H in this case is just a column of ones.

H=ones(nLandmarks,1);
for kk = 1: nLandmarks
Delta = Map(1,kk)-xTrue;
z(kk) = Delta + (sqrt(sigma_sensor)*randn(1,1)); % Noisy Observation
z_aux(kk)=Map(1,kk)-z(kk); %%% z_aux = l_i - z_i ; z_aux=H*x_true
end
plot(xOdom,0,'r.'); plot(xTrue,0,'bo');
%%% Linear model, solution is closed-based (observation affected by noise, but
such a noise is not considered in the equation. This will be done in the
weighted LSE).
xEst=inv(H'*H)*H'*z_aux
plot(xEst,0,'g.')
pause
loop=loop+1;
end
Weigted Least Squares:
More realistic situation: measurements are affected by gaussian

noise and are not equally reliable (still linear observation model):
𝑧 = 𝐻𝑥 + 𝑤, 𝑤 = [𝐻𝑥 − 𝑧]~𝑁(0, 𝜎 2 )
𝑧~𝑁(𝐻𝑥, 𝜎 2 )
For a number of meassurements, we will have a diagonal

covariance matrix that represent the uncertainty of the
meassurements:
2
𝜎𝑧1 0 …
𝑅 = 0 𝜎𝑧2 2
…
⋮ ⋮ ⋱
Weigthed Least Squares:

Recalling the expression for a RV following a gaussian distribution:
𝑧~𝑁(𝐻𝑥, 𝑅)
1
𝑝 𝑧 𝑥 = 𝐾 ∙ exp − [ 𝐻𝑥 − 𝑧 𝑇 𝑅−1 (𝐻𝑥 − 𝑧)]
2
This is what we are really minimizing
This expression is mínimum when e = 0, i. e. , 𝐻𝑥 − 𝑧 = 0 , and thus,

𝑝 𝑧 𝑥 is maximum. That is:
𝑥ො = arg min[ 𝐻𝑥 − 𝑧 𝑇 𝑅−1 (𝐻𝑥 − 𝑧)]

𝑥
𝑥ො = 𝐻 𝑇 𝑅−1 𝐻 −1 𝐻 𝑇 𝑅 −1 𝑧
Non-linear Least Squares
Realistic problems involve non-linear models, for example computing the

Euclidean distance between two points. In this cases:
𝑧 = ℎ(𝑥)
And thus we have to solve:
2
𝑥ො = arg min ℎ 𝑥 − 𝑧
𝑥
There are no closed-form solutions. We need an iterative estimation

starting with an initial guess of the solution 𝑥0 .
2
We seek a vector 𝛿, such that x1 = x0 + 𝛿 and minimize ℎ 𝑥1 − 𝑧
Using the Taylor expansion:
ℎ 𝑥1 = ℎ 𝑥0 + 𝛿 = ℎ 𝑥0 + 𝛻Hx 0 𝛿
Jacobian of the sensor model
n: d.o.f. of the pose
m: # observations
2 2 2
ℎ 𝑥1 − 𝑧 = ℎ 𝑥0 + 𝛻𝐻𝑥 0 𝛿 − 𝑧 = 𝛻𝐻𝑥 0 𝛿 − (𝑧 − ℎ 𝑥0 )
A 𝑥 B
Recalling that minimizing: 𝐴𝑥 − 𝑏 2 ⇒ 𝑥 = 𝐴𝑇 𝐴 −1 𝐴𝑇 𝑏
−1
𝛿= 𝛻𝐻𝑥 0 𝛻𝐻𝑥 0 𝛻𝐻𝑥 𝑇0 [𝑧
𝑇
− ℎ 𝑥0 ]
Once we know how to compute the vector 𝛿 we follow as:
1. Start with an initial guess of the robot pose 𝑥ො
2. Compute the vector

−1
𝛿 = 𝛻𝐻𝑥 𝑇0 𝛻𝐻𝑥 0 𝛻𝐻𝑥 𝑇0 [𝑧 − ℎ 𝑥0 ]
3. Set 𝑥ො = 𝑥ො + 𝛿
2
4. If ℎ 𝑥ො − 𝑧 > 𝜖 then goto 1
−1
For weighted sensor measurements: 𝛿 = 𝛻𝐻𝑥 𝑇 R−1 𝛻𝐻𝑥 𝛻𝐻𝑥 𝑇0 𝑅−1 [𝑧 − ℎ 𝑥0 ]
0 0
Example: Long Baseline Positioning in 2D
Estimate the position of a vehicle in 2D given the distances to n beacons.
𝑝 = 𝑥, 𝑦 𝑇 : pose of the robot

𝑝𝑖 = 𝑥𝑖 , 𝑦𝑖 𝑇 : position of the beacon 𝑖
1
Observation model: 𝑧𝑖 = ℎ𝑖 𝑝 = 𝑝𝑖 − 𝑝 = Δ𝑥𝑖2 + Δ𝑦𝑖2 2
𝑧1 𝑑1 𝑝1 − 𝑝 𝑥1 − 𝑥 2 + 𝑦1 − 𝑦 2 1/2
𝑧 = ℎ 𝑝 = 𝑧2 = 𝑑2 = 𝑝2 − 𝑝 = 𝑥2 − 𝑥 2 + 𝑦2 − 𝑦 2 1/2
𝑧3 𝑑3 𝑝3 − 𝑝 𝑥3 − 𝑥 2 + 𝑦3 − 𝑦 2 1/2
𝑥1 − 𝑥 2 + 𝑦1 − 𝑦 2 1/2
ℎ 𝑝 = 𝑥2 − 𝑥 2 + 𝑦2 − 𝑦 2 1/2
𝑥3 − 𝑥 2 + 𝑦3 − 𝑦 2 1/2
We also need the jacobian of the sensor model

𝜕 2 2 1/2
1
𝑥 −𝑥 + 𝑦𝑖 − 𝑦 =− (𝑥 − 𝑥)
𝜕𝑥 𝑖 𝑑𝑖 𝑖
𝜕 2 2 1/2
1
𝑥 −𝑥 + 𝑦𝑖 − 𝑦 = − (𝑦𝑖 − 𝑦)
𝜕𝑦 𝑖 𝑑𝑖
𝜕 2 2 1/2
𝑥 −𝑥 + 𝑦𝑖 − 𝑦 =0
𝜕𝜃 𝑖

1 1
− (𝑥1 − 𝑥) − (𝑦1 − 𝑦) 0
𝑑1 𝑑1
1 1
𝛻𝐻𝑥 = − (𝑥2 − 𝑥) − (𝑦2 − 𝑦) 0
𝑑2 𝑑2
1 1
− (𝑥3 − 𝑥) − (𝑦3 − 𝑦) 0
𝑑3 𝑑3
𝑟𝑎𝑛𝑘 𝛻𝐻𝑥𝑇 𝛻𝐻𝑥 = 2, so it is not invertible. We cannot computed

the heading of the robot when the angle is not measured.
In our case we drop this information and use

1 1
− (𝑥1 − 𝑥) − (𝑦1 − 𝑦)
𝑑1 𝑑1
1 1
𝛻𝐻𝑥 = − (𝑥2 − 𝑥) − (𝑦2 − 𝑦)
𝑑2 𝑑2
1 1
− (𝑥3 − 𝑥) − (𝑦3 − 𝑦)
𝑑3 𝑑3

clear all, close all
% Creating the mapnLandmarks = 5;
𝑝2 Map = 50*rand(2,nLandmarks)-25;
% Initialization
var_d = 0.05^2; %variance of the range measurement noise
𝑝1 R = zeros(nLandmarks);% Weigthed observations
U = diag([0.5,0.5,0.5*pi/180]).^2; %Odometry noise
z = zeros(nLandmarks,1);
xTrue = zeros(3,1); %True position
xEst = zeros(3,1); %Position estimated by the LSE method

xOdom = zeros(3,1); % Position given by odometry

𝑝3 jH = zeros(nLandmarks,2); %Jacobian of the observation
% Some parameters for the Gauss-Newton optimization loop

nIteration = 100;
tolerance = 1.0e-09;
%initial graphics
figure(1); hold on; grid off; axis equal;
plot(Map(1,:),Map(2,:),'sm','LineWidth',2);hold on;
loop=0;
num_loops=100;
while loop<num_loops
%%%% Moving the robot a fixed following a square

[…]

% Get the observation to all the landmarks
Delta = Map(1:2,kk)-xTrue(1:2);
z(kk) = norm(Delta) + sqrt(var_d)*randn(1,1); %Observation (distance)
end
% Loop for the iterative Least Square estimation
iteration = 0;
incr = ones(1,2);
xEst = xOdom; %Initial estimation is the odometry position
while (norm(incr) > tolerance && iteration < nIteration)

% Compute the Jacobian

Delta = Map(1:2,kk)-xEst(1:2);
h(kk) = norm(Delta);
jH(kk,1) = -Delta(1) / h(kk);
jH(kk,2) = -Delta(2) / h(kk);
end
R = diag(var_d*sqrt(z)); %Observation variances grow with the the distance
error = z - h';
incr = inv(jH'*inv(R)*jH) * jH'*inv(R)*error;
xEst (1:2) = xEst(1:2)+ incr;
iteration = iteration +1;
end
pause
loop=loop+1;
end
The robot thinks he is following

a square (odometry: red
points). But actually the
movement is not precise (blue
circles.
The localization method gives a

good estimation in green, in
spite of noisy movement and
noisy sensor readings.
Errors in localization. Top) difference between the ground truth position and
the one estimated by LSE. Bottom) Difference between the odometry
position and the ground truth,
Particle Filter
Problem:
Given a set of landmarks and a set of probable poses of the robot
(particles). We want to estimate the “best” pose 𝑥ො
Recalling from the motion model.

Each particle represents a possible
pose of the robot. They are
distributed following the

associated Gaussian distribution.
The idea is to simulate the sensor reading from each particle and compare it
with the actual measurement (affected by noise).
Particles with a coherent observation will have a high weight. Those with a
large discrepancy, will have a low weight.
Particle Filter
Weight Computation:
2
𝑧2
Motion model + 𝑧5
5 5 𝑙1
4 perturbation
2 4 𝑧4 𝑧
1
3
𝑧1 𝑧3
Starting sample set 1
3
Ground-truth pose
That is, particles with a 𝑧𝑖 closer to 𝑧 will better represent the actual pose of
the robot. So we define the “innovation”, i.e., the importance of each
particle as:
𝐼𝑛𝑛𝑜𝑣𝑖 = 𝑧 − 𝑧𝑖
Based on the innovation a weight is computed for each particle:

𝑊 𝑖 = 𝑓(𝑧 − 𝑧𝑖 )
Particle Filter
Weight Computation:
A variety of functions can be used to compute the particles’ weight; the
requisite is that they give a higher value to those particles with lower
innovation. A common function used is:
𝑇
𝑊 𝑖 = exp −0.5 ∗ 𝑧 − 𝑧𝑖 ∗ Σ𝑠−1 ∗ 𝑧 − 𝑧𝑖 +𝜖
That is, a gaussian function centered in 𝑧, and

the covariance of the sensor model. The 𝜖
term avoids null weights.
When 𝑧 = 𝑧𝑖 , 𝑊(𝑖) is maximum.
As long as 𝑧 − 𝑧𝑖 is larger, the value of 𝑊(𝑖)

will decrease.
𝑧
Particle Filter
Update (reselection):
2
Motion model +
5 perturbation 5
2 4 4
1
3

3
Ground-truth pose
Particles #4 and #1 have the highest weight, while #3 and #2 have a low
weight.
Now we give a new motion step but selecting randomly from the set of
particles according to their weight. Those with a high weight have higher
probability to survive.
Particle Filter
2
2
5
Motion model +
Motion model + 4
5 5 perturbation
4 perturbation 4
2 4
1
3
1
3
Ground-truth pose
Particles #4 has been selected twice and #3 has dissapeared.
The process continues by computing the weights again for the new set of
particles. In the long term particles that better represent the robot pose will
survive, while the rest will “die”.
The robot pose estimation can be computed as the average of all the particles or
taking the one with the highest weight
Particle Filter
How can I implement the randomly selection of particles according to their
weights?
%%% Let samples be a vector of 3xn_samples [x,y,phi]
%%% Let W be a vector 1xn_samples of the weights of each particle
%%% 1.- Compute the cumulative densitity function as:

CDF=cumsum(W)/sum(W);
%%% Select n_samples random numbers from

the standard uniform

%%% distribution [0,1) CDF
iSelect=rand(n_samples,1);
Particles in sloped areas

has a high probability.
Those in flat areas are low
probability
# particles
Particle Filter
%%%Interpolate the values randomly selected: options nearest and
extrap are needed to avoid values out of range. iNext are the indexes
of the chosen particles
iNext=interp1(CDF,1:n_samples,iSelect,'nearest','extrap');
%%%Update the samples set. Some particles can be repetitions

samples=samples(:,iNext);
[0.8,0.85]
Particle #21 selected
[0.25,0.26]
Particle #5 selected
# particles
Conclusions
• The localization problem has to be solved to enable

a mobile robot to operate within its workspace
• It relies on a map and on the reliability of sensors to
give measurements.
• Here we have considered maps of known landmarks

and distance sensors, but other more complex

solutions can be adopted.
• LSE is commonly used in controlled scenarios since it
is precise and robust.
• Particle Filter are normally used in uncontrolled
scenarios given its versatility.

Lecture1-Mobile Robotics (III)

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Lecture1-Mobile Robotics (III)

Uploaded by

Copyright:

Available Formats

Robot Programming and Control

Dpto. de Ingeniería de Sistemas y Automática

Dr. D. Cipriano Galindo Andrades

• How to mathematically model the motion of a robot

• Maps used for localization

form of gridmaps (occupancy or probabilistics)

Occupancy. A cell is occupied

• Maps used for localization

• Maps used for localization

Different types of landmarks: visual patterns, reflectors, etc.

• Maps used for (coarse) localization and other purposes

The graph permits the computation of paths and coarse localization,

𝑥ො = arg min 𝑒 𝑇 𝑒 = arg min 𝐻𝑥 − 𝑧 2 = arg min[ 𝐻𝑥 − 𝑧 𝑇 (𝐻𝑥 − 𝑧)]

We want to minimize this expression, so we derive and set to zero:

% H in this case is just a column of ones.

Weigted Least Squares:

More realistic situation: measurements are affected by gaussian

For a number of meassurements, we will have a diagonal

Weigthed Least Squares:

This is what we are really minimizing

This expression is mínimum when e = 0, i. e. , 𝐻𝑥 − 𝑧 = 0 , and thus,

𝑥ො = arg min[ 𝐻𝑥 − 𝑧 𝑇 𝑅−1 (𝐻𝑥 − 𝑧)]

Non-linear Least Squares

Realistic problems involve non-linear models, for example computing the

There are no closed-form solutions. We need an iterative estimation

Non-linear Least Squares

Using the Taylor expansion:

Recalling that minimizing: 𝐴𝑥 − 𝑏 2 ⇒ 𝑥 = 𝐴𝑇 𝐴 −1 𝐴𝑇 𝑏

Non-linear Least Squares

Once we know how to compute the vector 𝛿 we follow as:

1. Start with an initial guess of the robot pose 𝑥ො

2. Compute the vector

Example: Long Baseline Positioning in 2D

Estimate the position of a vehicle in 2D given the distances to n beacons.

𝑝 = 𝑥, 𝑦 𝑇 : pose of the robot

𝑝𝑖 = 𝑥𝑖 , 𝑦𝑖 𝑇 : position of the beacon 𝑖

Example: Long Baseline Positioning in 2D

We also need the jacobian of the sensor model

Example: Long Baseline Positioning in 2D

𝑟𝑎𝑛𝑘 𝛻𝐻𝑥𝑇 𝛻𝐻𝑥 = 2, so it is not invertible. We cannot computed

In our case we drop this information and use

Example: Long Baseline Positioning in 2D

xEst = zeros(3,1); %Position estimated by the LSE method

xOdom = zeros(3,1); % Position given by odometry

% Some parameters for the Gauss-Newton optimization loop

%%%% Moving the robot a fixed following a square

Example: Long Baseline Positioning in 2D

while (norm(incr) > tolerance && iteration < nIteration)

% Compute the Jacobian

The robot thinks he is following

The localization method gives a

Recalling from the motion model.

distributed following the

associated Gaussian distribution.

Based on the innovation a weight is computed for each particle:

That is, a gaussian function centered in 𝑧, and

When 𝑧 = 𝑧𝑖 , 𝑊(𝑖) is maximum.

As long as 𝑧 − 𝑧𝑖 is larger, the value of 𝑊(𝑖)

Starting sample set 1

Particles #4 has been selected twice and #3 has dissapeared.

%%% 1.- Compute the cumulative densitity function as:

%%% Select n_samples random numbers from