Этот пример показывает, как создать карту окружения с помощью показаний датчика области значений и положений робота для робота с дифференциальным приводом. Вы создаете карту из показаний датчика области значений, которые моделируются с помощью rangeSensor
объект. The differentialDriveKinematics
модель движения моделирует перемещение робота по комнате на основе скорости команд. The 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
Установите путь как путевые точки чистого контроллера преследования.
controller.Waypoints = path;
Установите начальное положение и окончательное местоположение цели на основе пути. Создайте глобальные переменные для хранения текущего положения и индекс для отслеживания итераций.
initPose = [path(1,1) path(1,2), pi/2]; goal = [path(end,1) path(end,2)]'; poses(:,1) = initPose';
Используйте предоставленную функцию helper exampleHelperDiffDriveControl
. Функция helper содержит основной цикл для навигации по пути, получения показаний области значений и отображения окружения.
The exampleHelperDiffDriveControl
функция имеет следующий рабочий процесс:
Сканируйте карту ссылки с помощью датчика области значений и текущего положения. Это имитирует показания нормальной области значений для вождения в неизвестном окружении.
Обновите карту с учетом показаний области значений.
Получите команды управления от чистого контроллера преследования до следующей путевой точки.
Вычислите производную движения робота на основе команд управления.
Увеличьте положение робота на основе производной.
Вы должны увидеть, как робот движется вокруг пустой карты и заполняет стенки, когда датчик области значений обнаруживает их.
exampleHelperDiffDriveCtrl(diffDrive,controller,initPose,goal,refMap,map,refFigure,mapFigure,sensor)
Goal position reached
The exampleHelperDiffDriveControl
функция имеет следующий рабочий процесс:
Сканируйте карту ссылки с помощью датчика области значений и текущего положения. Это имитирует показания нормальной области значений для вождения в неизвестном окружении.
Обновите карту с учетом показаний области значений.
Получите команды управления от чистого контроллера преследования до следующей путевой точки.
Вычислите производную движения робота на основе команд управления.
Увеличьте положение робота на основе производной.
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