Esta página aún no se ha traducido para esta versión. Puede ver la versión más reciente de esta página en inglés.

Rastrear un robot como un coche usando filtro de partículas

El filtro de partículas es un algoritmo de estimación Bayesiano recursivo basado en muestreo. Se implementa en.robotics.ParticleFilter En el ejemplo, hemos visto la aplicación de filtro de partículas para rastrear la pose de un robot contra un mapa conocido.Localize TurtleBot utilizando la localización de Montecarlo La medición se realiza mediante escaneo láser 2D. En este ejemplo, el escenario es un poco diferente: un robot de coche de control remoto se está rastreando en el ambiente al aire libre. La medición de pose de 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 ordenado debido a los pantalones mecánicos de la parte y/o la inexactitud 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.robotics.ParticleFilter El modelo cinemático de un robot auto-como se describe por el siguiente sistema no lineal. El filtro de partículas es ideal para estimar el estado de este tipo de sistemas, ya que puede lidiar con 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>

:Scenario El robot maneja y cambia su velocidad y su ángulo de dirección de forma continua. La pose del robot se mide por un sistema externo ruidoso, por ejemplo, un GPS o un sistema Vicon. A lo largo del camino se conduce a través de una zona techada donde no se puede realizar ninguna medición.

:Input

  • La medición ruidosa en la pose 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.Note 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 ordenado y el movimiento real del robot.Note

:Goal 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 de auto-como. otra vez que la orientación de la rueda (Note
<math display="block">
<mrow>
<mi>ϕ</mi>
</mrow>
</math>
) no se incluye en la estimación. , el estado completo del coche es sólo [From the observer's perspective
<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>
].

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

  • En la etapa, actualizamos los Estados de las partículas con un modelo de robot simplificado, semejante a un monociclo, como se muestra a continuación.predict 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 se Capture bien en el ruido del sistema (representado 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 está determinado por su norma de error a partir de la medición actual (correct

    <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 mediciones en estos tres componentes. Para obtener más información, consulte.correct

Inicializar un robot como un coche

rng('default'); % for repeatable result dt = 0.05; % time step initialPose = [0  0  0  0]'; carbot = ExampleHelperCarBot(initialPose, dt);

Configure el filtro de partículas

Esta sección configura el filtro de partículas utilizando 5000 partículas. Inicialmente, todas las partículas se recogen aleatoriamente de una distribución normal con 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 se marca como circular, ya que es la orientación del automóvil. 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 = robotics.ParticleFilter;  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];

Main Loop

Tenga en cuenta en este ejemplo, las velocidades lineales y angulares comandadas al robot son funciones dependientes del tiempo que se recogen arbitrariamente. También, observe la sincronización de la tarifa fija del Loop se realiza a través.rateControl

Ejecute loop a 20 Hz durante 20 segundos usando soporte de tasa fija.

r = robotics.Rate(1/dt);

Restablezca el objeto de tasa 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 figuras muestran el rendimiento de rastreo del filtro de partículas.

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

  • En la segunda figura, el robot conduce a la zona techada, donde no se puede realizar ninguna medición, y las partículas solo evolucionan según el modelo de predicción (marcado con color naranja). Se puede ver que las partículas forman gradualmente un frente parecido a una herradura, y la pose estimada se desvía gradualmente de la actual.

  • En la tercera figura, el robot ha expulsado del área 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 en función de 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 como un monociclo. Para obtener más información sobre este modelo de movimiento, consulte el capítulo 5.[1] Disminuya y vea 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 pronosticada basándose en la norma de error entre la partícula y la medida. 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 de 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