DEPARTAMENTO DE INGENIERÍA

DE SISTEMAS Y AUTOMÁTICA

Control y Programación de Robots

Práctica 1. Modelo de Movimiento

En esta práctica vamos a implementar el modelo de movimiento basado en odometría

mediante la técnica de sampling. Comenzaremos implementando funciones básicas que
utilizaremos a lo largo de la práctica.

1.- Composición de dos poses. Implementa una función que realice la composición de dos
poses, es decir que dadas 𝑝1 = [𝑥1 𝑦1 𝜃1 ]𝑇 y 𝑝2 = [𝑥2 𝑦2 𝜃2 ]𝑇 , devuelva el resultado de la
operación 𝑝1 ⊕ 𝑝2 , definido como:

𝑥1 + 𝑥2 𝑐𝑜𝑠𝜃1 − 𝑦2 𝑠𝑖𝑛𝜃1
𝑝1 ⊕ 𝑝2 = (𝑦1 + 𝑥2 𝑠𝑖𝑛𝜃1 + 𝑦2 𝑐𝑜𝑠𝜃1 )
𝜃1 + 𝜃2

function p=pose_comp(p1,p2)
%This function computes the composition of two poses, p1,p2 of dimension 3x1
%p is the resultant pose
end

2.- Modelo de Odometría. Diseña un programa que iterativamente comande un robot para
seguir una trayectoria en forma de cuadrado de lado 10 metros. Para ello necesitarás hacer
uso de la composición de poses para calcular en cada iteración 𝑥𝑡 = 𝑥𝑡−1 ⊕ 𝑢𝑡 , siendo 𝑢𝑡 =
(Δ𝑥 Δy Δ𝜃)𝑇 . Considera que en cada paso el robot puede avanzar 1 metro o girar 90⁰.
Puedes utilizar el siguiente snippet como guía.

clear all;
close all;

%%pose of the robot; initially at (0,0,0)'

x = zeros(3,1);
%%control action; initially set to (0,0,0)
u = zeros(3,1);

figure(1);hold on;axis equal;grid on;

k=1;
%%length of the simulation
%num_steps=.........
 DEPARTAMENTO DE INGENIERÍA
DE SISTEMAS Y AUTOMÁTICA

while (k<num_steps)
%%Add here the control action to follow a rectangle in terms of increments of x,y,theta
%u(1)=.....
%u(2)=....
%u(3)=....

%%updating pose:
%% Here the pose of the robot "x" should be updated with the control action
%%.............

%%Drawing
r=drawrobot(x,'b');
plot(x(1),x(2),'r.');

%a small pause to see the animation

pause(0.025);
%increment the value of k
k=k+1;
%making the robot moves.
if (~isempty(r)) delete(r);end;

end

3- Comandando el robot mediante comandos de velocidad lineal y angular. Repite el

ejercicio anterior pero utilizando comandos basados en las ecuaciones:
t
x(t )   vP (t ) cos( wt )dt
0
t
y (t )   vP (t ) sin( wt )dt
0
t
 (t )   w(t ) dt
0

4.- Añadiendo ruido. Modifica el ejercicio 2 para que la acción de control se vea afectada
2
𝜎Δ𝑥 0 0
2
por ruido. Para ello considera que Σ𝑢𝑡 = [ 0 𝜎Δy 0 ]. Dibuja la trayectoria del robot
2
0 0 𝜎Δθ
2 2 2
con y sin ruido y comprueba cómo varía para diferentes valores de 𝜎Δ𝑥 , 𝜎Δy y 𝜎Δθ .

clear all;
close all;

%%pose of the robot; initially at (0,0,0)'

x = zeros(3,1);

%%True position of the robot, including noise,

%% initially at (0,0,0)'
xtrue = zeros(3,1);
%%control action; initially set to (0,0,0)
u = zeros(3,1);

%%Noisy control action

SigmaX = 0.07;
SigmaY = 0.07;
SigmaPhi = 0.0015;
Q = diag([SigmaX^2 SigmaY^2 SigmaPhi^2]);

figure(1);hold on;axis equal;grid on;

k=1;
 DEPARTAMENTO DE INGENIERÍA
DE SISTEMAS Y AUTOMÁTICA

%%length of the simulation

%num_steps=.........

while (k<num_steps)
%%Add here the control action to follow a rectangle in terms of increments of x,y,theta

%u(1)=.....
%u(2)=....
%u(3)=....

%%updating pose:
%% Here the pose of the robot "x" should be updated with the control action...
%%.............

%including noise to u(t):

%noise based on the matrix Q (covariance matrix of the movement)
%noise = .......

%composing the control action with the noise:

%unoise=..........

%compose the true pose of the robot with the noisy control action
%xtrue=.....

%%Drawing
r=drawrobot(x,'b');
plot(x(1),x(2),'r.');

plot(xtrue(1),xtrue(2),'k.');

%a small pause to see the aniamtion

pause(0.025);
%increment the value of k
k=k+1;
%making the robot moves..
if (~isempty(r)) delete(r);end;

end

5.- Usando partículas. Vamos a estimar la posición del robot en base a partículas, samples
de poses probables en las que se puede encontrar el vehículo. Para ello repite el ejercicio
2, mostrando la posición sin ruido (odometría) del robot junto a #particles posibles
posiciones en base a otras tantas acciones de control ruidosas. Considera #particles=100.

% Drawing particles with different colors

% Assuming particles are stored in a vector called "samples"
part=plot(samples(1,:),samples(2,:),'.');
set(part,'MarkerSize',5,'Color',[rand rand rand]);

6.- Ajustando el modelo odométrico. En los ejercicios anteriores se ha considerado que el

modelo odométrico considera 𝑢𝑡 = (Δ𝑥 Δy Δ𝜃)′, lo que conlleva algunas limitaciones a la
hora de reflejar ruidos aleatorios en el movimiento real de los robots. Una forma más
conveniente y realista consiste en considerar 𝑢𝑡 = (𝜃1 𝑑 𝜃2 )′, es decir, modelando un giro,
 DEPARTAMENTO DE INGENIERÍA
DE SISTEMAS Y AUTOMÁTICA

avance y giro. Estos parámetros se obtienen fácilmente de los valores odométricos

[ xt , yt ,t ] y [ xt 1, yt 1,t 1 ] :
1  atan2( yt  yt 1, xt  xt 1 )  t 1
d  ( xt  xt 1 )2  ( yt  yt 1 )2
2  t  t 1  1

 Implementa una función, que dados [ xt , yt ,t ] y [ xt 1, yt 1,t 1 ] calcule 𝑢𝑡 =

(𝜃1 𝑑 𝜃2 )′.
 Utilizando esta acción de control ut  (1 d 2 )' , se puede modelar la acción ruidosa
como:
ˆ1  1  sample(1 12   2 d 2 )
dˆ  d  sample( d 2   ( 2   2 )
3 4 1 2

ˆ2  2  sample(1   2 d )
2
2
2

Donde sample(b) es un valor aleatorio que sigue una distribución N(0,b) y 𝛼𝑖

modelan el ruido intrínseco del robot. Utilizando el modelo basado en giro-avance-
giro, la posición del robot se puede calcular como:

xt  xt 1  dˆ cos( t 1  ˆ1 )
y  y  dˆ sin(  ˆ )
t t 1 t 1 1

 t   t 1  ˆ1  ˆ2

Repite el ejercicio 5 considerando este modelo de ruido y varía los valores de αi

Valores de αi utilizados en esta simulación: a1=0.07;a2=0.07;a3=0.03;a4=0.05;