Локализуйте TurtleBot Используя локализацию Монте-Карло

Этот пример использует:

Этот пример демонстрирует применение алгоритма Локализации Монте-Карло (MCL) на TurtleBot® в симулированной среде Gazebo®.

Локализация Монте-Карло (MCL) является алгоритмом, чтобы локализовать робота с помощью фильтра частиц. Алгоритм требует известной карты, и задача состоит в том, чтобы оценить положение (положение и ориентация) робота в рамках карты на основе движения и обнаружения робота. Алгоритм запускается с начальной веры вероятностного распределения положения робота, которое представлено частицами, распределенными согласно такой вере. Эти частицы распространены после модели движения робота каждый раз изменения положения робота. После получения новых показаний датчика каждая частица оценит свою точность путем проверки, с какой вероятностью это получило бы такие показания датчика в своем текущем положении. Затем алгоритм перераспределит (передискретизируют) частицы, чтобы сместить частицы, которые более точны. Продолжите выполнять итерации этих перемещение, обнаружение и передискретизация шагов, и все частицы должны сходиться к одному кластеру около истинного положения робота, если локализация успешна.

Адаптивная локализация Монте-Карло (AMCL) является вариантом MCL, реализованного в monteCarloLocalization. AMCL динамически настраивает количество частиц на основе KL-расстояния [1], чтобы гарантировать, что распределение частицы сходится к истинному распределению состояния робота на основе всего прошлого датчика и измерений движения с высокой вероятностью.

Текущая реализация MATLAB® AMCL может быть применена к любому роботу с дифференциальным приводом, оборудованному средством поиска области значений.

Симуляция Gazebo TurtleBot должна запускаться для этого примера, чтобы работать.

Предпосылки: Начало работы с Gazebo и Симулированным TurtleBot (ROS Toolbox), доступ к tf Дереву Преобразования в ROS (ROS Toolbox), обмениваются Данными с Издателями ROS и Подписчиками (ROS Toolbox).

Примечание: Начиная в R2016b, вместо того, чтобы использовать метод шага, чтобы выполнить операцию, заданную Системным объектом, можно вызвать объект с аргументами, как будто это была функция. Например, y = step(obj,x) и y = obj(x) выполните эквивалентные операции.

Соединитесь с TurtleBot в Gazebo

Во-первых, породите симулированный TurtleBot в офисной среде в виртуальной машине следующими шагами в Начало работы с Gazebo и Симулированным TurtleBot (ROS Toolbox), чтобы запустить Gazebo TurtleBot World с рабочего стола.

В вашем экземпляре MATLAB на хосте - компьютере запустите следующие команды, чтобы инициализировать глобальный узел ROS в MATLAB и соединить с ведущим устройством ROS в виртуальной машине через ее IP-адрес ipaddress. Замените ipaddress с IP-адресом вашего TurtleBot в виртуальной машине.

ipaddress = '192.168.233.133';
rosinit(ipaddress,11311);

Initializing global node /matlab_global_node_26847 with NodeURI http://192.168.233.1:64164/

Размещение симулированной офисной среды:

Загрузите карту мира симуляции

Загрузите бинарную сетку заполнения офисной среды в Gazebo. Карта сгенерирована путем управления TurtleBot в офисной среде. Карта создается с помощью переносящих область значений показаний из Kinect® и положений основной истины от gazebo/model_states тема.

load officemap.mat
show(map)

Setup лазерная модель движения модели и TurtleBot датчика

TurtleBot может быть смоделирован как робот с дифференциальным приводом, и его движение может быть оценено с помощью данных об одометрии. Noise свойство задает неопределенность во вращательном и линейном движении робота. Увеличение odometryModel.Noise свойство позволит более распространение при распространении частиц с помощью измерений одометрии. Обратитесь к odometryMotionModel для деталей свойства.

odometryModel = odometryMotionModel;
odometryModel.Noise = [0.2 0.2 0.2 0.2];

Датчик на TurtleBot является симулированным средством поиска области значений, преобразованным от показаний Kinect. Полевой метод вероятности используется для расчета вероятность восприятия набора измерений путем сравнения конечных точек измерений средства поиска области значений к карте заполнения. Если конечные точки совпадают с занятыми точками в карте заполнения, вероятности восприятия, что такие измерения высоки. Модель датчика должна быть настроена, чтобы совпадать с фактическим свойством датчика достигнуть лучших результатов испытаний. Свойство SensorLimits задает минимальную и максимальную область значений показаний датчика. Свойство Map задает используемое в вычислениях поле вероятности карты заполнения. Обратитесь к likelihoodFieldSensorModel для деталей свойства.

rangeFinderModel = likelihoodFieldSensorModel;
rangeFinderModel.SensorLimits = [0.45 8];
rangeFinderModel.Map = map;

Установите rangeFinderModel.SensorPose к координатному преобразованию фиксированной камеры относительно основы робота. Это используется, чтобы преобразовать лазерные показания от системы координат камеры до базовой системы координат TurtleBot. Обратитесь к доступу к tf Дереву Преобразования в ROS (ROS Toolbox) для получения дополнительной информации о координатных преобразованиях.

Обратите внимание на то, что в настоящее время 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 amcl. Смотрите monteCarloLocalization для получения дополнительной информации о классе.

amcl = 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 для локализации с начальной оценкой положения.

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 для визуализации и управления TurtleBot.

Setup ExampleHelperAMCLVisualization, чтобы построить карту и обновить предполагаемое положение робота, частицы и лазер сканирует показания на карте.

visualizationHelper = ExampleHelperAMCLVisualization(map);

Движение робота важно для алгоритма AMCL. В этом примере мы управляем TurtleBot случайным образом с помощью класса ExampleHelperAMCLWanderer, который управляет роботом в среде при предотвращении препятствий с помощью controllerVFH класс.

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

Остановите TurtleBot и завершите работу ROS в MATLAB

stop(wanderHelper);
rosshutdown

Shutting down global node /matlab_global_node_26847 with NodeURI http://192.168.233.1:64164/

Демонстрационные результаты для локализации AMCL с начальной оценкой положения

AMCL является вероятностным алгоритмом, результат симуляции на вашем компьютере может немного отличаться от демонстрационного запуска, показанного здесь.

После первого обновления AMCL частицы сгенерированы путем выборки Распределения Гаусса со средним значением, равным amcl.InitialPose и ковариация равняется amcl.InitialCovariance.

После 8 обновлений частицы начинают сходиться к областям с более высокой вероятностью:

После 60 обновлений все частицы должны сходиться к правильному положению робота, и лазерные сканирования должны тесно выровняться с основами карты.

Сконфигурируйте объект AMCL для глобальной локализации.

В случае, если никакая начальная оценка положения робота не доступна, AMCL попытается локализовать робота, не зная исходного положения робота. Алгоритм первоначально принимает, что робот имеет равную вероятность в том, чтобы быть где угодно в свободном пространстве офиса и генерирует равномерно распределенные частицы в таком пробеле. Таким образом Глобальная локализация требует значительно большего количества частиц по сравнению с локализацией с начальной оценкой положения.

Чтобы активировать глобальную опцию локализации AMCL, замените секции кода в объекте Configure AMCL для локализации с начальной оценкой положения с кодом в этом разделе.

     amcl.GlobalLocalization = true;
     amcl.ParticleLimits = [500 50000];

Демонстрационные результаты для глобальной локализации AMCL

После первого обновления AMCL частицы равномерно распределены в свободном офисе: