Отслеживание автомобильного робота с помощью фильтра частиц

Открыть сценарий в реальном времени

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

$\begin{array}{cl} _{}^{} & x˙=vcos (start \\ )_{}^{} & y˙=vsin \\ (_{}^{start} & ) \frac{}{} \\ _{}^{} \end{array}$ θ˙=vLtanϕϕ˙=ω

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

Входные данные:

Шумное измерение в частичной позе робота ( $x$ , $y$ , $start$ ). Обратите внимание, что это не полное измерение состояния. Никакое измерение не доступно на передней ориентации колеса (ϕ), а также все скорости ( $_{}^{x˙}$ , $_{}^{y˙}$ , ${θ˙}_{}^{,}$ ${ϕ˙}_{}^{}$ ).
Команда линейной и угловой скорости, посылаемая роботу ( $_{vc}$ , ${startc}_{}$ ). Обратите внимание, что существует некоторая разница между командным движением и фактическим движением робота.

Цель: Оценка частичной позы ( $x$ , $y$ , Обратите внимание на то, что ориентация колеса $не$ включена в оценку. С точки зрения наблюдателя, полное состояние автомобиля только [x $,$ y $,$ start $,$ ${x˙}_{}^{,}$ ${y˙}_{}^{,}$ θ˙ $_{}^{].}$

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

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

$\begin{array}{cl} _{}^{} & x˙=vcos (start \\ )_{}^{} & y˙=vsin \\ (_{}^{start} & ) \end{array}$ θ˙=ω

На правильном этапе вес важности (правдоподобие) частицы определяется её нормой погрешности по текущему измерению ( $\sqrt{(Δ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.

Настройка фильтра частиц

В этом разделе описывается настройка фильтра частиц с использованием 5000 частиц. Первоначально все частицы случайным образом отбираются из нормального распределения со средним значением в начальном состоянии и единичной ковариацией. Каждая частица содержит 6 переменных состояния ( $x$ , $y$ , $start$ , $_{}^{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 Press, 2006

Документация