Отследите подобный автомобилю робот используя фильтр частиц

Фильтр частиц является основанным на выборке рекурсивным Байесовым алгоритмом оценки, который реализован в stateEstimatorPF объект. В этом примере подобный автомобилю робот с дистанционным управлением прослеживается в наружной среде. Измерение положения робота обеспечивается встроенным GPS, который является шумным. Существуют известные команды движения, отправленные в робота, но робот не выполнит точное движение, которым управляют, из-за механической слабой или погрешности модели. Этот пример покажет, как использовать stateEstimatorPF уменьшать эффекты шума в данных об измерении и получить более точную оценку положения робота. Кинематическая модель подобного автомобилю робота описана следующей нелинейной системой. Фильтр частиц идеально подходит для оценки состояния такого вида систем, когда это может иметь дело со свойственной нелинейностью.

x˙=vcos(θ)y˙=vsin(θ)θ˙=vLtanϕϕ˙=ω

Сценарий: подобный автомобилю робот управляет и изменяет свою скорость и держащийся угол постоянно. Положение робота измеряется некоторой шумной внешней системой, e.g. GPS или система Vicon. Вдоль пути это проедет область с крышей, где никакое измерение не может быть сделано.

Входной параметр:

  • Шумное измерение на частичном положении робота (x, y, θ). Обратите внимание, что это не полное измерение состояния. Никакое измерение не доступно на передней ориентации колеса (ϕ) а также все скорости (x˙, y˙, θ˙, ϕ˙).

  • Линейные и угловые скорости отправляются в робота (vc, ωc). Примечание там будет некоторым различием между движением, которым управляют, и фактическим движением робота.

Цель: Оцените частичное положение (x, y, θ) из подобного автомобилю робота. Обратите внимание снова что ориентация колеса (ϕ) не включен в оценку. С точки зрения наблюдателя полное состояние автомобиля только [ x, y, θ, x˙, y˙, θ˙ ].

Подход: Использование stateEstimatorPF чтобы обработать два шумных входных параметров (ни одни из входных параметров не надежны отдельно) и делают лучшую оценку текущего (частичного) положения.

  • На предсказать этапе мы обновляем состояния частиц с упрощенной, подобной одноколесному велосипеду моделью робота, как показано ниже. Обратите внимание на то, что системная модель, используемая для оценки состояния, не является точным представлением фактической системы. Это приемлемо, пока различие модели хорошо получено в системном шуме (как представлено роем частицы). Для получения дополнительной информации смотрите predict.

x˙=vcos(θ)y˙=vsin(θ)θ˙=ω

  • На правильном этапе вес важности (вероятность) частицы определяется ее ошибочной нормой из текущего измерения ((Δx)2+(Δy)2+(Δθ)2), когда у нас только есть измерение на этих трех компонентах. Для получения дополнительной информации смотрите correct.

Инициализируйте подобного Автомобилю робота

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

Figure CarBot contains an axes. The axes with title t = 0 contains 11 objects of type rectangle, scatter, line, text. These objects represent actual pose, estimated pose.

Настройте Фильтр частиц

Этот раздел конфигурирует фильтр частиц с помощью 5 000 частиц. Первоначально все частицы случайным образом выбраны от нормального распределения со средним значением в модульной ковариации и начальном состоянии. Каждая частица содержит 6 переменных состояния (x, y, θ, x˙, y˙, θ˙). Обратите внимание на то, что третья переменная отмечена как Проспект, поскольку это - автомобильная ориентация. Также очень важно задать две функции обратного вызова StateTransitionFcn и MeasurementLikelihoodFcn. Эти две функции непосредственно определяют эффективность фильтра частиц. Детали этих двух функций могут быть найдены в последних двух разделах этого примера.

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];

Основной цикл

Отметьте в этом примере, линейное, которым управляют, и скорости вращения к роботу произвольно выбраны зависящие от времени функции. Кроме того, обратите внимание, что синхронизация с фиксированной процентной ставкой цикла понята через rateControl.

Запустите цикл на уровне 20 Гц в течение 20 секунд с помощью поддержки с фиксированной процентной ставкой.

r = rateControl(1/dt);

Сбросьте объект с фиксированной процентной ставкой перезапустить таймер. Сбросьте таймер прямо прежде, чем запустить зависящий от времени код.

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

Figure CarBot contains an axes. The axes with title t = 19.95 contains 11 objects of type rectangle, scatter, line, text. These objects represent actual pose, estimated pose.

Детали фигур результата

Три рисунка показывают эффективность отслеживания фильтра частиц.

  • На первом рисунке фильтр частиц отслеживает автомобиль хорошо, когда это уезжает от начального положения.

  • На втором рисунке робот управляет в область с крышей, где никакое измерение не может быть сделано, и частицы только развиваются на основе модели предсказания (отмеченный оранжевым цветом). Вы видите, что частицы постепенно формируют подобную подкове переднюю сторону, и предполагаемое положение постепенно отклоняется от фактического.

  • На третьем рисунке робот изгнал из области с крышей. С новыми измерениями предполагаемое положение постепенно сходится назад к фактическому положению.

Функция изменения состояния

Основанная на выборке функция изменения состояния развивает частицы на основе предписанной модели движения так, чтобы частицы сформировали представление из распределения предложения. Ниже пример функции изменения состояния на основе скоростной модели движения подобного одноколесному велосипеду робота. Для получения дополнительной информации об этой модели движения, см. Главу 5 в [1]. Уменьшите sd1, sd2 и sd3 чтобы видеть, как эффективность отслеживания ухудшается. Здесь sd1 представляет неопределенность в линейной скорости, sd2 представляет неопределенность в скорости вращения. sd3 дополнительное возмущение на ориентации.

   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

Функция правдоподобия измерения

Функция правдоподобия измерения вычисляет вероятность для каждой предсказанной частицы на основе ошибочной нормы между частицей и измерением. Вес важности для каждой частицы будет присвоен на основе вычисленной вероятности. В этом конкретном примере, predictParticles N x, 6 матриц (N количество частиц), и measurement 1 x 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

Ссылка

[1] С. Трун, В. Бергард, Д. Фокс, вероятностная робототехника, нажатие MIT, 2006

Для просмотра документации необходимо авторизоваться на сайте