Diese Seite wurde mithilfe maschineller Übersetzung übersetzt. Klicken Sie hier, um die neueste Version auf Englisch zu sehen.

Dynamische Neuplanung auf einer Innenraumkarte

Dieses Beispiel zeigt, wie eine dynamische Neuplanung auf einer Lagerkarte mit einem Entfernungsmesser und einem A*-Pfadplaner durchgeführt wird.

Lagerkarte erstellen

Definieren Sie ein binaryOccupancyMap, das den Grundriss des Lagers enthält. Anhand dieser Informationen wird eine erste Route vom Lagereingang zur Paketabholung erstellt.

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')

Route zur Abholung planen

Erstellen Sie ein plannerHybridAStar und verwenden Sie den zuvor erstellten Grundriss, um eine erste Route zu generieren.

% 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')

Platzieren Sie ein Hindernis auf der Route

Fügen Sie der Karte ein Hindernis hinzu, das auf der Route liegt, die der Gabelstapler zum Paket nehmen wird.

obstacleWidth = 6;
obstacleHeight = 11;
obstacleBottomLeftLocation = [34 49];
values = ones(obstacleHeight, obstacleWidth);
setOccupancy(map, obstacleBottomLeftLocation, values)

figure 
show(map)
title('Warehouse Floor Plan with Obstacle')

Geben Sie den Entfernungsmesser an

Erstellen Sie den Entfernungsmesser mit dem Objekt rangeSensor.

rangefinder = rangeSensor('HorizontalAngle', pi/3);
numReadings = rangefinder.NumReadings;

Aktualisieren Sie die Route anhand der Messwerte des Entfernungsmessers

Bewegen Sie den Gabelstapler vorwärts, indem Sie die Posen aus dem Pfadplaner verwenden. Holen Sie sich die neuen Hinderniserkennungen vom Entfernungsmesser und fügen Sie sie in die geschätzte Karte ein. Wenn die Route in der aktualisierten Karte nun belegt ist, berechnen Sie die Route neu. Wiederholen, bis das Ziel erreicht ist.

% 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