Связь с TurtleBot в Gazebo

Во-первых, создайте моделируемый TurtleBot в офисном окружении в виртуальной машине, выполнив шаги в Запуск with Gazebo и моделируемом 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/

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

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

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

load officemap.mat
show(map)

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

TurtleBot может быть смоделирован как робот с дифференциальным приводом, и его движение может быть оценено с помощью данных одометрии. The 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)];

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

Создайте абонентов ROS для извлечения измерений датчика и одометрии от 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, рыскание] для запуска обновления фильтра. Это препятствует слишком частым обновлениям из-за шума датчика. Повторная дискретизация частиц происходит после 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. Оценка начального положения должна быть получена в соответствии с вашими настройками. Этот пример помощника извлекает текущее истинное положение робота из Gazebo.

См. раздел Настройка объекта 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_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] S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics. Cambridge, MA: MIT Press, 2005.