Realizar el seguimiento de un robot similar a un automóvil utilizando un filtro de partículas

Abrir script en vivo

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.

$\begin{array}{cl} x_{}^{˙} & = v \cos (θ) \\ y_{}^{˙} & = v \sin (θ) \\ θ_{}^{˙} & = \frac{v}{L} \tan ϕ \\ ϕ_{}^{˙} & = ω \end{array}$

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 ( $x$ , $y$ , $θ$ ). 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 ( $x_{}^{˙}$ , $y_{}^{˙}$ , $θ_{}^{˙}$ , $ϕ_{}^{˙}$ ).
El comando de velocidad lineal y angular enviado al robot ( $v_{c}$ , $ω_{c}$ ). Observe que habrá alguna diferencia entre el movimiento indicado por comando y el movimiento real del robot.

Objetivo: estimar la pose parcial ( $x$ , $y$ , $θ$ ) 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 [ $x$ , $y$ , $θ$ , $x_{}^{˙}$ , $y_{}^{˙}$ , $θ_{}^{˙}$ ].

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.

$\begin{array}{cl} x_{}^{˙} & = v \cos (θ) \\ y_{}^{˙} & = v \sin (θ) \\ θ_{}^{˙} & = ω \end{array}$

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 ( $\sqrt{(Δ x)^{2} + (Δ y)^{2} + (Δ θ)^{2}}$ ), 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 ( $x$ , $y$ , $θ$ , $x_{}^{˙}$ , $y_{}^{˙}$ , $θ_{}^{˙}$ ). 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