Этот пример демонстрирует применение алгоритма Локализации Монте-Карло (MCL) на TurtleBot® в моделируемой среде Gazebo®.
Локализация Монте-Карло (MCL) является алгоритмом, чтобы локализовать робота с помощью фильтра частиц. Алгоритм требует известной карты, и задача состоит в том, чтобы оценить положение (положение и ориентация) робота в рамках карты на основе движения и обнаружения робота. Алгоритм запускается с начальной веры распределения вероятностей положения робота, которое представлено частицами, распределенными согласно такой вере. Эти частицы распространены после модели движения робота каждый раз изменения положения робота. После получения новых показаний датчика каждая частица оценит свою точность путем проверки, как, вероятно, это получило бы такие показания датчика в своем текущем положении. Затем алгоритм перераспределит (передискретизируют) частицы, чтобы сместить частицы, которые более точны. Продолжите выполнять итерации этих перемещение, обнаружение и передискретизация шагов, и все частицы должны сходиться к одному кластеру около истинного положения робота, если локализация успешна.
Адаптивная локализация Монте-Карло (AMCL) является вариантом MCL, реализованного в robotics.MonteCarloLocalization. AMCL динамически настраивает количество частиц на основе KL-расстояния [1], чтобы гарантировать, что распределение частицы сходится к истинному распределению состояния робота на основе всего прошлого датчика и измерений движения с высокой вероятностью.
Текущая реализация MATLAB® AMCL может быть применена к любому роботу с дифференциальным приводом, оборудованному средством поиска области значений.
Симуляция Gazebo TurtleBot должна запускаться для этого примера, чтобы работать.
Предпосылки: Запуск с Gazebo и Моделируемым TurtleBot, доступом к tf Дереву Преобразования в ROS, обменивается Данными с Издателями ROS и Подписчиками.
Примечание: Начиная в R2016b, вместо того, чтобы использовать метод шага, чтобы выполнить операцию, заданную Системным объектом, можно вызвать объект с аргументами, как будто это была функция. Например, y = step(obj,x) и y = obj(x) выполняют эквивалентные операции.
Во-первых, породите моделируемый TurtleBot в офисной среде в виртуальной машине следующими шагами в Запуске с Gazebo и Моделируемый TurtleBot, чтобы запустить Gazebo TurtleBot World с рабочего стола.
В вашем экземпляре MATLAB на хосте - компьютере запустите следующие команды, чтобы инициализировать глобальный узел ROS в MATLAB и соединить с ведущим устройством ROS в виртуальной машине через ее IP-адрес ipaddress. Замените ipaddress на IP-адрес вашего TurtleBot в виртуальной машине.
ipaddress = '192.168.203.129';
rosinit(ipaddress);Initializing global node /matlab_global_node_68982 with NodeURI http://192.168.203.1:55528/
Размещение моделируемой офисной среды:
Загрузите бинарную сетку заполнения офисной среды в Gazebo. Карта сгенерирована путем управления TurtleBot в офисной среде. Карта создается с помощью переносящих область значений показаний из Kinect® и наземных положений истины от темы gazebo/model_states. Обратитесь к Построению карты при известном местоположении для более подробного объяснения.
load officemap.mat
show(map)
TurtleBot может быть смоделирован как робот с дифференциальным приводом, и его движение может быть оценено с помощью данных об одометрии. Свойство Noise задает неуверенность во вращательном и линейном движении робота. Увеличение свойства odometryModel.Noise позволит более распространение при распространении частиц с помощью измерений одометрии. Обратитесь к robotics.OdometryMotionModel для деталей свойства.
odometryModel = robotics.OdometryMotionModel; odometryModel.Noise = [0.2 0.2 0.2 0.2];
Датчик на TurtleBot является моделируемым средством поиска области значений, преобразованным от показаний Kinect. Полевой метод вероятности используется, чтобы вычислить вероятность восприятия набора измерений путем сравнения конечных точек измерений средства поиска области значений к карте заполнения. Если конечные точки совпадают с занятыми точками в карте заполнения, вероятности восприятия, что такие измерения высоки. Модель датчика должна быть настроена, чтобы совпадать с фактическим свойством датчика достигнуть лучших результатов испытаний. Свойство SensorLimits задает минимальную и максимальную область значений показаний датчика. Свойство Map задает карту заполнения, используемую для вычислительного поля вероятности. Обратитесь к robotics.LikelihoodFieldSensorModel для деталей свойства.
rangeFinderModel = robotics.LikelihoodFieldSensorModel; rangeFinderModel.SensorLimits = [0.45 8]; rangeFinderModel.Map = map;
Установите rangeFinderModel.SensorPose на координатное преобразование фиксированной камеры относительно основы робота. Это используется, чтобы преобразовать лазерные показания от кадра камеры до опорной рамы TurtleBot. Обратитесь к доступу к tf Дереву Преобразования в ROS для получения дополнительной информации о координатных преобразованиях.
Обратите внимание на то, что в настоящее время SensorModel только совместим с датчиками, которые закрепляются на кадре робота, что означает, что датчик преобразовывает, является постоянным.
% Query the Transformation Tree (tf tree) in ROS. tftree = rostf; waitForTransform(tftree,'/base_link','/camera_depth_frame'); sensorTransform = getTransform(tftree,'/base_link', '/camera_depth_frame'); % Get the euler rotation angles. laserQuat = [sensorTransform.Transform.Rotation.W sensorTransform.Transform.Rotation.X ... sensorTransform.Transform.Rotation.Y sensorTransform.Transform.Rotation.Z]; laserRotation = quat2eul(laserQuat, 'ZYX'); % Setup the |SensorPose|, which includes the translation along base_link's % +X, +Y direction in meters and rotation angle along base_link's +Z axis % in radians. rangeFinderModel.SensorPose = ... [sensorTransform.Transform.Translation.X sensorTransform.Transform.Translation.Y laserRotation(1)];
Создайте подписчиков ROS для получения датчика и измерений одометрии от TurtleBot.
laserSub = rossubscriber('/scan'); odomSub = rossubscriber('/odom');
Создайте издателя ROS для отсылки скоростных команд к TurtleBot. TurtleBot подписывается на '/mobile_base/commands/velocity' для скоростных команд.
[velPub,velMsg] = ... rospublisher('/mobile_base/commands/velocity','geometry_msgs/Twist');
Инстанцируйте объекта AMCL amcl. Смотрите robotics.MonteCarloLocalization для получения дополнительной информации о классе.
amcl = robotics.MonteCarloLocalization; amcl.UseLidarScan = true;
Присвойте свойства MotionModel и SensorModel в объекте amcl.
amcl.MotionModel = odometryModel; amcl.SensorModel = rangeFinderModel;
Фильтр частиц только обновляет частицы, когда перемещение робота превышает UpdateThresholds, который задает минимальное смещение в [x, y, отклонение от курса], чтобы инициировать обновление фильтра. Это предотвращает слишком частые обновления из-за шума датчика. Передискретизация частицы происходит после обновлений фильтра amcl.ResamplingInterval. Используя большее число приводит к более медленному истощению частицы по цене более медленной сходимости частицы также.
amcl.UpdateThresholds = [0.2,0.2,0.2]; amcl.ResamplingInterval = 1;
amcl.ParticleLimits задает нижнюю и верхнюю границу на количестве частиц, которые будут сгенерированы во время процесса передискретизации. Разрешение большего количества частиц быть сгенерированным может улучшить шанс схождения к истинному положению робота, но оказывает влияние на скорость вычисления, и частицы могут занять время или даже могут не сходиться. Обратитесь к разделу 'KL-D Sampling' в [1] для вычисления разумного связанного значения на количестве частиц. Обратите внимание на то, что глобальная локализация, возможно, нуждается в значительно большем количестве частиц по сравнению с локализацией с начальной оценкой положения. Если робот знает свое начальное положение с некоторой неуверенностью, такая дополнительная информация может помочь AMCL локализовать роботов быстрее с меньшим количеством количества частиц, т.е. можно использовать меньшее значение верхней границы в amcl.ParticleLimits.
Теперь установите amcl.GlobalLocalization на ложь и предоставьте предполагаемое начальное положение AMCL. Путем выполнения так, AMCL содержит начальную веру, что истинное положение робота следует за Распределением Гаусса со средним значением, равным amcl.InitialPose и ковариационной матрице, равной amcl.InitialCovariance. Начальная оценка положения должна быть получена согласно вашей настройке. Этот помощник в качестве примера получает текущее истинное положение робота из Gazebo.
Обратитесь к объекту раздела Configure AMCL для глобальной локализации для примера при использовании глобальной локализации.
amcl.ParticleLimits = [500 5000]; amcl.GlobalLocalization = false; amcl.InitialPose = ExampleHelperAMCLGazeboTruePose; amcl.InitialCovariance = eye(3)*0.5;
Setup ExampleHelperAMCLVisualization, чтобы построить карту и обновить предполагаемое положение робота, частицы и лазер сканирует показания на карте.
visualizationHelper = ExampleHelperAMCLVisualization(map);
Движение робота важно для алгоритма AMCL. В этом примере мы управляем TurtleBot случайным образом с помощью класса ExampleHelperAMCLWanderer, который управляет роботом в среде при предотвращении препятствий с помощью robotics.VectorFieldHistogram класса.
wanderHelper = ...
ExampleHelperAMCLWanderer(laserSub, sensorTransform, velPub, velMsg);Алгоритм AMCL обновляется с одометрией и показаниями датчика на каждом временном шаге, когда робот перемещается. Позвольте за несколько секунд до того, как частицы инициализированы и построены в фигуре. В этом примере мы запустим numUpdates обновления AMCL. Если робот не сходится к правильному положению робота, рассмотрите использование большего numUpdates.
numUpdates = 60; i = 0; while i < numUpdates % Receive laser scan and odometry message. scanMsg = receive(laserSub); odompose = odomSub.LatestMessage; % Create lidarScan object to pass to the AMCL object. scan = lidarScan(scanMsg); % For sensors that are mounted upside down, you need to reverse the % order of scan angle readings using 'flip' function. % Compute robot's pose [x,y,yaw] from odometry message. odomQuat = [odompose.Pose.Pose.Orientation.W, odompose.Pose.Pose.Orientation.X, ... odompose.Pose.Pose.Orientation.Y, odompose.Pose.Pose.Orientation.Z]; odomRotation = quat2eul(odomQuat); pose = [odompose.Pose.Pose.Position.X, odompose.Pose.Pose.Position.Y odomRotation(1)]; % Update estimated robot's pose and covariance using new odometry and % sensor readings. [isUpdated,estimatedPose, estimatedCovariance] = amcl(pose, scan); % Drive robot to next pose. wander(wanderHelper); % Plot the robot's estimated pose, particles and laser scans on the map. if isUpdated i = i + 1; plotStep(visualizationHelper, amcl, estimatedPose, scan, i) end end
stop(wanderHelper); rosshutdown
Shutting down global node /matlab_global_node_68982 with NodeURI http://192.168.203.1:55528/
AMCL является вероятностным алгоритмом, результат симуляции на вашем компьютере может немного отличаться от демонстрационного выполнения, показанного здесь.
После первого обновления AMCL частицы сгенерированы путем выборки Распределения Гаусса со средним значением, равным amcl.InitialPose и ковариации, равной amcl.InitialCovariance.
После 8 обновлений частицы начинают сходиться к областям с более высокой вероятностью:
После 60 обновлений все частицы должны сходиться к правильному положению робота, и лазерные сканирования должны тесно выровняться с основами карты.
В случае, если никакая начальная оценка положения робота не доступна, AMCL попытается локализовать робота, не зная исходного положения робота. Алгоритм первоначально принимает, что робот имеет равную вероятность в том, чтобы быть где угодно в свободном пространстве офиса и генерирует равномерно распределенные частицы в таком пробеле. Таким образом Глобальная локализация требует значительно большего количества частиц по сравнению с локализацией с начальной оценкой положения.
Чтобы активировать глобальную опцию локализации AMCL, замените секции кода в объекте Configure AMCL для локализации с начальной оценкой положения с кодом в этом разделе.
amcl.GlobalLocalization = true;
amcl.ParticleLimits = [500 50000];
AMCL является вероятностным алгоритмом, результат симуляции на вашем компьютере может немного отличаться от демонстрационного выполнения, показанного здесь.
После первого обновления AMCL частицы равномерно распределены в свободном офисе:
После 8 обновлений частицы начинают сходиться к областям с более высокой вероятностью:
После 60 обновлений все частицы должны сходиться к правильному положению робота, и лазерные сканирования должны тесно выровняться с основами карты.
robotics.MonteCarloLocalization, построение карты при известном местоположении.
[1] С. Трун, В. Бергард и Д. Фокс, вероятностная робототехника. Кембридж, MA: нажатие MIT, 2005.