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

打开直播脚本

这个例子展示了如何使用一个距离查找器和一个a *路径规划器在一个仓库地图上执行动态重新规划。

创建仓库地图

定义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'，规划师.MinturningRadius）;startPoses =路线(1:end-1,:);endPoses =路线(2:,:);rsPathSegs = connect(rsConn, startpose, endpose);提出了= [];为i = 1:numel(rsPathSegs)的长度= 0:0.1:rsPathSegs{i}.Length;[pose， ~] =插值(rsPathSegs{i}，长度);提出了=[姿势;构成);结束图显示(计划)标题(“打包的初始路线”)

在路线上放置障碍物

向叉车拍摄的路由上的地图中添加障碍物。

obstacleWidth = 6;obstacleHeight = 11;obstacleBottomLeftLocation = [34 49];values = ones(obstacleHeight, obstacleWidth);settooccupancy (map, obstacleBottomLeftLocation, values)“有障碍的仓库平面图”)

指定测距仪

使用该测距查找器范围传感器目的。

测距仪= rangeSensor ('横向'π/ 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，:)）;TraveledLine = Plot（GCA，NaN，NaN）;标题（“叉车到包装路线”） 抓住从Idx = 1;tic;而Idx <=大小（姿势，1）%在地图中插入测距仪读数。[ranges, angles] = rangefinder(pose (idx，:)， map);insertRay(estMap, pose (idx，:)， range, angle，…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)的长度= 0:0.1:rsPathSegs{i}.Length;[pose， ~] =插值(rsPathSegs{i}，长度);提出了=[姿势;构成);% #好< AGROW >结束Routeline = Helperviz.updateroute（路由，路线）;Idx = 1;tic;其他的Idx = Idx + 1;结束结束