Построение карты при известном местоположении

В этом примере показано, как создать карту среды с помощью показаний датчика области значений, и робот позирует роботу с дифференциальным приводом. Вы создаете карту из показаний датчика области значений, которые симулированы с помощью rangeSensor объект. differentialDriveKinematics модель движения симулирует управление роботом вокруг комнаты на основе скоростных команд. rangeSensor дает показания области значений на основе положения робота, когда это следует за путем.

Ссылочная карта и фигуры

Загрузите набор двоичных сеток заполнения в качестве примера от exampleMaps, включая simpleMap, который использует этот пример.

load exampleMaps.mat

Создайте ссылочную бинарную карту заполнения с помощью simpleMap с разрешением 1. Покажите фигуре и сохраните указатель фигуры.

refMap = binaryOccupancyMap(simpleMap,1);
refFigure = figure('Name','SimpleMap');
show(refMap);

Создайте пустую карту тех же размерностей как выбранная карта с разрешением 10. Покажите фигуре и сохраните указатель фигуры. Заблокируйте оси в размере карты.

[mapdimx,mapdimy] = size(simpleMap);
map = binaryOccupancyMap(mapdimy,mapdimx,10);
mapFigure = figure('Name','Unknown Map');
show(map);

Инициализируйте модель движения и контроллер

Создайте дифференциальный диск кинематическая модель движения. Модель движения представляет движение симулированного робота с дифференциальным приводом. Эта модель берет левые и правые скорости колес или линейный и скорости вращения для заголовка робота. В данном примере используйте скорость транспортного средства и направляющийся уровень для VehicleInputs.

diffDrive = differentialDriveKinematics("VehicleInputs","VehicleSpeedHeadingRate");

Создайте чистый контроллер преследования. Этот контроллер генерирует скоростные входные параметры для симулированного робота, чтобы следовать за желаемым путем. Установите свою желаемую линейную скорость и максимальную скорость вращения, заданную в метрах в секунду и радианах в секунду соответственно.

controller = controllerPurePursuit('DesiredLinearVelocity',2,'MaxAngularVelocity',3);

Настройте датчик области значений

Создайте датчик с макс. областью значений 10 метров. Этот датчик симулирует показания области значений на основе данного положения и карты. Ссылочная карта используется с этим датчиком области значений, чтобы симулировать собирающиеся показания датчика в неизвестной среде.

sensor = rangeSensor;
sensor.Range = [0,10];

Создайте запланированный путь

Создайте путь, чтобы проехать карта для сбора показаний датчика области значений.

path = [4 6; 6.5 12.5; 4 22; 12 14; 22 22; 16 12; 20 10; 14 6; 22 3];

Постройте путь на ссылочной фигуре карты.

figure(refFigure);
hold on
plot(path(:,1),path(:,2), 'o-');
hold off

Установите путь как waypoints чистого контроллера преследования.

controller.Waypoints = path;

Следуйте за путем и сопоставьте среду

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

initPose = [path(1,1) path(1,2), pi/2];
goal = [path(end,1) path(end,2)]';
poses(:,1) = initPose';

Используйте обеспеченную функцию помощника exampleHelperDiffDriveControl. Функция помощника содержит основной цикл для навигации путь, получая показания области значений, и сопоставляя среду.

exampleHelperDiffDriveControl функция имеет следующий рабочий процесс:

  • Отсканируйте ссылочную карту с помощью датчика области значений и текущего положения. Это симулирует нормальные показания области значений для управления в неизвестной среде.

  • Обновите карту с показаниями области значений.

  • Заставьте команды управления от чистого диспетчера преследования управлять к следующему waypoint.

  • Вычислите производную движения робота на основе команд управления.

  • Постепенно увеличьте положение робота на основе производной.

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

exampleHelperDiffDriveCtrl(diffDrive,controller,initPose,goal,refMap,map,refFigure,mapFigure,sensor)

Goal position reached

Дифференциальная функция управления накопителями

exampleHelperDiffDriveControl функция имеет следующий рабочий процесс:

  • Отсканируйте ссылочную карту с помощью датчика области значений и текущего положения. Это симулирует нормальные показания области значений для управления в неизвестной среде.

  • Обновите карту с показаниями области значений.

  • Заставьте команды управления от чистого диспетчера преследования управлять к следующему waypoint.

  • Вычислите производную движения робота на основе команд управления.

  • Постепенно увеличьте положение робота на основе производной.

function exampleHelperDiffDriveControl(diffDrive,ppControl,initPose,goal,map1,map2,fig1,fig2,lidar)
sampleTime = 0.05;             % Sample time [s]
t = 0:sampleTime:100;         % Time array
poses = zeros(3,numel(t));    % Pose matrix
poses(:,1) = initPose';

% Set iteration rate
r = rateControl(1/sampleTime);

% Get the axes from the figures
ax1 = fig1.CurrentAxes;
ax2 = fig2.CurrentAxes;

    for idx = 1:numel(t)
        position = poses(:,idx)';
        currPose = position(1:2);
        
        % End if pathfollowing is vehicle has reached goal position within tolerance of 0.2m
        dist = norm(goal'-currPose);
        if (dist < .2)
            disp("Goal position reached")
            break;
        end
        
        % Update map by taking sensor measurements
        figure(2)
        [ranges, angles] = lidar(position, map1);
        scan = lidarScan(ranges,angles);
        validScan = removeInvalidData(scan,'RangeLimits',[0,lidar.Range(2)]);
        insertRay(map2,position,validScan,lidar.Range(2));
        show(map2);
        
        % Run the Pure Pursuit controller and convert output to wheel speeds
        [vRef,wRef] = ppControl(poses(:,idx));
    
        % Perform forward discrete integration step
        vel = derivative(diffDrive, poses(:,idx), [vRef wRef]);
        poses(:,idx+1) = poses(:,idx) + vel*sampleTime; 
    
    
        % Update visualization
        plotTrvec = [poses(1:2, idx+1); 0];
        plotRot = axang2quat([0 0 1 poses(3, idx+1)]);
        
        % Delete image of the last robot to prevent displaying multiple robots
        if idx > 1
           items = get(ax1, 'Children');
           delete(items(1)); 
        end
    
        %plot robot onto known map
        plotTransforms(plotTrvec', plotRot, 'MeshFilePath', 'groundvehicle.stl', 'View', '2D', 'FrameSize', 1, 'Parent', ax1);
        %plot robot on new map
        plotTransforms(plotTrvec', plotRot, 'MeshFilePath', 'groundvehicle.stl', 'View', '2D', 'FrameSize', 1, 'Parent', ax2);
    
        % waiting to iterate at the proper rate
        waitfor(r);
    end
end
Для просмотра документации необходимо авторизоваться на сайте