Main Content

Esta página es para la versión anterior. La página correspondiente en inglés ha sido eliminada en la versión actual.

Rastrear un robot similar a un coche usando un filtro de partículas

El filtro de partículas es un algoritmo de estimación bayesiana recursiva basado en muestreo. Se implementa en .stateEstimatorPF En el ejemplo, hemos visto la aplicación de filtro de partículas para rastrear la pose de un robot contra un mapa conocido. La medición se realiza a través de la exploración láser 2D. En este ejemplo, el escenario es un poco diferente: un robot similar a un coche controlado a distancia está siendo rastreado en el entorno al aire libre. La medición de la pose del robot ahora es proporcionada por un GPS a bordo, que es ruidoso. También conocemos los comandos de movimiento enviados al robot, pero el robot no ejecutará el movimiento exacto comandado debido a la holgura de las piezas mecánicas y / o la imprecisión del modelo. Este ejemplo mostrará cómo utilizar 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.stateEstimatorPF El modelo cinemático de un robot similar a un automóvil se describe mediante el siguiente sistema no lineal. El filtro de partículas es ideal para estimar el estado de este tipo de sistemas, ya que puede hacer frente a las no linealidades inherentes.

<math display="block">
<mrow>
<mtable columnalign="center left">
<mtr>
<mtd>
<mrow>
<munderover accent="true">
<mrow>
<mi>x</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>=</mo>
<mi>v</mi>
<mi mathvariant="normal">cos</mi>
<mo stretchy="false">(</mo>
<mi>θ</mi>
<mo stretchy="false">)</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<munderover accent="true">
<mrow>
<mi>y</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>=</mo>
<mi>v</mi>
<mi mathvariant="normal">sin</mi>
<mo stretchy="false">(</mo>
<mi>θ</mi>
<mo stretchy="false">)</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<munderover accent="true">
<mrow>
<mi>θ</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>=</mo>
<mfrac>
<mrow>
<mi>v</mi>
</mrow>
<mrow>
<mi>L</mi>
</mrow>
</mfrac>
<mi mathvariant="normal">tan</mi>
<mrow>
<mi>ϕ</mi>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<munderover accent="true">
<mrow>
<mi>ϕ</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>=</mo>
<mi>ω</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mrow>
</math>

:Escenario El robot similar al coche conduce y cambia su velocidad y ángulo de dirección continuamente. La pose del robot se mide por algún sistema externo ruidoso, por ejemplo, un GPS o un sistema Vicon. A lo largo del camino se conducirá a través de un área cubierta donde no se puede realizar ninguna medición.

:Entrada

  • La medida de ruidoso en la postura parcial del robot (

    <math display="block">
    <mrow>
    <mi>x</mi>
    </mrow>
    </math>
    ,
    <math display="block">
    <mrow>
    <mi>y</mi>
    </mrow>
    </math>
    ,
    <math display="block">
    <mrow>
    <mi>θ</mi>
    </mrow>
    </math>
    ). esto no es una medida de estado completo.Nota No hay medición disponible en la orientación de la rueda delantera (
    <math display="block">
    <mrow>
    <mi>ϕ</mi>
    </mrow>
    </math>
    ) así como todas las velocidades (
    <math display="block">
    <mrow>
    <munderover accent="true">
    <mrow>
    <mi>x</mi>
    </mrow>
    <mrow></mrow>
    <mrow>
    <mo stretchy="false">˙</mo>
    </mrow>
    </munderover>
    </mrow>
    </math>
    ,
    <math display="block">
    <mrow>
    <munderover accent="true">
    <mrow>
    <mi>y</mi>
    </mrow>
    <mrow></mrow>
    <mrow>
    <mo stretchy="false">˙</mo>
    </mrow>
    </munderover>
    </mrow>
    </math>
    ,
    <math display="block">
    <mrow>
    <munderover accent="true">
    <mrow>
    <mi>θ</mi>
    </mrow>
    <mrow></mrow>
    <mrow>
    <mo stretchy="false">˙</mo>
    </mrow>
    </munderover>
    </mrow>
    </math>
    ,
    <math display="block">
    <mrow>
    <munderover accent="true">
    <mrow>
    <mi>ϕ</mi>
    </mrow>
    <mrow></mrow>
    <mrow>
    <mo stretchy="false">˙</mo>
    </mrow>
    </munderover>
    </mrow>
    </math>
    ).

  • El comando de velocidad lineal y angular enviado al robot (

    <math display="block">
    <mrow>
    <msub>
    <mrow>
    <mi>v</mi>
    </mrow>
    <mrow>
    <mi>c</mi>
    </mrow>
    </msub>
    </mrow>
    </math>
    ,
    <math display="block">
    <mrow>
    <msub>
    <mrow>
    <mi>ω</mi>
    </mrow>
    <mrow>
    <mi>c</mi>
    </mrow>
    </msub>
    </mrow>
    </math>
    ). habrá alguna diferencia entre el movimiento comandado y el movimiento real del robot.Nota

