主要内容

室内地图上的动态再规划

打开生活的脚本

此示例展示了如何使用范围查找器和a *路径规划器在仓库映射上执行动态重规划。

创建仓库地图

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

map = binaryoccuancymap (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(地图));制作一张地图，为计划增加估计。inflateMap = occupancyMap (estMap);vMap = validatorOccupancyMap;vMap。地图= inflateMap;vMap。ValidationDistance = 1;规划师= plannerHybridAStar (vMap,“MinTurningRadius”4);入口= [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， ~] = interpolate(rsPathSegs{i}， length);提出了=[姿势;构成);结束图显示(计划)标题(“到包裹的初始路线”）

在路线上设置障碍

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

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

指定测距仪

方法创建测距器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 = plot(gca, NaN, NaN);标题(“叉车搬运包装路线”)举行从idx = 1;抽搐;而idx < =大小(姿势,1)将测距仪读数插入估计地图。[ranges，角度]= rangefinder(pose (idx，:)， map);insertRay(estMap，姿势(idx，:)，范围，角度，.．.rangefinder.Range(结束));重置和重新充气计划地图setOccupancy (inflateMap getOccupancy (estMap));充气(inflateMap 1“网格”）;%更新可视化。显示(inflateMap“FastUpdate”,真正的);helperViz。updateWorldMap (robotPatch rangesLine traveledLine,.．.姿势(idx，:)，范围，角度)现在绘制当在当前路径中检测到障碍物时，重新生成路径。。isRouteOccupied = any(checkOccupancy(inflateMap, pose (:，1:2)));如果isRouteOccupied && (toc > 0.1)计算新的路由。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， ~] = interpolate(rsPathSegs{i}， length);提出了=[姿势;构成);% #好< AGROW >结束routeLine = helperViz。updateRoute (routeLine、路线);idx = 1;抽搐;其他的Idx = Idx + 1;结束结束

Baidu
map