Realizar el seguimiento de un robot similar a un automóvil utilizando un filtro de partículas
El filtro de partículas es un algoritmo de estimación bayesiano recursivo basado en muestreo, implementado en el objeto stateEstimatorPF
. En este ejemplo, se realiza el seguimiento de un robot similar a un automóvil por control remoto en el entorno al aire libre. La medición de la pose del robot se proporciona con datos de GPS integrado, con ruido. Se envían comandos de movimiento conocidos al robot, pero el robot no ejecutará el movimiento exacto indicado por comando, debido a holguras mecánicas o inexactitudes del modelo. Este ejemplo mostrará cómo utilizar stateEstimatorPF
para reducir los efectos del ruido en los datos de medición y obtener una estimación más precisa de la pose del robot. El modelo cinemático del robot similar a un automóvil está descrito por el siguiente sistema no lineal. El filtro de partículas es ideal para estimar el estado de este tipo de sistemas, ya que es capaz de manejar las no linealidades inherentes.
Escenario: el robot similar a un automóvil conduce cambiando la velocidad y ángulo de giro constantemente. La pose del robot se mide con un sistema externo con ruido, como un GPS o un sistema Vicon. A lo largo de la ruta, el robot conduce por una zona techada y no es posible realizar mediciones.
Entrada:
La medición con ruido en la pose parcial del robot (, , ). Observe que no se trata de una medición en estado completo. No hay ninguna medición disponible en la orientación de la rueda delantera () así como de las velocidades (, , , ).
El comando de velocidad lineal y angular enviado al robot (, ). Observe que habrá alguna diferencia entre el movimiento indicado por comando y el movimiento real del robot.
Objetivo: estimar la pose parcial (, , ) del robot similar a un automóvil. Observe de nuevo que la orientación de las ruedas () no está incluida en la estimación. Desde la perspectiva del observador, el estado completo del automóvil es solamente [, , , , , ].
Planteamiento: utilice stateEstimatorPF
para procesar las dos entradas con ruido (ninguna de las entradas es fiable por sí misma) y realice una mejor estimación de la pose actual (parcial).
En la etapa de predicción, actualizamos los estados de las partículas con un modelo simplificado de robot similar a un monociclo, como se muestra a continuación. Observe que el modelo del sistema utilizado para una estimación de estado no es una representación exacta del sistema actual. Esto es aceptable siempre y cuando la diferencia del modelo esté bien capturada en el sistema con ruido (como se representa en la nube de partículas). Para más información, consulte
predict
.
En la etapa correcta, la ponderación (probabilidad) de importancia de una partícula está determinada por su norma de error de la medida actual (), ya que solo contamos con mediciones en esos tres componentes. Para más información, consulte
correct
.
Inicializar un robot similar a un automóvil
rng('default'); % for repeatable result dt = 0.05; % time step initialPose = [0 0 0 0]'; carbot = ExampleHelperCarBot(initialPose, dt);
Configurar el filtro de partículas
Esta sección configura el filtro de partículas utilizando 5.000 partículas. Inicialmente, todas las partículas se capturan de manera aleatoria de una distribución normal con media en el estado inicial y covarianza unitaria. Cada partícula contiene 6 variables de estado (, , , , , ). Observe que la tercera variable está marcada como circular, ya que es la orientación del automóvil. También es muy importante especificar dos funciones de callback, StateTransitionFcn
y MeasurementLikelihoodFcn
. Estas dos funciones determinan directamente el rendimiento del filtro de partículas. Los detalles de estas dos funciones se pueden encontrar en las dos últimas secciones de este ejemplo.
pf = stateEstimatorPF; initialize(pf, 5000, [initialPose(1:3)', 0, 0, 0], eye(6), 'CircularVariables',[0 0 1 0 0 0]); pf.StateEstimationMethod = 'mean'; pf.ResamplingMethod = 'systematic'; % StateTransitionFcn defines how particles evolve without measurement pf.StateTransitionFcn = @exampleHelperCarBotStateTransition; % MeasurementLikelihoodFcn defines how measurement affect the our estimation pf.MeasurementLikelihoodFcn = @exampleHelperCarBotMeasurementLikelihood; % Last best estimation for x, y and theta lastBestGuess = [0 0 0];
Bucle principal
Observe que, en este ejemplo, las velocidades lineal y angular indicadas por comando para el robot son funciones dependientes del tiempo elegidas arbitrariamente. Además, observe que la temporización a tasa fija del bucle se ha realizado mediante rateControl
.
Ejecute el bucle a 20 Hz durante 20 segundos utilizando soporte a tasa fija.
r = rateControl(1/dt);
Restablezca el objeto a tasa fija para reiniciar el cronómetro. Restablezca el cronómetro justo antes de ejecutar el código dependiente del tiempo.
reset(r); simulationTime = 0; while simulationTime < 20 % if time is not up % Generate motion command that is to be sent to the robot % NOTE there will be some discrepancy between the commanded motion and the % motion actually executed by the robot. uCmd(1) = 0.7*abs(sin(simulationTime)) + 0.1; % linear velocity uCmd(2) = 0.08*cos(simulationTime); % angular velocity drive(carbot, uCmd); % Predict the carbot pose based on the motion model [statePred, covPred] = predict(pf, dt, uCmd); % Get GPS reading measurement = exampleHelperCarBotGetGPSReading(carbot); % If measurement is available, then call correct, otherwise just use % predicted result if ~isempty(measurement) [stateCorrected, covCorrected] = correct(pf, measurement'); else stateCorrected = statePred; covCorrected = covPred; end lastBestGuess = stateCorrected(1:3); % Update plot if ~isempty(get(groot,'CurrentFigure')) % if figure is not prematurely killed updatePlot(carbot, pf, lastBestGuess, simulationTime); else break end waitfor(r); % Update simulation time simulationTime = simulationTime + dt; end
Notas sobre las figuras de resultados
Las tres figuras muestran el rendimiento del seguimiento del filtro de partículas.
En la primera figura, el filtro de partículas realiza un seguimiento del automóvil correctamente cuando se aleja de su pose inicial.
En la segunda figura, el robot entra en la zona techada, donde no se pueden realizar mediciones, y las partículas evolucionan solo en función del modelo de predicción (marcado en color naranja). Puede ver cómo las partículas forman gradualmente un frente similar a una herradura y que la pose estimada se desvía de la real gradualmente.
En la tercera figura, el robot ha salido de la zona techada. Con las nuevas mediciones, la pose estimada converge gradualmente de nuevo con la pose actual.
Función de transición de estado
La función de transición de estado basada en muestreo hace que las partículas evolucionen basándose en un modelo de movimiento prescrito, de forma que las partículas formarán una representación de la distribución propuesta. A continuación, se muestra un ejemplo de una función de transición de estados basada en el modelo de movimiento de velocidad de un robot similar a un monociclo. Para más detalles sobre este modelo de movimiento, consulte el capítulo 5 en [1]. Reduzca sd1
, sd2
y sd3
para observar cómo empeora el rendimiento del seguimiento. Aquí, sd1
representa la incertidumbre de la velocidad lineal, sd2
representa la incertidumbre de la velocidad angular. sd3
es una perturbación adicional en la orientación.
function predictParticles = exampleHelperCarBotStateTransition(pf, prevParticles, dT, u)
thetas = prevParticles(:,3);
w = u(2); v = u(1);
l = length(prevParticles);
% Generate velocity samples sd1 = 0.3; sd2 = 1.5; sd3 = 0.02; vh = v + (sd1)^2*randn(l,1); wh = w + (sd2)^2*randn(l,1); gamma = (sd3)^2*randn(l,1);
% Add a small number to prevent div/0 error wh(abs(wh)<1e-19) = 1e-19;
% Convert velocity samples to pose samples predictParticles(:,1) = prevParticles(:,1) - (vh./wh).*sin(thetas) + (vh./wh).*sin(thetas + wh*dT); predictParticles(:,2) = prevParticles(:,2) + (vh./wh).*cos(thetas) - (vh./wh).*cos(thetas + wh*dT); predictParticles(:,3) = prevParticles(:,3) + wh*dT + gamma*dT; predictParticles(:,4) = (- (vh./wh).*sin(thetas) + (vh./wh).*sin(thetas + wh*dT))/dT; predictParticles(:,5) = ( (vh./wh).*cos(thetas) - (vh./wh).*cos(thetas + wh*dT))/dT; predictParticles(:,6) = wh + gamma;
end
Función de probabilidad de medición
La función de probabilidad de medición calcula la probabilidad para cada partícula prevista basándose en la norma de error entre partícula y medición. La ponderación de importancia para cada partícula se asignará en función de la probabilidad calculada. En este ejemplo concreto, predictParticles
es una matriz de N por 6 (N es el número de partículas) y measurement
es un vector de 1 por 3.
function likelihood = exampleHelperCarBotMeasurementLikelihood(pf, predictParticles, measurement)
% The measurement contains all state variables predictMeasurement = predictParticles;
% Calculate observed error between predicted and actual measurement % NOTE in this example, we don't have full state observation, but only % the measurement of current pose, therefore the measurementErrorNorm % is only based on the pose error. measurementError = bsxfun(@minus, predictMeasurement(:,1:3), measurement); measurementErrorNorm = sqrt(sum(measurementError.^2, 2));
% Normal-distributed noise of measurement % Assuming measurements on all three pose components have the same error distribution measurementNoise = eye(3);
% Convert error norms into likelihood measure. % Evaluate the PDF of the multivariate normal distribution likelihood = 1/sqrt((2*pi).^3 * det(measurementNoise)) * exp(-0.5 * measurementErrorNorm);
end
Referencia
[1] S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics, MIT Press, 2006