Фильтр частиц является основанным на выборке рекурсивным Байесовым алгоритмом оценки. Это реализовано в robotics.ParticleFilter
. В Локализовать TurtleBot Используя пример Локализации Монте-Карло мы видели приложение фильтра частиц, чтобы отследить положение робота против известной карты. Измерение сделано через 2D лазерное сканирование. В этом примере сценарий немного отличается: подобный автомобилю робот с дистанционным управлением прослеживается в наружной среде. Измерение положения робота теперь обеспечивается встроенным GPS, который является шумным. Мы также знаем команды движения, отправленные в робота, но робот не выполнит точное движение, которым управляют, из-за слаксов механической детали и/или образцовой погрешности. Этот пример покажет, как использовать robotics.ParticleFilter
, чтобы уменьшать эффекты шума в данных об измерении и получить более точную оценку положения робота. Кинематическая модель подобного автомобилю робота описана следующей нелинейной системой. Фильтр частиц идеально подходит для оценки состояния такого вида систем, когда это может иметь дело со свойственной нелинейностью.
Сценарий: подобный автомобилю робот управляет и изменяет свою скорость и держащийся угол постоянно. Положение робота измеряется некоторой шумной внешней системой, например, GPS или системой Vicon. Вдоль пути это проедет область с крышей, где никакое измерение не может быть сделано.
Входной параметр:
Шумное измерение на частичном положении робота (, , ). Обратите внимание, что это не полное измерение состояния. Никакое измерение не доступно на передней ориентации колеса () а также все скорости (, , , ).
Линейная и угловая скоростная команда отправляется в робота (, ). Примечание там будет некоторым различием между движением, которым управляют, и фактическим движением робота.
Цель: Оцените частичное положение (, , ) из подобного автомобилю робота. Обратите внимание снова что ориентация колеса () не включен в оценку. С точки зрения наблюдателя полное состояние автомобиля только [ , , , , , ].
Подход: Используйте robotics.ParticleFilter
, чтобы обработать два шумных входных параметров (ни одни из входных параметров не надежны отдельно), и сделайте лучшую оценку текущего (частичного) положения.
На предсказать этапе мы обновляем состояния частиц с упрощенной, подобной одноколесному велосипеду моделью робота, как показано ниже. Обратите внимание на то, что системная модель, используемая для оценки состояния, не является точным представлением фактической системы. Это приемлемо, пока образцовое различие хорошо получено в системном шуме (как представлено роем частицы). Для получения дополнительной информации см. predict
.
На правильном этапе вес важности (вероятность) частицы определяется ее ошибочной нормой от текущего измерения (), когда у нас только есть измерение на этих трех компонентах. Для получения дополнительной информации см. correct
.
rng('default'); % for repeatable result dt = 0.05; % time step initialPose = [0 0 0 0]'; carbot = ExampleHelperCarBot(initialPose, dt);
Этот раздел конфигурирует фильтр частиц с помощью 5 000 частиц. Первоначально все частицы случайным образом выбраны от нормального распределения со средним значением в модульной ковариации и начальном состоянии. Каждая частица содержит 6 переменных состояния (, , , , , ). Обратите внимание на то, что третья переменная отмечена как Проспект, поскольку это - автомобильная ориентация. Также очень важно задать две функции обратного вызова StateTransitionFcn
и MeasurementLikelihoodFcn
. Эти две функции непосредственно определяют производительность фильтра частиц. Детали этих двух функций могут быть найдены в последних двух разделах этого примера.
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];
Отметьте в этом примере, линейные и угловые скорости, которыми управляют, к роботу произвольно выбраны зависящие от времени функции. Кроме того, обратите внимание, что синхронизация с фиксированной процентной ставкой цикла понята через robotics.Rate
.
Запустите цикл на уровне 20 Гц в течение 20 секунд с помощью поддержки с фиксированной процентной ставкой.
r = robotics.Rate(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
Три данных показывают производительность отслеживания фильтра частиц.
В первой фигуре фильтр частиц отслеживает автомобиль хорошо, когда это уезжает от начального положения.
Во второй фигуре робот управляет в область с крышей, где никакое измерение не может быть сделано, и частицы только развиваются на основе модели прогноза (отмеченный оранжевым цветом). Вы видите, что частицы постепенно формируют подобную подкове переднюю сторону, и предполагаемое положение постепенно отклоняется от фактического.
В третьей фигуре робот изгнал из области с крышей. С новыми измерениями предполагаемое положение постепенно сходится назад к фактическому положению.
Основанная на выборке функция изменения состояния развивает частицы на основе предписанной модели движения так, чтобы частицы сформировали представление из распределения предложения. Ниже пример функции изменения состояния на основе скоростной модели движения подобного одноколесному велосипеду робота. Для получения дополнительной информации об этой модели движения, см. Главу 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