Replanificación dinámica en un mapa interior
Este ejemplo muestra cómo realizar una replanificación dinámica en un mapa de almacén con un telémetro y un planificador de ruta A*.
Crear mapa de almacén
Defina un binaryOccupancyMap
que contenga el plano del almacén. Esta información se utilizará para crear una ruta inicial desde la entrada del almacén hasta la recogida del paquete.
map = binaryOccupancyMap(100, 80, 1);
occ = zeros(80, 100);
occ(1,:) = 1;
occ(end,:) = 1;
occ([1:30, 51:80],1) = 1;
occ([1:30, 51:80],end) = 1;
occ(40,20:80) = 1;
occ(28:52,[20:21 32:33 44:45 56:57 68:69 80:81]) = 1;
occ(1:12, [20:21 32:33 44:45 56:57 68:69 80:81]) = 1;
occ(end-12:end, [20:21 32:33 44:45 56:57 68:69 80:81]) = 1;
setOccupancy(map, occ)
figure
show(map)
title('Warehouse Floor Plan')
Planificar ruta de recogida
Cree un plannerHybridAStar
y utilice el plano de planta creado previamente para generar una ruta inicial.
% Create map that will be updated with sensor readings estMap = occupancyMap(occupancyMatrix(map)); % Create a map that will inflate the estimate for planning inflateMap = occupancyMap(estMap); vMap = validatorOccupancyMap; vMap.Map = inflateMap; vMap.ValidationDistance = .1; planner = plannerHybridAStar(vMap, 'MinTurningRadius', 4); entrance = [1 40 0]; packagePickupLocation = [63 44 -pi/2]; route = plan(planner, entrance, packagePickupLocation); route = route.States; % Get poses from the route. rsConn = reedsSheppConnection('MinTurningRadius', planner.MinTurningRadius); startPoses = route(1:end-1,:); endPoses = route(2:end,:); rsPathSegs = connect(rsConn, startPoses, endPoses); poses = []; for i = 1:numel(rsPathSegs) lengths = 0:0.1:rsPathSegs{i}.Length; [pose, ~] = interpolate(rsPathSegs{i}, lengths); poses = [poses; pose]; end figure show(planner) title('Initial Route to Package')
Colocar un obstáculo en la ruta
Agregue un obstáculo al mapa que se encuentra en la ruta que tomará el montacargas hacia el paquete.
obstacleWidth = 6;
obstacleHeight = 11;
obstacleBottomLeftLocation = [34 49];
values = ones(obstacleHeight, obstacleWidth);
setOccupancy(map, obstacleBottomLeftLocation, values)
figure
show(map)
title('Warehouse Floor Plan with Obstacle')
Especificar el buscador de rango
Cree el telémetro usando el objeto rangeSensor
.
rangefinder = rangeSensor('HorizontalAngle', pi/3);
numReadings = rangefinder.NumReadings;
Actualizar la ruta según las lecturas del telémetro
Avanza el montacargas hacia adelante usando las poses del planificador de rutas. Obtenga las nuevas detecciones de obstáculos del telémetro e insértelas en el mapa estimado. Si la ruta ahora está ocupada en el mapa actualizado, vuelva a calcular la ruta. Repita hasta alcanzar el objetivo.
% Setup visualization. helperViz = HelperUtils; figure show(inflateMap); hold on robotPatch = helperViz.plotRobot(gca, poses(1,:)); rangesLine = helperViz.plotScan(gca, poses(1,:), ... NaN(numReadings,1), ones(numReadings,1)); axesColors = get(gca, 'ColorOrder'); routeLine = helperViz.plotRoute(gca, route, axesColors(2,:)); traveledLine = plot(gca, NaN, NaN); title('Forklift Route to Package') hold off idx = 1; tic; while idx <= size(poses,1) % Insert range finder readings into estimated map. [ranges, angles] = rangefinder(poses(idx,:), map); insertRay(estMap, poses(idx,:), ranges, angles, ... rangefinder.Range(end)); % Reset and reinflate planning map setOccupancy(inflateMap, getOccupancy(estMap)); inflate(inflateMap,1,'grid'); % Update visualization. show(inflateMap, 'FastUpdate', true); helperViz.updateWorldMap(robotPatch, rangesLine, traveledLine, ... poses(idx,:), ranges, angles) drawnow % Regenerate route when obstacles are detected in the current one. isRouteOccupied = any(checkOccupancy(inflateMap, poses(:,1:2))); if isRouteOccupied && (toc > 0.1) % Calculate new route. route = plan(planner, poses(idx,:), packagePickupLocation); route = route.States; % Get poses from the route. startPoses = route(1:end-1,:); endPoses = route(2:end,:); rsPathSegs = connect(rsConn, startPoses, endPoses); poses = []; for i = 1:numel(rsPathSegs) lengths = 0:0.1:rsPathSegs{i}.Length; [pose, ~] = interpolate(rsPathSegs{i}, lengths); poses = [poses; pose]; %#ok<AGROW> end routeLine = helperViz.updateRoute(routeLine, route); idx = 1; tic; else idx = idx + 1; end end