Sistema de Posicionamiento de Robots Móviles

Sistemas Electrónicos de Control

Sistema de
Posicionamiento de
Robots Móviles
Juan José Martínez Bautista

Página |2
1.) Desarrollar un Filtro de Kalman Extendido que combine el modelo de movimiento
obtenido a partir de las lecturas de odometría y las medidas del láser de barrido. Se deja
opción al alumno a elegir la función que relaciona las medidas del sensor láser y la pose delrobot.
Para este ejercicio se pide al alumno que compare la trayectoria real llevada a cabo por el
robot con la estimación de Kalman y la trayectoria obtenida mediante odometría.
El movimiento descrito por el robot es diferencial. Las ecuaciones del movimiento son:

xk  xk 1   vk  w2 k  *cos  k 1  k  w1k  

yk  yk 1   vk  w2 k  * sen  k 1  k  w1k   f  X ,U ,W 
 k   k 1  k  w1k

donde:

R
  wik  wdk 
2
R
 k    wik  wdk 
D
vk 

Estimación
Calculamos los jacobianos tomando el error como nulo:

J f ,x

 1 0 vk * sen  k 1  k  
 vk * sen  k 1  k  cos  k 1  k  




  0 1 vk *cos  k 1  k   J f , w   vk *cos  k 1  k  sen  k 1  k  
0 0



1
1
0



 ^
 ^

 X k 1  vk *cos   k 1   k  



~
^
^
^

 


X k  f  X k 1 ,U k , 0    Yk 1  vk * sen   k 1   k  

 


^


 k 1   k





Página |3
~

k  J f , x  k 1  J

T
f ,x

 J f ,w  w  J

T
f ,w

Para calcular Yk, es decir, las medidas lo que tenemos que hacer es encontrar la distancia máspequeña que nos da el láser siempre que sea mayor que cero y que el valor esté dentro del
rango de las rectas.
Una vez obtenemos la distancia mínima a una pared y sabiendo si es horizontal o vertical
tenemos que:

xkd  xk   d  vk  *cos k    
xkd  xk   d  vk  * sen k    


Vertical
 Horizontal
ykd  yk   d  vk  * sen k    
ykd  yk   d  vk  *cos k   



Por lo tanto, tendremos que calcular jacobianos diferentes según la pared sea horizontal o
vertical:

 1 0   d  vk   sen k    
J g ,x  

 0 1  d  vk   cos k    
Para las verticales
 cos k    
J g ,v  

 sen k    

 1 0  d  vk   cos k    
J g ,x  

 0 1   d  vk   sen k    
Para las horizontales
sen k    
J g ,v  

 cos k    

Los jacobianos hay que evaluarlos con el error Vk nulo.

Pasamos ahora a la fase de corrección.

Página |4
Correción

~
~
T
T
T 

K e   k  J g , x   J g , x   k  J g , x  J g ,v   v  J g ,v 


^
~

 ~ 
X k  X k  K e   Yk  g  X k , 0  




k   I  Ke  J g ,x   k
~

Todos loscálculos están hechos en el programa escrito en Matlab. Presento a continuación el
código:
% Apartado1.m
%
% SISTEMAS ELECTRÓNICOS DE CONTROL
%
% Juan José Martínez Bautista
clear all;
close all;
clc;
L=recbox(1500,1500,0,0);
L=[L; recbox(600,300,100,100)];
L=[L; recbox(300,600,600,600)];
% Datos
V=15.*ones(1,210);Omega=[zeros(1,10),pi/(2*20).*ones(1,20),zeros(1,25),pi/(2*30).*ones(1,30),zer
os(1,35),pi/(2*30).*ones(1,20),pi/(2*30).*ones(1,50),zeros(1,20)];
Rrueda = 20;
D = 100;
N = length(V);
% Velocidades angulares de las ruedas
wi = 1/2*((2*V)/Rrueda + (Omega*D)/Rrueda);
wd = 1/2*((2*V)/Rrueda - (Omega*D)/Rrueda);
%% Cálculo de las posiciones reales
xreal = zeros(1,N);
yreal = zeros(1,N);
phireal = zeros(1,N);
%Inicialización
xreal(1) = 900;
yreal(1) = 500;phireal(1) = 0;
for i = 2:N
wik = wi(i);
wdk = wd(i);
phireal(i) = phireal(i-1) + Rrueda/D*(wik-wdk);
xreal(i) = xreal(i-1) + Rrueda/2*(wik+wdk)*cos(phireal(i));
yreal(i) = yreal(i-1) + Rrueda/2*(wik+wdk)*sin(phireal(i));
end

Página |5
%% Cálculo de las posiciones estimadas
x_m = zeros(1,N);
y_m = zeros(1,N);
phi_m = zeros(1,N);
%Inicialización
x_m(1) = 900;
y_m(1) = 500;...