Документация

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

Во-первых, породите моделируемый 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)

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

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

Интерфейс для получения измерений датчика от TurtleBot и отправки скоростных команд к TurtleBot.

Создайте подписчиков 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. Смотрите 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 для локализации с начальной оценкой положения.

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, который управляет роботом в среде при предотвращении препятствий с помощью 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

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

stop(wanderHelper);
rosshutdown

Shutting down global node /matlab_global_node_68982 with NodeURI http://192.168.203.1:55528/

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Смотрите также

robotics.MonteCarloLocalization, построение карты при известном местоположении.

Ссылки

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