屋内マップでの動的再計画
この例では、距離計と A* パス プランナーを使用して、倉庫のマップで動的な再計画を行う方法を説明します。
倉庫のマップの作成
倉庫のフロア プランを含む binaryOccupancyMap
を定義します。この情報は、倉庫の入口から荷物のピックアップ場所までの初期ルートを作成するために使用します。
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')
ピックアップ用ルートの計画
plannerHybridAStar
を作成し、先に作成しておいたフロア プランを使用して初期ルートを生成します。
% 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')
ルートへの障害物の配置
マップで、フォークリフトが荷物を取りに行くルート上に障害物を追加します。
obstacleWidth = 6;
obstacleHeight = 11;
obstacleBottomLeftLocation = [34 49];
values = ones(obstacleHeight, obstacleWidth);
setOccupancy(map, obstacleBottomLeftLocation, values)
figure
show(map)
title('Warehouse Floor Plan with Obstacle')
距離計の指定
rangeSensor
オブジェクトを使用して距離計を作成します。
rangefinder = rangeSensor('HorizontalAngle', pi/3);
numReadings = rangefinder.NumReadings;
距離計の読み取り値に基づいたルートの更新
パス プランナーからの姿勢を使用してフォークリフトを前進させます。距離計から新規の障害物検出を取得して、それらを推定マップに挿入します。更新後のマップでルートが占有されている場合は、ルートを再計算します。ゴールに到達するまで繰り返します。
% 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