Подключение к TurtleBot в беседке

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

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

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

Initializing global node /matlab_global_node_73841 with NodeURI http://192.168.2.1:53461/

Структура моделируемой офисной среды:

Загрузить карту мира моделирования

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

load officemap.mat
show(map)

Настройка модели лазерного датчика и модели движения 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','/base_scan');
sensorTransform = getTransform(tftree,'/base_link', '/base_scan');

% 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)];

Прием измерений датчиков и отправка команд скорости

Создайте абонентов АФК для извлечения измерений датчиков и одометрии из TurtleBot.

laserSub = rossubscriber('/scan');
odomSub = rossubscriber('/odom');

Создайте издатель ROS для отправки команд скорости в TurtleBot. TurtleBot подписывается на '/mobile_base/commands/velocity' для команд скорости.

[velPub,velMsg] = ...
    rospublisher('/cmd_vel','geometry_msgs/Twist');

Инициализация объекта AMCL

Создание экземпляра объекта AMCL amcl. Посмотрите monteCarloLocalization для получения дополнительной информации о классе.

amcl = monteCarloLocalization;
amcl.UseLidarScan = true;

Назначить MotionModel и SensorModel свойства в amcl объект.

amcl.MotionModel = odometryModel;
amcl.SensorModel = rangeFinderModel;

Фильтр частиц обновляет частицы только тогда, когда движение робота превышает UpdateThresholds, которая определяет минимальное смещение в [x, y, yaw] для запуска обновления фильтра. Это предотвращает слишком частые обновления из-за шума датчика. Повторная выборка частиц происходит после amcl.ResamplingInterval обновления фильтра. Использование больших чисел приводит к более медленному истощению частиц по цене и более медленной конвергенции частиц.

amcl.UpdateThresholds = [0.2,0.2,0.2];
amcl.ResamplingInterval = 1;

Настройка объекта AMCL для локализации с начальной оценкой позы.

amcl.ParticleLimits определяет нижнюю и верхнюю границы количества частиц, которые будут генерироваться в процессе повторной дискретизации. Разрешение генерировать больше частиц может улучшить вероятность сходимости к истинной позе робота, но оказывает влияние на скорость вычислений, и частицы могут занимать больше времени или даже не сходиться. См. раздел «Выборка KL-D» в [1] для расчета разумного связанного значения количества частиц. Следует отметить, что глобальная локализация может потребовать значительно больше частиц по сравнению с локализацией с первоначальной оценкой позы. Если робот знает свою исходную позу с некоторой неопределенностью, такая дополнительная информация может помочь AMCL быстрее локализовать роботов с меньшим количеством частиц, то есть можно использовать меньшее значение верхней границы в amcl.ParticleLimits.

Теперь установлено amcl.GlobalLocalization ложным и предоставить оценочную начальную позу AMCL. Таким образом, AMCL придерживается первоначального мнения, что истинная поза робота следует гауссову распределению со средним равным amcl.InitialPose и ковариационная матрица, равная amcl.InitialCovariance. Первоначальная оценка позы должна быть получена в соответствии с настройкой. В этом примере помощник извлекает текущую истинную позу робота из Беседки.

Пример использования глобальной локализации см. в разделе Настройка объекта AMCL для глобальной локализации.

amcl.ParticleLimits = [500 5000];
amcl.GlobalLocalization = false;
amcl.InitialPose = ExampleHelperAMCLGazeboTruePose;
amcl.InitialCovariance = eye(3)*0.5;

Помощник по настройке для визуализации и управления TurtleBot.

Настройка 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 и остановка АФК в MATLAB

stop(wanderHelper);
rosshutdown

Shutting down global node /matlab_global_node_73841 with NodeURI http://192.168.2.1:53461/

Примеры результатов локализации AMCL с первоначальной оценкой позы

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

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

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

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

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

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

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

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

Примеры результатов глобальной локализации AMCL

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

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

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

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

Ссылки

[1] С. Трюн, В. Бургард и Д. Фокс, вероятностная робототехника. Кембридж, Массачусетс: MIT Press, 2005.