В этом примере показано, как создать карту среды с помощью показаний датчика области значений, и робот позирует роботу с дифференциальным приводом. Вы создаете карту из показаний датчика области значений, которые симулированы с помощью 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