Una conducta utilizando el Khepera La descripción del vehículo autónomo Khepera II se encuentra en este enlace. Se presenta a continuación un programa en Matlab que permite adquirir y reproducir una conducta (seguir una pared). El método aquí descrito puede generalizarse para implementar otras conductas basadas en el control por modelo inverso. % Con este programa se adquieren datos para reproducir un comportamiento
% Conexión con el robot ref = kopen([0,9600,1]) % Velocidad inicial de las ruedas
vi = 3; vd = 3;
N=300; % Número de datos a adquirir
%Inicializa las variables para la adquisición
infrarrojos=zeros(N,8); % Tamaño de matrices
vel=zeros(N,2);
% Condiciones iniciales: Igual velocidad a ambas ruedas
kSetSpeed(ref, vi, vd);
for i=1:N % Se toman N muestras
% Se leen sensores
infrarrojos(i,:)=kProximity(ref)';
% En este ejemplo en particular se sigue una pared
% Utiliza información de sensores 1 y 7
if ((infrarrojos(i,5)>300)||(infrarrojos(i,7)<100))
vd=1.2*vi;
vi=0.8*vi;
if ((vi>4)||(vd<2)) %Saturador
vi=3;
end
end
% Aplica velocidad a cada rueda
kSetSpeed(ref, vi, vd);
vel(i,:)=[vi,vd]; % Reales aplicados
if ((infrarrojos(i,7)>150)||(infrarrojos(i,5)<200))
vi=1.2*vd;
vd=0.8*vd;
if ((vi>4)||(vd<2)) %Saturador
vi=3;
end
end
kSetSpeed(ref, vi, vd);
vel(i,:)=[vi,vd]; % Se definen en la matriz los reales aplicados
% Si se desean los medidos se aplicaría:
%vel(i,:)=kGetSpeed(ref)';
end
% Se conforman las matrices para obtener modelo (anfis)
vel_i=[infrarrojos(:,5), infrarrojos(:,7), vel(:,1)];
vel_d=[infrarrojos(:,5), infrarrojos(:,7), vel(:,2)]; % Se detiene el robot
kSetSpeed(ref, 0, 0); |
Para obtener el modelo del anterior comportamiento, basado en los datos, puede utilizar el siguiente programa. % Ejemplo de como obtener un modelo de comportamiento
% Se definen las condiciones iniciales de
% un sistema borroso
%FIS = GENFIS1(DATA, NUMMFS, INPUTMF, OUTPUTMF)
FIS_vel_i=genfis1(vel_i, 7, 'gbellmf');
FIS_vel_d=genfis1(vel_d, 7, 'gbellmf');
% Adapta parámetros para obtener el modelo
modelo_vel_i = anfis(vel_i,FIS_vel_i,450);
modelo_vel_d = anfis(vel_d,FIS_vel_d,450);
% Verifica modelo real y estimado
plot(vel_i(:,3),'g');
hold on;
plot(evalfis(vel_i(:,1:2),modelo_vel_i),'r');
figure;
plot(vel_d(:,3),'g');
hold on;
|
Finalmente, se aplica el modelo obtenido como ley de control. % Se aplica ley de control basado en modelo
% Velocidad inicial
a=3;
infrarrojosc=zeros(1,8); %tamaño de lectura de sensores
%Igual velocidad a ambas ruedas
kSetSpeed(ref, a, a);
while 1
infrarrojosc=kproximity(ref)';
% Se limita entradas
if (infrarrojosc(1,5)>max(infrarrojos(:,5)))
infrarrojosc(1,5)=max(infrarrojos(:,5));
end
if (infrarrojosc(1,5)infrarrojosc(1,5)=min(infrarrojos(:,5));
end
if (infrarrojosc(1,7)>max(infrarrojos(:,7)))
infrarrojosc(1,7)=max(infrarrojos(:,7));
end
if (infrarrojosc(1,7)infrarrojosc(1,7)=min(infrarrojos(:,7));
end
infrarrojos_r=[infrarrojosc(1,5), infrarrojosc(1,7)];
vi=evalfis(infrarrojos_r,modelo_vel_i);
if ((vi<2)||(vi>4))
vi=3;
end
vd=evalfis(infrarrojos_r,modelo_vel_d);
if ((vd<2)||(vd>4))
vd=3;
end
kSetSpeed(ref, vi, vd); % aplica ley de control
end
|
|