主要内容

动态重新规划一个室内地图

打开生活的脚本

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

创建仓库地图

定义一个binaryOccupancyMap包含仓库的平面图。这些信息将被用于创建一个初始路线从仓库入口包皮卡。

地图= 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,结束)= 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:最终,[20:21 32:33 44:45 56:57 68:69 80:81])= 1; setOccupancy(map, occ) figure show(map) title(“仓库平面图”)

计划路线皮卡

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

%用传感器读数创建地图将被更新estMap = occupancyMap (occupancyMatrix(地图));%创建一个映射,将膨胀的估计计划inflateMap = occupancyMap (estMap);vMap = validatorOccupancyMap;vMap。地图= inflateMap;vMap。ValidationDistance = 1;规划师= plannerHybridAStar (vMap,“MinTurningRadius”4);入口= 40 0 [1];packagePickupLocation =[63 44 -π/ 2];路线=计划(规划师,入口,packagePickupLocation);路线= route.States;%获得路线的姿势。rsConn = reedsSheppConnection (“MinTurningRadius”,planner.MinTurningRadius);startPoses =路线(1:end-1,:);endPoses =路线(2:,:);rsPathSegs =连接(rsConn startPoses endPoses);提出了= [];为i = 1:元素个数(rsPathSegs)长度= 0:0.1:rsPathSegs{我}. length;(姿势,~)=插入(rsPathSegs{},长度);提出了=[姿势;构成);结束图显示(计划)标题(“初始路线方案”)

地方一个障碍的路线

添加一个障碍地图在叉车将路由到包中。

obstacleWidth = 6;obstacleHeight = 11;obstacleBottomLeftLocation = [34 49];值= 1 (obstacleHeight obstacleWidth);setOccupancy(地图,obstacleBottomLeftLocation,值)图显示(map)标题(“仓库平面图与障碍)

指定测距仪

创建测距仪使用rangeSensor对象。

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

更新基于测距仪读数的路线

向前推进叉车的使用提出了路径规划。从测距仪获取新的障碍检测并将它们插入到估计地图。如果路线现在占领地图更新,重新计算路线。重复,直到达到目标。

%安装可视化。helperViz = HelperUtils;图显示(inflateMap);持有在robotPatch = helperViz。:plotRobot (gca、姿势(1));rangesLine = helperViz。:plotScan (gca、姿势(1),…南(numReadings 1)的(numReadings,1));axesColors =得到(gca,“ColorOrder”);routeLine = helperViz。plotRoute (gca,路线,axesColors (2:));traveledLine =情节(gca,南南);标题(“叉车路线方案”)举行从idx = 1;抽搐;而idx < =大小(姿势,1)%测距仪读数插入估计地图。(范围、角度)=测距仪(姿势(idx:),地图);insertRay (estMap姿势(idx:),范围,角度,…rangefinder.Range(结束));%重置和再膨胀规划地图setOccupancy (inflateMap getOccupancy (estMap));充气(inflateMap 1“网格”);%更新可视化。显示(inflateMap“FastUpdate”,真正的);helperViz。updateWorldMap (robotPatch rangesLine traveledLine,…姿势(idx:),范围,drawnow角度)%再生路线时发现在当前的一个障碍。isRouteOccupied =任何(checkOccupancy (inflateMap姿势(:1:2)));如果isRouteOccupied & & (toc > 0.1)%计算新路线。路线=计划(规划师,姿势(idx:), packagePickupLocation);路线= route.States;%获得路线的姿势。startPoses =路线(1:end-1,:);endPoses =路线(2:,:);rsPathSegs =连接(rsConn startPoses endPoses);提出了= [];为i = 1:元素个数(rsPathSegs)长度= 0:0.1:rsPathSegs{我}. length;(姿势,~)=插入(rsPathSegs{},长度);提出了=[姿势;构成);% #好< AGROW >结束routeLine = helperViz。updateRoute (routeLine、路线);idx = 1;抽搐;其他的idx = idx + 1;结束结束