Алгоритм локализации Монте-Карло (MCL) используется для оценки положения и ориентации робота. Алгоритм использует известную карту окружения, данные датчика области значений и данные датчика одометрии. Чтобы увидеть, как создать объект и использовать этот алгоритм, смотрите monteCarloLocalization.
Чтобы локализовать робота, алгоритм MCL использует фильтр частиц, чтобы оценить его положение. Частицы представляют распределение вероятных состояний для робота. Каждая частица представляет возможное состояние робота. Частицы сходятся вокруг одного местоположения, когда робот перемещается в окружении и чувствует различные части окружения с помощью датчика области значений. Движение робота измеряется с помощью датчика одометрии.
Частицы обновляются в этом процессе:
Частицы распространяются на основе изменения положения и заданной модели движения, MotionModel.
Частицам присваивают веса на основе вероятности получения показания датчика области значений для каждой частицы. Это чтение основано на модели датчика, которую вы задаете в SensorModel.
Исходя из этих весов, оценка состояния робота извлекается на основе весов частиц. Группа частиц с наибольшим весом используется для оценки положения робота.
Наконец, частицы повторно дискретизируются на основе заданных ResamplingInterval. Повторная дискретизация регулирует положения частиц и улучшает эффективность, регулируя количество используемых частиц. Это ключевая возможность для корректировки изменений и сохранения частиц релевантными для оценки состояния робота.
Алгоритм выводит предполагаемое положение и ковариацию. Эти оценки являются средним значением и ковариацией наиболее взвешенного кластера частиц. Для непрерывного отслеживания повторите эти шаги в цикле, чтобы распространить частицы, оценить их вероятность и получить лучшую оценку состояния.
Для получения дополнительной информации о фильтрах частиц как общем приложении, смотрите Рабочий процесс Фильтра частиц.
При работе с алгоритмом локализации цель состоит в том, чтобы оценить состояние вашей системы. Для приложений робототехники это предполагаемое состояние обычно является положением робота. Для monteCarloLocalization объект, вы задаете это положение как трехэлементный вектор. Положение соответствует x-y положению, [x y], и угловой ориентации, theta.
Алгоритм MCL оценивает эти три значения на основе входных входов датчика окружения и заданной модели движения вашей системы. Значение выхода от использования monteCarloLocalization объект включает в себя pose, которое является лучшим расчетным состоянием [x y theta] значения. Частицы распределены вокруг начального положения, InitialPose, или дискретизация равномерно с использованием глобальной локализации. Положение вычисляется как среднее значение наивысшего взвешенного кластера частиц после того, как эти частицы были скорректированы на основе измерений.
Этот график показывает самую высокую взвешенный кластер и окончательное положение робота, отображаемое над частицами выборок в зеленом цвете. С большим количеством итераций алгоритма MCL и коррекциями на измерения, частицы сходятся к истинному местоположению робота. Однако возможно, что кластеры частиц могут иметь высокие веса для ложных оценок и сходиться на неправильном месте. Если происходит неправильное сходимость, переопределите частицы путем сброса алгоритма MCL с обновленным InitialPose.
Когда вы впервые создаете monteCarloLocalization алгоритм, задайте минимальный и максимальный пределы частиц при помощи ParticleLimits свойство. Большее количество частиц увеличивает вероятность того, что частицы сходятся на фактическом месте. Однако более низкое число частиц быстрее. Количество частиц динамически регулируется в пределах, основанных на весах кластеров частиц. Эта корректировка помогает уменьшить количество частиц с течением времени, так что локализация может работать более эффективно.
Частицы должны быть отобраны через заданное распределение. Чтобы инициализировать частицы в пространстве состояний, можно использовать либо начальное положение, либо глобальную локализацию. С помощью глобальной локализации можно равномерно распределить частицы через ожидаемое пространство состояний (вытащенное из Map свойство вашего SensorModel объект). В объекте MCL по умолчанию установите GlobalLocalization свойство к true.
mcl = monteCarloLocalization; mcl.GlobalLocalization = true;
Глобальная локализация требует большего количества частиц для эффективной выборки частиц через пространство состояний. Больше частиц увеличивают вероятность успешного сходимости на фактическом состоянии. Это большое распределение значительно уменьшает начальную эффективность, пока частицы не начнут сходиться, и число частиц может быть уменьшено.
По умолчанию для глобальной локализации задано значение false. Без глобальной локализации необходимо задать InitialPose и InitialCovariance свойства, что помогает локализовать частицы. Используя это начальное положение, частицы более тесно сгруппированы вокруг предполагаемого состояния. Тесная группировка частиц позволяет вам использовать меньше из них, и увеличивает скорость и точность отслеживания во время первых итераций.
Эти изображения были взяты из примера Localize TurtleBot Using Monte Carlo Localization, который показывает, как использовать алгоритм MCL с TurtleBot® в известном окружении.
Чтобы локализовать робота постоянно, вы должны повторно отобразить частицы и обновить алгоритм. Используйте UpdateThreshold и ResamplingInterval свойства для управления, когда происходит повторная дискретизация и обновления предполагаемого состояния.
The UpdateThreshold является трехэлементным вектором, который определяет минимальное изменение положения робота, [x y theta], для запуска обновления. Изменение переменной больше, чем на этот минимум, запускает обновление, заставляя объект возвращать новую оценку состояния. Это изменение положения робота основано на одометрии, которая задается в функциональной форме объекта. Настройте эти пороги на основе свойств датчика и движения вашего робота. Случайный шум или незначительные изменения, большие вашего порога, могут вызвать ненужное обновление и повлиять на вашу эффективность. The ResamplingInterval свойство определяет количество обновлений для запуска повторной дискретизации частиц. Для примера - интервал повторной дискретизации 2 повторяется при каждом другом обновлении.
Преимущество повторной дискретизации частиц в том, что вы обновляете возможные местоположения, которые способствуют окончательной оценке. Повторная дискретизация перераспределяет частицы на основе их весов и развивает частицы на основе Модели Движения. В этом процессе частицы с меньшим весом устраняются, помогая частицам сходиться к истинному состоянию робота. Количество частиц динамически изменяется, чтобы улучшить скорость или отслеживание.
Эффективность алгоритма зависит от правильной повторной дискретизации. Если частицы широко диспергированы и начальное положение робота не известно, алгоритм поддерживает высокое количество частиц. Когда алгоритм сходится к истинному местоположению, это уменьшает количество частиц и увеличивает скорость эффективности. Вы можете настроить свои ParticleLimits свойство для ограничения минимальных и максимальных частиц, используемых для помощи с эффективностью.
Модели движения и датчика для алгоритма MCL аналогичны StateTransitionFcn и MeasurementLikelihoodFcn функции для stateEstimatorPF объект, которые описаны в параметрах фильтра частиц. Для алгоритма MCL эти модели более специфичны для локализации робота. После вызова объекта, чтобы изменить MotionModel или SensorModel свойства, сначала необходимо вызвать release на вашем объекте.
По умолчанию в monteCarloLocalization использует likelihoodFieldSensorModel объект как модель датчика. Эта модель датчика содержит параметры, специфичные для используемого датчика области значений, 2-D информацию карты для окружения робота и характеристики шума измерения. Модель датчика использует параметры с измерениями области значений, чтобы вычислить вероятность измерений, учитывая текущее положение робота. Без факторизации этих параметров некоторые ошибки измерения могут перекосить оценку состояния или увеличить вес нерелевантных частиц.
Свойства датчика области значений:
SensorPose - Положение датчика области значений относительно местоположения робота. Это положение используется, чтобы преобразовать показания области значений в координатную систему координат робота.
SensorLimits - Минимальный и максимальный пределы области значений. Измерения за пределами этих областей значений не учитываются в вычислении вероятности.
NumBeams - Количество лучей, используемых для вычисления вероятности. Можно улучшить скорость эффективности путем уменьшения количества используемых балок.
Также известно, что измерения области значений дают ложные показания из-за шума системы или другого воздействия на окружающую среду. Для расчета ошибки датчика задайте следующие параметры:
MeasurementNoise - Стандартное отклонение для шума измерения. Это отклонение относится к считыванию области значений и учитывает любые помехи датчику. Установите это значение на основе информации от вашего датчика области значений.
RandomMeasurementWeight - Вес для вероятности случайного измерения. Установите низкую вероятность для случайных измерений. Значение по умолчанию является 0.05.
ExpectedMeasurementWeight - Вес для вероятности ожидаемого измерения. Установите высокую вероятность для ожидаемых измерений. Значение по умолчанию является 0.95.
Модель датчика также хранит карту окружения робота как сетку заполнения. Использование binaryOccupancyMap чтобы указать вашу карту с занятыми и свободными пространствами. Установите все неизвестные пространства на карте как свободные местоположения. Установка их в свободные местоположения препятствует тому, чтобы алгоритм совпадал с обнаруженными объектами в этих областях карты.
Также можно задать MaximumLikelihoodDistance, что ограничивает область для поиска препятствий. Значение MaximumLikelihoodDistance - максимальное расстояние до ближайшего препятствия, которое используется для расчета правдоподобия.
Модель движения для локализации робота помогает предсказать, как частицы развиваются в течение времени при повторной дискретизации. Это представление кинематики робота. Модель движения, включенная по умолчанию с алгоритмом MCL, является основанной на одометрии дифференциальной моделью движения привода (odometryMotionModel). Без модели движения предсказать следующий шаг сложнее. Важно знать возможности вашей системы, чтобы алгоритм локализации мог планировать распределения частиц, чтобы получить лучшие оценки состояния. Обязательно учитывайте ошибки от энкодеров колес или других датчиков, используемых для измерения одометрии. Ошибки в системе определяют разброс распределения частиц.
Можно задать ожидаемую ошибку, основанную на движении вашего робота как четырехэлементного вектора, Noise. Эти четыре элемента заданы как веса стандартных отклонений для [1]:
Вращательная ошибка из-за вращательного движения
Вращательная ошибка из-за поступательного движения
Поступательная ошибка из-за поступательного движения
Поступательная ошибка из-за вращательного движения
Для роботов с дифференциальным приводом, когда робот переходит от стартового положения к конечному положению, изменение положения может быть рассмотрено как:
Вращение в конечное положение
Перевод по прямой линии в конечное положение
Вращение к ориентации цели
Принимая эти шаги, можно визуализировать эффект ошибок в повороте и перемещении. Ошибки в начальном повороте приводят к тому, что ваши возможные положения распределяются в C-образной форме вокруг конечного положения.
Большие переводные ошибки приводят к тому, что ваши возможные позиции распределяются вокруг прямой линии в конечное положение.
Большие ошибки в перемещении и вращении могут привести к более широким позициям.
Кроме того, ошибки вращения влияют на ориентацию конечного положения. Понимание этих эффектов помогает вам задать Гауссов шум в Noise свойство MotionModel объект для вашего конкретного приложения. Как показывают изображения, каждый параметр не управляет непосредственно дисперсией и может варьироваться в зависимости от строения и геометрии робота. Кроме того, многочисленные изменения положения, когда робот перемещается по вашему окружению, могут увеличить эффекты этих ошибок на многих различных шагах. Путем точного определения этих параметров частицы распределяются соответственно, чтобы дать алгоритму MCL достаточно гипотез, чтобы найти лучшую оценку для местоположения робота.
[1] Трун, Себастьян и Дитер Фокс. Вероятностная робототехника. 3 эд. Cambridge, Mass: MIT Press, 2006. p.136.
likelihoodFieldSensorModel | monteCarloLocalization | odometryMotionModel