:Gol Estimar la pose parcial (

<math display="block">
<mrow>
<mi>x</mi>
</mrow>
</math>
,
<math display="block">
<mrow>
<mi>y</mi>
</mrow>
</math>
,
<math display="block">
<mrow>
<mi>θ</mi>
</mrow>
</math>
) del robot similar al coche. de nuevo que la orientación de la rueda (Nota
<math display="block">
<mrow>
<mi>ϕ</mi>
</mrow>
</math>
) no está incluido en la estimación. , el estado completo del coche es sólo [Desde la perspectiva del observador
<math display="block">
<mrow>
<mi>x</mi>
</mrow>
</math>
,
<math display="block">
<mrow>
<mi>y</mi>
</mrow>
</math>
,
<math display="block">
<mrow>
<mi>θ</mi>
</mrow>
</math>
,
<math display="block">
<mrow>
<munderover accent="true">
<mrow>
<mi>x</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</math>
,
<math display="block">
<mrow>
<munderover accent="true">
<mrow>
<mi>y</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</math>
,
<math display="block">
<mrow>
<munderover accent="true">
<mrow>
<mi>θ</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</math>
].

:Enfoque Se utiliza para procesar las dos entradas de noconsumo (ninguna de las entradas es fiable por sí misma) y hacer la mejor estimación de la pose actual (parcial).stateEstimatorPF

  • En la etapa, actualizamos los estados de las partículas con un modelo de robot simplificado, similar a un ciclo, como se muestra a continuación.Predecir Tenga en cuenta que el modelo de sistema utilizado para la estimación de estado no es una representación exacta del sistema real. Esto es aceptable, siempre y cuando la diferencia del modelo esté bien capturada en el ruido del sistema (representada por el enjambre de partículas). Para obtener más información, consulte .predict

<math display="block">
<mrow>
<mtable columnalign="center left">
<mtr>
<mtd>
<mrow>
<munderover accent="true">
<mrow>
<mi>x</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>=</mo>
<mi>v</mi>
<mi mathvariant="normal">cos</mi>
<mo stretchy="false">(</mo>
<mi>θ</mi>
<mo stretchy="false">)</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<munderover accent="true">
<mrow>
<mi>y</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>=</mo>
<mi>v</mi>
<mi mathvariant="normal">sin</mi>
<mo stretchy="false">(</mo>
<mi>θ</mi>
<mo stretchy="false">)</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<munderover accent="true">
<mrow>
<mi>θ</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>=</mo>
<mi>ω</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mrow>
</math>

  • En la etapa, el peso de importancia (probabilidad) de una partícula viene determinado por su norma de error de la medición de corriente (Correcto

    <math display="block">
    <mrow>
    <msqrt>
    <mrow>
    <mo stretchy="false">(</mo>
    <mi>Δ</mi>
    <mi>x</mi>
    <msup>
    <mrow>
    <mo stretchy="false">)</mo>
    </mrow>
    <mrow>
    <mn>2</mn>
    </mrow>
    </msup>
    <mo>+</mo>
    <mo stretchy="false">(</mo>
    <mi>Δ</mi>
    <mi>y</mi>
    <msup>
    <mrow>
    <mo stretchy="false">)</mo>
    </mrow>
    <mrow>
    <mn>2</mn>
    </mrow>
    </msup>
    <mo>+</mo>
    <mo stretchy="false">(</mo>
    <mi>Δ</mi>
    <mi>θ</mi>
    <msup>
    <mrow>
    <mo stretchy="false">)</mo>
    </mrow>
    <mrow>
    <mn>2</mn>
    </mrow>
    </msup>
    </mrow>
    </msqrt>
    </mrow>
    </math>
    ), ya que sólo tenemos medición de estos tres componentes. Para obtener más información, consulte .correct

