室内地图上的动态重新规划

这个示例展示了如何使用测距仪和a *路径规划器对仓库地图执行动态重新规划。

创建仓库地图

定义一个binaryOccupancyMap里面有仓库的平面图。此信息将用于创建从仓库入口到包裹提取的初始路线。

map = binaryOccupancyMap(100, 80, 1);Occ = 0 (80, 100);occ (1) = 1;occ (,) = 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(“仓库平面图”）

计划接车路线

创建一个plannerHybridAStar然后使用之前创建的平面图来生成初始路线。

estMap = occupancyMap (occupancyMatrix(地图));vMap = validatorOccupancyMap;vMap。地图= estMap;规划师= plannerHybridAStar (vMap,“MinTurningRadius”2);入口= [1 40 0];packagePickupLocation = [63 44 -pi/2];route = plan(planner，入口，packagePickupLocation);路线= route.States;从路线上获得姿势。rsConn = reedsSheppConnection (“MinTurningRadius”, planner.MinTurningRadius);startPoses =路线(1:end-1,:);endPoses =路线(2:,:);rsPathSegs = connect(rsConn, startpose, endpose);提出了= [];为i = 1:numel(rsPathSegs) length = 0:0.1:rsPathSegs{i}.Length;[pose， ~] =插值(rsPathSegs{i}，长度);提出了=[姿势;构成);结束图显示(计划)标题(“到达包裹的初步路线”）

在路线上设置障碍

在叉车到达包裹的路线上的地图上添加一个障碍物。

obstacleWidth = 6;obstacleHeight = 11;[34 49];values = ones(obstacleHeight, obstacleWidth);setooccuancy (map, obstlebottomleftlocation, values) figure show(map) title(“有障碍的仓库平面图”）

指定测距仪

创建测距仪使用rangeSensor对象。

测距仪= rangeSensor (“HorizontalAngle”π/ 2);numReadings = rangefinder.NumReadings;

根据测距仪读数更新路线

使用路径规划器给出的姿势将叉车向前推进。从测距仪获得新的障碍物检测，并将它们插入估计的地图中。如果更新后的地图中该路线已被占用，请重新计算该路线。重复，直到达到目标。

%安装可视化。helperViz = HelperUtils;图显示(estMap)在robotPatch = helperViz。: plotRobot (gca、姿势(1));rangesLine = helperViz。: plotScan (gca、姿势(1),．..南(numReadings 1)的(numReadings,1));axesColors =得到(gca,“ColorOrder”）;routeLine = helperViz。plotRoute (gca,路线,axesColors (2:));旅行线= plot(gca, NaN, NaN);标题(“前往包裹的叉车路线”)举行从idx = 1;抽搐;而idx < =大小(姿势,1)将测距仪读数插入地图。[range, angle] =测距仪(pose (idx，:)， map);insertRay(estMap, pose (idx，:)，范围，角度，．..rangefinder.Range(结束));%更新可视化。显示(estMap“FastUpdate”,真正的);helperViz。updateWorldMap (robotPatch rangesLine traveledLine,．..姿势(idx，:)，范围，角度)绘制%当当前路径中检测到障碍物时重新生成路径。isRouteOccupied = any(checkOccupancy(estMap, pose (:，1:2)));如果isRouteOccupied && (toc > 0.5)%计算新路由。planner.StateValidator.Map = estMap;route = plan(planner, pose (idx，:)， packagePickupLocation);路线= route.States;从路线上获得姿势。startPoses =路线(1:end-1,:);endPoses =路线(2:,:);rsPathSegs = connect(rsConn, startpose, endpose);提出了= [];为i = 1:numel(rsPathSegs) length = 0:0.1:rsPathSegs{i}.Length;[pose， ~] =插值(rsPathSegs{i}，长度);提出了=[姿势;构成);% #好< AGROW >结束routeLine = helperViz。updateRoute (routeLine、路线);idx = 1;抽搐;其他的Idx = Idx + 1;结束结束