Inicializar un robot similar a un coche

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 5000 partículas. Inicialmente todas las partículas se seleccionan aleatoriamente de una distribución normal con la media en el estado inicial y la covarianza de la unidad. Cada partícula contiene 6 variables de estado (

<math display="block">
<mrow>
<mi>x</mi>
</mrow>
</math>
,
<math display="block">
<mrow>
<mi>y</mi>
</mrow>
</math>
,
<math display="block">
<mrow>
<mi>θ</mi>
</mrow>
</math>
,
<math display="block">
<mrow>
<munderover accent="true">
<mrow>
<mi>x</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</math>
,
<math display="block">
<mrow>
<munderover accent="true">
<mrow>
<mi>y</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</math>
,
<math display="block">
<mrow>
<munderover accent="true">
<mrow>
<mi>θ</mi>
</mrow>
<mrow></mrow>
<mrow>
<mo stretchy="false">˙</mo>
</mrow>
</munderover>
</mrow>
</math>
). Tenga en cuenta que la tercera variable está marcada como Circular ya que es la orientación del coche. También es muy importante especificar dos funciones de devolución de llamada y .StateTransitionFcnMeasurementLikelihoodFcn 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

Tenga en cuenta que en este ejemplo, las velocidades lineales y angulares ordenadas para el robot son funciones dependientes del tiempo seleccionadas arbitrariamente. También, observe la sincronización de la velocidad fija del loop se realiza a través de .rateControl

Ejecute el bucle a 20 Hz durante 20 segundos utilizando el soporte de velocidad fija.

r = rateControl(1/dt);

Restablezca el objeto de velocidad fija para reiniciar el temporizador. Restablezca el temporizador 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

Detalles de las cifras de resultados

Las tres cifras muestran el rendimiento de seguimiento del filtro de partículas.

  • En la primera figura, el filtro de partículas está rastreando el coche bien, ya que se aleja de la pose inicial.

  • En la segunda figura, el robot conduce hacia el área cubierta, donde no se puede realizar ninguna medición, y las partículas sólo evolucionan en función del modelo de predicción (marcado con color naranja). Se pueden ver las partículas formar gradualmente un frente similar a una herradura, y la pose estimada se desvía gradualmente de la real.

  • En la tercera figura, el robot ha expulsado de la zona techada. Con nuevas mediciones, la pose estimada converge gradualmente de nuevo a la pose real.

Función de transición de estado

La función de transición de estado basada en muestreo evoluciona las partículas basándose en un modelo de movimiento prescrito para que las partículas formen una representación de la distribución de la propuesta. A continuación se muestra un ejemplo de una función de transición de estado basada en el modelo de movimiento de velocidad de un robot similar a un ciclo. Para obtener más detalles sobre este modelo de movimiento, consulte el capítulo 5 en .[1] Disminuir y ver cómo se deteriora el rendimiento de seguimiento.sd1sd2sd3 Aquí representa la incertidumbre en la velocidad lineal, representa la incertidumbre en la velocidad angular. es una perturbación adicional en la orientación.sd1sd2sd3

   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 de cada partícula predicha en función de la norma de error entre la partícula y la medición. El peso de importancia para cada partícula se asignará en función de la probabilidad calculada. En este ejemplo en particular, es una matriz N x 6 (N es el número de partículas), y es un vector de 1 x 3.predictParticlesmeasurement

   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